CN103389506B - 一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法 - Google Patents

Abstract

本发明公开了一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法，其目的是改善由于***噪声统计先验信息未知或者时变情况下导致常规卡尔曼滤波发散的问题，并提高捷联惯性/北斗卫星组合导航***的定位精度。该方法通过引入关于新息协方差的衰减记忆平滑器，并基于该衰减平滑器对滤波器中的增益矩阵和***噪声统计协方差进行在线估计与修正，能够根据新近新息序列的变化自适应地调节增益矩阵，进而达到改善滤波精度的目的。本发明所设计的自适应滤波方法用于捷联惯性/北斗卫星组合导航***中，能够准确估计出***的状态，而且经过输出补偿后能够解算出更加精确的姿态、速度和位置信息。

Description

一种用于捷联惯性/北斗卫星组合导航***的自适应滤波 方法

技术领域

本发明属于导航***技术领域，涉及一种自适应卡尔曼滤波的在线设计方法，特别是在***噪声统计特性未知或者时变的情况下，可有效提高滤波器的鲁棒性和导航***定位精度的自适应滤波方法。

背景技术

卡尔曼滤波算法可以估计***的平台失准角以及陀螺的常值漂移等状态变量，因此在组合导航领域中获得了广泛的应用。但是常规卡尔曼滤波在工作时需要满足模型精确和噪声统计特性准确已知的要求，对于捷联惯性与北斗卫星组合导航***而言，***中不可避免的含有诸如陀螺随机漂移等干扰因素，此时常规卡尔曼滤波将失去最优估计特性，不仅使得滤波精度下降，甚至引起滤波发散。

为解决噪声统计信息未知或者时变导致滤波精度下降的问题，近年来学者们提出了许多自适应滤波算法，主要集中在关于新息序列的自适应卡尔曼滤波方面。文献“一种基于极大似然准则的自适应卡尔曼滤波算法，西北工业大学学报，2005，第23卷第4期”，基于极大似然估计准则得到了噪声估值器，可以对噪声统计信息进行实时估计。然而该方法的滤波结果会受到R和Q估计窗尺寸N的影响：N越小滤波器对***动力学模型的变化越敏感，但是N太小会使得滤波结果是有偏估计,而且可能导致滤波发散；N越大滤波结果的无偏性越好,但是此时又无法及时跟踪***模型的变化。因此，滤波效果会受到平滑窗口宽度的限制，难以保证较高的滤波精度，进而不能满足捷联惯性与北斗卫星组合导航***的高精度导航定位要求。

本发明提出一种自适应卡尔曼滤波器的在线设计方法，通过引入衰减记忆调节因子，提出一种关于新息协方差的衰减记忆平滑器，并基于该衰减平滑器对滤波器中的增益矩阵和***噪声统计协方差进行在线修正，使得滤波器能够根据新近的新息序列自适应的调节增益矩阵，进而提高捷联惯性/北斗卫星组合导航***的定位精度，具有实际的工程应用意义。

发明内容

为了克服现有技术中的缺陷，解决上述技术问题，本发明提供一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法，该方法可以明显改善由于噪声统计信息不准或者时变导致常规卡尔曼滤波发散的情况，有效提高捷联惯性/北斗卫星组合导航***的鲁棒性和定位精度，其技术方案如下：

步骤一：建立捷联惯导***的误差方程，选取状态变量，得到组合导航***的状态方程；并利用北斗卫星***与捷联惯导***所提供速度的差值作为量测变量，建立组合导航***的量测方程；

状态方程：X_k＝Φ_kX_k-1+Γ_kW_k

量测方程：Z_k＝H_kX_k+V_k

其中，状态变量δλ、δL表示滤波器估计的经纬度误差值，δv_E、δv_N表示滤波器估计的东北向速度误差值，和表示计算导航系与真实导航系坐标轴之间的失准角；Φ_k表示状态一步转移矩阵，Γ_k表示***噪声系数矩阵，W_k表示***噪声，Z_k表示量测信息，H_k表示量测系数矩阵，V_k表示量测噪声；

状态一步转移矩阵

