CN106501832A - 一种容错矢量跟踪gnss/sins深组合导航方法 - Google Patents

Abstract

本发明公开了一种容错矢量跟踪GNSS/SINS深组合导航方法。该方法步骤如下：一、载波鉴别器和码鉴别器输出作为对应通道的子滤波器的量测信息，同时利用故障检测函数对子滤波器的运行状态进行检测，子滤波器输出的信息输入导航滤波器估计相应的误差；二、组合导航滤波器接收GNSS跟踪通道与SINS输出的伪距和伪距率误差作为量测信息，利用估计的SINS***误差对SINS进行反馈校正；三、利用修正的SINS和导航滤波器输出的钟差、钟漂信息计算矢量跟踪环路参数，用以控制接收机的本地伪码、载波数控振荡器，以保持对输入信号的稳定跟踪。本发明方法提高了深组合导航***的容错性能，适用于部分信号被遮挡的环境，应用前景广阔。

Description

一种容错矢量跟踪GNSS/SINS深组合导航方法

技术领域

本发明属于卫星导航、惯性导航和卫星、惯性组合导航领域，特别是一种容错矢量跟踪GNSS/SINS深组合导航方法。

背景技术

全球导航卫星***(GNSS)是指各种全球和区域性导航***的总称。卫星导航***具有全天候、全时间的优点，但某些恶劣环境下如信号被遮挡、高动态环境、丛林地区环境下可能出现丢星失锁情况；捷联惯性导航***(SINS)是一种不依赖于任何外部信息、也不向外辐射能量的完全自主式导航***，具有体积小、数据更新率高、短时精度高的优点，但是具有导航精度随时间降低的致命缺点。因此常将两者结合起来以构成SINS/GNSS组合导航***，按照信息融合层次分为松组合导航***、紧组合导航***、超紧组合导航***和深组合导航***。

基于矢量跟踪的深组合导航***具有抗干扰性强的优点，同时基于矢量跟踪的深组合导航***具有瞬时桥接信号的能力，国内外学者对此进行了大量的研究。但是矢量跟踪环路中故障通道会影响其它通道工作甚至导致矢量跟踪环路无法工作，尤其是在部分卫星信号频繁被遮挡的情况下。

发明内容

本发明的目的在于提供一种容错矢量跟踪GNSS/SINS深组合导航方法，以提高GNSS/SINS深组合导航***在部分卫星信号被遮挡的环境下的稳定性。

本发明的技术解决方案为：一种容错矢量跟踪GNSS/SINS深组合导航方法，包括下列步骤：

步骤1，矢量跟踪环路设计：载波鉴别器和码鉴别器输出作为对应通道的子滤波器的量测信息，用来估计对应通道的伪距和伪距率误差，同时利用故障检测函数对子滤波器的运行状态进行检测，子滤波器输出的信息输入导航滤波器估计相应的误差；

步骤2，组合导航滤波器的构建：组合导航滤波器接收GNSS跟踪通道与SINS输出的伪距和伪距率误差作为量测信息，利用估计的SINS***误差对SINS进行反馈校正；

步骤3，跟踪环路参数计算：利用修正的SINS和导航滤波器输出的钟差、钟漂信息计算矢量跟踪环路参数，用以控制接收机的本地伪码、载波数控振荡器，以保持对输入信号的稳定跟踪。

本发明与现有技术相比其显著效果是：(1)采用故障检测函数检测通道子滤波器运行状态，进而判断通道运行状况的方法，提高了深组合导航***的容错性能；(2)采用惯性辅助技术，利用修正后的SINS导航参数和星历信息推测GNSS伪码相位和多普勒频移等信号跟踪参数，并将估计结果反馈到接收机内部对跟踪环路进行辅助，提高了***的鲁棒性。

附图说明

图1是本发明容错矢量跟踪GNSS/SINS深组合导航方法的结构框图。

具体实施方式

以下结合附图，对本发明做进一步详细说明。

如图1本发明容错矢量跟踪GNSS/SINS深组合导航方法，具体实现如下：

GNSS天线所接收的射频信号，以GPS卫星L1频点为例，卫星发射信号表示为：

其中，S_L1(t)为t时刻信号幅值，A为信号最大幅值，D(t)表示数据码，C(t)表示测距码，ω_L1表示信号频率，φ(t)表示相位，P(t)表示P码。

