CN104062672A - 基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法 - Google Patents

Abstract

本发明公开了一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，首先将捷联惯导系统和GPS接收机安装在载体上；对捷联惯导系统预热后采集捷联惯导系统输出的位置信息与GPS输出的位置信息；建立SINS／GPS组合导航系统的状态方程与量测方程；设计强跟踪Kalman滤波器与Sage-Husa自适应滤波器；采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS／GPS组合导航方案。采用本发明能够满足SINS／GPS组合导航系统遇到的高动态应用环境，抑制滤波的发散，进一步提高导航精度。

Description

基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法

技术领域

本发明涉及一种SINS／GPS组合导航方法，尤其涉及一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法。

背景技术

组合导航系统有效地结合了多种导航技术，不仅可以取长补短，发挥各自的优点，提高导航精度，而且如果采用低精度的导航系统仍可以得到较高的导航精度，降低成本和技术难度。另外，组合导航系统具有更好的容错性和可靠性。SINS／GPS组合导航系统能够有效地对运载体定位误差进行补偿和校正，准确地给出位置信息，便于运载体精确定位，目前使用最为广泛。

Kalman滤波在组合导航系统中具有十分重要的作用，能够给出导航系统各导航参数误差的最优估计，以实现对导航参数进行校正，提供更精确的导航信息，国内外对基于Kalman滤波算法的组合导航系统做了大量的研究工作。恶劣环境的影响会使得SINS／GPS组合导航系统的误差模型复杂而多变，如果仍然采用常规Kalman滤波进行处理，会导致滤波精度降低，甚至发散，难以满足运载体的高精度导航任务。专利CN103281054A公开了“一种带噪声统计估值器的自适应滤波方法”，通过在线加入噪声统计估计器，在系统噪声和量测噪声统计信息未知或者时变的情况下，对其统计特性进行实时估计与修正。这种方法计算量较大，降低了实时性，且在高动态环境下无法保证滤波的收敛性和稳定性。

发明内容

本发明的目的就在于为了解决上述问题而提供一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法。

本发明通过以下技术方案来实现上述目的：

一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于，包括以下步骤：

步骤1：将捷联惯导系统和GPS接收机安装在载体上，对捷联惯导系统预热后采集捷联惯导系统输出的位置信息与GPS输出的位置信息；

步骤2：建立SINS／GPS组合导航系统的状态方程与量测方程；

步骤3：设计Sage-Husa自适应滤波器；

步骤4：设计强跟踪Kalman滤波器；

步骤5：采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS／GPS组合导航方案。

所述步骤2中建立SINS／GPS组合导航系统的状态方程与量测方程：

状态方程为：

$\left[\begin{array}{c}{\stackrel{.}{X}}_{\mathrm{SINS}}\\ {\stackrel{.}{X}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0\\ 0& F_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{GPS}}\end{array}\right]+\left[\begin{array}{c}{W}_{\mathrm{SINS}}\\ W_{\mathrm{GPS}}\end{array}\right]$

其中： $X_{\mathrm{SINS}}={\left[\begin{array}{cccccccccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y\end{array}\right]}^T$

X_GPS=[δt_u δt_ru]^T

$F_{\mathrm{GPS}}={\left[\begin{array}{cc}-1/{\mathrm{\τ}}_{\mathrm{tu}}& 0\\ 0& -1/{\mathrm{\τ}}_{\mathrm{tru}}\end{array}\right]}^T$

W_GPS=[ε_tu ε_tru]^T

$F_{\mathrm{SINS}}=\left[\begin{array}{ccccc}{F}_1& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}\\ F_2& F_4& F_5& 0_{3\×3}& F_7\\ F_3& 0_{2\×3}& 0_{2\×2}& F_6& 0_{2\×2}\\ & & 0_{5\×12}& & \end{array}\right]$

$F_1=\left[\begin{array}{cc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0\end{array}\right]$

$F_2=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]$

$F_3=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{\sec L}{R}& 0\end{array}\right]$

$F_4=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\end{array}\right],$

$F_5=\left[\begin{array}{cc}0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0\end{array}\right]$

$F_6=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\end{array}\right]$

$F_7=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\\ C_{31}& C_{32}\end{array}\right]$

$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]$