***噪声系数阵

$\begin{array}{c}{\mathrm{\Γ}}_k=\\ =T\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& & 0& 0& 0& 0\\ 0& 0& C_b^n(1,1)& C_b^n(1,2)& 0& 0& 0\\ 0& 0& C_b^n(2,1)& C_b^n(2,2)& 0& 0& 0\\ 0& 0& 0& 0& C_b^n(1,1)& C_b^n(1,2)& C_b^n(1,3)\\ 0& 0& 0& 0& C_b^n(2,1)& C_b^n(2,2)& C_b^n(2,3)\\ 0& 0& 0& 0& C_b^n(3,1)& C_b^n(3,2)& C_b^n(3,3)\end{array}\right]\end{array}$

量测系数阵

$H_k=\left[\begin{array}{ccccccc}0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]$

其中，w_ie表示地球自转角速度，L表示载体所在纬度，g表示地球重力加速度，R表示地球半径，T表示滤波器数据处理周期，v_E、v_N分别表示载体相对地球的东北向速度，f_E、f_N表示加速度计测得的比力信息，表示载体坐标系到导航坐标系之间的转换矩阵；

步骤二：首先更新状态一步预测值及其均方误差，并利用k时刻量测信息Z_k和滤波器的一步预测值计算k时刻的新息序列ε_k；再确定当前时刻衰减记忆调节因子d_k，建立关于新息协方差矩阵的衰减记忆平滑器C_k；

步骤三：借助由步骤二得到的关于新息协方差的衰减记忆平滑器C_k，对k时刻滤波器的增益矩阵K_k进行修正，同时利用该衰减记忆平滑器对***噪声协方差阵Q_k进行在线估计与修正；

步骤四：将步骤二和步骤三中得到的滤波参数带入到自适应滤波器中，完成对导航参数误差的估计，并通过输出校正对捷联惯导***输出的导航参数进行补偿，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息；

步骤五：不断重复以上步骤二、三、四过程，利用自适应滤波方法对导航参数误差进行在线估计，并实时修正捷联惯导***输出的导航信息，直至捷联惯性/北斗卫星组合导航过程结束。

所述的方法，所述步骤二中，

所述的一步预测值 ${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k-1}{\hat{X}}_{k-1}$

所述的一步预测均方误差P_k,k-1： $P_{k,k-1}={\mathrm{\Φ}}_{k-1}{P}_{k-1}{\mathrm{\Φ}}_{k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

所述的k时刻新息序列ε_k：

所述的新息协方差衰减记忆平滑器C_k：

其中，d_k＝(1-b)/(1-b^k+1)，0.95<b<0.99，表示k时刻状态一步预测值，P_k,k-1表示k时刻一步预测均方误差，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻***噪声协方差，表示k-1时刻状态转移矩阵的转置，表示k-1时刻***噪声系数矩阵的转置，ε_k表示k时刻的新息序列，C_k和C_k-1分别是k和k-1时刻的新息协方差衰减记忆平滑值，且初值

所述的方法，所述步骤三的具体步骤为：

1)修正后k时刻的滤波增益矩阵的计算公式为

$K_k=P_{k,k-1}{H}_k^T{\left(C_k\right)}^{-1}$

其中，K_k表示k时刻的滤波增益矩阵，P_k,k-1表示k时刻滤波器的一步预测均方误差，表示k时刻量测矩阵的转置，(C_k)^-1表示新息协方差的衰减记忆平滑值的求逆运算；

2)与此同时，利用得到的新息协方差的衰减记忆平滑器对***噪声协方差阵进行在线估计与修正，那么k时刻***噪声估值器的表达式写作：

$Q_k=K_k{C}_k{K}_k^T$

其中，Q_k表示k时刻***噪声协方差的估计值，表示k时刻滤波增益矩阵的转置。

所述的方法，所述的步骤四，其具体方法为：

1)自适应滤波器对导航参数误差的估计值：

k时刻状态估计值： ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\mathrm{\&epsiv;}}_k$

k时刻估计均方误差：P_k＝(I-K_kH_k)P_k,k-1

