CN104035110B - 应用于多模卫星导航系统中的快速卡尔曼滤波定位方法 - Google Patents

Abstract

本发明涉及一种应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，属于导航控制领域。该方法将多模卫星导航的系统模型采用多胞型微分包含系统(Ploytopic Linear Differential Inclusion，PLDIs)模型来描述，从而将非线性的多模卫星导航系统转换为线性定常的误差系统，降低了多模卫星导航系统中定位算法在线编程的复杂性。

Description

应用于多模卫星导航系统中的快速卡尔曼滤波定位方法

技术领域

本发明涉及一种应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，属于导航控制领域。

背景技术

全球导航卫星系统(Global Navigation Satellite Navigation-GNSS)是指美国的GPS系统，中国的北斗系统，俄罗斯的GLONASS系统和欧盟的伽利略系统。传统单星座卫星导航系统易受所有者权限限制，可靠性得不到保障；同时可见卫星的数量易受环境影响，在恶劣的环境下，可见卫星的数量较少，导致定位精度较低，甚至定位失败。把GNSS中两个或多个卫星导航系统组合，构成多模卫星导航系统可极大增加可见卫星的数量，提高导航的稳定性和精确性。

接收机定位算法是影响多模卫星导航系统中载体定位精度的关键因素之一，而其实质为对非线性系统的估计问题。对非线性系统的估计，由于需要无穷维的积分运算，因此在理论上很难找到最优解。当前对非线性系统的估计方法主要可分为三类，第一类是采用泰勒级数展开或插值多项式的方法对函数进行近似的方法；第二类是采用确定性采样的方法对非线性系统的概率密度分布进行近似；第三类是采用随机采样近似对非线性函数的概率密度分布进行近似。与三种算法对应的典型滤波算法分别是扩展卡尔曼滤波(ExtendKalman Filter，EKF)，无迹卡尔曼滤波(Unscented Kalman Filter，UKF)和粒子滤波(Particle Filter，PF)。粒子滤波是基于蒙特卡洛仿真的一种滤波方法，在理论上粒子滤波是最优的，而且适用于非高斯噪声的情况。但是粒子滤波存在的粒子退化、计算量大、实时性差等问题使其不能在卫星导航中广泛使用。UKF的基本思想是近似非线性函数的概率密度分布，而这要比近似非线性函数本身容易。它产生一组西格玛(sigma)点去近似非线性函数的概率密度分布，并通过非线性函数传播获得系统的验后均值和方差，与EKF相比，UKF无须计算雅克比矩阵，且估计精度较高。但是在数值计算过程中，UKF往往会存在舍入误差，这会破坏系统估计误差协方差矩阵的非负定性和对称性，导致滤波发散。EKF作为经典的非线性系统的估计方法，在卫星导航领域得到了广泛的应用。EKF采用泰勒级数的一阶展开式对非线性系统进行线性化，原理简单、易于实现、实时性好。但是EKF在每一时刻都需要更新雅克比矩阵，给编程制造了困难，同时增加了算法的复杂性。尤其在多模卫星导航系统中，由于可见卫星的数量较多，雅克比矩阵的维数较高，在每一时刻都更新雅克比矩阵将极大增加系统对硬件数据处理器的要求。

发明内容

本发明针对多模卫星导航系统中采用的扩展卡尔曼滤波(EKF)定位算法的接收机，在定位过程中需要实时更新雅克比矩阵导致在线编程复杂的问题，提出了一种应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，该方法将多模卫星导航的系统模型采用多胞型线性微分包含系统(Ploytopic Linear Differential Inclusion，PLDIs)模型来描述，从而将非线性的多模卫星导航系统转换为线性定常的误差系统，降低了多模卫星导航系统中定位算法在线编程的复杂性。

本发明的目的是通过以下技术方案实现的。

一种应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，具体包括如下步骤：

步骤1：针对全球定位系统(Global Positioning System,GPS)和北斗2号(BeiDou-2，BD-2)卫星导航系统，建立全球定位系统/北斗2号(GPS/BD-2)组合的多模卫星导航系统的数学模型，其中选取的坐标系为世界大地坐标系(World Geodetic System一1984Coordinate System，WGS-84)，坐标系的三轴分别用X_T，Y_T和Z_T表示。具体过程为：

步骤1.1：建立GPS/BD-2组合的多模卫星导航系统的状态方程。

用符号X表示GPS/BD-2组合的多模卫星导航系统状态方程的状态变量， $X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x},y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z},{\mathrm{\δt}}_u^{\mathrm{GPS}},{\stackrel{\·}{\mathrm{\δ}}t}_u^{\mathrm{GPS}},{\mathrm{\δt}}_u^{\mathrm{BD}},{\stackrel{\·}{\mathrm{\δ}}t}_u^{\mathrm{BD}}]}^T,$ 其中x,分别表示载体WGS-84坐标系下X_T轴方向上的位置、速度和加速度；y,分别表示载体在WGS-84坐标系下Y_T轴方向上的位置、速度和加速度；z,分别表示载体在Z_T方向上的位置、速度、和加速度。分别表示GPS接收机钟差和钟差漂移频率；分别表示BD-2接收机钟差和钟差漂移频率。建立离散状态方程如公式(1)所示。