W_SINS=[w_gx w_gy w_ax w_ax w_ax 0_1×8]^T，

其中：δV_x、δV_y分别为东、北向速度误差；φ_x、φ_y和φ_z分别为东向、北向和方位失准角；δλ、δL分别为经纬度误差；ω_ie为地球自转角速度；L为当地纬度；R为地球半径；为捷联姿态矩阵；ε_x、ε_y和ε_z分别为陀螺漂移；w_gx、w_gy和w_gz为陀螺噪声；分别为加速度计零偏；w_ax、w_ay为加速度计噪声；δt_u表示接收机的钟差误差；δt_ru表示接收机钟差的漂移误差；τ_tu、τ_tru分别表示接收机的相关时间；ε_tu、ε_tru分别表示白噪声向量；

量测方程为：

在SINS／GPS组合导航系统中，将惯导解算的位置信息相对应的伪距、伪距率与GPS测量的伪距、伪距率之差作为量测值：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\δ\ρ}}_1\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}_1\\ .\\ .\\ .\\ {\mathrm{\δ\ρ}}_i\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}_i\end{array}\right]=H\left(t\right)X\left(t\right)+v\left(t\right)$

其中：i为可见的卫星数目；δρ_i＝ρ_Ii-ρ_Gi，ρ_Ii为惯导输出的位置信息相对应的伪距，ρ_Gi为GPS接收机测量得到的伪距；H(t)为量测矩阵；v(t)为各元素为零均值的高斯白噪声，且互不相关。

步骤3中所述设计Sage-Husa自适应滤波器，其具体算法如下：

假设线性离散系统方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中：X_k为状态向量，Φ_k，k-1为t_k-1时刻至t_k时刻的一步转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为系统噪声向量，Z_k为量测向量，H_k为量测矩阵，V_k为量测噪声向量；

Sage-Husa自适应Kalman滤波器设计如下：

计算一步预均方误差P_k，k-1：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{\hat{Q}}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

计算滤波增益K_k：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+{\hat{R}}_{k-1}]}^{-1}$

计算均方误差P_k：

P_k=[I-K_kH_k]P_k，k-1

状态一步预测：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\hat{q}}_{k-1}$

状态更新：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\tilde{Z}}_k$

量测更新：

${\tilde{Z}}_K=Z_k-H_k{\hat{X}}_{k,k-1}-{\hat{r}}_{k-1}$

Sage-Husa自适应Kalman滤波中的量测噪声均值r和量测噪声方差阵R的计算方法如下：

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k(Z_k-H_k{\hat{X}}_{k,k-1})$