在不考虑噪声的情况下，GNSS接收机射频前端输出的中频信号模型为：

S_IF(t)表示信号中频信号幅值，D(t-τ)为时间延迟为τ的数据码，C(t-τ)为时间延迟为τ的测距码。

式中，τ为传输过程中的时间延迟，ω_IF为信号中频，Δω为多普勒频移，n(t)为噪声项。

输入的中频信号与本地振荡器发生的同相、正交信号相乘，经积分清和清零后产生I_P、Q_P、I_E、Q_E、I_L、Q_L六路信号，统称为基带I、Q信息：

式中，M为相干积分时间内的采样点数，d为本地C/A码超前滞后的间隔，T为相干积分时间，δτ为伪码相位跟踪误差，Δf和分别为积分间隔起始时刻本地参考信号与输入信号之间的载波频率差和载波相位差，R(δτ)为C/A码的相关函数， 为噪声项，A为信号最大幅值，D表示数据码；

跟踪环路中载波鉴频器输出z_carrier，计算公式如下：

式中，和分别为I通道和Q通道即时支路t₁时刻的采样值，和分别为I通道和Q通道即时支路紧接着t₁时刻之后的t₂时刻的采样值，t₁为k-1时刻的时间，t₂为k时刻的时间，sign(x)为符号函数，取值如下：

跟踪环路中码鉴别器选取归一化的非相干超前减滞后模型，得到码相位测量值z_code：

式中，I_E和Q_E分别为I通道和Q通道超前支路采样值，I_L和Q_L分别为I通道和Q通道滞后支路采样值；

步骤1，矢量跟踪环路设计：载波鉴别器和码鉴别器输出作为对应通道的子滤波器的量测信息，用来估计对应通道的伪距和伪距率误差，同时利用故障检测函数对子滤波器的运行状态进行检测，子滤波器输出的信息输入导航滤波器估计相应的误差；

(a)子滤波器模型的状态方程：

其中，Δρ_k+1、为k+1时刻伪距、伪距率误差，T是滤波周期，值为1ms，Δρ_k、为k时刻伪距、伪距率误差，v_ρ、分别为伪距、伪距率***噪声；

(b)子滤波器模型的量测方程：

其中，分别为通道n的伪距、伪距率量测值，由载波鉴别器和码鉴别器的输出经单位转换而来；Δρ_k、分别为k时刻伪距、伪距率误差，ω_ρ、分别为伪距、伪距率量测噪声；

c)子滤波器故障检测函数：

考虑线性离散***：

其中，x_k为k时刻的状态向量，Φ_k,k-1为状态转移矩阵，x_k-1为k-1时刻的状态向量，z_k为量测向量，H_k为量测矩阵，w_k-1和v_k分别为***噪声和量测噪声，并且满足

式中：Q_k≥0为***噪声方差阵；R_k＞0为量测噪声方差阵。w_k和为***噪声，v_k和为量测噪声。

P_k＝(I-K_kH_k)P_k,k-1 (21)

式中：为状态预测，x_k-1为k-1时刻状态向量，P_k,k-1为协方差阵预测值，P_k-1为k-1时刻的协方差矩阵，H_k是k时刻观测矩阵，是k时刻观测矩阵的转置，Q_k-1是k-1时刻***噪声方差阵，I是单位矩阵，K_k为滤波增益矩阵，P_k为协方差矩阵，定义残差：

当滤波器无故障时残差r_k是零均值高斯白噪声，方差为：

其中，A_k表示k时刻***方差阵；

当***发生故障时，残差r_k的均值将不为零，故障检测函数如下：

其中，λ_k表示k时刻故障检测函数输出的值；式中λ_k服从自由度为m的χ²分布，m为测量z_k维数；

判定准则如下：

其中，表示k时刻故障函数输出值服从卡方分布，T_D为设置的故障检测阈值；

c)导航滤波器模型的状态方程如下：

式中，t_b,k和t_b,k-1分别为k时刻和k-1时刻的接收机钟差，t_d,k和t_d,k-1分别为k时刻和k-1时刻的接收机钟漂；δx_k、δx_k-1分别为k时刻和k-1时刻x方向位置误差；δv_x,k、δv_x,k-1分别为k时刻和k-1时刻x方向速度误差；δy_k、δy_k-1分别为k时刻和k-1时刻y方向位置误差；δv_y,k、δv_y,k-1分别为k时刻和k-1时刻y方向速度误差；δz_k、δz_k-1分别为k时刻和k-1时刻z方向位置误差；δv_z,k、δv_z,k-1分别为k时刻和k-1时刻z方向速度误差；T_o为k时刻和k-1时刻之间间隔时间；ω_x、ω_vx、ω_y、ω_vy、ω_z、ω_vz、ω_b、ω_d为相应的噪声；c表示光在真空的传播速度；

