[发明专利]一种深组合导航系统中的卡尔曼滤波处理方法

摘要

本发明涉及一种深组合导航系统中的卡尔曼滤波处理方法，包括以下步骤：(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(t)=(fI)15×15015×202×15(fG)2×217×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=ZρZρ·,]]>H=HρHρ·=0i×30i×3Hρ10i×30i×3Hρ20i×3Hρ·10i×30i×30i×3Hρ·22i×17,]]>V=VρVρ·]]>Z_ρ＝[δρ₁ δρ₂…δρ_i]^T，Zρ·=δρ·1δρ·2···δρ·iT;]]>H_ρ＝[0_i×3 0_i×3 H_ρ1 0_i×3 0_i×3 H_ρ2]_i×17，Hρ·=0i×3Hρ·10i×30i×30i×3Hρ·2i×17;]]>Hρ1=a11a12a13a21a22a23·········ai1ai2ai3i×3,]]>Hρ·1=b11b12b13b21b22b23·········bi1bi2bi3i×3,]]>Hρ2=-10-10······-10i×2,]]>Hρ·2=0-10-1······0-1i×2]]>Vρ=ϵ~ρ1ϵ~ρ2···ϵ~ρi,]]>Vρ·=ϵ~ρ·1ϵ~ρ·2···ϵ~ρ·i]]>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，并按照如下公式计算当前时间段的可观测性矩阵Q~jT=[(Hj)T,(HjAj)T,(HjAj2)T,···(HjAjn-1)T]---(3)]]>(4)按照如下公式计算当前的SOM矩阵Q~s(r)=Q~1Q~2···Q~r---(4)]]>(5)由GPS系统和惯性导航系统直接计算得到当前时间段的外观测量(6)按照如下公式计算出当前时间段可观测性矩阵的奇异值σ_i：Q~s(r)=VS000m×nUH,]]>S＝diag(σ₁，σ₂，…σ_r)，σ₁≥…≥σ_r＞0    (5)(7)按照如下公式求出每一个奇异值所对应的状态变量X(0)的大小，并根据状态变量X(0)的大小判断出相应变量的可观测性和可观测度，并且计算出可观测变量的可观测程度：X(0)=Σi=1rui(viHzσi),]]>ηk=σiσ0,]]>i＝1，2，…n，σi~max[viTzuiσi]---(6)]]>其中，η_k表示第k个状态变量的可观测度；σ₀表示外观测量所对应的奇异值；σ_i表示使(i＝1，2，…n)取得最大的奇异值；(8)如果当前时间段不是最后的时间段，返回到第(3)步继续循环进行下一时间段的分析，直至完成分析的全部时间段；(9)根据状态变量X(0)的可观测性和可观测度分析的结果，判断各个状态变量的可观测性以及可观测变量的可观测度，并采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择：加入外界观测信息、去除系统状态方程中不可观测的变量和可观测度低的变量、改变系统的机动性。