初学卡尔曼滤波

由于研究需要，最近在看卡尔曼滤波，做个小总结。
最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Kолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。
首先通过一个例子了解什么是卡尔曼滤波：

例1

这个例子来自于百度百科，或者也可以参考下面的这个博客，其中给出了更详细的解答：

https://blog.csdn.net/baidu_38172402/article/details/82289998

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方差(covariance)来判断。因为Kg=5^2/(52+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：23+0.61*(25-23)=24.22度。可以看出，因为温度计的协方差(covariance)比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*5²⁾0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波器就不断的把协方差(covariance)递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的协方差(covariance)。上面的Kg，就是卡尔曼增益（Kalman Gain）。
。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。
如果只是想了解卡尔曼滤波是什么东西的话差不多到这里就可以了，如果还想要深入理解的话可以继续往下看：

例2

这个例子来源于一篇知乎的文章，给出了一个更接近于实际的分析：

https://zhuanlan.zhihu.com/p/77327349

首先卡尔曼滤波要解决的问题是什么？我以我军发射一枚导弹攻击敌方某固定位置目标为例，导弹需要每隔一秒开雷达测量下离目标的距离。然后由于雷达有误差，所以需要融合自己上个时刻的位置、速度等信息来更准确的确定当前时刻离目标的距离。这就是卡尔曼滤波需要解决的事。

从直观理解卡尔曼滤波是怎么解决这个问题的呢？

首先导弹已知“当前这秒雷达测量的导弹离目标的距离（我们称它为观测值，比如雷达直接测量导弹离目标距离7m）”，“上个时刻导弹离目标距离”和“导弹自己当前时刻的速度”这三个数据。而根据“上个时刻导弹离目标距离”和“导弹自己当前时刻的速度”可以估算出当前导弹离目标的距离（我们称它为估计值）。比如：上一秒离目标10m，速度是4m/s，那么现在这秒估计就离目标距离是6m。这个速度数据可以从传感器里面读取也可以从发动机那获得。
那么问题来了，导弹离目标的距离现在既有个观测值7m，又有个估计值6m。到底相信哪个？单纯相信观测值万一雷达被敌方干扰了呢？单纯相信估计值那么万一上个时刻的距离估计值或者速度不准呢？所以，我们要根据观测值和估计值的准确度来得到最终导弹离目标的距离估计值。准确度高的就最终结果比重高，准确度低就占比低。如果雷达测量的那个7m准确度是49%，根据速度估计出的那个6m准确度是1%，那么最终的距离估计结果就是

卡尔曼滤波怎么做的？

我们先回顾总结下直观理解中是怎么做的：
1、根据上一秒导弹的位置 和 导弹的的速度估计出当前时刻导弹的位置粗略估计值。
2、将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。
在前面我们也提到了导弹的位置和雷达测量值都是有误差的。所以卡尔曼想用概率来衡量数据的可信度。
比如：雷达测量的数据它就不只是一个数字了。而是说测量发现导弹有0.8的概率在7m那个位置，有0.1的概率在7.2m那个位置。有0.1的概率在6.9m那个位置。这些数据就叫做概率分布。概率分布的意思就是很多个值还有它们各自出现的概率多大所组成的数据就叫做概率分布。
卡尔曼认为导弹速度、导弹位置、雷达测距的测量值这些都服从正态分布
我们知道正态分布可以这么表示 N(均值，方差)。
现在我们是已知：

在前面我们知道了卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差，需要实施“两步走”战略：
1、根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值。
2、将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

我们来看看具体怎么做的：
1、计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动），大家可以对比着看下直观理解是怎么做的：

2、根据 粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。现在我们是用概率分布来计算。所以和直观理解中的计算方式有不同。因为直观理解中的计算方式是相当于粗略估计值这个概率分布的均值与雷达测量值的概率分布的均值进行加权和得到精确估计值的概率分布的均值。由于正态分布是均值那个地方的概率最大，所以当前时刻导弹位置的精确估计值就是它概率分布的均值。但是现在我们还是没有回答怎么根据粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。其实这个也很简单。直接把这两个概率分布相乘即可。这是由贝叶斯滤波所推导得到的。所以我们得到当前时刻导弹位置的精确估计是 ：

。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。
如果仔细看这两个例子其实会发现它们其实是有点区别的，主要在于计算协方差以及卡尔曼增益上面。对于两个正态分布的总的协方差，温度的例子中的计算其实是有问题的，如果你去推导一下会发现它的计算结果永远都更加偏向于观测值，而实际上卡尔曼滤波应该既有可能偏向于观测值也可能偏向于预测值，当测量协方差趋向于零时结果会偏向于测量值，当预测协方差偏向于零时结果会偏向于预测值。下面的链接视频中通过一个观测器很好的阐述了卡尔曼滤波的原理以及卡尔曼增益与测量协方差以及预测协方差直接的关系：

https://www.bilibili.com/video/BV1D441167Qz/?spm_id_from=333.788.videocard.0

所以第一个例子更多的给与一个很直观的理解什么是卡尔曼滤波以及它的工作原理，第二个例子算是一个比较合理的应用。对于正态分布的均值方差概念可以参考：

https://www.cnblogs.com/DemonHunter/p/10740124.html

另外下面的这篇文章对于卡尔曼滤波讲的其实也很好，有兴趣的也可以看一下：

https://blog.csdn.net/u012411498/article/details/82887417

以及它的英文版：

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

这里面关于协方差的计算跟例二是相同的，只是这里没有使用具体的数值而是以公式的形式表达了。
顺便放两个关于贝叶斯滤波的链接：

https://blog.csdn.net/varyshare/article/details/97642209

https://blog.csdn.net/wq1psa78/article/details/105849353

注意：卡尔曼滤波的前提是它认为系统都是线性的，如果状态转移函数是线性的，那么在经历线性变换之后，分布保持其高斯特性；如果状态转移函数是非线性的，那么在经历线性变换之后，那么得到的状态分布可能不是高斯分布。因此，卡尔曼滤波算法可能会不收敛。在这种情况下，可以使用扩张卡尔曼滤波算法（EKF），它把非线性函数在当前估算状态的平均值附近进行线性化，每一个时间步执行线性化，然后将得到的雅克比矩阵用于预测和更新卡尔曼滤波算法。当系统是非线性，并且可以通过线性化很好的近似时，那么扩展卡尔曼滤波算法是状态估算的一个很好的选择。然而，它有以下的缺点：
1、由于复杂的导数，可能难以解析计算雅克比矩阵；
2、而以数值方式计算它们可能需要很高的计算成本；
3、扩展卡尔曼滤波算法不适用于具有不连续模型的系统，因为系统不可微分时雅克比矩阵不存在
4、高度非线性系统的线性化效果不好。
为了解决这些问题，后面提出了无迹卡尔曼滤波算法（UKF），它并不像扩展卡尔曼那样近似非线性函数，无迹卡尔曼近似于概率分布。