CN107621264B - 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法 - Google Patents

Abstract

本发明公开车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法。该方法在当组合导航系统状态估计误差参数与量测噪声参数未知或时变时，在进行状态估计的同时，根据量测输出与状态信息实时地对量测噪声方差阵与状态噪声方差阵进行更新。利用指数渐消记忆加权平均方法，渐消陈旧量测噪声与系统噪声的影响，同时针对噪声方差阵可能失去正定性的问题，引入序贯滤波的方法对其矩阵中对角线上的每个元素的大小进行限制。本发明计算量小，鲁棒性好，相比传统卡尔曼滤波，能获得车辆更高精度的位置信息、速度信息与姿态信息。

Description

车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法

技术领域

本发明属于智能交通车辆导航领域，具体涉及一种车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法。

背景技术

目前，GPS/INS组合导航已经获得了广泛的应用，尤其在军事领域，但组成INS(惯性导航系统)的惯导级惯性传感器普遍比较昂贵，限制了其在民间的应用。随着MEMS(微机电系统)技术的不断进步，低成本的组合导航系统逐步开始在民用领域开始应用。广泛应用于组合导航的卡尔曼滤波，只有在随机动态模型与结构参数与噪声参数准确已知的条件下，才能获得状态的最优估计。而在实际应用中，环境的变化与影响，会导致噪声参数是时变的，致使传统卡尔曼滤波器的精度降低，严重时还会引起滤波发散。

大量的研究表明，在低成本组合导航的实际使用当中，MINS中的传感器会受到周围环境的温度、电磁以及车辆振动的影响，GPS接收机会受到建筑物、云层以及电磁的干扰，导致系统噪声与量测噪声都发生变化。而低精度的MEMS传感器由于精度较低，误差较多，很难进行准确的误差建模。自适应滤波会在滤波过程中，利用实时的测量信息与状态估计，对量测噪声与系统噪声进行实时的更新与纠正，保证滤波的有效性，防止滤波发散。申请号201410129008.8，发明名称“机载惯性/卫星组合导航系统的自适应滤波方法与滤波器”公开了一种利用数据采样窗口，结合历史数据计算量测噪声与系统噪声的方法。但是可以看出，虽然此方法对两种噪声都进行了计算与更新，但此方法计算量偏大，需要存储一定的历史数据，并且没有保证系统噪声矩阵与量测噪声矩阵的正定性，有可能会出现矩阵失去正定性导致滤波发散的严重情况。

发明内容

本发明的目的在克服已有组合导航方案的不足，提出一种车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法。该方法不仅可以明显改善由于噪声统计信息不准确或发散导致传统卡尔曼滤波发散的情况，同时降低传统自适应滤波的计算量，保证噪声矩阵的正定性，有效提高组合导航的精度与鲁棒性。

本发明的目的至少通过如下技术方案之一实现。

车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法，包括以下步骤：

步骤一：建立组合导航系统的状态方程与量测方程

x_k＝Φ_k/k-1x_k-1+w_k-1

z_k＝H_kx_k+v_k

x_k为状态矩阵，z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵；w_k-1为系统高斯白噪声，Q_k-1为系统噪声方差；v_k测量高斯白噪声，量测噪声方差为R_k；

步骤二：采用一种基于噪声方差的的自适应参数估计方法进行噪声参数估计，其具体实施过程为：首先进行状态一步预测得到X_k/k-1＝Φ_k/k-1X_k-1以及Z_k/k-1＝H_kX_k/k-1；其次计算量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1以及调节参数β_Q,k、β_R,k，再次利用R_k与Q_k-1的递推更新公式进行量测噪声方差R_k与系统噪声方差Q_k-1的更新；然后利用序贯滤波，限制R_k与Q_k-1中对角元素的大小，保证R_k与Q_k-1的正定性，防止滤波发散；最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1代入到卡尔曼滤波器增益计算方程与状态更新方程中，完成对状态误差的估计，得到最优状态误差；

步骤三：采用松组合导航方法，利用前面得到的最优状态误差去校正微惯性导航系统的输出，获得车辆姿态角、速度与位置的最优值。

进一步地，所述步骤一中状态方程与测量方程具体为：

以姿态误差的微分、速度误差的微分、经纬度误差的微分、陀螺仪误差以及加速度计误差作为状态变量，利用GPS系统提供的位置信息与速度信息与微惯性导航提供的位置与速度信息的差值作为观测变量，

状态变量为

量测变量为

状态转移矩阵具体元素如下，其中第一个数字为行，第二个数字为列：

φ(1,2)＝w_U+v_EtanL/R_Nh φ(1,3)＝w_U+v_E/R_Nh

