基于平方根序贯处理的两阶段卡尔曼滤波的优化方案

两阶段卡尔曼滤波分为无偏滤波器和偏差滤波器，针对无偏滤波器容易因计算误差导致滤波发散的不足，用基于序贯处理的平方根滤波替代无偏滤波，结合偏差滤波器就构成基于平方根序贯处理的两阶段卡尔曼滤波。该算法结合了两种滤波算法的优势，既可以降低计算量又可以提高滤波稳定性。为提高在线标定的实时性，将该改进算法应用于在线标定。

在3.4.2小节提出的标定路径的基础上，将改进的两阶段卡尔曼滤波算法用于在线标定，两阶段卡尔曼滤波的初值设置如下。

状态变量X、偏差量b的初值都为0向量。

初始状态协方差为

系统噪声协方差为

设定加速度计刻度系数误差为10-3，常值偏置为10-3*g m／s2，陀螺刻度系数误差为10-3，零偏为4*10-4rad／s，其滤波结果如图7-5和图7-6所示（直线为设定值，曲线为估计值）。

图7-5　利用改进滤波算法得到的刻度系数估计结果（书后附彩插）(www.xing528.com)

（a）X加计刻系误差；（b）Y加计刻系误差；（c）Z加计刻系误差；
（d）X陀螺刻系误差；（e）Y陀螺刻系误差；（f）Z陀螺刻系误差

图7-6　利用改进滤波算法得到的加计零偏和陀螺漂移的估计结果（书后附彩插）

（a）X加计零偏；（b）Y加计零偏

图7-6　利用改进滤波算法得到的加计零偏和陀螺漂移的估计结果（续）（书后附彩插）

（c）Z加计零偏；（d）X陀螺漂移；（e）Y陀螺漂移；（f）Z陀螺漂移

将不同滤波方式下各参数的收敛时间进行统计，如表7-3所示。

表7-3　滤波收敛时间对比表

从表7-3可以看出，改进的滤波算法使X加计刻度系数误差、Z加计刻度系数误差、Z加计零偏和Y陀螺零偏的收敛速度得到提高，同时根据式（7-48）和式（7-49）可得扩展卡尔曼滤波的计算量为69 873，两阶段卡尔曼滤波的计算量为36 681。可见滤波计算量大大降低，提高在线标定的实时性。从而验证了改进算法的正确性。