${\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k({\tilde{Z}}_k{\tilde{Z}}_k^T-H_k{P}_{k,k-1}{H}_k^T)$

其中：d_k=(1-b)／(1-b^k+1)，0<b<1表示遗忘因子；

所述设计强跟踪Kalman滤波器，具体为：

通过用加权系数λ_k+1乘以状态量估计误差的验前方差阵得以实现：

$P_{k,k-1}={\mathrm{\&lambda;}}_{k+1}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{\hat{Q}}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T$

其中：

λ_k+1＝diag[λ_1(k+1)，λ_2(k+1)，L，λ_m(k+1)]

${\mathrm{\&lambda;}}_{i(k+1)}=\left\{\begin{array}{cc}{\mathrm{\&alpha;}}_i{C}_{k+1}& ({\mathrm{\&alpha;}}_i{C}_{k+1}>1)\\ 1& ({\mathrm{\&alpha;}}_i{C}_{k+1}\&le;1)\end{array}\right.$

$C_{k+1}=\frac{\mathrm{Tr}[v_{0(k+1)}-R_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T]}{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_i\left[{\mathrm{\&Phi;}}_{k+1,k}{P}_{k+1}{\mathrm{\&Phi;}}_{k+1,k}^T{H}_{k+1}^T{H}_{k+1}\right]}$

$v_{0(k+1)}=\left\{\begin{array}{cc}{\tilde{Z}}_1{\tilde{Z}}_1^T& (k=0)\\ \frac{{\mathrm{\&rho;v}}_{0\left(k\right)}+{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T}{1+\mathrm{\&rho;}}& (k\&GreaterEqual;\mathrm{1,0}\&le;\mathrm{\&rho;}<1)\end{array}\right.$

其中：α_i由先验知识确定，为系统估计误差，v_0(k+1)为误差的方差阵。

所述步骤5中的采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS／GPS组合导航方案，具体为：采用收敛判据对滤波的发散趋势进行判断，若不发散，则采用Sage-Husa自适应Kalman滤波算法， 若发散，则采用强跟踪Kalman滤波算法：

滤波收敛判据：以新息序列的性质构造滤波收敛判据，即 由于表示新息序列平方和，其包括了实际估计误差，而理论预测误差则可描述为新息序列方差阵即：

$E\left[{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T\right]=H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1}$

${\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T\&le;\mathrm{\&gamma;Tr}=[H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1}]$

其中：γ≥1表示可调系数，如果成立，则代表滤波器正常工作，此时可以采用Sage-Husa自适应Kalman滤波算法对状态变量进行估计；如果不成立，则表示滤波实际误差超过理论预测误差的γ倍，滤波发散，此时采用强跟踪滤波算法计算加权系数λ_k+1自适应调整P_k，k-1，充分发挥当前时刻量测量作用，以抑制滤波的发散。

本发明的有益效果在于：

本发明是一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，与现有技术相比，本发明能在航向发生机动的情况下，强跟踪自适应Kalman滤波比自适应Kalman滤波能更好地跟踪真实轨迹。无论是速度误差估计，还是位置误差估计，强跟踪自适应Kalman滤波都比自适应Kalman滤波的估计精度高，尤其是对位置误差的估计。结果表明，强跟踪自适应Kalman滤波能够更好地适应实际应用环境，满足SINS／GPS组合导航系统的高精度导航需要。

附图说明

图1为本发明的设计框图；

图2为强跟踪自适应Kalman滤波算法流程；

图3为基于两种不同滤波算法的东向速度误差曲线；

图4为基于两种不同滤波算法的北向速度误差曲线；

图5为基于两种不同滤波算法的经度误差曲线；

图6为基于两种不同滤波算法的纬度误差曲线。

具体实施方式

下面结合附图对本发明作进一步说明：

如图1至图6所示：一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于，包括以下步骤：

步骤1：将捷联惯导系统和GPS接收机安装在载体上，对捷联惯导系统预热后采集捷联惯导系统输出的位置信息与GPS输出的位置信息；

步骤2：建立SINS／GPS组合导航系统的状态方程与量测方程；状态方程为：

$\left[\begin{array}{c}{\stackrel{.}{X}}_{\mathrm{SINS}}\\ {\stackrel{.}{X}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0\\ 0& F_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{GPS}}\end{array}\right]+\left[\begin{array}{c}{W}_{\mathrm{SINS}}\\ W_{\mathrm{GPS}}\end{array}\right]$

其中： $X_{\mathrm{SINS}}={\left[\begin{array}{cccccccccccc}\mathrm{\&delta;}{V}_x& \mathrm{\&delta;}{V}_y& {\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z& \mathrm{\&delta;L}& \mathrm{\&delta;\&lambda;}& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& {\&dtri;}_x& {\&dtri;}_y\end{array}\right]}^T$

X_GPS=[δ_tu δt_ru]^T

$F_{\mathrm{GPS}}={\left[\begin{array}{cc}-1/{\mathrm{\&tau;}}_{\mathrm{tu}}& 0\\ 0& -1/{\mathrm{\&tau;}}_{\mathrm{tru}}\end{array}\right]}^T$

W_GPS=[ε_tu ε_tru]^T

$F_{\mathrm{SINS}}=\left[\begin{array}{ccccc}{F}_1& 0_{2\&times;3}& 0_{2\&times;2}& 0_{2\&times;3}& 0_{2\&times;2}\\ F_2& F_4& F_5& 0_{3\&times;3}& F_7\\ F_3& 0_{2\&times;3}& 0_{2\&times;2}& F_6& 0_{2\&times;2}\\ & & 0_{5\&times;12}& & \end{array}\right]$

$F_1=\left[\begin{array}{cc}0& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\\ -2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0\end{array}\right]$

$F_2=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]$

$F_3=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{\sec L}{R}& 0\end{array}\right]$

$F_4=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& -{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L& 0& 0\end{array}\right],$

$F_5=\left[\begin{array}{cc}0& 0\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L& 0\end{array}\right]$

$F_6=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\end{array}\right]$

$F_7=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\\ C_{31}& C_{32}\end{array}\right]$

$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]$

