CN102830415B - 一种降维度的基于Carlson滤波算法的快速组合导航方法 - Google Patents

Abstract

本发明提出了一种降维度的基于Carlson滤波算法的快速组合导航方法，该方法主要包括数据融合、位置补偿两个部分。数据融合部分采用了降维度的基于Carlson滤波算法的数据融合算法，通过数据融合算法实现对INS模块速度与姿态信息以及INS器件的误差信息的建模。位置补偿部分利用卫星接收机观测到的伪距信息解算出位置信息对INS模块解算出的位置信息进行校正。本发明通过对状态方程与量测方程进行修改，在满足对INS导航模块的准确校正的基础上，使得系统的维数由15维降低到12维。降低了系统的运算的数据量，同时采用Carlson滤波器作为数据融合滤波器，在Carlson滤波器中对均方误差阵及均方误差阵的估计进行上三角分解来保证矩阵的正定性，可以有效地避免滤波器的发散。

Description

一种降维度的基于Carlson滤波算法的快速组合导航方法

技术领域

本发明涉及卫星导航与惯性导航领域，是一种应用于组合导航系统的快速抗发散的组合导航算法。具体为一种降维度的基于Carlson滤波算法的快速组合导航方法。

背景技术

近年来，随着卫星导航系统的发展，卫星导航系统在日常工作与生活中占有了越来越重要的地位。GPS接收机可以全天候，高精度的对载体的位置进行定位。在卫星信号良好的情况下，GPS接收机可以连续且有效地实现定位的功能。但是GPS接收机依赖于外部的信号，当外部信号缺失以及在强干扰以及高动态的条件下，传统的GPS接收机难以完成正常的定位与导航的功能。

INS导航系统属于自主式定位系统，不依赖于外部信号的辅助，但是由于INS导航系统属于自主积分式导航系统，导航系统会存在误差的积累，即定位误差随着时间的推移而发散。如果能够将GPS系统与INS系统进行组合，则可以有效的克服两种定位系统各自的缺点，实现更好的定位效果。

为了设计全天候可运行的导航系统，近年来众多学者针对GPS/INS组合导航系统展开了大量的研究工作。组合导航算法是INS利用GPS系统提高自身导航性能的关键。目前的研究工作基本围绕着组合导航算法展开，但是设计的组合导航算法仍有许多问题存在。首先，组合导航系统的状态方程很大多为15维的状态方程。如此大维度的状态方程提高了系统的硬件实现难度。其次，基于GPS/INS的组合系统，工程界常采用EKF（Extened Kalman Filter）算法，该算法通过Taylor展开对非线性系统函数进行局部近似，强非线性条件下易导致滤波器性能下降，有时也很难计算Jacobian矩阵。EKF因舍弃高阶项、采用局部线性化近似可能导致滤波精度下降、甚至发散等问题。许多研究者针对EKF算法的缺点提出了UKF算法等非线性滤波算法以解决滤波发散的问题，但是随之而来的问题是运算量近一步的加大，极大地增加了硬件实现的难度。

发明内容

要解决的技术问题

针对目前现有的组合导航算法数据量巨大，难以硬件实现以及数据融合滤波器易发散等缺点，本发明提出了一种降维度的基于Carlson滤波算法的快速组合导航方法。该方法通过降低组合导航滤波器的状态维度以及采用Carlson滤波算法，既降低了系统的运算量，又有效提高了系统的抗发散能力。

技术方案

本方法主要包括数据融合、位置补偿两个部分。数据融合部分采用了降维度的基于Carlson滤波算法的数据融合算法，通过数据融合算法实现对INS模块速度与姿态信息以及INS器件的误差信息的建模。位置补偿部分利用卫星接收机观测到的伪距信息解算出位置信息，以对INS模块解算出的位置信息进行校正。

本发明的技术方案为：

所述一种降维度的基于Carlson滤波算法的快速组合导航方法，其特征在于：包括以下步骤：

步骤1：利用由GPS接收机得到的卫星位置信息与伪距信息解算出用户的位置(x y z)和用户的速度(v_x v_y v_z)；

