卡尔曼滤波原理总结

原博客

https://blog.csdn.net/u012554092/article/details/78290223

这篇博客图文结合，形象的介绍了卡尔曼滤波的基本原理，不过其中的数学公式推导较少，初学者或数学基础较少者可以容易理解一点，但是还是不能很好的懂得它的原理和应用。

以下是看看过一些别人的博客之后自己总结的

    卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

    数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。

    特点：在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态或真实信号。

    这种理论是在时间域上来表述的，基本概念：在线性系统的状态空间表示基础上，从输出和输入观测数据求系统状态的最优估计。这里所说的系统状态，是总结系统所有过去的输入和扰动对系统的作用的最小参数的集合，知道了系统的状态就能够与未来的输入与系统的扰动一起确定系统的整个行为。

 

    卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

       状态估计

        状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。

      状态量：

        受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。

下面介绍一个例子：

      假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是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/(5^2+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^2)^0.5=3.12。

这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

    就是这样，卡尔曼滤波器就不断的把协方差(covariance)递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的协方差(covariance)。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

接下来就是卡尔曼滤波的模型：

     首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

                X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

    到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的协方差还没更新。我们用P表示协方差(covariance)：

                P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。式子1、2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

    现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：

                X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg为卡尔曼增益(Kalman Gain)：

               Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

    到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的协方差：

               P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

    其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

公式推导：

简单推导：

       考虑一个SLAM 问题，它由一个运动方程：

                                  

和一个观测方程组成：

                                  

就把它当作一个线性系统吧（非线性系统请看下一讲扩展卡尔曼滤波），并且为了简化推导，忽略路标的下标j，并把路标y 并入到状态向量一起优化，那么运动方程就可以写为：

                                  

这一步在卡尔曼滤波中也称为预测 (predict)。

类似地，测量方程可以写为：

                                  

卡尔曼滤波就是预测 - 测量之间不断循环迭代。

 

示例推导：

用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。

状态向量为小车的位置和速度： ，而司机要是踩了刹车或者油门，小车就会具有一个加速度，。

假设t 和t-1 时刻之间的时间差为​ 。根据物理知识，有：

                            

写成矩阵形式就有：

                            

跟之前的运动方程对比，就知道

                            

上式就写为

  表示t-1 时刻卡尔曼滤波的状态估计；则表示中t-1 到t 时刻，预测更新所得的预测值。再利用运动模型对状态向量进行更新后，还要继续更新状态向量的协方差矩阵P，公式为：

                                                         

假设    为t 时刻下状态向量的真值（自然是永远未知的），由之前的现形运动方程（式(3)）给出，将式(3) 与式(9) 相减可得：

                                     

而

   

考虑到状态向量和噪声是不相关的，则，上式就可以简化为          

推导完毕。

 

 可以看到，经过预测更新，协方差矩阵P 变大了。这是态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。

 预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。

 如果得到了测量值，那么我们就可以对状态向量进行测量更新了，对应的公式为

           

其中，为卡尔曼增益。

（测量更新未能写出）

小结：

     卡尔曼滤波就是在给定初始值的情况下，由预测和测量不断迭代、更新状态向量。而预测就是一个“加法”：状态转换和运动预测叠加；测量则是简单的高斯分布相乘，中间引入了一个转换矩阵将测量值和状态向量映射在同一个代数空间中。