X_k+1＝ΦX_k+U_k+w_k (1)

其中，X_k为k时刻GPS/BD-2组合卫星导航系统的状态；Φ为状态转移矩阵，其具体形式如公式(2)所示；U_k为GPS/BD-2组合卫星导航系统在k时刻的输入量，如公式(3)所示，U_k在每一采样周期为定值；w_k为系统在k时刻的过程噪声，其协方差矩阵用Q_k表示，计算公式如式(4)所示。

Φ＝diag(Φ_x,Φ_y,Φ_z,Φ_t,GPS,Φ_t,BD) (2)

其中， ${\mathrm{\Φ}}_x={\mathrm{\Φ}}_y={\mathrm{\Φ}}_z=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\λ}}^2}(-1+\mathrm{\λT}+e^{-\mathrm{\λT}})\\ 0& 1& \frac{1}{\mathrm{\λ}}(1-e^{-\mathrm{\λT}})\\ 0& 0& e^{-\mathrm{\λT}}\end{array}\right],{\mathrm{\Φ}}_{t,\mathrm{GPS}}={\mathrm{\Φ}}_{t,\mathrm{BD}}\left[\begin{array}{cc}1& (1-e^{-{\mathrm{\λ}}_{t}T})/{\mathrm{\λ}}_t\\ 0& e^{-{\mathrm{\λ}}_{t}T}\end{array}\right];$ Φ_x为在WGS-84坐标系中描述载体在X_T轴上的位置、速度和加速度的状态转移矩阵，Φ_y为在WGS-84坐标系中描述载体在Y_T轴上的位置、速度和加速度的状态转移矩阵，Φ_z为在WGS-84坐标系中描述载体在Z_T轴上的位置、速度和加速度的状态转移矩阵；Φ_t,GPS为描述GPS接收机钟差和钟差漂移频率的状态转移矩阵，Φ_t,BD为描述BD-2接收机钟差和钟差漂移频率的状态转移矩阵。λ为一阶时间相关常数的倒数，λ∈(0,1)；λ_t为一阶马尔科夫时间常数倒数，λ_t∈(0,1)。λ和λ_t的值由人为设定，T为系统采样周期。

$U_k={\left[\begin{array}{ccccc}{u}_x{\stackrel{\‾}{a}}_{x,k}& u_y{\stackrel{\‾}{a}}_{y,k}& u_z{\stackrel{\‾}{a}}_{z,k}& 0_{1\×2}& 0_{1\×2}\end{array}\right]}^T---\left(3\right)$

其中， $u_x=u_y=u_z=\left[\begin{array}{c}\frac{1}{\mathrm{\λ}}(-T+\frac{{\mathrm{\λT}}^2}{2}+\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}})\\ T-\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}}\\ 1-e^{-\mathrm{\λT}}\end{array}\right],$ 为载体在X_T轴上的加速度均值，为载体在Y_T轴上的加速度均值，为载体在Z_T轴上的加速度均值，和在每一采样周期内为常数；0_1×2表示1行2列的零值矩阵。

$Q_k=E\left[w_k{w}_k^T\right]=\mathrm{diag}(Q_{x,k},Q_{y,k},Q_{z.k},Q_{t,\mathrm{gps},k},Q_{t,\mathrm{bd},k})---\left(4\right)$

其中E(·)表示求期望。Q_x,k为WGS-84坐标系下k时刻载体在X_T轴上运动状态的过程噪声协方差矩阵；Q_y,k为WGS-84坐标系下k时刻载体在Y_T轴上运动状态的过程噪声协方差矩阵；Q_z,k为WGS-84坐标系下k时刻载体在Z_T轴上运动状态的过程噪声协方差矩阵；Q_t,gps,k为k时刻GPS接收机钟差和钟差漂移频率的过程噪声协方差矩阵，Q_t,bd为k时刻BD-2接收机钟差和钟差漂移频率的过程噪声协方差矩阵。

步骤1.2，建立GPS/BD-2组合卫星导航系统的观测方程。

用符号M表示在k时刻接收机观测到的GPS卫星的个数；用符号N表示在k时刻接收机观测到的BD-2卫星的个数，则建立k时刻的伪距观测方程如公式(5)和公式(6)所示。

${\mathrm{\ρ}}_{m,g}^k=\sqrt{{(x_k-x_{m,g}^k)}^2+{(y_k-y_{m,g}^k)}^2+{(z_k-z_{m,g}^k)}^2}+{\mathrm{\δt}}_{u,k}^{\mathrm{GPS}}+{\mathrm{\ϵ}}_{m,g}^k---\left(5\right)$