步骤2：利用GPS接收机得到的星历信息解算出所需卫星的位置信息和速度信息，其中第i号卫星的位置信息为(xⁱ yⁱ zⁱ)，速度信息为 $\left(\begin{array}{ccc}{\stackrel{.}{x}}^i& {\stackrel{.}{y}}^i& {\stackrel{.}{z}}^i\end{array}\right);$ 由公式

$\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}=\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^i-\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^j$

$=(e_1^i-e_1^j)\mathrm{\δ}{v}_x+(e_2^i-e_2^j)\mathrm{\δ}{v}_y+(e_3^i-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}$

$\begin{array}{c}.\\ .\\ .\end{array}$

$\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}=\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^m-\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^j$

$=(e_1^m-e_1^j)\mathrm{\δ}{v}_x+(e_2^m-e_2^j)\mathrm{\δ}{v}_y+(e_3^m-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}$

得到数据融合系统的观测量集合 $Z=\left[\begin{array}{ccc}\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}& \·\·\·& \mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}\end{array}\right],$ 其中δv_x、δv_y、δv_z为速度误差信息，i、j、m为相应的卫星序号，

为GPS接收机到的第i号卫星的方向矢量在大地坐标系上的投影；

为GPS接收机相对于第i号卫星的伪距率残差信息：

为第i号卫星相对GPS接收机的伪距率预测值，

为第i号卫星相对于GPS接收机的伪距率量测值，

为第i号卫星与第j号卫星伪距率的量测误差；

步骤3：建立数据融合系统的量测方程Z＝HX+V，X为数据融合系统的状态向量，H为量测矩阵，V为量测噪声，其中

X＝[δv_x δv_y δv_z δε_x δε_y δε_z d_x d_y d_z b_x b_y b_z]，

δε_x、δε_y、δε_z为平台失准角信息，d_x、d_y、d_z为陀螺仪零偏信息，b_x、b_y、b_z为加速度计零偏信息；由量测方程Z＝HX+V得到数据融合系统的量测矩阵：

$H=\left[\begin{array}{c}{e}_1^i-e_1^j,e_2^i-e_2^j,e_3^i-e_3^j,0_{1\×3},0_{1\×3},0_{1\×3}\\ .\\ .\\ .\\ e_1^m-e_1^s,e_2^m-e_2^s,e_3^m-e_3^s,0_{1\×3},0_{1\×3},0_{1\×3}\end{array}\right]$

其中s也为卫星序号；

步骤4：对数据融合系统的观测量Z进行Carlson滤波，具体滤波步骤为：

步骤4.1：对Carlson滤波器进行初始化；

步骤4.2：对数据融合系统的观测量集合Z进行Carlson滤波，其中第k次滤波过程包括Carlson滤波的时间更新过程和Carlson滤波的量测更新过程：

步骤4.2.1：对于第k次Carlson滤波的时间更新过程，记迭代结果矩阵E为

$E={\left[\begin{array}{c}{U}_{k-1}^T{\mathrm{\Φ}}_{k,k-1}^T\\ {\mathrm{\Γ}}_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]}_{l\×n}^{n\×n}$

其中l为数据融合系统驱动噪声的维数，n为数据融合系统的状态向量维数，Y_k-1为第k-1次滤波的协方差矩阵的上三角阵分解阵，Φ_k，k-1为数据融合系统的状态矩阵的离散化形式，Q_k-1为状态噪声，Γ_k-1为状态噪声驱动矩阵，Φ_k，k-1＝I+F·t，其中t为采样时间；F为数据融合系统的状态矩阵，由数据融合系统的状态方程：

${\stackrel{.}{X}}^i\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_{3\×1}\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\ϵ}}}_{3\×1}\\ {\stackrel{.}{d}}_{3\×1}\\ {\stackrel{.}{b}}_{3\×1}\end{array}\right]=F\·X=\left[\begin{array}{c}-F^i\mathrm{\δ}{\mathrm{\ϵ}}_{3\×1}^i+C_b^i{b}_{3\×1}\\ C_b^i{d}_{3\×1}\\ -A{d}_{3\×1}\\ -A{b}_{3\×1}\end{array}\right]$

得到，其中Fⁱ为比力矩阵，为由载体坐标系到大地坐标系的方向余弦矩阵，A为一阶马尔科夫过程系数矩阵；

对公式：

