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

摘要

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

说明书

技术领域

本发明属于导航技术领域，尤其是一种深组合导航系统中的卡尔曼滤波处 理方法。

背景技术

全球定位系统(GPS)与惯性导航系统(INS)具有极强的功能互补性，两者构 成的深组合导航系统得到了越来越广泛的应用。卡尔曼滤波处理方法是组合导 航系统处理数据最常用的算法。卡尔曼滤波器的有效性就取决于系统的可观测 性，对于不完全可观测的系统，不仅需要知道哪些状态变量是可观测的，而且 还需要知道每个状态变量的可观测程度。现有的卡尔曼滤波器虽然可以在对时 变系统进行卡尔曼滤波处理之前，应用PWCS方法定性地分析系统状态能否全 部被观测到，或者判断哪些系统状态(或状态的线性组合)的可观测，哪些系统 状态是不可观测的，然而其无法定量地给出某个系统状态在不同时段的可观测 度，系统的不完全可观测使得组合导航系统无法获得更高的定位精度。

发明内容

本发明的目的在于克服现有技术的不足，提供一种能够获得高定向精度的 深组合导航系统中的卡尔曼滤波处理方法。

本发明解决其技术问题是采取以下技术方案实现的：

一种深组合导航系统中的卡尔曼滤波处理方法，包括以下步骤：

(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，为速度误差，为位置误 差，ε_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{\text{·}}{\rho }1}\\ {\widetilde{ϵ}}_{\stackrel{·}{\rho }2}\\ .\\ .\\ .\\ {\widetilde{ϵ}}_{\stackrel{·}{\mathrm{\rho i}}}\end{array}\right)$

a_j3＝sinEl_j

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，并按照如下公式计算当前时间段的可观测性矩阵

${{\tilde{Q}}_j}^T=[{\left(H_j\right)}^T,{(H_j,A_j)}^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}{ccc}{\tilde{Q}}_1& {\tilde{Q}}_2···& {\tilde{Q}}_r\end{array}\right)---\left(4\right)$

(5)由GPS系统和惯性导航系统直接计算得到当前时间段的外观测量

$Z_j=\left(\begin{array}{c}{Z}_{\rho }\\ Z_{\stackrel{·}{\rho }}\end{array}\right);$

(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=\mathrm{1,2},...n,$${\sigma }_i~\max \left[\frac{v_i^T{\mathrm{zu}}_i}{{\sigma }_i}\right]---\left(6\right)$

其中，η_k表示第k个状态变量的可观测度；σ₀表示外观测量所对应的奇异 值；σ_i表示使(i＝1，2，…n)取得最大的奇异值；

(8)如果当前时间段不是最后的时间段，返回到第(3)步继续循环进行下一时 间段的分析，直至完成分析的全部时间段；

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

而且，所述步骤(9)中加入外界观测信息后建立如下的基于伪距/伪距率的量 测方程：

在公式(7)中：

其中：b_b为载体坐标系中已知基线向量，λ为载波波长，ΔN_j分别为差分载波相位及其整周模糊度，j为导航定位卫星号，s_j为卫星j至接 收机天线的方向余弦向量，b_e为地心地固坐标系中基线向量，为载波相位测 量噪声，与分别为计算姿态矩阵和真是姿态矩阵，且有φ为姿态误差角。

而且，所述的外界观测信息包括姿态信息、伪距和伪距率信息。

而且，所述的时间段的选值范围为0.5～1秒。

本发明的优点和积极效果是：

本发明通过PWCS方法对组合导航系统进行可观测性和可观测度分析，能够 判别出系统的可观测变量和不可观测变量以及可观测变量的可观测程度，并在 此基础上对卡尔曼滤波器进行改进设计，提高了组合导航系统的滤波估计效果， 在初始对准时能获得高对准精度、在导航阶段能获得高定向精度。本发明处理 方法简单，不需要进行实际的卡尔曼滤波计算，使得提前对卡尔曼滤波器进行 改进设计成为可能。

附图说明

图1是本发明的深组合导航系统框图；

图2是未加入外界观测信息的深组合导航系统可观测度直方图；

图3是加入外界观测信息后的全组合导航系统可观测度直方图。

具体实施方式

以下结合附图对本发明实施例做进一步详述：

一种深组合导航系统中的卡尔曼滤波处理方法，是在如图1所示的深组合 导航系统上实现的，该组合导航系统将GPS定位系统与惯性导航系统组合在一 起实现深组合导航功能。惯性导航系统将速度及位置信息传送给INS伪距伪距 率计算模块，INS伪距伪距率计算模块同时利用卫星星历计算卫星位置，进而得 到INS到GPS的伪距和伪距率，同时将计算得到的伪距和伪距率发送给滤波量 程计算模块；GPS接收机控制处理模块通过天线接收GPS信号并将惯性辅助变 量处理后也发送给滤波量程计算模块，滤波量程计算模块将计算处理后的伪距 伪距率的量测方程送入卡尔曼滤波器中，卡尔曼滤波器处理后将姿态误差、速 度误差、位置误差、陀螺漂移、加速度计零偏反馈给GPS接收机控制处理模块 和惯性导航系统中。

一种深组合导航系统中的卡尔曼滤波处理方法，包括以下步骤：

(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为速度误差，为位置 误差，ε_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{\text{·}}{\rho }1}\\ {\widetilde{ϵ}}_{\stackrel{·}{\rho }2}\\ .\\ .\\ .\\ {\widetilde{ϵ}}_{\stackrel{·}{\mathrm{\rho i}}}\end{array}\right)$

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并通过如下公式计算当前时间段的可观测性矩阵