${\mathrm{\ρ}}_{n,b}^k=\sqrt{{(x_k-x_{n,b}^k)}^2+{(y_k-y_{n,b}^k)}^2+{(z_k-z_{n,b}^k)}^2}+{\mathrm{\δt}}_{u,k}^{\mathrm{BD}}+{\mathrm{\ϵ}}_{n,b}^k---\left(6\right)$

其中，为k时刻观测到的第m颗GPS卫星的伪距，m＝1,2...M；为k时刻观测到的第n颗BD-2卫星的伪距，n＝1,2...N；表示k时刻观测到的第m颗GPS卫星在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标；表示k时刻观测到的第n颗BD-2卫星在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标。[xk,yk,zk]为k时刻待求接收机在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标；为k时刻GPS接收机钟差，为k时刻BD-2接收机钟差。为k时刻第m颗GPS卫星的观测噪声，为k时刻第n颗BD-2卫星的观测噪声。为下文描述算法方便，将公式(5)和公式(6)所示的观测方程简写为公式(7)。

y_k＝h(X_k)+ε_k (7)

其中y_k为系统的观测量， $y_k={\left[\begin{array}{cccccccc}{\mathrm{\ρ}}_{1,g}^k& {\mathrm{\ρ}}_{2,g}^k& ...& {\mathrm{\ρ}}_{M,g}^k& {\mathrm{\ρ}}_{1,b}^k& {\mathrm{\ρ}}_{2,b}^k& ...& {\mathrm{\ρ}}_{N,b}^k\end{array}\right]}^T.$ h(·)为观测方程中的非线性函数。ε_k为系统的观测误差，ε_k的协方差矩阵用符号R_k表示，

公式(1)和公式(7)构成了GPS/BD-2组合卫星导航系统的系统模型，写成公式(8)的形式：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\ΦX}}_k+U_k+w_k\\ y_k=h\left(X_k\right)+{\mathrm{\ϵ}}_k\end{array}\right.---\left(8\right)$

步骤2：将GPS/BD-2组合卫星导航的系统方程转换为GPS/BD-2组合卫星导航系统的误差方程。具体为：

用符号表示系统在k-1时刻的滤波值，多模卫星导航系统在k时刻的误差方程为：

$\left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ \mathrm{\Δ}{y}_k=H\left({\hat{X}}_{k-1}\right){\mathrm{\ΔX}}_k+{\mathrm{\ϵ}}_k\end{array}\right.---\left(9\right)$

其中，ΔX_k为GPS/BD-2组合卫星导航系统在k时刻的状态偏差，Δy_k为GPS/BD-2组合卫星导航系统在k时刻的测量偏差 ${\mathrm{\Δy}}_k=y_k-h\left({\hat{X}}_k\right);$ 表示系统在k-1时刻的雅克比矩阵，即 $H\left({\hat{X}}_{k-1}\right)=\frac{\∂h}{\∂X}{|}_{X={\hat{X}}_{k-1}},$ 表示求函数的偏导。

步骤3：将GPS/BD-2组合卫星导航系统的误差方程(8)通过张量积模型转换的方法转换为多胞型线性微分包含系统模型。具体操作为：

步骤3.1：确定GPS/BD-2组合卫星导航系统状态X中X_TY_TZ_T三坐标轴上位置变量(x,y,z)的值域集合。值域集合用符号Ω_p表示，即Ω_p＝[c_x×d_x]×[c_y×d_y]×[c_z×d_z]，其中[c_x,d_x]为变量x的取值范围，[c_y,d_y]为变量y的取值范围，[c_z,d_z]为变量z的取值范围；Ω_p包含三个维度，分别用符号p_xp_yp_z表示。

步骤3.2：将值域集合Ω_p的每一个维度p_n(n＝x,y,z)划分为L个均匀分布的网格，L∈(0,100]，每个网格点用符号p_n,l表示，l＝1,2...L，c_n＜p_n,1＜p_n,2＜...＜p_n,L＜d_n。

步骤3.3：计算每个网格点的雅克比矩阵H(p_nl),n＝x,y,z,l＝1,2...L，并将得到的矩阵存入张量空间(用符号表示)中；

步骤3.4：对张量空间内的每模矩阵执行高阶奇异值分解，在每模矩阵奇异值分解时舍弃零和部分非零每模矩阵奇异值，并将每模矩阵奇异值分解得到的酉矩阵(用符号U_j表示，j＝1,2,3)进行标准化转换，转换后的矩阵用符号表示。经过3次高阶奇异值分解，将张量空间转换为近似张量空间(用符号表示)，其中为的近似张量空间，为经过高阶奇异值分解后产生的子张量空间。

步骤3.5：从子张量空间中提取多胞型线性微分包含系统模型的顶点矩阵(用符号H_i表示)，i＝1,2...r，H_i为常值，r为多胞型线性微分包含系统模型总的顶点个数。