$\left\{\begin{array}{c}{U}_{k/k-1}(g,g)=\sqrt{E^{\left(g\right)T}{E}^{\left(g\right)}}\\ V_g=\frac{1}{U_{k/k-1}(g,g)}{E}^{\left(g\right)}\\ U_{k/k-1}(b,g)=E^{\left(b\right)T}{V}_g,b=\mathrm{1,2},\·\·\·,g-1\\ E^{\left(b\right)}=E^{(b-1)}-U_{k/k-1}(b,g)V_g,b=\mathrm{1,2},\·\·\·,g-1\end{array}\right.$

按照g＝n,n-1,n-2,…,2,1顺序迭代，经过n次递推后，得到第k次滤波的协方差预测矩阵的上三角阵分解阵U_k/k-1，其中U_k/k-1(g,g)为U_k/k-1中第g行第g列的元素值，E^(g)为E的第g列；

步骤4.2.2：对于第k次Carlson滤波的量测更新过程，采用序贯处理的方法：对于数据融合系统的观测量集合Z中的每个观测量采用如下步骤处理：

对于第r个观测量的处理过程为：对公式

进行迭代运算，q从1到n，其中d_q＝d_q-1+a(q)，

为第r个观测量的噪声协防差， $a=U_{k/k-1}^T{\left(H_k^r\right)}^T={\left[\begin{array}{cccc}a\left(1\right)& a\left(2\right)& \·\·\·& a\left(n\right)\end{array}\right]}^T,$

为数据融合系统的量测矩阵的第r行，

e₀＝0，式中

和

分别表示U_k的第q列和U_k/k-1的第q列；得到第k次滤波协方差矩阵的上三角分解阵 $U_k=\left[\begin{array}{cccc}{U}_k^{\left(1\right)}& U_k^{\left(2\right)}& \·\·\·& U_k^{\left(n\right)}\end{array}\right];$

对第r个观测量序贯处理的状态估计为

其中

为第k-1次滤波的状态向量值，

为第k次滤波得到的第r个观测量的值， ${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1};$

最终得到数据融合系统的观测量集合Z中的最后一个观测量m的状态估计为

步骤5：利用步骤4得到的

对INS解算模块进行校正：利用INS求解出的速度信息减去由Carlson滤波器得到的速度误差即可得到校正后的速度信息；运用公式

${\hat{C}}_E^b=\left[\begin{array}{ccc}1& {\mathrm{\ϵ}}_z& -{\mathrm{\ϵ}}_y\\ -{\mathrm{\ϵ}}_z& 1& {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y& -{\mathrm{\ϵ}}_x& 1\end{array}\right]\·C_E^b$

得到校正后的方向余弦矩阵

为由INS模块得到的大地坐标系到载体坐标系的方向余弦矩阵，将由INS器件得到的加速度与解算出的加速度计零偏信息b_x、b_y、b_z相减得到校正后的加速度，将由INS器件得到的角速度与解算出的陀螺仪零偏信息d_x、d_y、d_z减得到校正后的角速度信息。

有益效果

本发明设计的降维度的基于Carlson滤波的数据融合滤波器，通过对状态方程与量测方程进行修改，在满足对INS导航模块的准确校正的基础上，使得系统的维数由15维降低到12维。降低了系统的运算的数据量，对单个矩阵来说系统的运算量由225次降低到144次，降低了约三分之一。同时采用Carlson滤波器作为数据融合滤波器，在Carlson滤波器中对均方误差阵及均方误差阵的估计进行上三角分解来保证矩阵的正定性，可以有效地避免滤波器的发散。

附图说明

图1：本发明提出的降维度的基于Carlson滤波的数据融合方法方案图；

图2：本发明的流程图。

具体实施方式

下面结合具体实施例描述本发明：

本实施例的数据融合方法的具体实施方案图如图1所示，用户利用采集的INS器件的原始数据进行INS导航的数据解算，解算出载体的姿态信息、位置信息与速度信息，对INS解算模块进行计数，每计够5次进行一次组合导航数据融合。利用降维度的基于Carlson滤波的数据融合方法，实现GPS导航系统与INS导航系统的数据融合，具体步骤为：

步骤1：利用由GPS接收机得到的卫星位置信息与伪距信息，利用以上信息通过最小二乘法解算出当前用户的位置(x y z)和用户的速度(v_x v_y v_z)；将该值传递给INS模块更新INS模块的位置信息；

步骤2：利用GPS接收机得到的星历信息解算出所需卫星的位置信息和速度信息，其中第i号卫星的位置信息为(xⁱ yⁱ zⁱ)，速度信息为 $\left(\begin{array}{ccc}{\stackrel{.}{x}}^i& {\stackrel{.}{y}}^i& {\stackrel{.}{z}}^i\end{array}\right);$ 由公式

$\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}=\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^i-\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^j$

$=(e_1^i-e_1^j)\mathrm{\δ}{v}_x+(e_2^i-e_2^j)\mathrm{\δ}{v}_y+(e_3^i-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}$

$\begin{array}{c}.\\ .\\ .\end{array}$

$\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}=\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^m-\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^j$