d)导航滤波器模型的观测方程为：

观测量选取各通道的码相位测量值和载波频率测量值，观测量与状态量之间的关系如下：

Δρ＝h_xδx+h_yδy+h_zδz+c·t_b+w_code (27)

式中，h_x、h_y、h_z为视线矢量在x、y和z轴的分量；t_b为钟差；δx为x方向位置误差，δy为y方向位置误差，δz为z方向位置误差，δv_x为x方向速度误差，δv_y为y方向速度误差，δv_z为z方向的速度误差，t_d为钟漂，w_code和w_carrier分别为码相位跟踪误差噪声和载波频率跟踪误差噪声。

步骤2，组合导航滤波器的构建：组合导航滤波器接收GNSS跟踪通道与SINS输出的伪距和伪距率误差作为量测信息，利用估计的SINS***误差对SINS进行反馈校正；

a)***状态方程：

式中，X为***状态矢量，表示***状态矢量的导数，F为***状态转移矩阵，G为***噪声驱动矩阵，W为***噪声矢量，具体如下：

式中，ψ_e、ψ_n、ψ_u分别为东、北、天方向的平台失准角误差；δV_e、δV_n、δV_u分别为东、北、天方向的速度误差；δL、δλ、δh分别为纬度、经度及高度误差；ε_x、ε_y、ε_z分别为陀螺常值漂移在x、y、z轴上的分量；▽_x、▽_y、▽_z分别为加速度计常值偏置在x、y、z轴上的分量；δt_u为钟差引起的等效距离误差；δt_ru为钟漂引起的伪距率误差；F_N为对应9个基本导航参数的***阵；为在载体系到导航系转换矩阵；(τ为相关时间)；分别为陀螺在x、y、z三个轴向的量测零均值高斯白噪声；分别为加速度计在x、y、z三个轴向的量测零均值高斯白噪声； 分别为钟差和钟漂零均值高斯白噪声；

b)***量测方程：

Z＝HX+V (30)

式中，Z为观测矢量，H为观测矩阵，V为观测噪声矩阵，具体如下：

式中，δρ为伪距偏差，为伪距率偏差；

H为***量测矩阵，a_n1a_n2a_n3为卫星n和用户的视线矢量在三个坐标轴方向的投影分量；

上式中，E为导航星方向余弦阵，为地理系下位置误差到ECEF系的转换关系矩阵：

式中，H为***量测矩阵，a_n1a_n2a_n3为卫星n和用户的视线矢量在三个坐标轴方向的投影分量；E为导航星方向余弦阵，为地理系下位置误差到ECEF系的转换关系；e_ij为SINS解算位置到第i颗导航星的方向余弦，L、λ、h分别为载体的真实纬度、经度和高度，基准椭球体长半径：R_e＝6378137.0m，椭圆度：f＝1/298.257223563，卯酉圈曲率半径：R_n＝R_e(1+fsin²L)，基准椭球偏心率：

上式中，i＝1,2...n,j＝1,2,3，b_ij具体展开如下：

式中，V_ρ为伪距观测高斯白噪声，为伪距率观测高斯白噪声。

步骤3，跟踪环路参数计算：利用修正的SINS和导航滤波器输出的钟差、钟漂等信息计算矢量跟踪环路参数，用以控制接收机的本地伪码、载波数控振荡器，以保持对输入信号的稳定跟踪；

式中，卫星n和用户之间的k时刻的视线矢量，表示k卫星n和用户之间的距离，V_k-1分别为k-1时刻解算得到的修正之后的SINS***速度，x⁽ⁿ⁾、y⁽ⁿ⁾、z⁽ⁿ⁾为卫星号为n的GPS卫星的位置，为修正后的SINS***在地心地固坐标系下的位置；为卫星n与接收机之间的视线矢量，为k-1时刻通过星历解算出来的卫星n的速度；为k时刻的卫星位置；是指k时刻的卫星位置和k-1时刻卫星n的位置的差值；分别为k时刻的码相位、码频率和载波频率预测值；为k-1时刻的码相位解算值；c为真空中光速；t_d,k-1为k-1时刻的钟漂；f_code为C/A码的基准频率，为1.023MHz；f_carrier为载波L1的频率，为1575.42MHz。

