kalman滤波理解二：预测和更新过程

这篇主要讲kalman滤波的预测和更新过程，首相强调以下上篇（kalman滤波理解一：理论框架）所强调的连个理论原则：

• 预测过程符合全概率法则，是卷积过程，即采用概率分布相加；
• 感知过程符合贝叶斯法则，是乘积过程，即采用概率分布相乘；

---

（一）预测过程

假设有一辆小车在路上行驶，其状态有位置p，速度v，我们用一个列向量来表示此时的状态：

                                                                         

如果我问你，经过时间后，其位置和速度分别是多少？计算也很简单，我们假设该系统是无控制系统，即没有控制输入，保持匀速直线行驶：

                                                                       

表示成矩阵形式就是：

                                                                

即

                                                                         

有了该方程我们就可以预测下一时刻小车的位置和速度，同时，我们还要衡量状态的分布，kalman滤波中所有状态的分布都是高斯分布，分布的中心值就是状态值估计值，不确定性就是协方差矩阵，状态 的协方差矩阵为： 

                                                                

可得的协方差矩阵为

                                                                   

注意，在状态转移过程中，除了状态具有不确定性外，移动也具有不确定性，我们将该不确定性定义为过程噪声矩阵 ，该噪声源自于外部干扰，如小车打滑等。

由于预测过程符合全概率法则，总体的不确定度为过程不确定度的和，即：

                                                            

此时，状态和协方差都得到了预测，下面开始状态更新

（二）状态观测更新过程（感知过程）

状态观测的物理意义就是我们的状态不是所有都可以观测到的，比如上面例子中状态X包含距离和速度两个量，如果我么的传感器只能观测距离，那速度就无法观测，另外可能在观测中还有尺度上的变化，因此观测结果和状态量之间需要一个转换。将该过程表述为：

                                                              

其观测的过程协方差为：

                                                          

该协方差来源于状态本身的协方差P，另外观测过程也具有不确定度，该不确定度来自于传感器的误差，我们将该不确定度称为观测误差协方差矩阵R ，其均值就是读取的数据，该矩阵的维度和Z中的量的个数一致。 

目前，我们得到了两个分布，分别是观测状态的分布和观测过程的分布，分别为：

观测状态分布：

观测过程分布：

这两个分布符合贝叶斯法则，结合（kalman滤波理解一：理论框架）中提到的多维贝叶斯融合分布结果：

                                                

 我们可得，

                                              

将上面第二个方程左乘，将第三个方程左乘，右乘，可得：

                                                

其中，

                                           

上面三个式子得到的就是状态观测更新过程的方程，经过该方程得到的状态X就是状态的最优估计。

（三）kalman滤波总结

经过上面的过程描述，kalman滤波最终落到操作上只有5各公式，分别是预测的两个公式和观测更新的三个公式：

预测：

                                                 

更新：

                                               

使用时按照下面图示过程即可：

                          

为了公式看起来简洁，将公式中的上标和下标都略去了，使用时注意以下两点：