$=(e_1^m-e_1^j)\mathrm{\δ}{v}_x+(e_2^m-e_2^j)\mathrm{\δ}{v}_y+(e_3^m-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}$

得到数据融合系统的观测量集合 $Z=\left[\begin{array}{ccc}\mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{ij}}& \·\·\·& \mathrm{\δ}{\stackrel{.}{\mathrm{\ρ}}}^{\mathrm{mj}}\end{array}\right],$ 其中δv_x、δv_y、δy_z为速度误差信息，i、j、m为相应的卫星序号，

为GPS接收机到的第i号卫星的方向矢量在大地坐标系上的投影：

$e_1^i=\frac{x-x^i}{[{(x-x^i)}^2+{(y-y^i)}^2+{(z-z^i)}^2]}$

$e_2^i=\frac{y-y^i}{[{(x-x^i)}^2+{(y-y^i)}^2+{(z-z^i)}^2]}$

$e_3^i=\frac{z-z^i}{[{(x-x^i)}^2+{(y-y^i)}^2+{(z-z^i)}^2]},$

为GPS接收机相对于第i号卫星的伪距率残差信息：

为第i号卫星相对GPS接收机的伪距率预测值：

${\stackrel{.}{\mathrm{\ρ}}}_c^i=e_1^i\·(v_x-{\stackrel{.}{x}}^i)+e_2^i\·(v_y-{\stackrel{.}{y}}^i)+e_3^i\·(v_z-{\stackrel{.}{z}}^i),$

为第i号卫星相对于GPS接收机的伪距率量测值，

为第i号卫星与第j号卫星伪距率的量测误差；

步骤3：建立数据融合系统的量测方程Z＝HX+V，X为数据融合系统的状态向量，H为量测矩阵，V为量测噪声，其中

X＝[δv_x δv_y δv_z δε_x δε_y δε_z d_x d_y d_z b_x b_y b_z]，

δε_x、δε_y、δε_z为平台失准角信息，d_x、d_y、d_z为陀螺仪零偏信息，b_x、b_y、b_z为加速度计零偏信息；系统的量测矩阵为U×12，其中U为观测到的卫星数目。而原有的观测矩阵为2U×15，相比原有的观测矩阵，数据量缩减了60%。

由量测方程Z＝HX+V得到数据融合系统的量测矩阵：

$H=\left[\begin{array}{c}{e}_1^i-e_1^j,e_2^i-e_2^j,e_3^i-e_3^j,0_{1\×3},0_{1\×3},0_{1\×3}\\ .\\ .\\ .\\ e_1^m-e_1^s,e_2^m-e_2^s,e_3^m-e_3^s,0_{1\×3},0_{1\×3},0_{1\×3}\end{array}\right]$

其中s也为卫星序号；

步骤4：对数据融合系统的观测量Z进行Carlson滤波，具体滤波步骤为：

步骤4.1：对Carlson滤波器进行初始化；

步骤4.2：对数据融合系统的观测量集合Z进行Carlson滤波，其中第k次滤波过程包括Carlson滤波的时间更新过程和Carlson滤波的量测更新过程：

步骤4.2.1：对于第k次Carlson滤波的时间更新过程，记迭代结果矩阵E为

$E={\left[\begin{array}{c}{U}_{k-1}^T{\mathrm{\Φ}}_{k,k-1}^T\\ {\mathrm{\Γ}}_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]}_{l\×n}^{n\×n}$