本发明方法保留了矢量接收机的导航滤波器，设计了一种子滤波器和故障检测函数用来判断矢量跟踪接收机每一个通道是否存在异常，如果有异常能够及时检测出然后及时对故障通道进行隔离，提高了深组合导航***的容错性能，适用于部分信号被遮挡的环境，应用前景广阔。

Claims (4)

1.一种容错矢量跟踪GNSS/SINS深组合导航方法，其特征在于，包括下列步骤：

步骤1，矢量跟踪环路设计：载波鉴别器和码鉴别器输出作为对应通道的子滤波器的量测信息，用来估计对应通道的伪距和伪距率误差，同时利用故障检测函数对子滤波器的运行状态进行检测，子滤波器输出的信息输入导航滤波器估计相应的误差；

步骤2，组合导航滤波器的构建：组合导航滤波器接收GNSS跟踪通道与SINS输出的伪距和伪距率误差作为量测信息，利用估计的SINS***误差对SINS进行反馈校正；

步骤3，跟踪环路参数计算：利用修正的SINS和导航滤波器输出的钟差、钟漂信息计算矢量跟踪环路参数，用以控制接收机的本地伪码、载波数控振荡器，以保持对输入信号的稳定跟踪。

2.根据权利要求1所述的容错矢量跟踪GNSS/SINS深组合导航方法，其特征在于，步骤1所述利用故障检测函数对子滤波器的运行状态进行检测，具体如下：

(a)子滤波器模型的状态方程：

$\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\ρ}}_{k+1}\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ρ}}}_{k+1}\end{array}\right]=\left[\begin{array}{cc}0& T\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\ρ}}_k\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ρ}}}_k\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{\ρ}}\\ v_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]---\left(1\right)$

其中，Δρ_k+1、为k+1时刻伪距、伪距率误差，T是滤波周期，值为1ms，Δρ_k、为k时刻伪距、伪距率误差，v_ρ、分别为伪距、伪距率***噪声；

(b)子滤波器模型的量测方程：

$\left[\begin{array}{c}{z}_{code}^n\\ z_{carrier}^n\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\ρ}}_k\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ρ}}}_k\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{\ρ}}\\ {\mathrm{\ω}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]---\left(2\right)$

其中，分别为通道n的伪距、伪距率量测值，由载波鉴别器和码鉴别器的输出经单位转换而来；Δρ_k、分别为k时刻伪距、伪距率误差，ω_ρ、分别为伪距、伪距率量测噪声；

c)子滤波器故障检测函数：

考虑线性离散***：

$\left\{\begin{array}{c}{x}_k={\mathrm{\Φ}}_{k,k-1}{x}_{k-1}+w_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.---\left(3\right)$

其中，x_k为k时刻的状态向量，Φ_k,k-1为状态转移矩阵，x_k-1为k-1时刻的状态向量，z_k为量测向量，H_k为量测矩阵，w_k-1和v_k分别为***噪声和量测噪声，并且满足

$\left\{\begin{array}{cc}E\left\{w_k\right\}=0,& E\left\{w_k{w}_j^T\right\}=Q_k\\ E\left\{v_k\right\}=0,& E\left\{v_k{v}_j^T\right\}=R_k\\ E\left\{v_k{w}_j^T\right\}=0& \end{array}\right.---\left(4\right)$

式中：Q_k≥0为***噪声方差阵；R_k＞0为量测噪声方差阵，w_k和为***噪声，v_k和为量测噪声；

${\hat{x}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{x}}_{k-1}---\left(5\right)$

${\hat{x}}_k={\hat{x}}_{k,k-1}+K_k(z_k-H_k{\hat{x}}_{k,k-1})---\left(6\right)$

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(7\right)$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}---\left(8\right)$

P_k＝(I-K_kH_k)P_k,k-1 (9)式中：为状态预测，x_k-1为k-1时刻状态向量，P_k,k-1为协方差阵预测值，P_k-1为k-1时刻的协方差矩阵，H_k是k时刻观测矩阵，是k时刻观测矩阵的转置，Q_k-1是k-1时刻***噪声方差阵，I是单位矩阵，K_k为滤波增益矩阵，P_k为协方差矩阵，定义残差：

$r_k=(z_k-H_k{\hat{x}}_{k,k-1})---\left(10\right)$

当滤波器无故障时残差r_k是零均值高斯白噪声，方差为：

$A_k=H_k{P}_{k|k-1}{H}_k^T+R_k---\left(11\right)$

其中，A_k表示k时刻***方差阵；

当***发生故障时，残差r_k的均值将不为零，故障检测函数如下：

${\mathrm{\λ}}_k=r_k^T{A}_k^{-1}{r}_k---\left(12\right)$

其中，λ_k表示k时刻故障检测函数输出的值；式中λ_k服从自由度为m的χ²分布，m为测量z_k维数；

判定准则如下：

其中，表示k时刻故障函数输出值服从卡方分布，T_D为设置的故障检测阈值；

c)导航滤波器模型的状态方程如下：

