CN103389095A - 一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法 - Google Patents

Abstract

本发明公开了一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法，其目的是提高滤波器在动态条件下的响应速度和滤波精度，并提高捷联惯性/多普勒组合导航系统的定位精度。该方法通过引入关于新息协方差的限定窗口平滑器，利用该新息协方差平滑值直接修正滤波器中的增益矩阵，并引入调节因子对一步预测均方误差进行修正，进而达到改善滤波器动态响应速度以及提高滤波精度的目的。本发明所设计的自适应滤波方法用于捷联惯性/多普勒组合导航系统中，有效地提高了组合系统在动态条件下的导航定位精度。

Description

一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法

技术领域

本发明属于组合导航系统技术领域，尤其涉及的是一种捷联惯性/多普勒组合导航系统的自适应滤波方法。

背景技术

常规卡尔曼滤波可以估计组合导航系统的位置、速度误差以及平台失准角等状态变量，因此在组合导航领域中获得了广泛的应用。但是常规卡尔曼滤波在工作时需要满足模型精确和噪声统计特性准确已知的要求，对于捷联惯性与多普勒组合导航系统而言，系统中不可避免的含有不确定性噪声，例如陀螺随机漂移、随机风速以及海流等干扰因素，此时常规卡尔曼滤波将失去最优估计特性；而且当常规卡尔曼滤波在特定条件下达到稳态时，滤波参数也会收敛于稳态值，若此时系统的外部运动发生剧烈变化，滤波参数不能立刻做出调整，使得滤波精度下降，甚至引起滤波发散。

为解决常规卡尔曼滤波在噪声统计信息不准确以及动态条件下导致滤波精度下降等问题，近年来学者们提出了许多自适应滤波算法，主要集中在新息自适应滤波和渐消记忆衰减自适应滤波两方面。文献“基于新息协方差的自适应渐消卡尔曼滤波器，系统工程与电子技术，2012，第13卷第12期”，提出了一种利用新息协方差计算渐消因子的方法，该方法利用新息序列的协方差自适应的改变渐消因子调整新息的权重，减小了陈旧量测值对估计的影响，而且通过渐消因子自适应地调整误差协方差，补偿不完整信息的影响；但是该方法需要同时求取两个调节因子，不仅增加了计算量，还由于调节因子本身会带有一定的误差，可能会引起估计精度下降，甚至导致滤波器的发散。

发明内容

为了克服现有技术中的缺陷，解决上述技术问题，本发明提供一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法，该方法可以明显改善由于噪声统计信息不准导致常规卡尔曼滤波发散的情况，提高滤波器在动态条件下的响应速度和滤波精度，可有效改进捷联惯性/多普勒组合导航系统的定位精度，其技术方案如下：

一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法，包括以下步骤：

步骤一：利用捷联惯性导航系统中惯性测量单元测得沿载体系相对惯性空间的角速率和加速度分量信息，并与系统的初始位置、速度以及姿态信息一起进行捷联惯导解算，得到载体实时的位置、速度和姿态信息；

步骤二：根据捷联惯导系统的误差方程选取状态变量，建立系统的状态方程，并选取经纬度误差、速度误差以及失准角作为状态变量；利用多普勒导航系统与捷联惯导系统所提供速度的差值作为量测变量，建立组合导航系统的量测方程；

步骤三：经过连续系统离散化处理后，对组合导航系统进行滤波估计；首先更新状态一步预测值

及其均方误差P_k,k-1，并利用k时刻量测信息Z_k和滤波器的一步预测值

计算k时刻的新息序列ε_k；再确定平滑窗口的宽度N，并借助量测新息序列ε_k建立关于新息协方差的N步限定窗口平滑器

