Kalman Filter 公式及推导

1.介绍

卡尔曼滤波的本质是参数化的贝叶斯模型，通过对下一时刻系统的初步状态估计（即状态的先验估计）以及测量得出的反馈相结合，最终得到该时刻较为准确的的状态估计（即状态的后验估计）。

其核心思想即为预测+测量反馈。

 Kalman fliter是线性滤波器

 

2.经典的5个公式：

状态预测

 

“小帽子”表示估计，“ - ”表示用上一时刻的量估计而来的，

F 是状态转移矩阵，表示怎么由上一状态量怎么推测当前时刻状态；

B是控制矩阵，表示控制量u怎么作用当前状态。

所有推测都包含噪声，所有噪声都假定为高斯白噪声，噪声越大不确定性越大，用协方差矩阵来表示不确定性。

主对角线上表示两个维度的方差，反对角线上表示两维度的协方差kalman filter中所有不确定性的描述都要用协方差矩阵。

噪声协方差矩阵的传递

每一时刻的协方差用P表示，协方差矩阵在每个时刻间传递

             

协方差矩阵Q表示P预测时的不确定性。

备注1：噪声协方差矩阵的推导

协方差矩阵的性质：

观察矩阵

                       

v表示测量误差（噪声），观测噪声的协方差矩阵：R

测量矩阵H可以表示多种测量方式，这也体现了kalman filter的数据融合的特性

状态更新

                          

括号里表示实际测量值与预测值之间的残差，为什么预测值要乘上观测矩阵H？因为让测量值和观测值维度一致，可以相减。

K为卡尔曼增益

           

定性分析:

1.权衡预测协方差P和观测协方差R的大小来决定相信预测模型多一点还是相信观测模型多一点

2.把残差的表现形式从观察域变到状态域（即让残差的维度与前者相同，可以相加）

噪声协方差矩阵的更新

            

这个值是留给下一次迭代用的

备注2：卡尔曼增益和协方差矩阵推导：

测量值与估计值之间的误差为：

由公式（２）（３）可得

     

此处有个小错误，应该是-K，结果是对的。

协方差矩阵为：

    

因为为向量，所以要加转置。

同理可得：

      

所以对展开得到：

     

因为协方差矩阵的对角线元素就是方差，而方差表示的就是噪声的能量，所以求的方差最小，则噪声也越小，所以求迹tr

    

对未知量K求导为零的值，则是方差和最小的对应的K

   

上式中的P越大，则K越大，从实际上P越大表示噪声越大估计值越不准确，所以需要测量值来修正，则K需越大

带入式中的：

  

公式（1）（2）表示预测，公式（3）（4）（5）用来更新

3.例子：小车位置与速度

    

参考：1.卡尔曼滤波器的原理以及在matlab中的实现

           2.http://blog.youkuaiyun.com/u012211419/article/details/49821393

转载请注明出处