一种深组合导航系统中的卡尔曼滤波处理方法

摘要

本发明涉及一种深组合导航系统中的卡尔曼滤波处理方法，包括以下步骤：(1)建立滤波状态方程和基于伪距/伪距率量测方程；(2)选取时变系统的第一时间段；(3)计算当前时间段的可观测性矩阵；(4)计算当前的SOM矩阵；(5)计算当前时间段的外观测量；(6)计算当前时间段可观测性矩阵的奇异值；(7)求出每一个奇异值所对应的状态变量X(0)的大小，进行可观测性分析和可观测度计算；(8)循环执行(3)～(8)直至完成分析的全部时间段；(9)根据状态变量X(0)的可观测性和可观测度分析的结果，进行卡尔曼滤波器的改进设计和参数选择。本发明通过对组合导航系统进行可观测性和可观测度分析，根据分析结果对卡尔曼滤波器进行改进设计，提高了组合导航系统的滤波估计效果和导航精度。

主权项

1.一种深组合导航系统中的卡尔曼滤波处理方法，其特征在于：包括以下步骤：(1)建立卡尔曼滤波器的滤波状态方程和基于伪距/伪距率量测方程：①按如下公式建立卡尔曼滤波器的滤波状态方程：X(t)＝A(t)X(t)+w，w～N(0，Q)          (1)在公式(1)中$A\left(t\right)={\left[\begin{array}{cc}{\left(f_I\right)}_{15\times 15}& 0_{15\times 2}\\ 0_{2\times 15}& {\left(f_G\right)}_{2\times 2}\end{array}\right]}_{17\times 17},$w为白噪声，其中：为姿态误差，δv_E，δv_N，δv_U为速度误差，δλ，δh为位置误差，ε_x，ε_y，ε_z为陀螺漂移，为加速度计零偏；δt_u，δt_ru为接收机时钟偏差和时钟漂移，f_I为INS误差状态矩阵，f_G为GPS误差状态矩阵；②按如下公式建立基于伪距/伪距率的量测方程：Z＝HX+V                               (2)在公式(2)中$Z=\left[\begin{array}{c}{Z}_{\rho }\\ Z_{\stackrel{·}{\rho }}\end{array}\right],$$H=\left[\begin{array}{c}{H}_{\rho }\\ H_{\stackrel{·}{\rho }}\end{array}\right]={\left[\begin{array}{cccccc}{0}_{i\times 3}& 0_{i\times 3}& H_{\rho 1}& 0_{i\times 3}& 0_{i\times 3}& H_{\rho 2}\\ 0_{i\times 3}& H_{\stackrel{·}{\rho }1}& 0_{i\times 3}& 0_{i\times 3}& 0_{i\times 3}& H_{\stackrel{·}{\rho }2}\end{array}\right]}_{2i\times 17},$$V=\left[\begin{array}{c}{V}_{\rho }\\ V_{\stackrel{·}{\rho }}\end{array}\right]$Z_ρ＝[δρ₁ δρ₂…δρ_i]^T，$Z_{\stackrel{·}{\rho }}={\left[\begin{array}{cccc}\delta {\stackrel{·}{\rho }}_1& \delta {\stackrel{·}{\rho }}_2& ···& \delta {\stackrel{·}{\rho }}_i\end{array}\right]}^T;$H_ρ＝[0_i×3 0_i×3 H_ρ1 0_i×3 0_i×3 H_ρ2]_i×17，$H_{\stackrel{·}{\rho }}={\left[\begin{array}{cccccc}{0}_{i\times 3}& H_{\stackrel{·}{\rho }1}& 0_{i\times 3}& 0_{i\times 3}& 0_{i\times 3}& H_{\stackrel{·}{\rho }2}\end{array}\right]}_{i\times 17};$$H_{\rho 1}={\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ ·& ·& ·\\ ·& ·& ·\\ ·& ·& ·\\ a_{i1}& a_{i2}& a_{i3}\end{array}\right]}_{i\times 3},$$H_{\stackrel{·}{\rho }1}={\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ ·& ·& ·\\ ·& ·& ·\\ ·& ·& ·\\ b_{i1}& b_{i2}& b_{i3}\end{array}\right]}_{i\times 3},$$H_{\rho 2}={\left[\begin{array}{cc}-1& 0\\ -1& 0\\ ·& ·\\ ·& ·\\ ·& ·\\ -1& 0\end{array}\right]}_{i\times 2},$$H_{\stackrel{·}{\rho }2}={\left[\begin{array}{cc}0& -1\\ 0& -1\\ ·& ·\\ ·& ·\\ ·& ·\\ 0& -1\end{array}\right]}_{i\times 2}$$V_{\rho }=\left[\begin{array}{c}{\widetilde{ϵ}}_{\rho 1}\\ {\widetilde{ϵ}}_{\rho 2}\\ ·\\ ·\\ ·\\ {\widetilde{ϵ}}_{\mathrm{\rho i}}\end{array}\right],$$V_{\stackrel{·}{\rho }}=\left[\begin{array}{c}{\widetilde{ϵ}}_{\stackrel{·}{\rho }1}\\ {\widetilde{ϵ}}_{\stackrel{·}{\rho }2}\\ ·\\ ·\\ ·\\ {\widetilde{ϵ}}_{\stackrel{·}{\rho }i}\end{array}\right]$a_j3＝sinE_ljb_j1＝cosEl_jsinAz_j，b_j2＝cosEl_jcosAz_j，b_j3＝sinEl_j其中：i为参与导航定位可见卫星数，j为卫星号(j＝1，2，...，i)；δρ_i和分别为伪距差和伪距率差；V_ρ和分别为伪距和伪距率残留误差；Az_j和El_j分别为第j颗卫星的高度角和方位角；R_N地球子午圈曲率半径；h为高度；纬度；e为椭球扁率；(2)选取时变系统的第一时间段，令时间段变量j＝1；(3)定义A_j和H_j，并按照如下公式计算当前时间段的可观测性矩阵${\tilde{Q}}_j^T=[{\left(H_j\right)}^T,{\left(H_j{A}_j\right)}^T,{\left(H_j{A}_j^2\right)}^T,···{\left(H_j{A}_j^{n-1}\right)}^T]---\left(3\right)$(4)按照如下公式计算当前的SOM矩阵${\tilde{Q}}_s\left(r\right)=\left[\begin{array}{cccc}{\tilde{Q}}_1& {\tilde{Q}}_2& ···& {\tilde{Q}}_r\end{array}\right]---\left(4\right)$(5)由GPS系统和惯性导航系统直接计算得到当前时间段的外观测量(6)按照如下公式计算出当前时间段可观测性矩阵的奇异值σ_i：${\tilde{Q}}_s\left(r\right)=V{\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]}_{m\times n}{U}^H,$S＝diag(σ₁，σ₂，…σ_r)，σ₁≥…≥σ_r＞0    (5)(7)按照如下公式求出每一个奇异值所对应的状态变量X(0)的大小，并根据状态变量X(0)的大小判断出相应变量的可观测性和可观测度，并且计算出可观测变量的可观测程度：$X\left(0\right)=\underset{i=1}{\overset{r}{\Sigma }}{u}_i\left(\frac{v_i^{H}z}{{\sigma }_i}\right),$${\eta }_k=\frac{{\sigma }_i}{{\sigma }_0},$i＝1，2，…n，${\sigma }_i~\max \left[\frac{v_i^{T}z{u}_i}{{\sigma }_i}\right]---\left(6\right)$其中，η_k表示第k个状态变量的可观测度；σ₀表示外观测量所对应的奇异值；σ_i表示使(i＝1，2，…n)取得最大的奇异值；(8)如果当前时间段不是最后的时间段，返回到第(3)步继续循环进行下一时间段的分析，直至完成分析的全部时间段；(9)根据状态变量X(0)的可观测性和可观测度分析的结果，判断各个状态变量的可观测性以及可观测变量的可观测度，并采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择：加入外界观测信息、去除系统状态方程中不可观测的变量和可观测度低的变量、改变系统的机动性。