其中，状态变量 表示状态估计值，表示k时刻状态一步预测值，K_k表示k时刻的滤波增益矩阵，ε_k表示k时刻的新息序列，H_k表示量测系数矩阵，P_k表示状态估计均方误差。

2)得到导航参数误差的估计值后，利用输出校正对捷联惯导***输出的导航参数进行补偿：

①位置修正：λ＝λ_SINS-δλ

L＝L_SINS-δL

其中，λ、L分别表示修正后的经纬度信息，λ_SINS、L_SINS分别表示捷联惯导***输出的经纬度信息，δλ、δL分别表示滤波器估计的经纬度误差值；

②速度修正：v_E＝v_SINS-E-δv_E

v_N＝v_SINS-N-δv_N

其中，v_E、v_N分别表示修正后的东北向速度信息，v_SINS-E、v_SINS-N分别表示捷联惯导***输出的东北向速度信息，δv_E、δv_N分别表示滤波器估计的东北向速度误差值；

③姿态修正：

a)计算导航坐标系与真实导航坐标系之间的转移矩阵

其中，和分别表示计算导航系与真实导航系坐标轴之间的失准角。

b)对捷联惯导***解算的姿态角矩阵进行修正；

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}$

且修正后的姿态角矩阵可以表示成矩阵的形式：

$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

c)计算补偿后的姿态角，再根据

可以求取航向角θ，横摇角γ，纵摇角的主值：

由于航向角θ，横摇角γ，纵摇角的定义域分别为：θ∈[0,2π]，γ∈[-π,+π]，那么它们对应的真值应为：

至此，得到的θ，γ和即为通过自适应卡尔曼滤波修正后的姿态角信息。

本发明的有益效果：

1.本发明通过对新近的新息协方差序列进行衰减记忆平滑，并对滤波器的增益矩阵进行修正，使得滤波增益矩阵能够根据新息序列的变化自动做出调整，提高了滤波器的鲁棒性。

2.本发明通过对新近的新息协方差序列进行衰减记忆平滑，并利用该衰减记忆平滑值对***噪声统计信息进行估计与修正，从而为滤波器提供更加准确的噪声统计信息，改善了滤波器的估计精度。

3.本发明通过引入衰减记忆调节因子，提出一种关于新息协方差的衰减记忆平滑器，并基于该衰减平滑器对滤波器中的增益矩阵和***噪声统计协方差进行在线修正，用于捷联惯性/北斗卫星组合导航***中，可以保证导航***可靠的完成组合导航过程，而且提高组合导航***定位精度和姿态精度。

4.本发明的有益效果可以通过Matlab仿真实验进行验证，在捷联惯性/北斗卫星组合导航***中仿真参数设置如下：

①假定对***噪声统计信息未知，设噪声方差理论值为Q，取初始***噪声方差为20Q；计算步长设为0.1s，仿真时间1200s；初始位置姿态：经度为126.67°，纬度为45.77°，初始速度(6m/s,8m/s,0),姿态角(0,0,5°)。

②自适应滤波器的初始参数设置如下：

$\hat{X}\left(0\right)=0_{7\&times;1}$

P(0)＝diag{(20m/Re)²,(20m/Re)²,(0.5m/s)²,(0.5m/s)²,(0.1°)²,(0.1°)²,(0.1°)²}

Q(0)＝20Q＝20′diag{(100ug)²,(100ug)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²}

R(0)＝diag{(0.5m/s)²,(0.5m/s)²}

仿真结果表明，在***噪声统计信息未知或者不准确的情况下，本发明提出的自适应滤波器通过对新息协方差序列进行衰减平滑处理，实时调节滤波增益矩阵，不仅使得滤波精度明显提高，鲁棒性得到显著提高；而且本发明提出的自适应滤波方法进一步提高了导航***的定位精度，具有更加广阔的工程应用前景。

附图说明

图1是本发明自适应滤波方法的工作流程图；

图2是本发明自适应滤波方法在捷联惯性/北斗卫星组合***方框图；

图3是本发明自适应滤波方法的姿态误差角仿真图；