${{\tilde{Q}}_j}^T=[{\left(H_j\right)}^T,{(H_j,A_j)}^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}{ccc}{\tilde{Q}}_1& {\tilde{Q}}_2...& {\tilde{Q}}_r\end{array}\right)---\left(4\right)$

(5)由GPS和INS直接计算得到当前时间段的外观测量Z_j

(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=\mathrm{1,2},...n,$${\sigma }_i~\max \left[\frac{v_i^T{\mathrm{zu}}_i}{{\sigma }_i}\right]---\left(6\right)$

其中，η_k表示第k个状态变量的可观测度；σ₀表示外观测量所对应的奇异 值；σ_i表示使(i＝1，2，…n)取得最大的奇异值；

(8)如果当前时间段不是最后的时间段，返回到第(3)步继续循环进行下一时 间段的分析，直至完成分析的全部时间段；

(9)根据状态变量X(0)的可观测性和可观测度分析的结果，能够判断出系统 的各个变量的可观测性以及可观测变量的可观测度，采用如下三种方式之一进 行卡尔曼滤波器的改进设计和参数选择：加入外界观测信息(比如姿态信息、 速度信息等)来提高系统滤波的效果，通过去掉系统状态方程中不可观测的变 量和可观测度低的变量，即采用降阶滤波的方法来提高滤波估计的效果和减少 系统运算量；此外还可以通过改变系统的机动性来提高滤波估计效果；

下面以增加外界观测信息方式提高系统滤波估计效果为例进行说明，即通 过加入GPS载波相位姿态观测量后按照如下公式建立全组合导航系统的量测方 程如下：

在公式(7)中：

其中：b_b为载体坐标系中已知基线向量，λ为载波波长，ΔN_j分别为差分载波相位及其整周模糊度，j为导航定位卫星号，s_j为卫星j至接 收机天线的方向余弦向量，b_e为地心地固坐标系(简称e系)中基线向量，为 载波相位测量噪声。与分别为计算姿态矩阵和真是姿态矩阵，且有 φ为姿态误差角。

通过上述步骤可以实现本发明的卡尔曼滤波处理方法。在表1中给出了在 深组合导航系统加入外界观测信息(GPS载波相位姿态观测量)后构成的全组 合导航系统和深组合导航系统在第2时间段(2#)的可观测度计算结果，为确保系 统矩阵的变化量可以忽略不计，系统分段时间间隔应该取为较小值，例如0.5～ 1秒，本实施例取时间段为0.5秒。从表1可以看出深组合导航系统为不完全可 观测系统，某些误差状态变量无法估计，而全组合导航系统为完全可观测系统。 图2和图3分别给出了未加入外界观测信息的深组合导航系统可观测度直方图 和加入外界观测信息后的全组合导航系统可观测度直方图，通过对比分析可以 看出，加入外界观测信息后的全组合导航系统各状态变量的可观测度明显提高。

从计算结果可以看出，加入GPS姿态测量信息组成的全组合导航系统在任 何时刻都是完全可观测系统，相对深组合导航系统具有更好的滤波效果，使得 全组合导航系统在初始对准时能获得更高的对准精度、在导航阶段能获得更高 的定位和定向精度。

表1全组合导航系统和深组合导航系统可观测度

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此 本发明并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本 发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。