使用松组合算法实现误差状态方程建立

9.4.2.1　状态方程的建立

将MEMS惯性导航系统输出的导航参数误差作为组合导航的状态量,分别为纬度、经度、高度的误差值,东北天坐标系下的速度误差值和失准角,以及陀螺仪和加速度计的漂移量：

根据惯性导航系统的误差模型建立相应的状态方程为

式中,

过程噪声向量W＝[ωg3×3ωa3×3],其中ωg3×3、ωa3×3分别为陀螺仪和加速度计的白噪声,可假设为零均值的高斯白噪声。

由于采用上述求解矩阵微分方程的方法列出的卡尔曼滤波方程是连续型的,在计算机上难以仿真,因此在工程中主要应用离散型滤波方程,应用式(4.78)对卡尔曼连续型状态方程进行离散化。

9.4.2.2　观测方程的建立

设载体真实位置为(Lr,λr,hr),卫星接收机输出的位置信息为(LG,λG,hG),则存在如下关系：

式中,Ne,Nn,Nu——卫星解算量在东、北、天方向的位置误差。

惯导解算位置为

则取惯导解算和卫星解算的位置值之差为其中一组观测值,即

设载体的真实速度为(Vre,Vrn,Vru),接收机输出的速度信息为(VGe,VGn,VGu),则存在如下关系：

式中,Me,Mn,Mu——接收机在东向、北向和天向的测速误差。

惯导解算速度信息为

则取惯导解算与卫星解算的速度信息的差值作为另外一组观测信息,即(www.xing528.com)

则观测方程为

式中,

9.4.2.3　仿真及结果

根据上述所建模型,以某型高速旋转稳定弹为例,利用GNSS模拟数据和惯导解算模拟数据进行仿真验证,结果如图9.7～图9.11所示。图中,GNSS输出结果分别为接收机输出东北天位置或速度数据,捷联解算结果分别为MIMU 捷联解算出的东北天位置或速度,滤波修正结果是经过卡尔曼滤波修正后的东北天位置或速度,GNSS数据误差为GNSS输出数据与对应参数真实值之差,滤波估计误差是经卡尔曼滤波修正后的捷联解算结果与真实值之差。从图9.7可以看出,离散卡尔曼滤波可以校正位置捷联解算误差,大幅提高东北天位置输出结果的精度,使位置误差小于GNSS输出数据误差,实际位置误差不大于5 m。从图9.8可以看出,离散卡尔曼滤波可以校正捷联解算速度误差,大幅提高东北天速度输出精度,使速度误差小于GNSS输出数据误差,实际速度误差不大于0.2 m/s。

图9.7　东、北、天位置滤波结果(书后附彩插)

(a)东向位置滤波结果；(b)东向位置滤波误差；(c)北向位置滤波结果；(d)北向位置滤波误差；(e)天向位置滤波结果；(f)天向位置滤波误差

旋转稳定弹的俯仰、偏航和滚转姿态初始对准结果如图9.9～图9.11所示,其中,捷联解算结果为捷联解算出的姿态数据,姿态参考值为仿真所用弹道的无误差姿态,滤波修正结果是经过卡尔曼滤波修正后的姿态数据,捷联解算误差是捷联解算姿态与姿态参考值之差,滤波估计误差是经卡尔曼滤波修正后的捷联解算姿态与姿态参考值之差。可知,可以通过离散卡尔曼滤波来估计捷联解算姿态误差,经误差修正后的姿态数据的误差不大于±2°。

由姿态滤波结果可以看出,系统工作过程中模型不准或模型变化都会导致离散卡尔曼滤波的结果发散或精度下降。另外,计算机字长可能导致在计算过程中出现较大舍入误差,进而导致方差阵不对称而引起滤波发散。再者,若传感器故障或外部条件改变而出现数据突变,则会对滤波器的收敛性产生严重影响,甚至导致发散。总之,利用离散卡尔曼滤波进行旋转稳定GNSS/MIMU组合系统进行初始对准,对准误差较大甚至可能发散。

图9.8　东、北、天速度滤波结果(书后附彩插)

(a)东向速度滤波结果；(b)东向速度滤波误差；(c)北向速度滤波结果；(d)北向速度滤波误差；(e)天向速度滤波结果；(f)天向速度滤波误差

图9.9　俯仰角滤波结果(书后附彩插)

(a)俯仰角滤波结果；(b)俯仰角滤波误差

图9.10　滚转角滤波结果(书后附彩插)

(a)滚转角滤波结果；(b)滚转角滤波误差

图9.11　偏航角滤波结果(书后附彩插)

(a)偏航角滤波结果；(b)偏航角滤波误差