其中l为数据融合系统驱动噪声的维数，n为数据融合系统的状态向量维数，U_k-1为第k-1次滤波的协方差矩阵的上三角阵分解阵，Φ_k，k-1为数据融合系统的状态矩阵的离散化形式，Q_k-1为状态噪声，Γ_k-1为状态噪声驱动矩阵，Φ_k，k-1＝I+F·t，其中t为采样时间；F为数据融合系统的状态矩阵，由数据融合系统的状态方程：

${\stackrel{.}{X}}^i\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_{3\×1}\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\ϵ}}}_{3\×1}\\ {\stackrel{.}{d}}_{3\×1}\\ {\stackrel{.}{b}}_{3\×1}\end{array}\right]=F\·X=\left[\begin{array}{c}-F^i\mathrm{\δ}{\mathrm{\ϵ}}_{3\×1}^i+C_b^i{b}_{3\×1}\\ C_b^i{d}_{3\×1}\\ -A{d}_{3\×1}\\ -A{b}_{3\×1}\end{array}\right]$

得到，其中Fⁱ为比力矩阵，为由载体坐标系到大地坐标系的方向余弦矩阵，A为一阶马尔科夫过程系数矩阵；这里利用由INS器件得到的加速度信息以及由INS解算模块得到的方向余弦矩阵信息列写出数据融合系统的状态矩阵F_12×12的微分形式。系统的状态矩阵相对于原有系统由15维缩减为12维，数据量由225个缩减为144个，数据处理时间也得到了相应的缩减。

对公式：

$\left\{\begin{array}{c}{U}_{k/k-1}(g,g)=\sqrt{E^{\left(g\right)T}{E}^{\left(g\right)}}\\ V_g=\frac{1}{U_{k/k-1}(g,g)}{E}^{\left(g\right)}\\ U_{k/k-1}(b,g)=E^{\left(b\right)T}{V}_g,b=\mathrm{1,2},\·\·\·,g-1\\ E^{\left(b\right)}=E^{(b-1)}-U_{k/k-1}(b,g)V_g,b=\mathrm{1,2},\·\·\·,g-1\end{array}\right.$

按照g＝n,n-1,n-2,…,2,1顺序迭代，经过n次递推后，得到第k次滤波的协方差预测矩阵的上三角阵分解阵U_k/k-1，其中U_k/k-1(g,g)为U_k/k-1中第g行第g列的元素值，E^(g)为E的第g列；

步骤4.2.2：对于第k次Carlson滤波的量测更新过程，采用序贯处理的方法：对于数据融合系统的观测量集合Z中的每个观测量采用如下步骤处理：

对于第r个观测量的处理过程为：对公式

进行迭代运算，q从1到n，其中

d_q＝d_q-1+a(q)，

为第r个观测量的噪声协防差， $a=U_{k/k-1}^T{\left(H_k^r\right)}^T={\left[\begin{array}{cccc}a\left(1\right)& a\left(2\right)& \·\·\·& a\left(n\right)\end{array}\right]}^T,$ 为数据融合系统的量测矩阵的第r行，

e₀＝0，式中

和

分别表示U_k的第q列和U_k/k-1的第q列；得到第k次滤波协方差矩阵的上三角分解阵 $U_k=\left[\begin{array}{cccc}{U}_k^{\left(1\right)}& U_k^{\left(2\right)}& \·\·\·& U_k^{\left(n\right)}\end{array}\right],$ 将U_k存储以参与Carlson滤波的下一次的时间更新过程，在序贯处理结束之前应将U_k作为下一次的协方差估计矩阵；

对第r个观测量序贯处理的状态估计为

其中

为第k-1次滤波的状态向量值，

为第k次滤波得到的第r个观测量的值， ${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1};$

继续利用观测量对状态进行估计，最终得到数据融合系统的观测量集合Z中的最后一个观测量m的状态估计为

即为最终的状态估计值；

步骤5：利用步骤4得到的对INS解算模块进行校正：利用INS求解出的速度信息减去由Carlson滤波器得到的速度误差即可得到校正后的速度信息；运用公式

${\hat{C}}_E^b=\left[\begin{array}{ccc}1& {\mathrm{\ϵ}}_z& -{\mathrm{\ϵ}}_y\\ -{\mathrm{\ϵ}}_z& 1& {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y& -{\mathrm{\ϵ}}_x& 1\end{array}\right]\·C_E^b$

得到校正后的方向余弦矩阵

为由INS模块得到的大地坐标系到载体坐标系的方向余弦矩阵，将由INS器件得到的加速度与解算出的加速度计零偏信息b_x、b_y、b_z相减得到校正后的加速度，将由INS器件得到的角速度与解算出的陀螺仪零偏信息d_x、d_y、d_z减得到校正后的角速度信息。

Claims (1)

1.一种降维度的基于Carlson滤波算法的快速组合导航方法，其特征在于：包括以下步骤：

步骤1：利用由GPS接收机得到的卫星位置信息与伪距信息解算出用户的位置(x y z)和用户的速度(v_x v_y v_z)；

步骤2：利用GPS接收机得到的星历信息解算出所需卫星的位置信息和速度信息，其中第i号卫星的位置信息为(xⁱ yⁱ zⁱ)，速度信息为 $\left(\begin{array}{ccc}{\stackrel{\·}{x}}^i& {\stackrel{\·}{y}}^i& {\stackrel{\·}{z}}^i\end{array}\right);$ 由公式

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{ij}}=\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^i-\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^j$

$=(e_1^i-e_1^j)\mathrm{\δ}{v}_x+(e_2^i-e_2^j)\mathrm{\δ}{v}_y+(e_3^i-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{ij}}$

$\begin{array}{c}\·\\ \·\\ \·\end{array}$

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{mj}}=\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^m-\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^j$

