概率机器人（3）-卡尔曼滤波

       卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包含系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

       卡尔曼用矩参数表示置信度：在时间,置信度用均值和方差表达。除了贝叶斯滤波的马尔可夫假设之外，还具有其他的三个特征，则后验就是高斯的。马尔可夫假设设定过去和未来数据都是独立的，即当前时刻的状态至于上一时刻有关。

        1）状态转移概率必须是带有随机高斯噪声的参数的线性函数：

                                                                                                                                                             (1)

式1定义了状态转移概率。这个概率可由式1带入到多元正态分布。后验状态的均值由给定，方差由给定：

                                                       (2)

        2）测量概率也与带有高斯噪声的自变量呈线性关系：

                                                                                                                                                                              (3)

同样的测量概率的多元正态分布给定为：

                                                                                                    (4)

         3）初始置信度必须是正态分布的。用和表示置信度的均值和方差：

                                                                                                (5)

         卡尔曼滤波算法：KF表示均值为、方差为在时间t的置信度。KF的输入是时刻的置信度，其均值和方差，为了更新这些参数，KF需要控制向量和测量向量。