步骤4：利用步骤3.5得到的顶点矩阵H_i将GPS/BD-2组合卫星导航系统的误差方程转换为GPS/BD-2组合卫星导航多胞型线性微分包含系统模型，即转换为r个顶点系统的凸组合形式，如公式(10)所示。

$\begin{array}{c}\left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_1{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\\ \left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_2{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\\ .\\ .\\ .\\ \left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_r{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\end{array}---\left(10\right)$

步骤5：设置GPS/BD-2组合卫星导航多胞型线性微分包含系统的在0时刻的滤波初值，其中在多胞型线性微分包含系统中状态在0时刻的初值用符号表示，估计误差协方差矩阵的初值用符号P₀表示，同时设置系统定位结束时间(用符号time表示)。

步骤6：状态一步预测。根据步骤5或步骤11得到的k-1时刻滤波值用公式(11)进行状态一步预测得到系统在k时刻的一步预测值(用符号表示)。其中当k＝1时，过步骤5获得，当k≥2时，通过步骤11获得。

${\hat{X}}_{k,k-1}={\mathrm{\Φ}\hat{X}}_{k-1}+U_{k-1}---\left(11\right)$

步骤7：对GPS/BD-2组合卫星导航多胞型线性微分包含系统模型的每个顶点系统进行卡尔曼滤波，获得每个顶点系统的误差估计值(用符号表示)和每个顶点的估计误差协方差矩阵(用符号表示)，其中i＝1,2...r。具体操作步骤为：

步骤7.1：估计误差协方差矩阵一步预测。根据步骤5或步骤11得到的k-1时刻的估计误差协方差矩阵(用符号P_k-1表示)通过公式(12)进行一步预测，获得系统在k时刻估计误差协方差矩阵的一步预测值(用符号P_k,k-1表示)。其中当k＝1时，P_k-1通过步骤5获得，当k≥2时，P_k-1通过步骤11获得。

P_k,k-1＝ΦP_k-1Φ^T+Q_k-1 (12)

步骤7.2：通过公式(13)计算每个顶点系统在k时刻的滤波增益(用符号表示)，其中i＝1,2...r。

$K_k^i=P_{k-1}{H}_i^T{[H_i{P}_{k,k-1}{H}_i^T+R_k]}^{-1}---\left(13\right)$

步骤7.3：通过公式(14)计算系统在k时刻的测量偏差(用符号Δy_k表示)。

${\mathrm{\Δy}}_k=y_k-{\hat{y}}_k=y_k-h\left({\hat{X}}_{k,k-1}\right)---\left(14\right)$

步骤7.4：通过公式(15)计算每个顶点在k时刻估计误差的校正量(用符号表示)。

${\mathrm{\ΔX}}_k^i=K_k^i{\mathrm{\Δy}}_k---\left(15\right)$

步骤7.5：通过公式(16)校正每个顶点在k时刻的估计误差协方差矩阵(用符号表示)。

$P_k^i=(I-K_k^i{H}_i)P_{k,k-1}{(I-K_k^i{H}_i)}^T+K_k^i{R}_k{\left(K_k^i\right)}^T---\left(16\right)$

步骤8：通过公式(17)计算顶点系统的融合系数(用符号α_i表示)。

${\mathrm{\α}}_i=\frac{{\left({\mathrm{\σ}}_i^{\left(3\right)}\right)}^{-2}}{\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\left({\mathrm{\σ}}_i^{\left(3\right)}\right)}^{-2}}---\left(17\right)$

其中为张量空间的3模矩阵的高阶奇异值分解的第i个奇异值，i＝1,2...r。

步骤9：在公式(17)的基础上通过公式(18)将每个顶点的估计误差进行融合，得到GPS/BD-2组合卫星导航多胞型线性微分包含系统模型误差的最优估计值(用符号表示)。

$\mathrm{\Δ}{\hat{X}}_k=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\α}}_i\mathrm{\Δ}{\hat{X}}_k^i---\left(18\right)$

步骤10：在公式(17)的基础上通过公式(19)对每个顶点系统的估计误差协方差矩阵融合，获得GPS/BD-2组合卫星导航系统在k时刻最优的估计误差协方差矩阵

$P_k^{-1}=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\α}}_i{\left(P_k^i\right)}^{-1}---\left(19\right)$

其中，为P_k的逆矩阵。

步骤11：在公式(18)的基础上通过公式(20)求取GPS/BD-2组合卫星导航系统在k时刻状态的最优估计值(用符号表示)。

${\hat{X}}_k={\hat{X}}_{k/k-1}+{\mathrm{\Δ}\hat{X}}_k---\left(20\right)$

步骤12：判断k是否到达设定的系统结束时间time，若未到达设定时间，则返回步骤(6)，重复步骤(6)到步骤(11)的操作；若到达设定时间，则结束操作。

经过上述步骤的操作，即可实现对运动载体的实时定位。

有益效果