$\left[\begin{array}{c}{\mathrm{\δx}}_k\\ {\mathrm{\δv}}_{x,k}\\ {\mathrm{\δy}}_k\\ {\mathrm{\δv}}_{y,k}\\ {\mathrm{\δz}}_k\\ {\mathrm{\δv}}_{z,k}\\ c\·t_{b,k}\\ c\·t_{d,k}\end{array}\right]=\left[\begin{array}{cccccccc}0& T_o& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& T_o& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& T_o& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& T_o\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}{\mathrm{\δx}}_{k-1}\\ {\mathrm{\δv}}_{x,k-1}\\ {\mathrm{\δy}}_{k-1}\\ {\mathrm{\δv}}_{y,k-1}\\ {\mathrm{\δz}}_{k-1}\\ {\mathrm{\δv}}_{z,k-1}\\ c\·t_{b,k-1}\\ c\·t_{d,k-1}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_{vx}\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_{vy}\\ {\mathrm{\ω}}_z\\ {\mathrm{\ω}}_{vz}\\ {\mathrm{\ω}}_b\\ {\mathrm{\ω}}_d\end{array}\right]---\left(14\right)$

式中，t_b,k和t_b,k-1分别为k时刻和k-1时刻的接收机钟差，t_d,k和t_d,k-1分别为k时刻和k-1时刻的接收机钟漂；δx_k、δx_k-1分别为k时刻和k-1时刻x方向位置误差；δv_x,k、δv_x,k-1分别为k时刻和k-1时刻x方向速度误差；δy_k、δy_k-1分别为k时刻和k-1时刻y方向位置误差；δv_y,k、δv_y,k-1分别为k时刻和k-1时刻y方向速度误差；δz_k、δz_k-1分别为k时刻和k-1时刻z方向位置误差；δv_z,k、δv_z,k-1分别为k时刻和k-1时刻z方向速度误差；T_o为k时刻和k-1时刻之间间隔时间；ω_x、ω_vx、ω_y、ω_vy、ω_z、ω_vz、ω_b、ω_d为相应的噪声；c表示光在真空的传播速度；

d)导航滤波器模型的观测方程为：

观测量选取各通道的码相位测量值和载波频率测量值，观测量与状态量之间的关系如下：

Δρ＝h_xδx+h_yδy+h_zδz+c·t_b+w_code (15)

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}=h_x{\mathrm{\δv}}_x+h_y{\mathrm{\δv}}_y+h_z{\mathrm{\δv}}_z+c\·t_d+w_{carrier}---\left(16\right)$

式中，h_x、h_y、h_z为视线矢量在x、y和z轴的分量；t_b为钟差；δx为x方向位置误差，δy为y方向位置误差，δz为z方向位置误差，δv_x为x方向速度误差，δv_y为y方向速度误差，δv_z为z方向的速度误差，t_d为钟漂，w_code和w_carrier分别为码相位跟踪误差噪声和载波频率跟踪误差噪声。

3.根据权利要求1所述的容错矢量跟踪GNSS/SINS深组合导航方法，其特征在于，步骤2中所述组合导航滤波器的构建，具体如下：

(a)***状态方程：

$\stackrel{\·}{X}=FX+GW---\left(17\right)$

式中，X为***状态矢量，表示***状态矢量的导数，F为***状态转移矩阵，G为***噪声驱动矩阵，W为***噪声矢量，具体如下：