φ(2,1)＝-(w_U+v_EtanL/R_Nh) φ(2,3)＝-v_N/R_Mh

φ(3,1)＝w_N+v_E/R_Nh φ(3,2)＝v_N/R_Mh

φ(3,4)＝tanL/R_Nh φ(3,7)＝w_N+v_Esec²L/R_Nh

φ(4,5)＝2w_U+v_EtanL/R_Nh φ(4,6)＝-(2w_N+v_E/R_Nh)

φ(4,7)＝2(v_Nw_N+v_Uw_U)+v_Ev_Nsec²L/R_Nh

φ(5,5)＝-v_U/R_Mh φ(5,6)＝-v_N/R_Mh

φ(6,4)＝2(w_N+v_E/R_Nh) φ(6,5)＝v_N/R_Mh

φ(8,4)＝secL/R_Nh φ(8,7)＝v_EsecLtanL/R_Nh

量测系数矩阵为：

其中

分别代表俯仰角误差的微分、横滚角误差的微分、航向角误差的微分。

分别代表东向速度误差的微分、北向速度误差的微分、天向速度误差的微分。

分别代表经度误差的微分、纬度误差的微分、高度误差的微分。v_E v_N v_U代表东向速度、北向速度、天向速度，Lλh代表经度、纬度与高度；ε代表陀螺仪误差，

代表加速度计误差；R_Mh、R_Nh分别代表子午面、卯酉面地球半径；

为地球自转角速度；

代表载体系到地理坐标系的转换矩阵；f^b代表加速度计测得的比力信息。0_n×m代表n列m行的零矩阵，I_n×m代表n列m行的单位矩阵。

进一步地，所述步骤二中，对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时的自适应计算方法为：

首先计算状态一步预测：X_k/k-1＝Φ_k/k-1X_k-1，以及量测一步预测：Z_k/k-1＝H_kX_k/k-1，

然后得到量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1，

在噪声平稳的条件下，以时间指数渐消记忆加权平均方法代替集总平均，并转化为递推算法，噪声递推公式如下：

R_k＝(1-β_R,k)R_k-1+β_R,k(e_z,k/k-1e_z,k/k-1 ^T-H_kP_k/k-1H_k ^T)

Q_k-1＝(1-β_Q,k)Q_k-2+β_Q,k(K_ke_x,k/k-1K_k ^T+P_k-Φ_k/k-1P_k-1Φ_k/k-1 ^T)

以及参数递推公式：

初值β_R,0＝1β_Q,0＝1，渐消因子b₁与b₂分别取0.98、0.95；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差。利用以上计算方法可以对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新；

最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1带入到卡尔曼滤波器增益计算方程与状态更新方程中，增益计算方程为K_k＝(Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k-1)H_k ^T(H_k/k-1P_k-1H_k/k-1 ^T+R_k)，状态更新方程为：X_k＝X_k/k-1+K_k(Z_k-(H_kX_k/k-1+v_k))，得到最优状态误差。

进一步地，所述步骤二中，对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时，序贯滤波方法为：

标量量测方程为：

记为

利用上限条件

与下限条件

将测量噪声

限定在合理的范围内，具体如下：

Q_k-1计算如下：

在以上公式中，上标i代表的是相应矩阵中的第i行标量。X_k为状态矩阵，Z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵，Q_k-1为系统噪声方差；v_k为测量高斯白噪声，R_k为量测噪声方差；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差。R_max＝100×R₀，R_min＝0.01×R₀，Q_max＝100×Q₀，Q_min＝0.01×Q₀，R₀为量测噪声方差初值，Q₀为系统噪声方差初值。

进一步地，所述步骤三中利用前面得到的最优状态误差去校正微惯性导航系统输出的计算方法为:

姿态角修正att＝att_INS-φ_E，速度修正V＝V_INS-δV，位置修正P＝P_INS-δP其中att_INS、V_INS、P_INS为MINS解算出的姿态信息、速度信息、位置信息，φ_E、δV、δP为卡尔曼滤波估计出的误差最优值；att、V、P分别为修正后的姿态信息、速度信息、位置信息，将其作为输出。

与现有技术相比，本发明的有益效果：

1、本发明通过在自适应滤波中利用递归的方法实时的更新测量噪声方差R_k与系统噪声方差Q_k-1，同时采用指数渐消记忆加权平均方法淡化历史数据的影响，加大对于新近信息的利用，更能有效的计算出滤波器的量测噪声方差R_k与系统噪声方差Q_k-1。