该方法基于多胞型线性微分包含理论采用TP模型变换的方法将GPS/BD-2组合卫星导航误差系统转换为多胞型线性微分包含系统模型，从而将对非线性系统的滤波问题转化为对线性系统的滤波问题。与已有的非线性滤波方法相比，该方法无须在线更新雅克比矩阵，降低了算法在线计算的复杂性，提高了多模卫星导航算法的实时性。

附图说明

图1为本发明具体实施方式中应用于多模卫星导航系统中的快速卡尔曼滤波定位方法的操作流程图；

图2为本发明具体实施方式中东向位置误差结果图；

图3为本发明具体实施方式中北向位置误差结果图；

图4为本发明具体实施方式中高程误差结果图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实施例加以进一步说明。

本实验采用GPS/BD-2组合的卫星导航系统进行实验，仿真时间time设为400秒，在此时间内载体做速度为15m/s的匀速直线运动。在0时刻的速度初值和位置初值由一次最小二乘法获得，采用单点定位的方式。仿真方式为蒙特卡洛仿真，蒙特卡洛仿真次数为100。采用本发明提出的应用于多模卫星导航系统中的快速卡尔曼滤波定位方法对载体进行定位，其操作流程如图1所示，具体实施步骤为：

步骤1：建立GPS/BD-2组合卫星导航系统的数学模型。具体为：

步骤1.1：建立GPS/BD-2组合卫星导航系统的状态方程如公式(1)所示。公式(1)中， ${\mathrm{\Φ}}_x={\mathrm{\Φ}}_y={\mathrm{\Φ}}_z=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\λ}}^2}(-1+\mathrm{\λT}+e^{-\mathrm{\λT}})\\ 0& 1& \frac{1}{\mathrm{\λ}}(1-e^{-\mathrm{\λT}})\\ 0& 0& e^{-\mathrm{\λT}}\end{array}\right],{\mathrm{\Φ}}_{t,\mathrm{GPS}}={\mathrm{\Φ}}_{t,\mathrm{BD}}\left[\begin{array}{cc}1& (1-e^{-{\mathrm{\λ}}_{t}T})/{\mathrm{\λ}}_t\\ 0& e^{-{\mathrm{\λ}}_{t}T}\end{array}\right].$ λ为一阶时间相关时间常数的倒数，λ_t为一阶马尔科夫时间常数倒数。λ和λ_t设为0.1。 $U_k={\left[\begin{array}{ccccc}{u}_x{\stackrel{\‾}{a}}_{x,k}& u_y{\stackrel{\‾}{a}}_{y,k}& u_z{\stackrel{\‾}{a}}_{z,k}& 0_{1\×2}& 0_{1\×2}\end{array}\right]}^T,$ 其中 $u_x=u_y=u_z=\left[\begin{array}{c}\frac{1}{\mathrm{\λ}}(-T+\frac{{\mathrm{\λT}}^2}{2}+\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}})\\ T-\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}}\\ 1-e^{-\mathrm{\λT}}\end{array}\right],$ 为载体在x轴上的加速度均值，为载体在y轴上的加速度均值，为载体在z轴上的加速度均值。的值由人为设定，k＝1,2...400。在本实施例中，0_1×2表示1行2列的零值矩阵。

k时刻的过程噪声w_k的协方差矩阵用Q_k表示，计算公式如式(4)所示。在本实施例中， $Q_x=Q_y=Q_z=0.2\×\left[\begin{array}{ccc}\frac{T^5}{20}& \frac{T^4}{8}& \frac{T^3}{6}\\ \frac{T^4}{8}& \frac{T^3}{6}& \frac{T^2}{2}\\ \frac{T^3}{6}& \frac{T^2}{2}& T\end{array}\right]Q_{t,\mathrm{gps}}=Q_{t,\mathrm{bd}}=\begin{array}{}\end{array}\left[\begin{array}{cc}\frac{T^3}{3}& \frac{T^2}{2}\\ \frac{T^2}{2}& T\end{array}\right].$

步骤1.2：在k时刻观测到5颗GPS卫星和4颗北斗卫星，即M＝5，N＝4，则建立GPS/BD-2组合卫星导航系统的观测方程如公式(7)所示。GPS/BD-2组合卫星导航系统滤波模型如公式(8)所示。

步骤2：将GPS/BD-2组合卫星导航系统方程转换为GPS/BD-2组合卫星导航系统误差方程如公式(9)所示。

步骤3：将GPS/BD-2组合卫星导航系统误差方程(9)通过张量积模型转换的方法转换为GPS/BD-2组合卫星导航多胞型线性微分包含系统模型。

步骤3.1：确定GPS/BD-2组合卫星导航系统状态X中X_TY_TZ_T三坐标轴上位置变量(x,y,z)的值域集合。值域集合用符号Ω_p表示，即Ω_p＝[c_x×d_x]×[c_y×d_y]×[c_z×d_z]，其中[c_x,d_x]为变量x的取值范围，[c_y,d_y]为变量y的取值范围，[c_z,d_z]为变量z的取值范围；Ω_p包含三个维度，分别用符号p_xp_yp_z表示。