$=(e_1^m-e_1^j)\mathrm{\δ}{v}_x+(e_2^m-e_2^j)\mathrm{\δ}{v}_y+(e_3^m-e_3^j)\mathrm{\δ}{v}_z+{\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{mj}}$

得到数据融合系统的观测量集合 $Z=\left[\begin{array}{ccc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{ij}}& \·\·\·& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{mj}}\end{array}\right],$ 其中δv_x、δv_y、δv_z为速度误差信息，i、j、m为相应的卫星序号，为GPS接收机到的第i号卫星的方向矢量在大地坐标系上的投影；

为GPS接收机相对于第i号卫星的伪距率残差信息：

为第i号卫星相对GPS接收机的伪距率预测值，

为第i号卫星相对于GPS接收机的伪距率量测值，

为第i号卫星与第j号卫星伪距率的量测误差；

步骤3：建立数据融合系统的量测方程Z＝HX+V，X为数据融合系统的状态向量，H为量测矩阵，V为量测噪声，其中

X＝[δv_x δv_y δv_z δε_x δε_y δε_z d_x d_y d_z b_x b_y b_z]，

δε_x、δε_y、δε_z为平台失准角信息，d_x、d_y、d_z为陀螺仪零偏信息，b_x、b_y、b_z为加速度计零偏信息；由量测方程Z＝HX+V得到数据融合系统的量测矩阵：

$H=\left[\begin{array}{c}{e}_1^i-e_1^j,e_2^i-e_2^j,e_3^i-e_3^j,0_{1\×3},0_{1\×3},0_{1\×3}\\ \·\\ \·\\ \·\\ e_1^m-e_1^s,e_2^m-e_2^s,e_3^m-e_3^s,0_{1\×3},0_{1\×3},0_{1\×3}\end{array}\right]$

其中s也为卫星序号；

步骤4：对数据融合系统的观测量Z进行Carlson滤波，具体滤波步骤为：

步骤4.1：对Carlson滤波器进行初始化；

步骤4.2：对数据融合系统的观测量集合Z进行Carlson滤波，其中第k次滤波过程包括Carlson滤波的时间更新过程和Carlson滤波的量测更新过程：

步骤4.2.1：对于第k次Carlson滤波的时间更新过程，记迭代结果矩阵E为

$E={\left[\begin{array}{c}{U}_{k-1}^T{\mathrm{\Φ}}_{k,k-1}^T\\ {\mathrm{\Γ}}_{k-1}{Q}_{k-1}^{1/2}\end{array}\right]}_{l\×n}^{n\×n}$