2、本发明通过序贯滤波对测量噪声方差R_k与系统噪声方差Q_k-1的对角线上的元素单独进行计算，利用最大值与最小值对其进行约束，防止在计算过程中R_k与Q_k-1失去正定性，提高了组合导航的定位精度。

3、本发明的有益效果可以通过Matlab仿真实验进行验证，在MINS/GPS组合导航系统中仿真参数设置如下：

①陀螺仪与加速度计的噪声设置，因为是MEMS惯性器件，误差较大，陀螺仪的零漂为2deg/h，角度随机游走为3deg/sqrt(h)，加速度计零漂为2mg，速度随机游走为150ug/sqrt(Hz)。

②初始位置经纬度为113.3455°，纬度为23.1559°，海拔为39m，经纬度与高度误差分别为10m，10m，15m。初始速度都为0，误差为0.1m/s。初始姿态角都为0°，因为微惯性系统精度较低，所以初始横滚角与俯仰角误差都为1度，航向角误差为2度。MEMS惯性器件更新频率为100Hz，GPS更新频率为1Hz。

③系统噪声与量测噪声的理论值为

Q＝diag{(3deg/sqrt(h))²,3deg/sqrt(h))²,3deg/sqrt(h))²,(150ug/sqrt(Hz))²,

(150ug/sqrt(Hz))²,(150ug/sqrt(Hz))²,zeros(9,1)}

R＝diag{0.1,0.1,0.1,10,10,15}²

假设系统噪声与测量噪声未知，初始量测噪声方差R₀取0.5倍的R，初始系统噪声方差Q₀取0.5倍的Q，R_max＝100×R₀，R_min＝0.01×R₀，Q_max＝100×Q₀，Q_min＝0.01×Q₀。

P值的初始值为：

P₀＝diag{30/3600°,30/3600°,20/60°,0.1m/s,0.1m/s,0.1m/s,10m,10m,15m,

2deg/h,2deg/h,2deg/h,2mg,2mg,2mg}

仿真结果表明，在系统噪声统计信息未知或不准确的情况下，本发明提出的自适应滤波，利用指数渐消记忆滤波算法与序贯滤波，不断调整更新量测噪声方差R_k与系统噪声方差Q_k-1，相比于传统卡尔曼滤波，在没有增加计算量的条件下，有效提高了滤波精度，提高了系统的鲁棒性，提高了组合导航系统的精度。

附图说明

图1为本发明组合导航自适应卡尔曼滤波流程图。

图2为本发明组合导航松组合流程图。

图3为本发明与传统卡尔曼滤波对比，角度误差仿真图。

图4为本发明与传统卡尔曼滤波对比，速度误差仿真图。

图5为本发明与传统卡尔曼滤波对比，位置误差仿真图。

具体实施方式

以下结合附图和实例对本发明的具体实施作进一步说明，但本发明的实施和保护不限于此。

如图1，步骤一：以姿态误差、速度误差、经纬度误差、陀螺仪误差以及加速度计误差作为状态变量，利用GPS系统提供的位置信息与速度信息与微惯性导航提供的位置与速度信息的差值作为观测变量，建立组合导航系统的状态方程与量测方程

x_k＝Φ_k/k-1x_k-1+w_k-1

z_k＝H_kx_k+v_k

x_k为状态矩阵，z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测矩阵，w_k-1为系统高斯白噪声，系统噪声方差为Q_k-1，v_k测量高斯白噪声，量测噪声方差为R_k。

状态变量为

量测变量为

状态转移矩阵为(第一个数字为行，第二个数字为列)：

φ(1,2)＝w_U+v_EtanL/R_Nh φ(1,3)＝w_U+v_E/R_Nh

φ(2,1)＝-(w_U+v_EtanL/R_Nh) φ(2,3)＝-v_N/R_Mh

φ(3,1)＝w_N+v_E/R_Nh φ(3,2)＝v_N/R_Mh

φ(3,4)＝tanL/R_Nh φ(3,7)＝w_N+v_Esec²L/R_Nh

φ(4,5)＝2w_U+v_EtanL/R_Nh φ(4,6)＝-(2w_N+v_E/R_Nh)

φ(4,7)＝2(v_Nw_N+v_Uw_U)+v_Ev_Nsec²L/R_Nh

φ(5,5)＝-v_U/R_Mh φ(5,6)＝-v_N/R_Mh

φ(6,4)＝2(w_N+v_E/R_Nh) φ(6,5)＝v_N/R_Mh

φ(8,4)＝secL/R_Nh φ(8,7)＝v_EsecLtanL/R_Nh

量测系数矩阵为：

