传感器融合小结（3）：非线性卡尔曼滤波器

内容主要提取自 edX 平台上Chalmers的micromaster项目：Emerging Automotive Technologies: Sensor Fusion and Non-linear Filtering for Automotive Systems。

内容主要包括高斯滤波器，扩展卡尔曼滤波器（Extended KF），无迹卡尔曼滤波器（Unscented KF）和容积卡尔曼滤波器（Cubature KF）。

 

1 模型说明

模型包括系统动态模型（Motion Model）和观测模型（Measurement Model）：

                                                      

                                                      

其中  为状态变量， 为测量变量， 和  为独立噪声。

 

2 高斯滤波器

滤波器的通用形式为贝叶斯滤波器，采用递推算法，每次递推包括预测（prediction）和更新（update），即：

                            已知：

                            预测：

                            更新：

递推方式要求  对任意 k 都保持同一种概率分布，以便进行下一次递推。如果  服从高斯分布，则称为高斯滤波器。

对于非线性模型，我们近似预测和更新阶段结果均服从高斯分布。即：

                            已知：

                            预测：

                            更新：

高斯分布由均值和方差决定，每个阶段只需求解对应的均值和方差，其中更新后的均值即为所求对状态变量的估计。具体步骤如下：

预测（设  均值为零，方差为 ）：

                              

                                           

                   

                               

                               

更新（设  均值为零，方差为 ）：

假设  服从联合高斯分布：

                                        

其中：

                    

                                 

                                        

                                    

                                          

求得联合高斯分布后  ，可根据如下性质求  ，即：

                                              

                                                     

其中： 即为卡尔曼增益  。

观察可以发现，高斯滤波器的关键在于在预测步求  和 ，在更新步求 ， 和 。五者均具有以下形式：

                           ；其中  为非线性函数， 为高斯分布

不同的积分求解策略将形成不同的卡尔曼滤波器。

 

3 扩展卡尔曼滤波器（EKF）

扩展卡尔曼滤波器通过泰勒级数来线性化函数  来求解积分。

预测步：

                      

                                  

                                  

       

                  

                  

更新步：

                                    

                             

                                    

                                    

                               

                                    

扩展卡尔曼滤波器因为线性化了模型中的非线性函数，因此当模型非线性度较大时，误差较大。

 

4 无迹卡尔曼滤波器（UKF）和容积卡尔曼滤波器（CKF）

这两种卡尔曼滤波器采用类似高斯求积的方式，求解积分，即：

                                                

与高斯求积略有不同的是，这里  的影响被归入权重系数  中。

无迹卡尔曼滤波器按如下述方法取  和 。

预测步：

                                                 

                                                 

                                             

                                                       

其中  为状态向量  的维度； 表示  的第  列，； 为可调参数，当  服从高斯分布时，取  。

                               

                

                            

更新步：

                                        

                                       

                                   

                                               

                               

                                 

                                        

                            

                                 

容积卡尔曼滤波器则按下述方法取  和  ：

                                               

                                             

                                                    

其中  和   根据预测步和更新步分别取值。

无迹卡尔曼滤波计算 2n+1 个点；容积卡尔曼滤波计算 2n 个点，当状态向量维度增加时，点  的位置越远离均值 。