步骤3.2：将值域集合Ω_p的每一个维度p_n(n＝x,y,z)划分为L个均匀分布的网格，L＝10，每个网格点用符号p_n,l表示，l＝1,2...L，c_n＜p_n,1＜p_n,2＜...＜p_n,L＜d_n。

步骤3.3：计算每个网格点的雅克比矩阵H(p_nl),n＝x,y,z,l＝1,2...L，并将得到的矩阵存入张量空间中；

步骤3.4：对张量空间内的每模矩阵执行高阶奇异值分解，在每模矩阵奇异值分解时舍弃零和部分非零每模矩阵奇异值，并将每模矩阵奇异值分解得到的酉矩阵U_j进行标准化转换，j＝1,2,3，转换后的矩阵用符号表示。经过3次高阶奇异值分解，将张量空间转换为近似张量空间其中为的近似张量空间，为经过高阶奇异值分解后产生的子张量空间。

步骤3.5：从子张量中提取多胞型线性微分包含系统模型的顶点矩阵H₁,H₂其中H₁,H₂为常值矩阵。

步骤4：利用步骤3.5得到的顶点矩阵H₁,H₂将GPS/BD-2组合卫星导航系统的误差方程转换为GPS/BD-2组合卫星导航多胞型线性微分包含系统模型，即转换为r个顶点系统的凸组合形式，如公式(10)所示，其中r＝2。

步骤5：设置GPS/BD-2组合卫星导航多胞型线性微分包含系统的在0时刻的滤波初值，其中在多胞型线性微分包含系统中状态在0时刻的初值用符号表示，估计误差协方差矩阵的初值用符号P₀表示，同时设置系统定位结束时间time。

步骤6：状态一步预测。根据步骤5或步骤11得到的k-1时刻滤波值用公式(11)进行状态一步预测得到系统在k时刻的一步预测值其中当k＝1时，通过步骤5获得，当k≥2时，通过步骤11获得。

步骤7：对GPS/BD-2组合卫星导航多胞型线性微分包含系统模型的每个顶点系统进行卡尔曼滤波，获得每个顶点系统的误差估计值和每个顶点的估计误差协方差矩阵具体操作步骤为：

步骤7.1：估计误差协方差矩阵一步预测。根据步骤5或步骤11得到的k-1时刻的估计误差协方差矩阵P_k-1通过公式(12)进行一步预测，获得系统在k时刻估计误差协方差矩阵的一步预测值P_k,k-1。其中当k＝1时，P_k-1通过步骤5获得，当k≥2时，P_k-1通过步骤11获得。

步骤7.2：通过公式(13)计算每个顶点系统在k时刻的滤波增益和

步骤7.3：通过公式(14)计算系统在k时刻的测量偏差Δy_k。

步骤7.4：通过公式(15)计算每个顶点在k时刻估计误差的校正量和

步骤7.5：通过公式(16)校正每个顶点在k时刻的估计误差协方差矩阵和

步骤8：通过公式(17)计算顶点系统的融合系数α₁和α₂。

步骤9：在公式(17)的基础上通过公式(18)将每个顶点的估计误差进行融合，得到GPS/BD-2组合卫星导航多胞型线性微分包含系统模型误差在k时刻的最优估计值

步骤10：在公式(17)的基础上通过公式(19)对每个顶点系统的估计误差协方差矩阵融合，获得GPS/BD-2组合卫星导航系统在k时刻最优的估计误差协方差矩阵

步骤11：在公式(18)的基础上通过公式(20)求取GPS/BD-2组合卫星导航系统在k时刻状态的最优估计值

步骤12：判断k是否到达设定的系统结束时间400，若未到达设定时间，则返回步骤(6)，重复步骤(6)到步骤(11)的操作；若到达设定时间，则结束操作。

图2、图3和图4分别是蒙特卡洛仿真实验中东向位置误差、北向位置误差和高程误差结果图。由图2至图4可知，本发明提出的方法稳定在3西格玛(σ)误差内，说明本发明提出的方法是稳定的。

表1是本发明提出的快速卡尔曼滤波定位算法与已有的扩展卡尔曼滤波定位方法的比较。

表1扩展卡尔曼滤波定位算法与快速卡尔曼滤波定位算法定位误差的均值和方差

由表1可见，扩展卡尔曼滤波和快速卡尔曼滤波定位算法定位结果的均值和方差几乎相等，说明该发明提出的方法与已有的传统的扩展卡尔曼滤波定位方法相比估计性能相当。但是本发明提出的快速卡尔曼滤波算法降低了算法在线的编程量。

Claims (1)

1.应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，其特征在于：其操作步骤为：