进行初始化车辆的初始位置、姿态角、速度信息，初始化状态转移矩阵与量测矩阵以及量测噪声方差R₀与系统噪声方差Q₀，最后初始化状态误差协方差P₀。加权系数初始值都为β₀＝1，渐消因子b₁与b₂分别取0.98、0.95。

步骤二：利用加速度计测得的比力信息与陀螺仪测得的角速度信息进行捷联解算，获得MINS计算出来的车辆的速度、姿态与位置信息。如果此时没有GPS信息，只进行时间更新。若有GPS车辆的位置信息与速度信息，联合微惯性系统获得的信息进行自适应卡尔曼滤波解算。

自适应卡尔曼滤波解算过程为：首先进行状态一步预测X_k/k-1＝Φ_k/k-1X_k-1，计算量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1，其次利用测量噪声方差R_k与系统噪声方差Q_k的递推公式：

R_k＝(1-β_R,k)R_k-1+β_R,k(e_z,k/k-1e_z,k/k-1 ^T-H_kP_k/k-1H_k ^T)

Q_k-1＝(1-β_Q,k)Q_k-2+β_Q,k(K_ke_x,k/k-1K_k ^T+P_k-Φ_k/k-1P_k-1Φ_k/k-1 ^T)

以及渐消因子更新公式：

对R_k、Q_k-1、β_Q,k+1、β_R,k+1进行更新。

再次利用序贯滤波对R_k、Q_k-1进行限制，保证其正定性。

最后将更新后的R_k、Q_k-1代入卡尔曼滤波方程进行解算，计算出最优误差估计，同时更新状态估计均方误差P_k，以供下一次使用

P_k/k-1＝Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k-1

K_k＝(Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k-1)H_k ^T(H_k/k-1P_k-1H_k/k-1 ^T+R_k-1)

X_k＝X_k/k-1+K_k(z_k-(H_kX_k/k-1))

P_k＝(I-K_kH_k)P_k/k-1

步骤三：获得状态估计最优值后，采用松组合组合导航方法(如图2)，利用最优值去校正微惯性导航系统的输出，获得最优的姿态角、速度与位置。

姿态角修正att＝att_INS-φ_E，速度修正V＝V_INS-δV，位置修正P＝P_INS-δP其中att_INS、V_INS、P_INS为MINS解算出的姿态、速度、位置信息，φ_E、δV、δP为卡尔曼滤波估计出的误差最优值，att、V、P为修正后的姿态信息，速度信息，位置信息信息，将其进行输出。

从图3，图4，图5可以看出，改进后的自适应卡尔曼滤波估计出的速度误差、姿态角误差，明显好于传统卡尔曼滤波。在位置误差上，传统卡尔曼滤波的纬度、经度、高度误差均值分别为4.29m、4.70m、2.92m，而自适应卡尔曼滤波相应的误差均值为3.30m、3.37m、1.87，纬度误差均值减少了30％，经度误差均值减少了28.2％，高度误差减少了35.9％。

Claims (2)

1.车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法，其特征在于，包括以下步骤：

步骤一：建立组合导航系统的状态方程与量测方程

x_k＝Φ_k/k-1x_k-1+w_k-1

z_k＝H_kx_k+v_k

x_k为状态矩阵，z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵；w_k-1为系统高斯白噪声，系统噪声方差为Q_k-1；v_k测量高斯白噪声，量测噪声方差为R_k；状态方程与测量方程具体为：

以姿态误差的微分、速度误差的微分、经纬度误差的微分、陀螺仪误差以及加速度计误差作为状态变量，利用GPS系统提供的位置信息与速度信息与微惯性导航提供的位置与速度信息的差值作为观测变量，

状态变量为

量测变量为

状态转移矩阵具体元素如下，其中第一个数字为行，第二个数字为列：

φ(1,2)＝w_U+v_EtanL/R_Nh φ(1,3)＝w_U+v_E/R_Nh

φ(1,5)＝-1/R_Mh

φ(2,1)＝-(w_U+v_EtanL/R_Nh) φ(2,3)＝-v_N/R_Mh

φ(2,4)＝1/R_Nh φ(2,7)＝-w_U

φ(3,1)＝w_N+v_E/R_Nh φ(3,2)＝v_N/R_Mh

φ(3,4)＝tanL/R_Nh φ(3,7)＝w_N+v_Esec²L/R_Nh

φ(4,4)＝(v_EtanL-v_U)/R_Nh

φ(4,5)＝2w_U+v_EtanL/R_Nh φ(4,6)＝-(2w_N+v_E/R_Nh)

φ(4,7)＝2(v_Nw_N+v_Uw_U)+v_Ev_Nsec²L/R_Nh

φ(5,4)＝-2(w_U+v_EtanL/R_Nh)

