卡尔曼滤波器使用介绍

卡尔曼滤波的五个公式分为时间更新方程以及测量更新方程。时间更新方程根据前一时刻的状态估计值推算当前时刻的状态变量的先验估计值和误差协方差先验估计值；测量更新方程将先验估计和测量值结合起来进行测量校正。

系统的状态方程：

系统的观测方程：

五个公式：

• 时间更新方程：

                                                                

                                                                  

• 测量更新方程：

                                                       

                                                            

                                                                    

 

参数意义介绍：

方程1：为k-1时刻的后验状态估计，为k-1时刻输入，A状态转移矩阵，B输入矩阵，  为k时刻的先验状态估计值。

方程2：A状态转移矩阵，为k-1时刻后验估计协方差矩阵，Q状态转移协方差矩阵（难以确定），为k时刻先验估计协方差

方程3：为k时刻先验估计协方差，H测量转换阵，R为测量噪声协方差阵（一般可确定），为中间量卡尔曼系数

方程4： 为k时刻的先验状态估计值，卡尔曼系数，实际测量值，H测量转换阵，  为k时刻的先验状态估计值

方程5：I单位阵，卡尔曼系数，H测量转换阵，为k时刻先验估计协方差

 

卡尔曼滤波使用

场景介绍：开发一台无人机，装备有GPS系统用于获取位置={x,y,z}T,装备有其它传感器获得速度={vx,vy,vz}T，控制量为加速度a={ax,ay,az}T。

因此我们可以得到系统的状态方程为：

                                                                

                                                                            

因此可以写成状态方程，得到A与B：

                                         

其中，

                                        

写出测量方程，得到H（只想知道无人机在哪里）：

                                                                       

 

表征状态方程中~的协方差阵，用来表征状态转移矩阵与实际的误差，由于我们无法直接测量到过程信号，所以难以取得Q的值，我们这里采用拍脑袋方法：

                        

表征测量方程中~的协方差阵，一般可以测量得到，不过我们继续拍脑袋：

 

                                   

目前为止，该有的数据都有了，我们就可以愉快地套公式了：

对于方程1：

假设一开始无人机没有初始速度，位置在原点，则，然后这时候给了加速度

我们就可以计算出。

对于方程2：

为k-1时刻后验估计协方差矩阵，我们设初始值为：

                                                     

我们就计算出了。

对于方程3：

都是已知的量，直接求出了

对于方程4：

根据实际测量值,直接求出

对于方程5：

都是已知的量，更新一下

便完成了一个周期的卡尔曼滤波使用啦！

 

主要参考资料：

[1] https://www.jianshu.com/p/f6ce8943560c?from=singlemessage

[2] https://blog.youkuaiyun.com/wccsu1994/article/details/84643221