图4是本发明自适应滤波方法的速度误差仿真图；

图5是本发明自适应滤波方法的位置误差仿真图；

具体实施方式

下面结合附图和具体实施例对本发明的技术方案作进一步详细地说明。

参照图1-图2，步骤一：建立捷联惯导***的误差方程；在捷联惯性/北斗卫星组合导航模式下，捷联惯导***作为主导航***建立***的状态方程，并选取经纬度误差、速度误差以及失准角作为状态变量；利用北斗卫星***与捷联惯导***所提供速度的差值作为量测变量，建立组合导航***的量测方程；

状态方程：X_k＝Φ_kX_k-1+Γ_kW_k

量测方程：Z_k＝H_kX_k+V_k

其中，状态变量δλ、δL表示滤波器估计的经纬度误差值，δv_E、δv_N表示滤波器估计的东北向速度误差值，和表示计算导航系与真实导航系坐标轴之间的失准角；Φ_k表示状态一步转移矩阵，Γ_k表示***噪声系数矩阵，W_k表示***噪声，Z_k表示量测信息，H_k表示量测系数矩阵，V_k表示量测噪声；

状态一步转移矩阵

***噪声系数阵

$=T\begin{array}{c}{\mathrm{\&Gamma;}}_k=\\ \left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& & 0& 0& 0& 0\\ 0& 0& C_b^n(1,1)& C_b^n(1,2)& 0& 0& 0\\ 0& 0& C_b^n(2,1)& C_b^n(2,2)& 0& 0& 0\\ 0& 0& 0& 0& C_b^n(1,1)& C_b^n(1,2)& C_b^n(1,3)\\ 0& 0& 0& 0& C_b^n(2,1)& C_b^n(2,2)& C_b^n(2,3)\\ 0& 0& 0& 0& C_b^n(3,1)& C_b^n(3,2)& C_b^n(3,3)\end{array}\right]\end{array}$

量测系数阵

$H_k=\left[\begin{array}{ccccccc}0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]$

其中，w_ie表示地球自转角速度，L表示载体所在纬度，g表示地球重力加速度，R表示地球半径，T表示滤波器数据处理周期，v_E、v_N分别表示载体相对地球的东北向速度，f_E、f_N表示加速度计测得的比力信息，表示载体坐标系到导航坐标系之间的转换矩阵。

步骤二：首先更新状态一步预测值及其均方误差，并利用k时刻量测信息Z_k和滤波器的一步预测值计算k时刻的新息序列ε_k；再确定当前时刻衰减记忆调节因子d_k，建立关于新息协方差矩阵的衰减记忆平滑器C_k；

一步预测值 ${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k-1}{\hat{X}}_{k-1}$

一步预测均方误差P_k,k-1： $P_{k,k-1}={\mathrm{\&Phi;}}_{k-1}{P}_{k-1}^T{\mathrm{\&Phi;}}_{k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T$

k时刻新息序列ε_k： ${\mathrm{\&epsiv;}}_k=Z_k-H_k{\hat{X}}_{k,k-1}$

新息协方差衰减记忆平滑器C_k：

其中，d_k＝(1-b)/(1-b^k+1)，0.95<b<0.99，表示k时刻状态一步预测值，P_k,k-1表示k时刻一步预测均方误差，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻***噪声协方差，表示k-1时刻状态转移矩阵的转置，表示k-1时刻***噪声系数矩阵的转置，ε_k表示k时刻的新息序列，C_k和C_k-1分别是k和k-1时刻的新息协方差衰减记忆平滑值，且初值

步骤三：借助由步骤二得到的关于新息协方差的衰减记忆平滑器C_k，对k时刻滤波器的增益矩阵K_k进行修正，同时利用该衰减记忆平滑器对***噪声协方差阵Q_k进行在线估计与修正；

1)修正后k时刻的滤波增益矩阵的计算公式为

$K_k=P_{k,k-1}{H}_k^T{\left(C_k\right)}^{-1}$