φ(5,5)＝-v_U/R_Mh φ(5,6)＝-v_N/R_Mh

φ(5,7)＝-v_E(2w_N+v_Esec²L/R_Nh)

φ(6,4)＝2(w_N+v_E/R_Nh) φ(6,5)＝v_N/R_Mh

φ(6,7)＝-2w_Uv_E

φ(7,5)＝1/R_Mh

φ(8,4)＝secL/R_Nh φ(8,7)＝v_EsecLtanL/R_Nh

φ(9,6)＝1

量测系数矩阵为：

其中

分别代表俯仰角误差的微分、横滚角误差的微分、航向角误差的微分，

分别代表东向速度误差的微分、北向速度误差的微分、天向速度误差的微分，

分别代表经度误差的微分、纬度误差的微分、高度误差的微分， ν_Eν_Nν_U代表东向速度、北向速度、天向速度，Lλh代表经度、纬度与高度；ε代表陀螺仪误差，

代表加速度计误差；R_Mh、R_Nh分别代表子午面、卯酉面地球半径；

为地球自转角速度；

代表载体系到地理坐标系的转换矩阵；f^b代表加速度计测得的比力信息， 0_n×m代表n列m行的零矩阵，I_n×m代表n列m行的单位矩阵；

步骤二：采用一种基于噪声方差的自适应参数估计方法进行噪声参数估计，其具体实施过程为：首先进行状态一步预测得到X_k/k-1＝Φ_k/k-1X_k-1以及Z_k/k-1＝H_kX_k/k-1；其次计算量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1以及调节参数β_Q,k、β_R,k，再次利用R_k与Q_k-1的递推更新公式进行测量噪声方差R_k与系统噪声方差Q_k-1的更新；然后利用序贯滤波，限制R_k与Q_k-1中对角元素的大小，保证R_k与Q_k-1的正定性，防止滤波发散；最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1代入到卡尔曼滤波器增益计算方程与状态更新方程中，完成对状态误差的估计，得到最优状态误差；对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时的自适应计算方法为：

首先计算状态一步预测：X_k/k-1＝Φ_k/k-1X_k-1，以及量测一步预测：Z_k/k-1＝H_kX_k/k-1

然后得到量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1，

在噪声平稳的条件下，以时间指数渐消记忆加权平均方法代替集总平均，并转化为递推算法，噪声递推公式如下：

R_k＝(1-β_R,k)R_k-1+β_R,k(e_z,k/k-1e_z,k/k-1 ^T-H_kP_k/k-1H_k ^T)

Q_k-1＝(1-β_Q,k)Q_k-2+β_Q,k(K_ke_y,k/k-1K_k ^T+P_k-Φ_k/k-1P_k-1Φ_k/k-1 ^T)

以及参数递推公式：

初值β_R,0β_Q,0＝1，渐消因子b₁与b₂分别取0.98、0.95；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差。利用以上计算方法可以对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新；

最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1带入到卡尔曼滤波器增益计算方程与状态更新方程中，增益计算方程为K_k＝(Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k-1)H_k ^T(H_k/k-1P_k-1H_k/k-1 ^T+R_k)，状态更新方程为：X_k＝X_k/k-1+K_k(Z_k-(H_kX_k/k-1+v_k))，得到最优状态误差；对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时，序贯滤波方法为：

标量量测方程为：

记为

利用上限条件

与下限条件

将测量噪声

限定在合理的范围内，具体如下：

Q_k-1计算如下：

在以上公式中，上标i代表的是相应矩阵中的第i行标量。X_k为状态矩阵，Z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵，Q_k-1为系统噪声方差；v_k为测量高斯白噪声，R_k为量测噪声方差；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差， R_max＝100×R₀，R_min＝0.01×R₀，Q_max＝100×Q₀，Q_min＝0.01×Q₀，R₀为量测噪声方差初值，Q₀为系统噪声方差初值；

步骤三：采用松组合导航方法，利用前面得到的最优状态误差去校正微惯性导航系统的输出，获得车辆姿态角、速度与位置的最优值。

2.如权利要求1要求的车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法，其特征在于,所述步骤三中利用前面得到的最优状态误差去校正微惯性导航系统输出的计算方法为:

姿态角修正att＝att_INS-φ_E，速度修正V＝V_INS-δV，位置修正P＝P_INS-δP其中att_INS、V_INS、P_INS为MINS解算出的姿态信息、速度信息、位置信息，φ_E、δV、δP为卡尔曼滤波估计出的误差最优值；att、V、P分别为修正后的姿态信息、速度信息、位置信息，将其作为输出。

Images