其中l为数据融合系统驱动噪声的维数，n为数据融合系统的状态向量维数，U_k-1为第k-1次滤波的协方差矩阵的上三角阵分解阵，Φ_k,k-1为数据融合系统的状态矩阵的离散化形式，Q_k-1为状态噪声，Γ_k-1为状态噪声驱动矩阵，Φ_k,k-1＝I+F·t，其中t为采样时间；F为数据融合系统的状态矩阵，由数据融合系统的状态方程：

${\stackrel{\·}{X}}^i\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_{3\×1}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ϵ}}}_{3\×1}\\ {\stackrel{\·}{d}}_{3\×1}\\ {\stackrel{\·}{b}}_{3\×1}\end{array}\right]=F\·X=\left[\begin{array}{c}-F^i\mathrm{\δ}{\mathrm{\ϵ}}_{3\×1}^i+C_b^i{b}_{3\×1}\\ C_b^i{d}_{3\×1}\\ -A{d}_{3\×1}\\ -A{b}_{3\×1}\end{array}\right]$

得到，其中Fⁱ为比力矩阵，

为由载体坐标系到大地坐标系的方向余弦矩阵，A为一阶马尔科夫过程系数矩阵；

对公式：

$\left\{\begin{array}{c}{U}_{k/k-1}(g,g)=\sqrt{E^{\left(g\right)T}{E}^{\left(g\right)}}\\ V_g=\frac{1}{U_{k/k-1}(g,g)}{E}^{\left(g\right)}\\ U_{k/k-1}(b,g)=E^{\left(b\right)T}{V}_g,b=\mathrm{1,2},\·\·\·,g-1\\ E^{\left(b\right)}=E^{(b-1)}-U_{k/k-1}(b,g)V_g,b=\mathrm{1,2},\·\·\·,g-1\end{array}\right.$

按照g＝n,n-1,n-2,…，2,1顺序迭代，经过n次递推后，得到第k次滤波的协方差预测矩阵的上三角阵分解阵U_k/k-1，其中U_k/k-1(g,g)为U_k/k-1中第g行第g列的元素值，E^(g)为E的第g列；

步骤4.2.2：对于第k次Carlson滤波的量测更新过程，采用序贯处理的方法：对于数据融合系统的观测量集合Z中的每个观测量采用如下步骤处理：

对于第r个观测量的处理过程为：对公式

进行迭代运算，q从1到n，其中

d_q＝d_q-1+a(q)，

为第r个观测量的噪声协防差， $a={U_{k/k-1}^T{\left(H_k^r\right)}^T=\left[\begin{array}{cccc}a\left(1\right)& a\left(2\right)& \·\·\·& a\left(n\right)\end{array}\right]}^T,$

为数据融合系统的量测矩阵的第r行，

e₀＝0，式中

和

分别表示U_k的第q列和U_k/k-1的第q列；得到第k次滤波协方差矩阵的上三角分解阵 $U_k=\left[\begin{array}{cccc}{U}_k^{\left(1\right)}& U_k^{\left(2\right)}& \·\·\·& U_k^{\left(n\right)}\end{array}\right];$

对第r个观测量序贯处理的状态估计为 ${\hat{X}}_k^r={\hat{X}}_k^{r-1}+e_n[(Z_k^r-H_k^r{\hat{X}}_{k/k-1})/d_n],$ 其中

为第k-1次滤波的状态向量值，为第k次滤波得到的第r个观测量的值， ${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1};$

最终得到数据融合系统的观测量集合Z中的最后一个观测量m的状态估计为

步骤5：利用步骤4得到的

对INS解算模块进行校正：利用INS求解出的速度信息减去由Carlson滤波器得到的速度误差即可得到校正后的速度信息；运用公式

${\hat{C}}_E^b=\left[\begin{array}{ccc}1& {\mathrm{\ϵ}}_z& -{\mathrm{\ϵ}}_y\\ -{\mathrm{\ϵ}}_z& 1& {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y& -{\mathrm{\ϵ}}_x& 1\end{array}\right]\·C_E^b$

得到校正后的方向余弦矩阵

为由INS模块得到的大地坐标系到载体坐标系的方向余弦矩阵，将由INS器件得到的加速度与解算出的加速度计零偏信息b_x、b_y、b_z相减得到校正后的加速度，将由INS器件得到的角速度与解算出的陀螺仪零偏信息d_x、d_y、d_z相减得到校正后的角速度信息。

Images