其中，K_k表示k时刻的滤波增益矩阵，P_k,k-1表示k时刻滤波器的一步预测均方误差，表示k时刻量测矩阵的转置，(C_k)^-1表示新息协方差的衰减记忆平滑值的求逆运算。

2)与此同时，利用得到的新息协方差的衰减记忆平滑器对***噪声协方差阵进行在线估计与修正，那么k时刻***噪声估值器的表达式写作：

$Q_k=K_k{C}_k{K}_k^T$

其中，Q_k表示k时刻***噪声协方差的估计值，表示k时刻滤波增益矩阵的转置。

步骤四：将步骤二和步骤三中得到的滤波参数带入到自适应滤波器中，完成对导航参数误差的估计，并通过输出校正对捷联惯导***输出的导航参数进行补偿，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息。

1)自适应滤波器对导航参数误差的估计值：

k时刻状态估计值： ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\mathrm{\&epsiv;}}_k$

k时刻估计均方误差：P_k＝(I-K_kH_k)P_k,k-1

其中，状态变量

2)得到导航参数误差的估计值后，利用输出校正对捷联惯导***输出的导航参数进行补偿：

①位置修正：λ＝λ_SINS-δλ

L＝L_SINS-δL

其中，λ、L分别表示修正后的经纬度信息，λ_SINS、L_SINS分别表示捷联惯导***输出的经纬度信息，δλ、δL分别表示滤波器估计的经纬度误差值。

②速度修正：v_E＝v_SINS-E-δv_E

v_N＝v_SINS-N-δv_N

其中，v_E、v_N分别表示修正后的东北向速度信息，v_SINS-E、v_SINS-N分别表示捷联惯导***输出的东北向速度信息，δv_E、δv_N分别表示滤波器估计的东北向速度误差值。

③姿态修正：

a)计算导航坐标系与真实导航坐标系之间的转移矩阵

其中，和分别表示计算导航系与真实导航系坐标轴之间的失准角。

b)对捷联惯导***解算的姿态角矩阵进行修正

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}$

且修正后的姿态角矩阵可以表示成矩阵的形式：

$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

c)计算补偿后的姿态角，再根据

可以求取航向角θ，横摇角γ，纵摇角的主值：

由于航向角θ，横摇角γ，纵摇角的定义域分别为：θ∈[0,2π]，γ∈[-π,+π]，那么它们对应的真值应为：

至此，得到的θ，γ和即为通过自适应卡尔曼滤波修正后的姿态角信息。

步骤五：不断重复以上步骤二、三、四过程，利用自适应滤波方法对导航参数误差进行在线估计，并实时修正捷联惯导***输出的导航信息，直至捷联惯性/北斗卫星组合导航过程结束。

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (4)

1.一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法，其特征包括以下几个步骤：

步骤一：建立捷联惯导***的误差方程，选取状态变量，得到组合导航***的状态方程；并利用北斗卫星***与捷联惯导***所提供速度的差值作为量测变量，建立组合导航***的量测方程；

状态方程：X_k＝Φ_kX_k-1+Γ_kW_k

量测方程：Z_k＝H_kX_k+V_k

其中，状态变量δλ、δL表示滤波器估计的经纬度误差值，δv_E、δv_N表示滤波器估计的东北向速度误差值，和表示计算导航系与真实导航系坐标轴之间的失准角；Φ_k表示状态一步转移矩阵，Γ_k表示***噪声系数矩阵，W_k表示***噪声，Z_k表示量测信息，H_k表示量测系数矩阵，V_k表示量测噪声；

状态一步转移矩阵

***噪声系数阵

量测系数阵

其中，w_ie表示地球自转角速度，L表示载体所在纬度，g表示地球重力加速度，R表示地球半径，T表示滤波器数据处理周期，v_E、v_N分别表示载体相对地球的东北向速度，f_E、f_N表示加速度计测得的比力信息，表示载体坐标系到导航坐标系之间的转换矩阵；

步骤二：首先更新状态一步预测值及其均方误差，并利用k时刻量测信息Z_k和滤波器的一步预测值计算k时刻的新息序列ε_k；再确定当前时刻衰减记忆调节因子d_k，建立关于新息协方差矩阵的衰减记忆平滑器C_k；

