卡尔曼滤波

 

https://blog.csdn.net/heyijia0327/article/details/17487467

首先假设我们知道一个线性系统的状态差分方程为

                                                   

其中x是系统的状态向量，大小为n*1列。A为转换矩阵，大小为n*n。u为系统输入，大小为k*1。B是将输入转换为状态的矩阵，大小为n*k。随机变量w为系统噪声。注意这些矩阵的大小，它们与你实际编程密切相关。

       看一个具体的匀加速运动的实例。

       有一个匀加速运动的小车，它受到的合力为 ft , 由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式：

                                                                                           

该系统系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。控制输入变量为u，也就是加速度，于是有如下形式:

                                           

所以这个系统的状态的方程为：

                                         

这里对应的的矩阵A大小为 2*2 ,矩阵B大小为 2*1。 

      貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。回到最开始的那个笑话，如果那个完全凭预测的特种兵在某一步偏离了正确的路径，当他站在错误的路径上（而他自己以为是正确的）做下一步预测时，肯定走的路径也会错了，到最后越走越偏。

     既然如此，我们就引进反馈。从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。

 测量值当然是由系统状态变量映射出来的，方程形式如下：

                                        

注意Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。大小为m*n。随机变量v是测量噪声。

     同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。

      

这里总结下递推的过程，理一下思路：

首先要计算预测值、预测值和真实值之间误差协方差矩阵。

                    

有了这两个就能计算卡尔曼增益K，再然后得到估计值，

                     

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

                     

至此，卡尔曼滤波的理论推导到此结束。