$X={\left[\begin{array}{ccccccccccccccccc}{\mathrm{\ψ}}_e& {\mathrm{\ψ}}_n& {\mathrm{\ψ}}_u& {\mathrm{\δV}}_e& {\mathrm{\δV}}_n& {\mathrm{\δV}}_u& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\δt}}_u& {\mathrm{\δt}}_{ru}\end{array}\right]}^T$

$F_{17\×17}=\left[\begin{array}{ccc}{F}_N& F_S& 0_{9\×2}\\ 0_{6\×9}& 0_{6\×6}& 0_{6\×2}\\ 0_{2\×9}& 0_{2\×6}& F_G\end{array}\right],F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],F_G=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{{\mathrm{\δt}}_{ru}}\end{array}\right],$

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×2}\\ 0_{3\×3}& C_b^n& 0_{3\×2}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×2}\\ 0_{2\×3}& 0_{2\×3}& I_{2\×2}\end{array}\right],W={\left[\begin{array}{cccccccc}{W}_{{\mathrm{\ϵ}}_x}& W_{{\mathrm{\ϵ}}_y}& W_{{\mathrm{\ϵ}}_z}& W_{{\▿}_x}& W_{{\▿}_y}& W_{{\▿}_z}& W_{{\mathrm{\δt}}_u}& W_{{\mathrm{\δt}}_{ru}}\end{array}\right]}^T$

式中，ψ_e、ψ_n、ψ_u分别为东、北、天方向的平台失准角误差；δV_e、δV_n、δV_u分别为东、北、天方向的速度误差；δL、δλ、δh分别为纬度、经度及高度误差；ε_x、ε_y、ε_z分别为陀螺常值漂移在x、y、z轴上的分量；分别为加速度计常值偏置在x、y、z轴上的分量；δt_u为钟差引起的等效距离误差；δt_ru为钟漂引起的伪距率误差；F_N为对应9个基本导航参数的***阵；为在载体系到导航系转换矩阵；τ为相关时间；分别为陀螺在x、y、z三个轴向的量测零均值高斯白噪声；分别为加速度计在x、y、z三个轴向的量测零均值高斯白噪声； 分别为钟差和钟漂零均值高斯白噪声；

(b)***量测方程：

Z＝HX+V (18)

式中，Z为观测矢量，H为观测矩阵，V为观测噪声矩阵，具体如下：

$Z={\left[\begin{array}{cccccccc}{\mathrm{\δ\ρ}}^1& {\mathrm{\δ\ρ}}^2& ...& {\mathrm{\δ\ρ}}^n& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^1& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^2& ...& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

式中，δρ为伪距偏差，为伪距率偏差；

$H=\left[\begin{array}{cccc}{0}_{n\×6}& H_{\mathrm{\ρ}1}& 0_{n\×6}& H_{\mathrm{\ρ}2}\\ 0_{n\×3}& H_{\stackrel{\·}{\mathrm{\ρ}}1}& 0_{n\×9}& H_{\stackrel{\·}{\mathrm{\ρ}}2}\end{array}\right],H_{\mathrm{\ρ}1}=E\·D_{LLA}^{XYZ}={\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]}_{n\×3},H_{\mathrm{\ρ}2}={\left[\begin{array}{cc}1& 0\\ \·& \·\\ \·& \·\\ \·& \·\\ 1& 0\end{array}\right]}_{n\×2},$

矩阵： $E={\[e_{ij}\]}_{n\×3}=\left[\begin{array}{ccc}{e}_{11}& e_{12}& e_{13}\\ e_{21}& e_{22}& e_{23}\\ .& .& .\\ .& .& .\\ .& .& .\\ e_{n1}& e_{n2}& e_{n3}\end{array}\right],(i=1,2...n,j=1,2,3)$

$D_{LLA}^{XYZ}=\left[\begin{array}{ccc}-(R_n+h)\sin L\cos \mathrm{\λ}& -(R_n+h)\cos L\sin \mathrm{\λ}& \cos L\cos \mathrm{\λ}\\ -(R_n+h)\sin L\sin \mathrm{\λ}& (R_n+h)\cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}\\ \[R_n(1-e^2)+h\]\cos L& 0& \sin L\end{array}\right]$

式中，H为***量测矩阵，a_n1a_n2a_n3为卫星n和用户的视线矢量在三个坐标轴方向的投影分量；E为导航星方向余弦阵，为地理系下位置误差到ECEF系的转换关系；e_ij为SINS解算位置到第i颗导航星的方向余弦，L、λ、h分别为载体的真实纬度、经度和高度，基准椭球体长半径：R_e＝6378137.0m，椭圆度：f＝1/298.257223563，卯酉圈曲率半径：R_n＝R_e(1+fsin²L)，基准椭球偏心率：

