关于卡尔曼滤波器的数学推导的学习笔记

卡尔曼滤波器学习笔记，包含推导过程中的具体计算。在学习理解过程中主要参考了以下两篇博客：

1）卡尔曼滤波 -- 从推导到应用(一)

2）详解卡尔曼滤波原理

本笔记主要是为了方便自己理解与将来再回顾，文中若有不足之处，欢迎指正交流！

 

1、系统描述

离散系统状态方程为：

                                              

其中：U, X, Y 是向量，F, G, C 是矩阵。理想情况下，只要有初始值 X(0) 便可“精确地”推出任意 X(k)。

 

2、预测值

实际情况下，因为系统建模过程可能存在简化和近似以及存在外界干扰，根据上式求得的结果不可能是准确的。因此不妨定义预测值：

                                              

式中： 表示初始赋值的估计值或者上一时刻的估计值。因为最后得到的结果仍然只是接近真实值，所以这里使用“估计值”一词。

 

3、引入测量

为了获得更精确的值，需要引入额外信息，通常引入测量校正。测量值可用如下结果表示：

                                                            
式中，H 表示传感器增益，V(k) 表示测量误差。注意，这里  是真实值，  也是传感器的实际示数。

将预测值换算为传感器的等效示数：

                                                           

设：

                                      （利用了模型信息和测量信息）

则可推出：

                                                 
                                             

其中：

                                                      

所以目标可转化为求K(k)。

 

4、问题转化

 又可表式成：

                                                        
因为测量误差 V(k) 是随机变量，所以计算得到的  也是随机变量，我们希望  尽可能地接近 ，也就是说我们希望 相对于  的距离尽可能的小，这正好可以用方差来衡量。我们可以把  看作是  的期望，但不要求  真的是  的期望，从而引入协方差矩阵：

                                                            

协方差矩阵的迹（对角线元素的和）就是 X 向量中各元素的方差之和。

因此我们的目标变为：

                                                               求合适的K(k)，最小化矩阵  的迹。

 

5、算式推导

展开并化简 。

因此:

         

记：

                                                    

                                                   
则：

               

 的迹：

                                     

化简过程中用到的公式：

                                                              

对K求导并取0：

                                    

求导过程中用到的公式：

                                       

最后得到：

                                                                 

将 K 代回  的表达式，可以得到：

                                 

补充：关于的计算

因为

                                               

又

                                               

式中：W(k)是干扰。

则：

                                        

假设  和 W(k-1) 相互独立，且 E(W) = 0

 

这里指出，如果 W(k) 和 V(k) 服从高斯分布，则卡尔曼滤波器是针对该问题的最优滤波器。如果两者是均值为0，无关的白噪声，但不服从高斯分布，则卡尔曼滤波器是其估计的最优线性滤波器（一个非线性滤波器可能可以找到更优的解）。

则：

           

 

6、总结：卡尔曼滤波器的使用步骤

1）给定初始估计值  和  的初始值 

2）计算

                                                       

                                                               

其中Q为外界对系统的干扰的协方差矩阵，要求扰动的均值为0；F和G为系统状态方程参数矩阵，U为输入。

3）计算卡尔曼增益 K(k) 和  ， 

                                                         

                                                        

                                                             

其中 H 为传感器增益矩阵；R 为测量误差的协方差矩阵，要求测量误差的均值为0。

4）返回1）。

 

关于具体用例可参考：

卡尔曼滤波 -- 从推导到应用(二)