步骤1：针对全球定位系统GPS和北斗2号BD-2卫星导航系统，建立全球定位系统/北斗2号GPS/BD-2组合的多模卫星导航系统的数学模型，其中选取的坐标系为世界大地坐标系WGS-84，坐标系的三轴分别用X_T，Y_T和Z_T表示；具体过程为：

步骤1.1：建立GPS/BD-2组合的多模卫星导航系统的状态方程；

用符号X表示GPS/BD-2组合的多模卫星导航系统状态方程的状态变量，其中分别表示载体WGS-84坐标系下X_T轴方向上的位置、速度和加速度；分别表示载体在WGS-84坐标系下Y_T轴方向上的位置、速度和加速度；分别表示载体在Z_T方向上的位置、速度、和加速度；分别表示GPS接收机钟差和钟差漂移频率；分别表示BD-2接收机钟差和钟差漂移频率；建立离散状态方程如公式(1)所示；

X_k+1＝ΦX_k+U_k+w_k (1)

其中，X_k为k时刻GPS/BD-2组合卫星导航系统的状态；Φ为状态转移矩阵，其具体形式如公式(2)所示；U_k为GPS/BD-2组合卫星导航系统在k时刻的输入量，如公式(3)所示，U_k在每一采样周期为定值；w_k为系统在k时刻的过程噪声，其协方差矩阵用Q_k表示，计算公式如式(4)所示；

Φ＝diag(Φ_x,Φ_y,Φ_z,Φ_t,GPS,Φ_t,BD) (2)

其中，Φ_x为在WGS-84坐标系中描述载体在X_T轴上的位置、速度和加速度的状态转移矩阵，Φ_y为在WGS-84坐标系中描述载体在Y_T轴上的位置、速度和加速度的状态转移矩阵，Φ_z为在WGS-84坐标系中描述载体在Z_T轴上的位置、速度和加速度的状态转移矩阵；Φ_t,GPS为描述GPS接收机钟差和钟差漂移频率的状态转移矩 阵，Φ_t,BD为描述BD-2接收机钟差和钟差漂移频率的状态转移矩阵；λ为一阶时间相关常数的倒数，λ∈(0,1)；λ_t为一阶马尔科夫时间常数倒数，λ_t∈(0,1)；λ和λ_t的值由人为设定，T为系统采样周期；

其中， 为载体在X_T轴上的加速度均值， 为载体在Y_T轴上的加速度均值， 为载体在Z_T轴上的加速度均值， 和在每一采样周期内为常数；0_1×2表示1行2列的零值矩阵；

其中E(·)表示求期望；Q_x,k为WGS-84坐标系下k时刻载体在X_T轴上运动状态的过程噪声协方差矩阵；Q_y,k为WGS-84坐标系下k时刻载体在Y_T轴上运动状态的过程噪声协方差矩阵；Q_z,k为WGS-84坐标系下k时刻载体在Z_T轴上运动状态的过程噪声协方差矩阵；Q_t,gps,k为k时刻GPS接收机钟差和钟差漂移频率的过程噪声协方差矩阵，Q_t,bd,k为k时刻BD-2接收机钟差和钟差漂移频率的过程噪声协方差矩阵；

步骤1.2，建立GPS/BD-2组合卫星导航系统的观测方程；

用符号M表示在k时刻接收机观测到的GPS卫星的个数；用符号N表示在k时刻接收机观测到的BD-2卫星的个数，则建立k时刻的伪距观测方程如公式(5)和公式(6)所示；

其中，为k时刻观测到的第m颗GPS卫星的伪距，m＝1,2...M；为k时刻观测到的第n颗BD-2卫星的伪距，n＝1,2...N；表示k时刻观测到的第m颗GPS卫星在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标；表示k时刻观测到的第n颗BD-2卫星在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标；[x_k,y_k,z_k]为k时刻待求接收机在WGS-84坐标系下X_T，Y_T和Z_T轴上的位置坐标；为k时刻GPS接收机钟差，为k时刻BD-2接收机钟差；为k时刻第m颗GPS卫星的观测噪声，为k时刻第n颗BD-2卫星的观测噪声；为下文描述算法方便，将公式(5)和公式(6)所示的观测方程简写为公式(7)；

y_k＝h(X_k)+ε_k (7)

其中y_k为系统的观测量，h(·)为观测方程中的非线性函数；ε_k为系统的观测误差，ε_k的协方差矩阵用符号R_k表示，

公式(1)和公式(7)构成了GPS/BD-2组合卫星导航系统的系统模型，写成公式(8)的形式：

步骤2：将GPS/BD-2组合卫星导航的系统方程转换为GPS/BD-2组合卫星导航系统的误差方程；具体为：

用符号表示系统在k-1时刻的滤波值，多模卫星导航系统在k时刻的误差方程为：

其中，ΔX_k为GPS/BD-2组合卫星导航系统在k时刻的状态偏差，Δy_k为GPS/BD-2组合卫星导航系统在k时刻的测量偏差 表示系统在k-1时刻的雅克比矩阵，即 表示求函数的偏导；