W_SINS＝[w_gx w_gy w_ax w_ax w_ax 0_1×8]^T，

其中：δV_x、δV_y分别为东、北向速度误差；φ_x、φ_y和φ_z分别为东向、北向和方位失准角；δλ、δL分别为经纬度误差；ω_ie为地球自转角速度；L为当地纬度；R为地球半径；为捷联姿态矩阵；ε_x、ε_y和ε_z分别为陀螺漂移；w_gx、w_gy和w_gz为陀螺噪声；分别为加速度计零偏；w_ax、w_ay为加速度计噪声；δt_u表示接收机的钟差误差；δt_ru表示接收机钟差的漂移误差；τ_tu、τ_tru分别表示接收机的相关时间；ε_tu、ε_tru分别表示白噪声向量；

量测方程为：

在SINS／GPS组合导航系统中，将惯导解算的位置信息相对应的伪距、伪距率与GPS测量的伪距、伪距率之差作为量测值：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\&delta;\&rho;}}_1\\ \mathrm{\&delta;}{\stackrel{.}{\mathrm{\&rho;}}}_1\\ .\\ .\\ .\\ {\mathrm{\&delta;\&rho;}}_i\\ \mathrm{\&delta;}{\stackrel{.}{\mathrm{\&rho;}}}_i\end{array}\right]=H\left(t\right)X\left(t\right)+v\left(t\right)$

其中：i为可见的卫星数目；δρ_i＝ρ_Ii-ρ_Gi，ρ_Ii为惯导输出的位置信息相对应的伪距，ρ_Gi为GPS接收机测量得到的伪距；H(t)为量测矩阵；v(t)为各元素为零均值的高斯白噪声，且互不相关。

步骤3：设计Sage-Husa自适应滤波器；其具体算法如下：

假设线性离散系统方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

其中：X_k为状态向量，Φ_k，k-1为t_k-1时刻至t_k时刻的一步转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为系统噪声向量，Z_k为量测向量，H_k为量测矩阵，V_k为量测噪声向量；

Sage-Husa自适应Kalman滤波器设计如下：

计算一步预均方误差P_k，k-1：

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

计算滤波增益K_k：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+{\hat{R}}_{k-1}]}^{-1}$

计算均方误差P_k：

P_k=[I-K_kH_k]P_k，k-1

状态一步预测：

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+{\hat{q}}_{k-1}$

状态更新：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\tilde{Z}}_k$

量测更新：

${\tilde{Z}}_K=Z_k-H_k{\hat{X}}_{k,k-1}-{\hat{r}}_{k-1}$

Sage-Husa自适应Kalman滤波中的量测噪声均值r和量测噪声方差阵R的计算方法如下：

${\hat{r}}_k=(1-d_k){\hat{r}}_{k-1}+d_k(Z_k-H_k{\hat{X}}_{k,k-1})$