$H_{\stackrel{\·}{\mathrm{\ρ}}1}={\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ b_{n1}& b_{n2}& b_{n3}\end{array}\right]}_{n\×3},H_{\stackrel{\·}{\mathrm{\ρ}}2}={\left[\begin{array}{cc}0& 1\\ \·& \·\\ \·& \·\\ \·& \·\\ 0& 1\end{array}\right]}_{n\×2},$

上式中，i＝1,2…n,j＝1,2,3，b_ij具体展开如下：

$\left\{\begin{array}{c}{b}_{i1}=-e_{i1}\sin \mathrm{\λ}+e_{i2}\cos \mathrm{\λ}\\ b_{i2}=-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}+e_{i3}\cos L\\ b_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

$V={\left[\begin{array}{cccccccc}{v}_{\mathrm{\ρ}}^1& v_{\mathrm{\ρ}}^2& ...& v_{\mathrm{\ρ}}^n& v_{\stackrel{\·}{\mathrm{\ρ}}}^1& v_{\stackrel{\·}{\mathrm{\ρ}}}^2& ...& v_{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

式中，V_ρ为伪距观测高斯白噪声，为伪距率观测高斯白噪声。

4.根据权利要求1所述的容错矢量跟踪GNSS/SINS深组合导航方法，其特征在于，步骤3中所述环路参数计算，具体如下：

$h_k^{\left(n\right)}={\left[\begin{array}{ccc}{h}_x^{\left(n\right)}& h_y^{\left(n\right)}& h_z^{\left(n\right)}\end{array}\right]}^T---\left(19\right)$

$h_x^{\left(n\right)}=\frac{x^{\left(n\right)}-\hat{x}}{{\hat{r}}^{\left(n\right)}},h_y^{\left(n\right)}=\frac{y^{\left(n\right)}-\hat{y}}{{\hat{r}}^{\left(n\right)}},h_z^{\left(n\right)}=\frac{z^{\left(n\right)}-\hat{z}}{{\hat{r}}^{\left(n\right)}}---\left(20\right)$

${\hat{r}}^{\left(n\right)}=\sqrt{{(x^{\left(n\right)}-\hat{x})}^2+{(y^{\left(n\right)}-\hat{y})}^2+{(z^{\left(n\right)}-\hat{z})}^2}---\left(21\right)$

${\widehat{\mathrm{\τ}}}_k^{\left(n\right)}={\mathrm{\τ}}_{k-1}^{\left(n\right)}+{({\mathrm{\ΔX}}_{k,k-1}^{\left(n\right)}-t_{k,k-1}{V}_{k-1})}^T{h}_k^{\left(n\right)}+t_{k,k-1}c---\left(22\right)$

${\hat{f}}_{code,k}^{\left(n\right)}\left(n\right)=\[1+t_{d,k-1}+{(V_{k-1}^{\left(n\right)}-V_{k-1})}^T{h}_k^{\left(n\right)}\]f_{code}/c---\left(23\right)$

${\hat{f}}_{carrier,k}^{\left(n\right)}=\[1+t_{d,k-1}+{(V_{k-1}^{\left(n\right)}-V_{k-1})}^T{h}_k^{\left(n\right)}\]f_{carrier}/c---\left(24\right)$

式中，卫星n和用户之间的k时刻的视线矢量，表示k卫星n和用户之间的距离，V_k-1分别为k-1时刻解算得到的修正之后的SINS***速度，x⁽ⁿ⁾、y⁽ⁿ⁾、z⁽ⁿ⁾为卫星号为n的GPS卫星的位置，为修正后的SINS***在地心地固坐标系下的位置；为卫星n与接收机之间的视线矢量，为k-1时刻通过星历解算出来的卫星n的速度；为k时刻的卫星位置；是指k时刻的卫星位置和k-1时刻卫星n的位置的差值；分别为k时刻的码相位、码频率和载波频率预测值；为k-1时刻的码相位解算值；c为真空中光速；t_d,k-1为k-1时刻的钟漂；f_code为C/A码的基准频率，为1.023MHz；f_carrier为载波L1的频率，为1575.42MHz。