步骤三：借助由步骤二得到的关于新息协方差的衰减记忆平滑器C_k，对k时刻滤波器的增益矩阵K_k进行修正，同时利用该衰减记忆平滑器对***噪声协方差阵Q_k进行在线估计与修正；

步骤四：将步骤二和步骤三中得到的滤波参数带入到自适应滤波器中，完成对导航参数误差的估计，并通过输出校正对捷联惯导***输出的导航参数进行补偿，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息；

步骤五：不断重复以上步骤二、三、四过程，利用自适应滤波方法对导航参数误差进行在线估计，并实时修正捷联惯导***输出的导航信息，直至捷联惯性/北斗卫星组合导航过程结束。

2.根据权利要求1所述的方法，其特征在于：所述步骤二中，

所述的一步预测值

所述的一步预测均方误差P_k,k-1：

所述的k时刻新息序列ε_k：

所述的新息协方差衰减记忆平滑器C_k：

其中，d_k＝(1-b)/(1-b^k+1)，0.95<b<0.99，表示k时刻状态一步预测值，P_k,k-1表示k时刻一步预测均方误差，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻***噪声协方差，表示k-1时刻状态转移矩阵的转置，表示k-1时刻***噪声系数矩阵的转置，ε_k表示k时刻的新息序列，C_k和C_k-1分别是k和k-1时刻的新息协方差衰减记忆平滑值，且初值

3.根据权利要求2所述的方法，其特征在于：所述步骤三的具体步骤为：

1)修正后k时刻的滤波增益矩阵的计算公式为

其中，K_k表示k时刻的滤波增益矩阵，P_k,k-1表示k时刻滤波器的一步预测均方误差，表示k时刻量测矩阵的转置，(C_k)^-1表示新息协方差的衰减记忆平滑值的求逆运算；

2)与此同时，利用得到的新息协方差的衰减记忆平滑器对***噪声协方差阵进行在线估计与修正，那么k时刻***噪声估值器的表达式写作：

其中，Q_k表示k时刻***噪声协方差的估计值，表示k时刻滤波增益矩阵的转置。

4.根据权利要求1所述的方法，其特征在于：所述的步骤四，其具体方法为：

1)自适应滤波器对导航参数误差的估计值：

k时刻状态估计值：

k时刻估计均方误差：P_k＝(I-K_kH_k)P_k,k-1

其中，状态变量 表示状态估计值，表示k时刻状态一步预测值，K_k表示k时刻的滤波增益矩阵，ε_k表示k时刻的新息序列，H_k表示量测系数矩阵，P_k表示状态估计均方误差；

2)得到导航参数误差的估计值后，利用输出校正对捷联惯导***输出的导航参数进行补偿：

①位置修正：λ＝λ_SINS-δλ

L＝L_SINS-δL

其中，λ、L分别表示修正后的经纬度信息，λ_SINS、L_SINS分别表示捷联惯导***输出的经纬度信息，δλ、δL分别表示滤波器估计的经纬度误差值；

②速度修正：v_E＝v_SINS-E-δv_E

v_N＝v_SINS-N-δv_N

其中，v_E、v_N分别表示修正后的东北向速度信息，v_SINS-E、v_SINS-N分别表示捷联惯导***输出的东北向速度信息，δv_E、δv_N分别表示滤波器估计的东北向速度误差值；

③姿态修正：

a)计算导航坐标系与真实导航坐标系之间的转移矩阵

其中，和分别表示计算导航系与真实导航系坐标轴之间的失准角；

b)对捷联惯导***解算的姿态角矩阵进行修正；

且修正后的姿态角矩阵可以表示成矩阵的形式：

c)计算补偿后的姿态角，再根据

可以求取航向角θ，横摇角γ，纵摇角的主值：

由于航向角θ，横摇角γ，纵摇角的定义域分别为：θ∈[0,2π]，γ∈[-π,+π]，那么它们对应的真值应为：

至此，得到的θ，γ和即为通过自适应卡尔曼滤波修正后的姿态角信息。