所述的一步预测值：     ${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k-1}{\hat{X}}_{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时刻新息序列：     ${\mathrm{\ϵ}}_k=Z_k-H_k{\hat{X}}_{k,k-1}$

所述的N步新息协方差平滑器：     $C_k^N=C_{k-1}^N+\frac{1}{N}[{\mathrm{\ϵ}}_k{\mathrm{\ϵ}}_k^T-{\mathrm{\ϵ}}_{k-N}{\mathrm{\ϵ}}_{k-N}^T]$

其中，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻系统噪声协方差，

表示k-1时刻状态转移矩阵的转置，

表示k-1时刻系统噪声系数矩阵的转置，ε_k表示k时刻的新息序列，

和

分别是k和k-1时刻的新息协方差N步平滑值，N=10，且当k<N时，

步骤四：借助步骤三中得到的关于新息协方差的限定窗口平滑器

对k时刻滤波器的增益矩阵K_k进行修正，使得增益矩阵能够根据新息序列的变化直接做出调整；

步骤五：利用步骤三中得到的限定窗口平滑器

并引入调节因子ρ_k，对滤波器的一步预测估计均方误差P_k,k-1进行修正；

步骤六：将步骤三至步骤五中得到的滤波参数带入到自适应滤波器中，并不断重复以上过程完成对导航参数误差的估计；利用输出校正实时修正捷联惯导系统输出的导航信息，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息，直至捷联惯性/多普勒组合导航过程结束。

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

所述的状态方程：X_k=Φ_kX_k-1+Γ_kW_k

所述的量测方程：Z_k=H_kX_k+V_k

其中，状态变量

状态一步转移矩阵

Φ_k=

$=\left[\begin{array}{ccccccc}1& 0& \frac{T}{R}& 0& 0& 0& 0\\ \frac{v_E}{R}T\sec L\tan L& 1& \frac{\sec L}{R}T& 0& 0& 0& 0\\ {2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_N+\frac{v_E{v}_N}{R}{T\sec }^{2}L& 0& 1+\frac{v_N}{R}T\tan L& {2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& 0& -\mathrm{gT}& f_{N}T\\ -({2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_E+\frac{v_E^2}{R}{T\sec }^{2}L)& 0& -({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \mathrm{gT}& 0& {-f}_{E}T\\ 0& 0& 0& \frac{T}{R}& 1& w_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& -(w_{\mathrm{ie}}T\cos L+\frac{v_E}{R})\\ {-w}_{\mathrm{ie}}T\sin L& 0& \frac{T}{R}& 0& -(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& -\frac{v_N}{R}T\\ w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}{T\sec }^{2}L& 0& \frac{T}{R}\tan L& 0& w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T& \frac{v_N}{R}T& 1\end{array}\right]$

系统噪声系数阵

Γ_k=

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

量测系数阵

$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表示加速度计测得的比力信息，

表示载体坐标系到导航坐标系之间的转换矩阵。

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

(1)建立k时刻新息协方差的限定窗口平滑器

其递推形式：

$C_k^N=C_{k-1}^N+\frac{1}{N}[{\mathrm{\&epsiv;}}_k{\mathrm{\&epsiv;}}_k^T-{\mathrm{\&epsiv;}}_{k-N}{\mathrm{\&epsiv;}}_{k-N}^T]$

其中，

是k-1时刻的新息协方差平滑器，N为限定平滑窗口的宽度，ε_k与ε_k-N分别表示k时刻和k-N时刻的量测新息值；

(2)修正k时刻的滤波增益矩阵K_k，其计算公式写作：

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

其中，P_k,k-1表示k时刻一步预测均方误差，H_k表示k时刻量测系数矩阵。

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

(1)得到k时刻的调节因子ρ_k，其表达式写作：

${\mathrm{\&rho;}}_k=\max \{1,\frac{\mathrm{tr}\left[(C_k^N-{\hat{R}}_k){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}{\mathrm{tr}\left[\left(H_k{P}_{k,k-1}{H}_k^T\right){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}\}$

其中，max{□}表示取内部所有元素的最大值，tr[□]表示对内部矩阵求迹；

(2)修正一步预测均方误差P_k,k-1，其表达式写作：

$P_{k,k-1}={\mathrm{\&rho;}}_k({\mathrm{\&Phi;}}_{k-1}{P}_{k-1}{\mathrm{\&Phi;}}_k^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T)$

其中，Φ_k-1表示k-1时刻的系统状态转移矩阵，P_k-1表示k-1时刻的滤波估计均方误差，Γ_k-1表示k-1时刻的系统噪声系数阵，Q_k-1表示k-1时刻的系统噪声协方差。

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

(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)计算导航坐标系与真实导航坐标系之间的转移矩阵

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\&phi;}}_z& {\mathrm{\&phi;}}_y\\ {\mathrm{\&phi;}}_z& 1& {-\mathrm{\&phi;}}_x\\ {-\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_x& 1\end{array}\right]$

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

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、本发明的有益效果可以通过Matlab仿真实验进行验证，在捷联惯性/多普勒组合导航系统中仿真参数设置如下：

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

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

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

Q(0)=diag{(100ug)²,(100ug)²,(0.01°/h)²,(0.01°/h)2,(0.01°/h)²}

R(0)=10R=10×diag{(0.1m/s)²,(0.1m/s)²}

②初始位置姿态：经度为126.67°，纬度为45.77°，初始速度（6m/s,8m/s,0）,

姿态角（0,0,30°）；仿真时间3600s，计算步长设为0.1s。

由仿真结果图3-图5可以得出，本发明提出的自适应滤波方法通过对新息协方差序列进行限定窗口平滑处理，并直接调节增益矩阵，而且通过引入调节因子来修正一步预测均方误差，不仅改善了滤波器的鲁棒性，而且进一步提高了滤波精度；本发明提出的自适应滤波方法用于捷联惯性/多普勒组合导航系统中，提高了组合导航系统的定位精度，具有广阔的工程应用前景。

本发明提出一种自适应卡尔曼滤波的设计方法，通过引入关于新息协方差的限定窗口平滑器，对滤波器的增益矩阵进行实时修正，通过引入调节因子对一步预测均方误差进行修正，使得滤波器能够根据新近的新息序列自适应的调节增益矩阵，提高滤波器在动态条件下的响应速度和滤波精度，进而提高捷联惯性/多普勒组合导航系统的定位精度，具有实际的工程应用意义。

附图说明

图1是本发明自适应滤波方法在组合导航系统中的工作流程图；

图2是本发明自适应滤波方法用于捷联惯性/多普勒组合系统的方框图；

图3是本发明自适应滤波方法在组合导航系统中的速度误差仿真图；

图4是本发明自适应滤波方法在组合导航系统中的位置误差仿真图；

图5是本发明自适应滤波方法在组合导航系统中的姿态误差角仿真图；

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法，如图1-图2所示，通过下列步骤实现自适应滤波过程：

步骤一：利用捷联惯性导航系统中惯性测量单元测得沿载体系相对惯性空间的角速率和加速度分量信息，并与系统的初始位置、速度以及姿态信息一起进行捷联惯导解算，得到载体实时的位置、速度和姿态信息；

步骤二：根据捷联惯导系统的误差方程选取状态变量，建立系统的状态方程，并选取经纬度误差、速度误差以及失准角作为状态变量；利用多普勒导航系统与捷联惯导系统所提供速度的差值作为量测变量，建立组合导航系统的量测方程；

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

量测方程：Z_k=H_kX_k+V_k

其中，状态变量

由位置误差、速度误差以及平台失准角组成；Z_k表示k时刻的量测信息，由北斗接收机和捷联惯导系统提供；W_k和V_k分别表示k时刻的系统噪声和量测噪声。

状态一步转移矩阵

Φ_k=

$=\left[\begin{array}{ccccccc}1& 0& \frac{T}{R}& 0& 0& 0& 0\\ \frac{v_E}{R}T\sec L\tan L& 1& \frac{\sec L}{R}T& 0& 0& 0& 0\\ {2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_N+\frac{v_E{v}_N}{R}{T\sec }^{2}L& 0& 1+\frac{v_N}{R}T\tan L& {2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& 0& -\mathrm{gT}& f_{N}T\\ -({2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_E+\frac{v_E^2}{R}{T\sec }^{2}L)& 0& -({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \mathrm{gT}& 0& {-f}_{E}T\\ 0& 0& 0& \frac{T}{R}& 1& w_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& -(w_{\mathrm{ie}}T\cos L+\frac{v_E}{R})\\ {-w}_{\mathrm{ie}}T\sin L& 0& \frac{T}{R}& 0& -(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& -\frac{v_N}{R}T\\ w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}{T\sec }^{2}L& 0& \frac{T}{R}\tan L& 0& w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T& \frac{v_N}{R}T& 1\end{array}\right]$

系统噪声系数阵

Γ_k=

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

量测系数阵

$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表示加速度计测得的比力信息，

表示载体坐标系到导航坐标系之间的转换矩阵。

步骤三：经过连续系统离散化处理后，对组合导航系统进行滤波估计。首先更新状态一步预测值

及其均方误差P_k,k-1，并利用k时刻量测信息Z_k和滤波器的一步预测值

计算k时刻的新息序列ε_k；再确定平滑窗口的宽度N，并借助量测新息序列ε_k建立关于新息协方差的N步限定窗口平滑器

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

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

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

N步新息协方差平滑器：     $C_k^N=C_{k-1}^N+\frac{1}{N}[{\mathrm{\&epsiv;}}_k{\mathrm{\&epsiv;}}_k^T-{\mathrm{\&epsiv;}}_{k-N}{\mathrm{\&epsiv;}}_{k-N}^T]$

其中，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻系统噪声协方差，

表示k-1时刻状态转移矩阵的转置，

表示k-1时刻系统噪声系数矩阵的转置，ε_k表示k时刻的新息序列，

和

分别是k和k-1时刻的新息协方差N步平滑值，N=10，且当k<N时，

步骤四：借助步骤三中得到的关于新息协方差的限定窗口平滑器

对k时刻滤波器的增益矩阵K_k进行修正，使得增益矩阵能够根据新息序列的变化直接做出调整；

(1)建立k时刻新息协方差的限定窗口平滑器其递推形式：

$C_k^N=C_{k-1}^N+\frac{1}{N}[{\mathrm{\&epsiv;}}_k{\mathrm{\&epsiv;}}_k^T-{\mathrm{\&epsiv;}}_{k-N}{\mathrm{\&epsiv;}}_{k-N}^T]$

其中，

是k-1时刻的新息协方差平滑器，N为限定平滑窗口的宽度，ε_k与ε_k-N分别表示k时刻和k-1时刻的量测新息值；

(2)修正k时刻的滤波增益矩阵K_k，其计算公式写作：

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

其中，P_k,k-1表示k时刻一步预测均方误差，H_k表示k时刻量测系数矩阵。

步骤五：利用步骤三中得到的限定窗口平滑器

并引入调节因子ρ_k，对滤波器的一步预测估计均方误差P_k,k-1进行修正；

(1)得到k时刻的调节因子ρ_k，其表达式写作：

${\mathrm{\&rho;}}_k=\max \{1,\frac{\mathrm{tr}\left[(C_k^N-{\hat{R}}_k){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}{\mathrm{tr}\left[\left(H_k{P}_{k,k-1}{H}_k^T\right){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}\}$

其中，max{□}表示取内部所有元素的最大值，tr[□]表示对内部矩阵求迹；

(2)修正一步预测均方误差P_k,k-1，其表达式写作：

$P_{k,k-1}={\mathrm{\&rho;}}_k({\mathrm{\&Phi;}}_{k-1}{P}_{k-1}{\mathrm{\&Phi;}}_k^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T)$

其中，Φ_k-1表示k-1时刻的系统状态转移矩阵，P_k-1表示k-1时刻的滤波估计均方误差，Γ_k-1表示k-1时刻的系统噪声系数阵，Q_k-1表示k-1时刻的系统噪声协方差。

步骤六：将步骤三至步骤五中得到的滤波参数带入到自适应滤波器中，并不断重复以上过程完成对导航参数误差的估计；利用输出校正实时修正捷联惯导系统输出的导航信息，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息，直至捷联惯性/多普勒组合导航过程结束。

(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)计算导航坐标系与真实导航坐标系之间的转移矩阵

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\&phi;}}_z& {\mathrm{\&phi;}}_y\\ {\mathrm{\&phi;}}_z& 1& {-\mathrm{\&phi;}}_x\\ {-\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_x& 1\end{array}\right]$

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

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 (5)

1.一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法，其特征在于，包括以下步骤：

步骤一：利用捷联惯性导航系统中惯性测量单元测得沿载体系相对惯性空间的角速率和加速度分量信息，并与系统的初始位置、速度以及姿态信息一起进行捷联惯导解算，得到载体实时的位置、速度和姿态信息；

步骤二：根据捷联惯导系统的误差方程选取状态变量，建立系统的状态方程，并选取经纬度误差、速度误差以及失准角作为状态变量；利用多普勒导航系统与捷联惯导系统所提供速度的差值作为量测变量，建立组合导航系统的量测方程；

步骤三：经过连续系统离散化处理后，对组合导航系统进行滤波估计；首先更新状态一步预测值

及其均方误差P_k,k-1，并利用k时刻量测信息Z_k和滤波器的一步预测值计算k时刻的新息序列ε_k；再确定平滑窗口的宽度N，并借助量测新息序列ε_k建立关于新息协方差的N步限定窗口平滑器

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

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

所述的k时刻新息序列：    

所述的N步新息协方差平滑器：     $C_k^N=C_{k-1}^N+\frac{1}{N}\left[{\mathrm{\&epsiv;}}_k{\mathrm{\&epsiv;}}_k^T{\mathrm{\&epsiv;}}_{k-N}{\mathrm{\&epsiv;}}_{k-N}^T\right]$

其中，P_k-1表示k-1时刻的滤波均方误差，Q_k-1表示k-1时刻系统噪声协方差，表示k-1时刻状态转移矩阵的转置，

表示k-1时刻系统噪声系数矩阵的转置，ε_k表示k时刻的新息序列，

和

分别是k和k-1时刻的新息协方差N步平滑值，N=10，且当k<N时，

步骤四：借助步骤三中得到的关于新息协方差的限定窗口平滑器对k时刻滤波器的增益矩阵K_k进行修正，使得增益矩阵能够根据新息序列的变化直接做出调整；

步骤五：利用步骤三中得到的限定窗口平滑器

并引入调节因子ρ_k，对滤波器的一步预测估计均方误差P_k,k-1进行修正；

步骤六：将步骤三至步骤五中得到的滤波参数带入到自适应滤波器中，并不断重复以上过程完成对导航参数误差的估计；利用输出校正实时修正捷联惯导系统输出的导航信息，从而得到更高精度的导航参数信息，即补偿后的位置、速度和姿态信息，直至捷联惯性/多普勒组合导航过程结束。

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

所述的状态方程：X_k=Φ_kX_k-1+Γ_kW_k

所述的量测方程：Z_k=H_kX_k+V_k

其中，状态变量

状态一步转移矩阵

Φ_k=

$=\left[\begin{array}{ccccccc}1& 0& \frac{T}{R}& 0& 0& 0& 0\\ \frac{v_E}{R}T\sec L\tan L& 1& \frac{\sec L}{R}T& 0& 0& 0& 0\\ {2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_N+\frac{v_E{v}_N}{R}{T\sec }^{2}L& 0& 1+\frac{v_N}{R}T\tan L& {2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& 0& -\mathrm{gT}& f_{N}T\\ -({2w}_{\mathrm{ie}}T\cos {\mathrm{Lv}}_E+\frac{v_E^2}{R}{T\sec }^{2}L)& 0& -({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \mathrm{gT}& 0& {-f}_{E}T\\ 0& 0& 0& \frac{T}{R}& 1& w_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& -(w_{\mathrm{ie}}T\cos L+\frac{v_E}{R})\\ {-w}_{\mathrm{ie}}T\sin L& 0& \frac{T}{R}& 0& -(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& -\frac{v_N}{R}T\\ w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}{T\sec }^{2}L& 0& \frac{T}{R}\tan L& 0& w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T& \frac{v_N}{R}T& 1\end{array}\right]$

系统噪声系数阵

Γ_k=

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

量测系数阵

$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表示加速度计测得的比力信息，

表示载体坐标系到导航坐标系之间的转换矩阵。

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

(1)建立k时刻新息协方差的限定窗口平滑器

其递推形式：

$C_k^N=C_{k-1}^N+\frac{1}{N}[{\mathrm{\&epsiv;}}_k{\mathrm{\&epsiv;}}_k^T-{\mathrm{\&epsiv;}}_{k-N}{\mathrm{\&epsiv;}}_{k-N}^T]$

其中，

是k-1时刻的新息协方差平滑器，N为限定平滑窗口的宽度，ε_k与ε_k-N分别表示k时刻和k-N时刻的量测新息值；

(2)修正k时刻的滤波增益矩阵K_k，其计算公式写作：

其中，P_k,k-1表示k时刻一步预测均方误差，H_k表示k时刻量测系数矩阵。

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

(1)得到k时刻的调节因子ρ_k，其表达式写作：

${\mathrm{\&rho;}}_k=\max \{1,\frac{\mathrm{tr}\left[(C_k^N-{\hat{R}}_k){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}{\mathrm{tr}\left[\left(H_k{P}_{k,k-1}{H}_k^T\right){\left(H_k{P}_{k,k-1}{H}_k^T\right)}^T\right]}\}$

其中，max{□}表示取内部所有元素的最大值，tr[□]表示对内部矩阵求迹；

(2)修正一步预测均方误差P_k,k-1，其表达式写作：

$P_{k,k-1}={\mathrm{\&rho;}}_k({\mathrm{\&Phi;}}_{k-1}{P}_{k-1}{\mathrm{\&Phi;}}_k^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T)$

其中，Φ_k-1表示k-1时刻的系统状态转移矩阵，P_k-1表示k-1时刻的滤波估计均方误差，Γ_k-1表示k-1时刻的系统噪声系数阵，Q_k-1表示k-1时刻的系统噪声协方差。

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

(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)计算导航坐标系与真实导航坐标系之间的转移矩阵

$C_{n^{\&prime;}}^n=\left[\begin{array}{ccc}1& {-\mathrm{\&phi;}}_z& {\mathrm{\&phi;}}_y\\ {\mathrm{\&phi;}}_z& 1& {-\mathrm{\&phi;}}_x\\ {-\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_x& 1\end{array}\right]$

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

b)对捷联惯导系统解算的姿态角矩阵

进行修正

$C_b^n=C_{n^{\text{\&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π]，

γ∈[-π,+π]，

那么它们对应的真值应为：

至此，得到的θ，γ和

即为通过自适应卡尔曼滤波修正后的姿态角信息。

Images