步骤3：将GPS/BD-2组合卫星导航系统的误差方程(8)通过张量积模型转换的方法转换为多胞型线性微分包含系统模型；具体操作为：

步骤3.1：确定GPS/BD-2组合卫星导航系统状态X中X_T Y_T Z_T三坐标轴上位 置变量(x,y,z)的值域集合；值域集合用符号Ω_p表示，即Ω_p＝[c_x×d_x]×[c_y×d_y]×[c_z×d_z]，其中[c_x,d_x]为变量x的取值范围，[c_y,d_y]为变量y的取值范围，[c_z,d_z]为变量z的取值范围；Ω_p包含三个维度，分别用符号p_x p_y p_z表示；

步骤3.2：将值域集合Ω_p的每一个维度p_n划分为L个均匀分布的网格，n＝x,y,z，L∈(0,100]，每个网格点用符号p_n,l表示，l＝1,2...L，c_n＜p_n,1＜p_n,2＜...＜p_n,L＜d_n；

步骤3.3：计算每个网格点的雅克比矩阵H(p_nl),n＝x,y,z,l＝1,2...L，并将得到的矩阵存入张量空间中；

步骤3.4：对张量空间内的每模矩阵执行高阶奇异值分解，在每模矩阵奇异值分解时舍弃零和部分非零每模矩阵奇异值，并将每模矩阵奇异值分解得到的酉矩阵U_j进行标准化转换，j＝1,2,3，转换后的矩阵用符号表示；经过3次高阶奇异值分解，将张量空间转换为近似张量空间 其中为的近似张量空间，为经过高阶奇异值分解后产生的子张量空间；

步骤3.5：从子张量空间中提取多胞型线性微分包含系统模型的顶点矩阵H_i，i＝1,2...r，H_i为常值，r为多胞型线性微分包含系统模型总的顶点个数；

步骤4：利用步骤3.5得到的顶点矩阵H_i将GPS/BD-2组合卫星导航系统的误差方程转换为GPS/BD-2组合卫星导航多胞型线性微分包含系统模型，即转换为r个顶点系统的凸组合形式，如公式(10)所示；

步骤5：设置GPS/BD-2组合卫星导航多胞型线性微分包含系统的在0时刻的滤波初值，其中在多胞型线性微分包含系统中状态在0时刻的初值用符号表示，估计误差协方差矩阵的初值用符号P₀表示，同时设置系统定位结束时间time；

步骤6：状态一步预测；根据步骤5或步骤11得到的k-1时刻滤波值用 公式(11)进行状态一步预测得到系统在k时刻的一步预测值其中当k＝1时，通过步骤5获得，当k≥2时，通过步骤11获得；

步骤7：对GPS/BD-2组合卫星导航多胞型线性微分包含系统模型的每个顶点系统进行卡尔曼滤波，获得每个顶点系统的误差估计值和每个顶点的估计误差协方差矩阵其中i＝1,2...r；具体操作步骤为：

步骤7.1：估计误差协方差矩阵一步预测；根据步骤5或步骤10得到的k-1时刻的估计误差协方差矩阵P_k-1通过公式(12)进行一步预测，获得系统在k时刻估计误差协方差矩阵的一步预测值P_k,k-1；其中当k＝1时，P_k-1通过步骤5获得，当k≥2时，P_k-1通过步骤10获得；

P_k,k-1＝ΦP_k-1Φ^T+Q_k-1 (12)

步骤7.2：通过公式(13)计算每个顶点系统在k时刻的滤波增益其中i＝1,2...r；

步骤7.3：通过公式(14)计算系统在k时刻的测量偏差Δy_k；

步骤7.4：通过公式(15)计算每个顶点在k时刻估计误差的校正量

步骤7.5：通过公式(16)校正每个顶点在k时刻的估计误差协方差矩阵

步骤8：通过公式(17)计算顶点系统的融合系数α_i；

其中为张量空间的3模矩阵的高阶奇异值分解的第i个奇异值，i＝1,2...r；

步骤9：在公式(17)的基础上通过公式(18)将每个顶点的估计误差进行融合，得到GPS/BD-2组合卫星导航多胞型线性微分包含系统模型误差的最优估计值

步骤10：在公式(17)的基础上通过公式(19)对每个顶点系统的估计误差协方差矩阵融合，获得GPS/BD-2组合卫星导航系统在k时刻的估计误差协方差矩阵P_k；

其中，为P_k的逆矩阵；

步骤11：在公式(18)的基础上通过公式(20)求取GPS/BD-2组合卫星导航系统在k时刻状态的最优估计值

步骤12：判断k是否到达设定的系统结束时间time，若未到达设定时间，则返回步骤6，重复步骤6到步骤11的操作；若到达设定时间，则结束操作；

经过上述步骤的操作，即可实现对运动载体的实时定位。