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

权利要求书

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_lj

b_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)的可观测性和可观测度分析的结果，判断各个状态变量的可观测性以及可观测变量的可观测度，并采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择：加入外界观测信息、去除系统状态方程中不可观测的变量和可观测度低的变量、改变系统的机动性。