自动驾驶（一）---------KalmanFilter 原理

向我男神致敬：https://me.csdn.net/AdamShan

网上kalman filter 的文章漫天飞，字符乱用，看的晕得不行，例子又是万年不变的小车，温度

本文直接看公式，不举例子，看懂公式你自己举例子：

1.  状态预测方程：

                                                  

            B 是系统变量方程，Uk-1是状态变化

            还是举例子吧：是位置，上一时刻的位置，小车的速度，B 是时间间隔

             一般的方程要加上预测的误差：其中误差为方差Q，期望0的正态分布

           得到完整的状态预测方程：

 

2. 预测方程的协方差传递：

                                              

           计算的目的是描述预测方程的不确定性，这里就是这种不确定性的传递方程。

 

3 .观测方程和系数：

                    没什么好讲的吧，Vk是误差呀，呀 我这暴脾气......

              其中     R是观察方程的方差，

 

4.计算卡尔曼增益K，再然后得到当前估计值，

                                      

             公式推导我推不出来，这个增益是预测和观察的权重。就是最终的滤波结果。

5.最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。

                                 

               I为单位矩阵 是为了下一步的 

 

最后只有我能看懂，你要是也看懂了，算我佩服你。