${\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k({\tilde{Z}}_k{\tilde{Z}}_k^T-H_k{P}_{k,k-1}{H}_k^T)$

其中：d_k=(1-b)／(1-b^k+1)，0<b<1表示遗忘因子；

步骤4：设计强跟踪Kalman滤波器；具体为：

通过用加权系数λ_k+1乘以状态量估计误差的验前方差阵得以实现：

$P_{k,k-1}={\mathrm{\&lambda;}}_{k+1}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{\hat{Q}}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T$

其中：

λ_k+1＝diag[λ_1(k+1)，λ_2(k+1)，L，λ_m(k+1)]

${\mathrm{\&lambda;}}_{i(k+1)}=\left\{\begin{array}{cc}{\mathrm{\&alpha;}}_i{C}_{k+1}& ({\mathrm{\&alpha;}}_i{C}_{k+1}>1)\\ 1& ({\mathrm{\&alpha;}}_i{C}_{k+1}\&le;1)\end{array}\right.$

$C_{k+1}=\frac{\mathrm{Tr}[v_{0(k+1)}-R_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T]}{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_i\left[{\mathrm{\&Phi;}}_{k+1,k}{P}_{k+1}{\mathrm{\&Phi;}}_{k+1,k}^T{H}_{k+1}^T{H}_{k+1}\right]}$

$v_{0(k+1)}=\left\{\begin{array}{cc}{\tilde{Z}}_1{\tilde{Z}}_1^T& (k=0)\\ \frac{{\mathrm{\&rho;v}}_{0\left(k\right)}+{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T}{1+\mathrm{\&rho;}}& (k\&GreaterEqual;\mathrm{1,0}\&le;\mathrm{\&rho;}<1)\end{array}\right.$

其中：α_i由先验知识确定，为系统估计误差，v_0(k+1)为误差的方差阵。

步骤5：采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS/GPS组合导航方案。具体为：采用收敛判据对滤波的发散趋势进行判断，若不发散，则采用Sage-Husa自适应Kalman滤波算法，若发散，则采用强跟踪Kalman滤波算法：

滤波收敛判据：以新息序列的性质构造滤波收敛判据，即 由于表示新息序列平方和，其包括了实际估计误差，而理论预测误差则可描述为新息序列方差阵即：

$E\left[{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T\right]=H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1}$

${\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T\&le;\mathrm{\&gamma;Tr}=[H_{k+1}{P}_{k+1,k}{H}_{k+1}^T+R_{k+1}]$

其中：γ≥1表示可调系数，如果成立，则代表滤波器正常工作，此时可以采用Sage-Husa自适应Kalman滤波算法对状态变量进行估计；如果不成立，则表示滤波实际误差超过理论预测误差的γ倍，滤波发散，此时采用强跟踪滤波算法计算加权系数λ_k+1自适应调整P_k，k-1，充分发挥当前时刻量测量作用，以抑制滤波的发散。

本发明的有益效果通过以下方法得以验证：

假设载体的航向角、纵摇角及横摇角的摇摆模型分别为：

$\left\{\begin{array}{c}\mathrm{\&psi;}={\mathrm{\&psi;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&psi;}}t+{\mathrm{\&phi;}}_{\mathrm{\&psi;}})\\ \mathrm{\&theta;}={\mathrm{\&theta;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&theta;}}t+{\mathrm{\&phi;}}_{\mathrm{\&theta;}})\\ \mathrm{\&gamma;}={\mathrm{\&gamma;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&gamma;}}t+{\mathrm{\&phi;}}_r)\end{array}\right.$

其中，ψ_m、θ_m和γ_m为相应的摇摆角幅值；ω_ψ、ω_θ和ω_γ为相应的摇摆角频率；ω_i＝2π／T_i(i=ψ，θ，r)，T_i为相应的摇摆周期；仿真时取ψ_m＝10°，θ_m=6°，γ_m＝12°，T_ψ＝6s，T_θ=8s，T_γ＝10s；φ_ψ、φ_θ和φ_r为相应的初始相位，由于受海浪等外界因素影响，取初始相位为φ_ψ＝φ_θ=φ_r=30°。

假设载体的初始位置为北纬45.7796°，东经126.6705°；系统状态初始估计值X(0)=0；初始位置误差10m；初始速度误差0.01m／s；陀螺漂移误差0.01°／h；加速度计误差10^-4g；GPS接收机的时钟偏差为20m，时钟噪声为0.1m／s，伪距测量标准差为15m，伪距率测量标准差为0.1m／s。自适应Kalman滤波中的遗忘因子b=0.6，强跟踪Kalman滤波中的α_i由先验知识确定为α_i＝a(i，i)，其中，

a=diag(1 1 2.3 2.3 1 1 1 1 1 1 1 1 1 1)

根据上述仿真条件，并假设真实量测噪声为R′＝10R，且载体做航向变化的机动，分别利用自适应Kalman滤波和强跟踪自适应Kalman滤波对水下航行器SINS／GPS组合导航系统进行仿真分析。失准角误差、速度误差和位置误差的仿真结果如图3至图6所示，其中，左图为自适应Kalman滤波的估计结果，右图为强跟踪自适应Kalman滤波的估计结果。

Claims (5)

1.一种基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于，包括以下步骤：

步骤1：将捷联惯导系统和GPS接收机安装在载体上，对捷联惯导系统预热后采集捷联惯导系统输出的位置信息与GPS输出的位置信息；

步骤2：建立SINS／GPS组合导航系统的状态方程与量测方程；

步骤3：设计Sage-Husa自适应滤波器；

步骤4：设计强跟踪Kalman滤波器；

步骤5：采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS／GPS组合导航方案。

2.根据权利要求1中所述的基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于：所述步骤2中建立SINS／GPS组合导航系统的状态方程与量测方程：

状态方程为：

其中：

X_GPS=[δt_u δt_ru]^T

W_GPS=[ε_tu ε_tru]^T

W_SINS=[w_gx w_gy w_ax w_ax w_ax 0_1×8]^T，

其中：δV_x、δV_y分别为东、北向速度误差；φ_x、φ_y和φ_z分别为东向、北向和方位失准角；δλ、δL分别为经纬度误差；ω_ie为地球自转角速度；L为当地纬度；R为地球半径；为捷联姿态矩阵；ε_x、ε_y和ε_z分别为陀螺漂移；w_gx、w_gy和w_gz为陀螺噪声；分别为加速度计零偏；w_ax、w_ay为加速度计噪声；δt_u表示接收机的钟差误差；δt_ru表示接收机钟差的漂移误差；τ_tu、τ_tru分别表示接收机的相关时间；ε_tu、ε_tru分别表示白噪声向量；

量测方程为：

在SINS／GPS组合导航系统中，将惯导解算的位置信息相对应的伪距、伪距率与GPS测量的伪距、伪距率之差作为量测值：

其中：i为可见的卫星数目；δρ_i=ρ_Ii-ρ_Gi，ρ_Ii为惯导输出的位置信息相对应的伪距，ρ_Gi为GPS接收机测量得到的伪距；H(t)为量测矩阵；v(t)为各元素为零均值的高斯白噪声，且互不相关。

3.根据权利要求1中所述的基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于：步骤3中所述设计Sage-Husa自适应滤波器，其具体算法如下：

假设线性离散系统方程为：

其中：X_k为状态向量，Φ_k，k-1为t_k-1时刻至t_k时刻的一步转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为系统噪声向量，Z_k为量测向量，H_k为量测矩阵，V_k为量测噪声向量；

Sage-Husa自适应Kalman滤波器设计如下：

计算一步预均方误差P_k，k-1：

计算滤波增益K_k：

计算均方误差P_k：

P_k=[I-K_kH_k]P_k，k-1

状态一步预测：

状态更新：

量测更新：

Sage-Husa自适应Kalman滤波中的量测噪声均值r和量测噪声方差阵R的计算方法如下：

其中：d_k=(1-b)／(1-b^k+1)，0<b<1表示遗忘因子；

4.根据权利要求1中所述的基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于：所述设计强跟踪Kalman滤波器，具体为：

通过用加权系数λ_k+1乘以状态量估计误差的验前方差阵得以实现：

其中：

λ_k+1＝diag[λ_1(k+1)，λ_2(k+1)，…，λ_m(k+1)]

其中：α_i由先验知识确定，为系统估计误差，v_0(k+1)为误差的方差阵。

5.根据权利要求1中所述的基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法，其特征在于：所述步骤5中的采用收敛判据对滤波的发散趋势进行判断，设计基于强跟踪自适应Kalman滤波的SINS／GPS组合导航方案，具体为：采用收敛判据对滤波的发散趋势进行判断，若不发散，则采用Sage-Husa自适应Kalman滤波算法，若发散，则采用强跟踪Kalman滤波算法：

滤波收敛判据：以新息序列的性质构造滤波收敛判据，即 由于表示新息序列平方和，其包括了实际估计误差，而理论预测误差则可描述为新息序列方差阵即：

其中：γ≥1表示可调系数，如果成立，则代表滤波器正常工作，此时可以采用Sage-Husa自适应Kalman滤波算法对状态变量进行估计；如果不成立，则表示滤波实际误差超过理论预测误差的γ倍，滤波发散，此时采用强跟踪滤波算法计算加权系数λ_k+1自适应调整P_k，k-1，充分发挥当前时刻量测量作用，以抑制滤波的发散。