CN104567880A - 一种基于多源信息融合的火星最终接近段自主导航方法 - Google Patents

Abstract

本发明公开的一种基于多源信息融合的火星最终接近段自主导航方法，涉及火星最终接近段自主导航方法，属于深空探测技术领域。基于火星最终接近段动力学模型，结合光学测量、基于火星轨道器的无线电测量以及X射线脉冲星测量信息，基于联邦滤波结构，在其适用范围内有效融合，互为备份，充分发挥各种导航系统的优势。在建立火星最终接近段状态模型和火星最终接近段自主导航测量模型基础上，利用联邦结构的组合导航滤波算法解算探测器实时导航状态信息，实现火星最终接近段实时自主导航。本发明可提高火星最终接近段自主导航的精度与实时性，并可提高导航系统的可靠性及容错性。本发明可为未来火星探测任务最终接近段自主导航方案设计提供技术支持。

Description

一种基于多源信息融合的火星最终接近段自主导航方法

技术领域

本发明涉及一种火星最终接近段自主导航方法，属于深空探测技术领域。

背景技术

为探寻火星上存在的生命痕迹，了解其地质成分及演化过程，新一代的火星探测任务需要探测器具备定点着陆的能力(着陆精度<100m)，以安全到达具有科学研究价值的特定区域。火星最终接近段的导航性能直接决定了探测器在大气进入点处的状态估计精度，对进入段导航与制导效果，以及着陆器最终着陆精度，都会产生重要影响。

在过去已实施的火星探测任务中，探测器在最终接近段大多采用基于地面深空网络的无线电跟踪测量。由于地火距离遥远，地面测控信号的强度随距离的增加逐渐衰减，导航误差也随之增大。且探测器与测控站之间存在较大的通信延时，地面缺乏对探测器实时导航与控制的能力，无法及时处理复杂深空环境中的突发事件。因此，需要构建火星最终接近段自主导航方案以提高最终着陆精度。

有学者提出增加光学导航辅助地面测量，但在接近段末端，由于成像面积过大而难以提取火星的中心点坐标，导致光学导航无法进行。另有学者提出利用探测器与装配有无线电收发装置的轨道器进行通信，获取测距及多普勒测速信息，辅助地面定轨。但现阶段轨道器数量有限，由于火星遮挡，探测器受可见弧段约束，测量信息无法实时获得。且以上导航方案都主要依赖于地面测控，仍然难以保证探测器最终接近段的实时自主导航。

本发明针对现有技术存在的问题，提出一种基于多源信息融合的自主导航方案，以期为未来火星探测任务最终接近段自主导航方案设计提供技术支持。

发明内容

本发明要解决的技术问题是提高火星最终接近段自主导航的精度与实时性，并可提高导航系统的可靠性及容错性。本发明公开的一种基于多源信息融合的火星最终接近段自主导航方法，针对火星最终接近段自主导航问题，结合光学导航、无线电导航以及X射线脉冲星导航的适用范围和测量特性，提出一种基于多源信息融合的火星最终接近段自主导航方案，提高火星最终接近段自主导航的精度与实时性。

本发明是通过以下述技术方案实现的：

本发明公开的一种基于多源信息融合的火星最终接近段自主导航方法，基于火星最终接近段动力学模型，结合了光学测量、基于火星轨道器的无线电测量以及X射线脉冲星测量信息，基于联邦滤波结构，在其适用范围内有效融合，互为备份，充分发挥各种导航系统的优势，以无线电径向测量信息弥补光学导航沿光轴方向估计信息不足的缺陷，并引入脉冲星测量覆盖接近段全过程。在建立火星最终接近段状态模型和火星最终接近段自主导航测量模型基础上，利用联邦结构的组合导航滤波算法解算探测器实时导航状态信息，实现火星最终接近段实时自主导航。

本发明公开的一种基于多源信息融合的火星最终接近段自主导航方法，具体实施步骤如下：

步骤1：建立火星最终接近段状态模型。

在日心惯性坐标系下建立探测器状态模型。探测器的状态矢量为位置矢量r_s＝[r_x,r_y,r_z]^T和速度矢量v_s＝[v_x,v_y,v_z]^T。考虑太阳引力、火星引力以及其他摄动力，火星最终接近段探测器的状态模型建立为：

$\stackrel{\&CenterDot;}{X}=\left[\begin{array}{c}{\stackrel{\&CenterDot;}{r}}_s\\ {\stackrel{\&CenterDot;}{v}}_s\end{array}\right]=\left[\begin{array}{c}{v}_s\\ -{\mathrm{\&mu;}}_S\frac{r_s}{r_s^3}-{\mathrm{\&mu;}}_M(\frac{r_{\mathrm{Ms}}}{r_{\mathrm{Ms}}^3}-\frac{r_M}{r_M^3})+a\end{array}\right]---\left(1\right)$

其中μ_S和μ_M分别为太阳和火星的引力常数，r_M为火星的位置矢量，a为其他未建模摄动力矢量。此外r_Ms为探测器相对于火星的位置矢量，满足：

r_Ms＝r_s-r_M  (2)

进而火星最终接近段探测器的动力学模型可描述为其中x＝[r_s ^T,v_s ^T]^T。

步骤2：建立火星最终接近段自主导航测量模型。火星最终接近段自主导航测量模型包括光学导航测量模型，无线电导航测量模型和X射线脉冲星导航测量模型。

利用安装在探测器上的导航相机对火星进行拍摄。通过调整探测器姿态，使光学敏感器的光轴始终指向火星，并对拍摄到的火星图像进行处理，可以提取出火星光心在平面上的中心点坐标。

在日心惯性系中，拍摄时刻火星中心火相对探测器的方向矢量为：

$n^i={\left[\begin{array}{ccc}{n}_x^i& n_y^i& n_z^i\end{array}\right]}^T=-\frac{r_{\mathrm{Ms}}}{\left|r_{\mathrm{Ms}}\right|}=\frac{1}{\sqrt{{(r_{\mathrm{Mx}}-r_x)}^2+{(r_{\mathrm{My}}-r_y)}^2+{(r_{\mathrm{Mz}}-r_z)}^2}}\left[\begin{array}{c}{r}_{\mathrm{Mx}}-r_x\\ r_{\mathrm{My}}-r_y\\ r_{\mathrm{Mz}}-r_z\end{array}\right]---\left(3\right)$

其中，r_Ms＝[r_Mx,r_My,r_Mz]^T为火星的位置矢量。

由日心惯性系转换到相机本体坐标系的转换矩阵为T_ci，可以通过姿态确定系统获得。火星中心相对探测器的方向矢量在相机本体系中可表示为：

$n^c={\left[\begin{array}{ccc}{n}_x^c& n_y^c& n_z^c\end{array}\right]}^T=T_{\mathrm{ci}}\&CenterDot;n^i---\left(4\right)$

将该方向矢量向相机像平面投影，不考虑相机电磁畸变和光学畸变，得到火星中心像素坐标为：

$\left[\begin{array}{c}p\\ l\end{array}\right]=\frac{f}{n_z^c}\left[\begin{array}{cc}{K}_x& 0\\ 0& K_y\end{array}\right]\left[\begin{array}{c}{n}_x^c\\ n_y^c\end{array}\right]---\left(5\right)$

其中，f为导航相机的焦距，K_x,K_y为像素转换系数。

光学导航测量模型为：

$y_1=h_1\left(x\right)=\left[\begin{array}{c}p\\ l\end{array}\right]+{\mathrm{\&epsiv;}}_1---\left(6\right)$

式中，ε₁为测量误差，认为服从高斯分布。在火星最终接近段采用光学测量信息，自主性好，可实施性强。

同时，通过探测器与装备有无线电收发装置的位置确定的火星轨道器的无线电测量及通信(无线电可采用UHF波段或X波段)，可以得到探测器与火星轨道器之间的相对距离及速度：

$\begin{array}{c}{R}_i=|r_s-r_{\mathrm{mi}}|\\ V_i=\frac{(v_s-v_{\mathrm{mi}})\&CenterDot;(r_s-r_{\mathrm{mi}})}{|r_s-r_{\mathrm{mi}}|}\end{array}i=\mathrm{1,2},...,n---\left(7\right)$

式中R_i与V_i分别为探测器到第i颗火星轨道器的相对距离与相对速度，r_mi＝[r_mi,r_mi,r_mi]^T，v_mi＝[v_mi,v_mi,v_mi]^T分别为第i颗火星轨道器的位置矢量和速度矢量，n为无线电信标的个数。

无线电导航测量模型为：

y₂＝h₂(x)＝[R₁,V₁,…,R_i,V_i]^T+ε₂ i＝1,2,…,n  (8)

式中，ε₂为测量误差，认为服从高斯分布。在火星最终接近段采用基于轨道器的无线电测量信息，观测数据更新率高，有直接的测速信息，测量精度高。

另外通过脉冲星发射的X射线的接收并与标准波形的比对，可以得到X射线到达探测器与到达太阳系质心SSB的时间差

$\begin{array}{c}\mathrm{\&Delta;}{t}_j=t_{\mathrm{bj}}-t_{\mathrm{sj}}=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{2c{D}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2+2(n_j\&CenterDot;b)(n_j\&CenterDot;r_b)\\ -2(b\&CenterDot;r_b)]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|\\ j=\mathrm{1,2},...,m\end{array}---\left(9\right)$

式中n_j为太阳系质心SSB到第j颗脉冲星的单位矢量，b是SSB在日心惯性坐标系下的位置矢量，r_b为探测器相对SSB的位置矢量，满足：

r_s＝b+r_b  (10)

D_0j为第j颗脉冲星到日心的距离，m为所用到的脉冲星数量。忽略小项的影响，简化的脉冲星导航测量模型为：

y₃＝h₃(x)＝[△t₁,△t₂,…,△t_m]+ε₃  (11a)

$\mathrm{\&Delta;}{t}_j=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{{2\mathrm{cD}}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|,j=\mathrm{1,2},...,m---\left(11b\right)$

式中，ε₃为测量误差，认为服从高斯分布。在火星最终接近段采用脉冲星测量信息，可以覆盖接近段全过程，满足了自主导航的实时性要求。

对光学测量信息、无线电测量信息及X射线脉冲星测量信息进行组合，式(6)(8)(11)构建了火星最终接近段的自主导航测量模型，可描述为y_k＝h_k(x),k＝1,2,3。

步骤3：基于联邦结构的组合导航滤波算法，解算探测器实时导航状态信息。

根据火星最终接近段状态模型及测量模型y_k＝h_k(x),k＝1,2,3，通过导航滤波计算可以对探测器状态进行估计。由于最终接近过程中，探测器通过光学导航测量，无线电导航测量和X射线脉冲星导航测量获取多源信息，光学导航对垂直光轴方向上的状态估计精度较高，而基于轨道器的无线电导航对径向状态估计精度较高。为了有效融合多源观测信息，采用联邦结构的组合导航滤波算法进行解算，最终输出探测器实时导航状态信息。

由于状态模型及测量模型均呈现非线性，故局部滤波器宜选用扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)等滤波算法。局部滤波器1结合系统动力学模型(状态模型)处理X射线脉冲星测量信息，局部滤波器2结合系统动力学模型(状态模型)处理光学测量信息，局部滤波器3结合系统动力学模型(状态模型)处理无线电测量信息，此外，局部滤波器1、局部滤波器2、局部滤波器3分别滤波输出局部状态估计值和误差协方差阵。主滤波器对局部滤波器1、局部滤波器2、局部滤波器3的局部滤波结果进行最优融合，得到全局滤波结果，并重置局部滤波器1、局部滤波器2、局部滤波器3。当各敏感器均有观测量输出时，对局部滤波器1、局部滤波器2、局部滤波器3的状态估计值进行最优融合，当光学导航不可用或轨道器不可见时，主滤波器只采用剩余的局部滤波器估计值进行融合或直接输出。以扩展卡尔曼滤波为例，局部滤波算法为：

${\hat{X}}_{i,k/k+1}={\mathrm{\&Phi;}}_k{\hat{X}}_{i,k}---\left(12\right)$

$P_{i,k/k+1}={\mathrm{\&Phi;}}_k{P}_{i,k}{\mathrm{\&Phi;}}_k^T+Q_{i,k}---\left(13\right)$

$K_{i,k+1}=P_{i,k/k+1}{H}_{i,k+1}^T{[H_{i,k+1}{P}_{i,k/k+1}{H}_{i,k+1}^T+R_{i,k+1}]}^{-1}---\left(14\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{i,k/k+1}+K_{i,k+1}[Z_{i,k+1}-H_{i,k+1}{\hat{X}}_{i,k/k+1}]---\left(15\right)$

P_i,k+1＝[I-K_i,k+1H_i,k+1]P_i,k/k+1  (16)

信息融合算法为：

$P_{g,k+1}={[P_{1,k+1}^{-1}+P_{2,k+1}^{-1}+P_{3,k+1}^{-1}]}^{-1}---\left(17\right)$

${\hat{X}}_{g,k+1}=P_{g,k+1}[P_{1,k+1}^{-1}{\hat{X}}_{1,k+1}+P_{2,k+1}^{-1}{\hat{X}}_{2,k+1}+P_{3,k+1}^{-1}{\hat{X}}_{3,k+1}]---\left(18\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{g,k+1},P_{i,k+1}={\mathrm{\&beta;}}_i^{-1}\&CenterDot;P_{g,k+1},Q_{i,k+1}={\mathrm{\&beta;}}_i^{-1}{Q}_{k+1}---\left(19\right)$

式中，表示系统状态向量，Z为系统观测向量，P表示估计误差方差阵，Φ为根据状态模型得到的状态转移矩阵，H为根据测量模型得到的系统观测矩阵，Q和R分别为系统过程噪声方差阵和观测噪声方差阵。下角标k和k+1表示前一时刻和当前时刻，下角标i表示所属第i个子系统(i＝1,2,3)，下角标g表示全局估计，β_i为信息分配因子，与第i个子系统的误差方差成反比，且满足信息守恒原则， $\underset{i}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_i=1.$

在火星最终接近段利用基于联邦结构的组合导航滤波算法，局部采用非线性滤波器，提高了导航滤波精度，同时增强了系统的可靠性和容错性。

有益效果

1、本发明采用光学测量信息，自主性好，可实施性强。

2、本发明采用基于轨道器的无线电测量信息，观测数据更新率高，有直接的测速信息，测量精度高。

3、本发明采用脉冲星测量信息，满足了自主导航的实时性要求。

4、本发明利用基于联邦结构的组合导航滤波算法，局部采用非线性滤波器，提高了导航滤波精度，同时增强了系统的可靠性和容错性。

附图说明

图1为基于多源信息融合的火星最终接近段自主导航的流程图；

图2为基于联邦结构的组合导航滤波器结构框图；

图3为以扩展卡尔曼滤波为局部滤波器的导航误差结果图；

图4为以无迹卡尔曼滤波为局部滤波器的导航误差结果图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

实施例1：

本实例针对火星最终接近段，采用光学测量、基于一颗轨道器的无线电测量以及三颗脉冲星测量信息，结合基于联邦滤波的组合导航滤波方法，选用扩展卡尔曼滤波算法(EKF)作为局部滤波器，对探测器位置、速度状态进行估计，实现高精度实时自主导航。本实例的具体实施方法如下：

步骤1：建立火星最终接近段状态模型。

在日心惯性坐标系下建立探测器状态模型。探测器的状态矢量为位置矢量r_s＝[r_x,r_y,r_z]^T和速度矢量v_s＝[v_x,v_y,v_z]^T。考虑太阳引力、火星引力以及其他摄动力，火星最终接近段探测器的状态模型建立为：

$\stackrel{\&CenterDot;}{X}=\left[\begin{array}{c}{\stackrel{\&CenterDot;}{r}}_s\\ {\stackrel{\&CenterDot;}{v}}_s\end{array}\right]=\left[\begin{array}{c}{v}_s\\ -{\mathrm{\&mu;}}_S\frac{r_s}{r_s^3}-{\mathrm{\&mu;}}_M(\frac{r_{\mathrm{Ms}}}{r_{\mathrm{Ms}}^3}-\frac{r_M}{r_M^3})+a\end{array}\right]---\left(1\right)$

其中μ_S和μ_M分别为太阳和火星的引力常数，r_M为火星的位置矢量，a为其他未建模摄动力矢量。此外r_Ms为探测器相对于火星的位置矢量，满足：

r_Ms＝r_s-r_M              (2)

进而火星最终接近段探测器的动力学模型可描述为其中x＝[r_s ^T,v_s ^T]^T。

步骤2：建立火星最终接近段自主导航测量模型。火星最终接近段自主导航测量模型包括光学导航测量模型，无线电导航测量模型和X射线脉冲星导航测量模型。

利用安装在探测器上的导航相机对火星进行拍摄。通过调整探测器姿态，使光学敏感器的光轴始终指向火星，并对拍摄到的火星图像进行处理，可以提取出火星光心在平面上的中心点坐标。

在日心惯性系中，拍摄时刻火星中心火相对探测器的方向矢量为：

$n^i={\left[\begin{array}{ccc}{n}_x^i& n_y^i& n_z^i\end{array}\right]}^T=-\frac{r_{\mathrm{Ms}}}{\left|r_{\mathrm{Ms}}\right|}=\frac{1}{\sqrt{{(r_{\mathrm{Mx}}-r_x)}^2+{(r_{\mathrm{My}}-r_y)}^2+{(r_{\mathrm{Mz}}-r_z)}^2}}\left[\begin{array}{c}{r}_{\mathrm{Mx}}-r_x\\ r_{\mathrm{My}}-r_y\\ r_{\mathrm{Mz}}-r_z\end{array}\right]---\left(3\right)$

其中，r_Ms＝[r_Mx,r_My,r_Mz]^T为火星的位置矢量。

由日心惯性系转换到相机本体坐标系的转换矩阵为T_ci，可以通过姿态确定系统获得。火星中心相对探测器的方向矢量在相机本体系中可表示为：

$n^c={\left[\begin{array}{ccc}{n}_x^c& n_y^c& n_z^c\end{array}\right]}^T=T_{\mathrm{ci}}\&CenterDot;n^i---\left(4\right)$

将该方向矢量向相机像平面投影，不考虑相机电磁畸变和光学畸变，得到火星中心像素坐标为：

$\left[\begin{array}{c}p\\ l\end{array}\right]=\frac{f}{n_z^c}\left[\begin{array}{cc}{K}_x& 0\\ 0& K_y\end{array}\right]\left[\begin{array}{c}{n}_x^c\\ n_y^c\end{array}\right]---\left(5\right)$

其中，f为导航相机的焦距，K_x,K_y为像素转换系数。

光学导航测量模型为：

$y_1=h_1\left(x\right)=\left[\begin{array}{c}p\\ l\end{array}\right]+{\mathrm{\&epsiv;}}_1---\left(6\right)$

式中，ε₁为测量误差，认为服从高斯分布。

同时，通过探测器与一颗装备有无线电收发装置的位置确定的定性轨道器的无线电测量及通信(无线电可采用UHF波段或X波段)，可以得到探测器与火星轨道器之间的相对距离及速度：

$\begin{array}{c}{R}_1=|r_s-r_m|\\ V_1=\frac{(v_s-v_m)\&CenterDot;(r_s-r_m)}{|r_s-r_m|}\end{array}---\left(7\right)$

式中R₁与V₁分别为探测器到火星轨道器的相对距离与相对速度，r_m＝[r_m,r_m,r_m]^T，v_m＝[v_m,v_m,v_m]^T分别为火星轨道器的位置矢量和速度矢量。

无线电导航测量模型为：

y₂＝h₂(x)＝[R₁,V₁]^T+ε₂           (8)

式中，ε₂为测量误差，认为服从高斯分布。

另外通过脉冲星发射的X射线的接收并与标准波形的比对，可以得到X射线到达探测器与到达太阳系质心SSB的时间差

$\begin{array}{c}\mathrm{\&Delta;}{t}_j=t_{\mathrm{bj}}-t_{\mathrm{sj}}=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{2c{D}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2+2(n_j\&CenterDot;b)(n_j\&CenterDot;r_b)\\ -2(b\&CenterDot;r_b)]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|\\ j=\mathrm{1,2},3\end{array}---\left(9\right)$

式中n_j为太阳系质心SSB到第j颗脉冲星的单位矢量，b是SSB在日心惯性坐标系下的位置矢量，r_b为探测器相对SSB的位置矢量，满足：

r_s＝b+r_b             (10)

D_0j为第j颗脉冲星到日心的距离，共观测三颗脉冲星。忽略小项的影响，简化的脉冲星导航测量模型为：

y₃＝h₃(x)[△t₁,△t₂,…,△t_m]+ε₃             (11a)

$\mathrm{\&Delta;}{t}_j=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{{2\mathrm{cD}}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|,j=\mathrm{1,2,3}---\left(11b\right)$

式中，ε₃为测量误差，认为服从高斯分布。

对光学测量信息、无线电测量信息及X射线脉冲星测量信息进行组合，式(6)(8)(11)构建了火星最终接近段的自主导航测量模型，可描述为y_k＝h_k(x),k＝1,2,3。

步骤3：基于联邦结构的组合导航滤波解算

根据火星最终接近段状态模型及测量模型y_k＝h_k(x),k＝1,2,3，通过导航滤波计算可以对探测器状态进行估计。由于最终接近过程中，探测器通过不同观测方式获取多源信息，光学导航对垂直光轴方向上的状态估计精度较高，而基于轨道器的无线电导航对径向状态估计精度较高。为了有效融合多源观测信息，采用联邦结构的组合导航滤波算法进行解算，最终输出探测器状态信息。

由于状态模型及测量模型均呈现非线性，局部滤波器选用扩展卡尔曼滤波器(EKF)。局部滤波器各局部滤波器结合系统动力学模型(状态模型)，处理各自的量测信息，滤波输出局部状态估计值和误差协方差阵，主滤波器对各局部滤波结果进行最优融合，得到全局滤波结果，并重置各局部滤波器。当各敏感器均有观测量时，对三个局部滤波器的状态估计值进行最优融合，当光学导航不可用或轨道器不可见时，只采用剩余的局部滤波器估计值进行融合或直接输出。以扩展卡尔曼滤波为例，局部滤波算法为：

${\hat{X}}_{i,k/k+1}={\mathrm{\&Phi;}}_k{\hat{X}}_{i,k}---\left(12\right)$

$P_{i,k/k+1}={\mathrm{\&Phi;}}_k{P}_{i,k}{\mathrm{\&Phi;}}_k^T+Q_{i,k}---\left(13\right)$

$K_{i,k+1}=P_{i,k/k+1}{H}_{i,k+1}^T{[H_{i,k+1}{P}_{i,k/k+1}{H}_{i,k+1}^T+R_{i,k+1}]}^{-1}---\left(14\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{i,k/k+1}+K_{i,k+1}[Z_{i,k+1}-H_{i,k+1}{\hat{X}}_{i,k/k+1}]---\left(15\right)$

P_i,k+1＝[I-K_i,k+1H_i,k+1]P_i,k/k+1  (16)

信息融合算法为：

$P_{g,k+1}={[P_{1,k+1}^{-1}+P_{2,k+1}^{-1}+P_{3,k+1}^{-1}]}^{-1}---\left(17\right)$

${\hat{X}}_{g,k+1}=P_{g,k+1}[P_{1,k+1}^{-1}{\hat{X}}_{1,k+1}+P_{2,k+1}^{-1}{\hat{X}}_{2,k+1}+P_{3,k+1}^{-1}{\hat{X}}_{3,k+1}]---\left(18\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{g,k+1},P_{i,k+1}={\mathrm{\&beta;}}_i^{-1}\&CenterDot;P_{g,k+1},Q_{i,k+1}={\mathrm{\&beta;}}_i^{-1}{Q}_{k+1}---\left(19\right)$

式中，表示系统状态向量，Z为系统观测向量，P表示估计误差方差阵，Φ为根据状态模型得到的状态转移矩阵，H为根据测量模型得到的系统观测矩阵，Q和R分别为系统过程噪声方差阵和观测噪声方差阵。下角标k和k+1表示前一时刻和当前时刻，下角标i表示所属第i个子系统(i＝1,2,3)，下角标g表示全局估计，β_i为信息分配因子，与第i个子系统的误差方差成反比，且满足信息守恒原则， $\underset{i}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_i=1.$

对该组合导航方案进行仿真验证，探测器及轨道器的轨道参数如表1所示。探测器初始状态位置误差为50km，速度误差为10m/s。仿真时间为自探测器进入火星大气前24h至进入火星大气层(距火星表面高度125km)。在接近过程中，光学导航相机每10分钟对火星进行一次拍摄，相机测量精度为0.1像素；轨道器位置误差0.5km，速度误差0.5m/s，无线电测距精度为10m，测速精度为1mm/s；脉冲星观测时间为10min，TOA测量误差为10μs。

表1 探测器与轨道器的轨道根数

基于多源信息融合的火星最终接近段自主导航方案性能如图3所示。由仿真结果可以看出，探测器的位置及速度估计误差均随时间快速收敛，最终能得到高精度的状态估计信息。以扩展卡尔曼滤波为局部滤波器的导航误差结果图如图3所示。

实施例2：

本实例针对火星最终接近段，采用光学测量、基于一颗轨道器的无线电测量以及三颗脉冲星测量信息，结合基于联邦滤波的组合导航滤波方法，选用无迹卡尔曼滤波算法(UKF)作为局部滤波器，对探测器位置、速度状态进行估计，实现高精度实时自主导航。本实例的具体实施方法如下：

步骤1：建立火星最终接近段状态模型。

在日心惯性坐标系下建立探测器状态模型。探测器的状态矢量为位置矢量r_s＝[r_x,r_y,r_z]^T和速度矢量v_s＝[v_x,v_y,v_z]^T。考虑太阳引力、火星引力以及其他摄动力，火星最终接近段探测器的状态模型建立为：

$\stackrel{\&CenterDot;}{X}=\left[\begin{array}{c}{\stackrel{\&CenterDot;}{r}}_s\\ {\stackrel{\&CenterDot;}{v}}_s\end{array}\right]=\left[\begin{array}{c}{v}_s\\ -{\mathrm{\&mu;}}_S\frac{r_s}{r_s^3}-{\mathrm{\&mu;}}_M(\frac{r_{\mathrm{Ms}}}{r_{\mathrm{Ms}}^3}-\frac{r_M}{r_M^3})+a\end{array}\right]---\left(1\right)$

其中μ_S和μ_M分别为太阳和火星的引力常数，r_M为火星的位置矢量，a为其他未建模摄动力矢量。此外r_Ms为探测器相对于火星的位置矢量，满足：

r_Ms＝r_s-r_M              (2)

进而火星最终接近段探测器的动力学模型可描述为其中x＝[r_s ^T,v_s ^T]^T。

步骤2：建立火星最终接近段自主导航测量模型。火星最终接近段自主导航测量模型包括光学导航测量模型，无线电导航测量模型和X射线脉冲星导航测量模型。

利用安装在探测器上的导航相机对火星进行拍摄。通过调整探测器姿态，使光学敏感器的光轴始终指向火星，并对拍摄到的火星图像进行处理，可以提取出火星光心在平面上的中心点坐标。

在日心惯性系中，拍摄时刻火星中心火相对探测器的方向矢量为：

$n^i={\left[\begin{array}{ccc}{n}_x^i& n_y^i& n_z^i\end{array}\right]}^T=-\frac{r_{\mathrm{Ms}}}{\left|r_{\mathrm{Ms}}\right|}=\frac{1}{\sqrt{{(r_{\mathrm{Mx}}-r_x)}^2+{(r_{\mathrm{My}}-r_y)}^2+{(r_{\mathrm{Mz}}-r_z)}^2}}\left[\begin{array}{c}{r}_{\mathrm{Mx}}-r_x\\ r_{\mathrm{My}}-r_y\\ r_{\mathrm{Mz}}-r_z\end{array}\right]---\left(3\right)$

其中，r_Ms＝[r_Mx,r_My,r_Mz]^T为火星的位置矢量。

由日心惯性系转换到相机本体坐标系的转换矩阵为T_ci，可以通过姿态确定系统获得。火星中心相对探测器的方向矢量在相机本体系中可表示为：

$n^c={\left[\begin{array}{ccc}{n}_x^c& n_y^c& n_z^c\end{array}\right]}^T=T_{\mathrm{ci}}\&CenterDot;n^i---\left(4\right)$

将该方向矢量向相机像平面投影，不考虑相机电磁畸变和光学畸变，得到火星中心像素坐标为：

$\left[\begin{array}{c}p\\ l\end{array}\right]=\frac{f}{n_z^c}\left[\begin{array}{cc}{K}_x& 0\\ 0& K_y\end{array}\right]\left[\begin{array}{c}{n}_x^c\\ n_y^c\end{array}\right]---\left(5\right)$

其中，f为导航相机的焦距，K_x,K_y为像素转换系数。

光学导航测量模型为：

$y_1=h_1\left(x\right)=\left[\begin{array}{c}p\\ l\end{array}\right]+{\mathrm{\&epsiv;}}_1---\left(6\right)$

式中，ε₁为测量误差，认为服从高斯分布。

同时，通过探测器与一颗装备有无线电收发装置的位置确定的定性轨道器的无线电测量及通信(无线电可采用UHF波段或X波段)，可以得到探测器与火星轨道器之间的相对距离及速度：

$\begin{array}{c}{R}_1=|r_s-r_m|\\ V_1=\frac{(v_s-v_m)\&CenterDot;(r_s-r_m)}{|r_s-r_m|}\end{array}---\left(7\right)$

式中R₁与V₁分别为探测器到火星轨道器的相对距离与相对速度，r_m＝[r_m,r_m,r_m]^T，v_m＝[v_m,v_m,v_m]^T分别为火星轨道器的位置矢量和速度矢量。

无线电导航测量模型为：

y₂＝h₂(x)＝[R₁,V₁]^T+ε₂          (8)

式中，ε₂为测量误差，认为服从高斯分布。

另外通过脉冲星发射的X射线的接收并与标准波形的比对，可以得到X射线到达探测器与到达太阳系质心SSB的时间差

$\begin{array}{c}\mathrm{\&Delta;}{t}_j=t_{\mathrm{bj}}-t_{\mathrm{sj}}=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{2c{D}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2+2(n_j\&CenterDot;b)(n_j\&CenterDot;r_b)\\ -2(b\&CenterDot;r_b)]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|\\ j=\mathrm{1,2},3\end{array}---\left(9\right)$

式中n_j为太阳系质心SSB到第j颗脉冲星的单位矢量，b是SSB在日心惯性坐标系下的位置矢量，r_b为探测器相对SSB的位置矢量，满足：

r_s＝b+r_b             (10)

D_0j为第j颗脉冲星到日心的距离，共观测三颗脉冲星。忽略小项的影响，简化的脉冲星导航测量模型为：

y₃＝h₃(x)[△t₁,△t₂,…,△t_m]+ε₃         (11a)

$\mathrm{\&Delta;}{t}_j=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{{2\mathrm{cD}}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2]+\frac{2{\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|,j=\mathrm{1,2,3}---\left(11b\right)$

式中，ε₃为测量误差，认为服从高斯分布。

对光学测量信息、无线电测量信息及X射线脉冲星测量信息进行组合，式(6)(8)(11)构建了火星最终接近段的自主导航测量模型，可描述为y_k＝h_k(x),k＝1,2,3。

步骤3：基于联邦结构的组合导航滤波解算

根据火星最终接近段状态模型及测量模型y_k＝h_k(x),k＝1,2,3，通过导航滤波计算可以对探测器状态进行估计。由于最终接近过程中，探测器通过不同观测方式获取多源信息，光学导航对垂直光轴方向上的状态估计精度较高，而基于轨道器的无线电导航对径向状态估计精度较高。为了有效融合多源观测信息，采用联邦结构的组合导航滤波算法进行解算，最终输出探测器状态信息。

由于状态模型及测量模型均呈现非线性，局部滤波器选用无迹卡尔曼滤波器(UKF)。各局部滤波器结合系统动力学模型(状态模型)，处理各自的量测信息，滤波输出局部状态估计值和误差协方差阵，主滤波器对各局部滤波结果进行最优融合，得到全局滤波结果，并重置各局部滤波器。当各敏感器均有观测量时，对三个局部滤波器的状态估计值进行最优融合，当光学导航不可用或轨道器不可见时，只采用剩余的局部滤波器估计值进行融合或直接输出。以无迹卡尔曼滤波为例，局部滤波算法为：

$M_{i,k}=[{\hat{X}}_{i,k},...{\hat{X}}_{i,k}]+\sqrt{n+{\mathrm{\&lambda;}}_i}\left[\begin{array}{ccc}0& \sqrt{P_{i,k}}& -\sqrt{P_{i,k}}\end{array}\right]---\left(12\right)$

${\hat{M}}_{i,k/k+1}=f(M_{i,k},k)---\left(13\right)$

${\hat{X}}_{k/k+1}={\hat{M}}_{i,k/k+1}{W}_{i,m}---\left(14\right)$

$P_{i,k/k+1}={\hat{M}}_{i,k/k+1}{W}_{i,c}{\hat{M}}_{i,k/k+1}^T+Q_{i,k}---\left(15\right)$

$M_{i,k/k+1}=[{\hat{X}}_{i,k/k+1},...,{\hat{X}}_{i,k/k+1}]+\sqrt{n+{\mathrm{\&lambda;}}_i}\left[\begin{array}{ccc}0& \sqrt{P_{i,k/k+1}}& -\sqrt{P_{i,k/k+1}}\end{array}\right]---\left(16\right)$

Y_i,k/k+1＝h_i(M_i,k/k+1,k+1)         (17)

μ_i,k+1＝Y_i,k/k+1W_i,m           (18)

$S_{i,k+1}=Y_{i,k/k+1}{W}_{i,c}{Y}_{i,k/k+1}^T+R_{i,k+1}---\left(19\right)$

$C_{i,k+1}=M_{i,k/k+1}{W}_{i,c}{M}_{i,k/k+1}^T---\left(20\right)$

$K_{i,k+1}=C_{i,k+1}{S}_{i,k+1}^{-1}---\left(21\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{i,k/k+1}+K_{i,k+1}[Z_{i,k+1}-{\mathrm{\&mu;}}_{i,k+1}]---\left(22\right)$

$P_{i,k+1}=P_{i,k/k+1}-K_{i,k+1}{S}_{i,k+1}{K}_{i,k+1}^T---\left(23\right)$

信息融合算法为：

$P_{g,k+1}={[P_{1,k+1}^{-1}+P_{2,k+1}^{-1}+P_{3,k+1}^{-1}]}^{-1}---\left(24\right)$

${\hat{X}}_{g,k+1}=P_{g,k+1}[P_{1,k+1}^{-1}{\hat{X}}_{1,k+1}+P_{2,k+1}^{-1}{\hat{X}}_{2,k+1}+P_{3,k+1}^{-1}{\hat{X}}_{3,k+1}]---\left(25\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{g,k+1},P_{i,k+1}={\mathrm{\&beta;}}_i^{-1}\&CenterDot;P_{g,k+1},Q_{i,k+1}={\mathrm{\&beta;}}_i^{-1}{Q}_{k+1}---\left(26\right)$

式中，表示系统状态向量，M为无迹变换的采样点，W_m和W_c为权系数，λ为尺度参数，Z为系统观测向量，P表示估计误差方差阵，Q和R分别为系统过程噪声方差阵和观测噪声方差阵。下角标k和k+1表示前一时刻和当前时刻，下角标i表示所属第i个子系统(i＝1,2,3)，下角标g表示全局估计，β_i为信息分配因子，与第i个子系统的误差方差成反比，且满足信息守恒原则，

对该组合导航方案进行仿真验证，探测器及轨道器的轨道参数如表1所示。探测器初始状态位置误差为50km，速度误差为10m/s。仿真时间为自探测器进入火星大气前24h至进入火星大气层(距火星表面高度125km)。在接近过程中，光学导航相机每10分钟对火星进行一次拍摄，相机测量精度为0.1像素；轨道器位置误差0.5km，速度误差0.5m/s，无线电测距精度为10m，测速精度为1mm/s；脉冲星观测时间为10min，TOA测量误差为10μs。

表1 探测器与轨道器的轨道根数

基于多源信息融合的火星最终接近段自主导航方案性能如图3所示。由仿真结果可以看出，探测器的位置及速度估计误差均随时间快速收敛，最终能得到高精度的状态估计信息。以无迹卡尔曼滤波为局部滤波器的导航误差结果图如图4所示。

本发明保护范围不仅局限于实施例，实施例用于解释本发明，凡与本发明在相同原理和构思条件下的变更或修改均在本发明公开的保护范围之内。

Claims (5)

1.一种基于多源信息融合的火星最终接近段自主导航方法，其特征在于：基于火星最终接近段动力学模型，结合了光学测量、基于火星轨道器的无线电测量以及X射线脉冲星测量信息，基于联邦滤波结构，在其适用范围内有效融合，互为备份，充分发挥各种导航系统的优势，以无线电径向测量信息弥补光学导航沿光轴方向估计信息不足的缺陷，并引入脉冲星测量覆盖接近段全过程；在建立火星最终接近段状态模型和火星最终接近段自主导航测量模型基础上，利用联邦结构的组合导航滤波算法解算探测器实时导航状态信息，实现火星最终接近段实时自主导航。

2.如权利要求1所述的一种基于多源信息融合的火星最终接近段自主导航方法，其特征在于：具体实现步骤如下，

步骤1：建立火星最终接近段状态模型；

在日心惯性坐标系下建立探测器状态模型；探测器的状态矢量为位置矢量r_s＝[r_x,r_y,r_z]^T和速度矢量v_s＝[v_x,v_y,v_z]^T；考虑太阳引力、火星引力以及其他摄动力，火星最终接近段探测器的状态模型建立为：

$\stackrel{.}{X}=\left[\begin{array}{c}{\stackrel{.}{r}}_s\\ {\stackrel{.}{v}}_s\end{array}\right]=\left[\begin{array}{c}{v}_s\\ -{\mathrm{\&mu;}}_S\frac{r_s}{r_s^3}-{\mathrm{\&mu;}}_M(\frac{r_{\mathrm{Ms}}}{r_{\mathrm{Ms}}^3}-\frac{r_M}{r_M^3})+a\end{array}\right]---\left(1\right)$

其中μ_S和μ_M分别为太阳和火星的引力常数，r_M为火星的位置矢量，a为其他未建模摄动力矢量；此外r_Ms为探测器相对于火星的位置矢量，满足：

r_Ms＝r_s-r_M  (2)

进而火星最终接近段探测器的动力学模型可描述为其中 $x={\left[\begin{array}{cc}{r}_s^T,& v_s^T\end{array}\right]}^T;$

步骤2：建立火星最终接近段自主导航测量模型；火星最终接近段自主导航测量模型包括光学导航测量模型，无线电导航测量模型和X射线脉冲星导航测量模型；

利用安装在探测器上的导航相机对火星进行拍摄；通过调整探测器姿态，使光学敏感器的光轴始终指向火星，并对拍摄到的火星图像进行处理，提取出火星光心在平面上的中心点坐标；

在日心惯性系中，拍摄时刻火星中心火相对探测器的方向矢量为：

$n^i={\left[\begin{array}{ccc}{n}_x^i& n_y^i& n_z^i\end{array}\right]}^T=-\frac{r_{\mathrm{Ms}}}{\left|r_{\mathrm{Ms}}\right|}=\frac{1}{\sqrt{{(r_{\mathrm{Mx}}-r_x)}^2+{(r_{\mathrm{My}}-r_y)}^2+{(r_{\mathrm{Mz}}-r_z)}^2}}\left[\begin{array}{c}{r}_{\mathrm{Mx}}-r_x\\ r_{\mathrm{My}}-r_y\\ r_{\mathrm{Mz}}-r_z\end{array}\right]---\left(3\right)$

其中，r_Ms＝[r_Mx, r_My, r_Mz]^T为火星的位置矢量；

由日心惯性系转换到相机本体坐标系的转换矩阵为T_ci，通过姿态确定系统获得；火星中心相对探测器的方向矢量在相机本体系中可表示为：

$n^c={\left[\begin{array}{ccc}{n}_x^c& n_y^c& n_z^c\end{array}\right]}^T=T_{\mathrm{ci}}\&CenterDot;n^i---\left(4\right)$

将该方向矢量向相机像平面投影，不考虑相机电磁畸变和光学畸变，得到火星中心像素坐标为：

$\left[\begin{array}{c}p\\ l\end{array}\right]=\frac{f}{n_z^c}\left[\begin{array}{cc}{K}_x& 0\\ 0& K_y\end{array}\right]\left[\begin{array}{c}{n}_x^c\\ n_y^c\end{array}\right]---\left(5\right)$

其中，f为导航相机的焦距，K_x,K_y为像素转换系数；

光学导航测量模型为：

$y_1=h_1\left(x\right)=\left[\begin{array}{c}p\\ l\end{array}\right]+{\mathrm{\&epsiv;}}_1---\left(6\right)$

式中，ε₁为测量误差，认为服从高斯分布；在火星最终接近段采用光学测量信息，自主性好，可实施性强；

同时，通过探测器与装备有无线电收发装置的位置确定的火星轨道器的无线电测量及通信，得到探测器与火星轨道器之间的相对距离及速度：

R_i＝|r_s-r_mi|

$V_i=\frac{(v_s-v_{\mathrm{mi}})\&CenterDot;(r_s-r_{\mathrm{mi}})}{|r_s-r_{\mathrm{mi}}|},i=\mathrm{1,2},...,n---\left(7\right)$

式中R_i与V_i分别为探测器到第i颗火星轨道器的相对距离与相对速度，r_mi＝[r_mi, r_mi, r_mi]^T，v_mi＝[v_mi, v_mi, v_mi]^T分别为第i颗火星轨道器的位置矢量和速度矢量，n为无线电信标的个数；

无线电导航测量模型为：

y₂＝h₂(x)＝[R₁,V₁,…,R_i,V_i]^T+ε₂  i＝1,2,…,n(8)

式中，ε₂为测量误差，认为服从高斯分布；在火星最终接近段采用基于轨道器的无线电测量信息，观测数据更新率高，有直接的测速信息，测量精度高；

另外通过脉冲星发射的X射线的接收并与标准波形的比对，可以得到X射线到达探测器与到达太阳系质心SSB的时间差

$\begin{array}{c}{\mathrm{\&Delta;t}}_j=t_{\mathrm{bj}}-t_{\mathrm{sj}}=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{2c{D}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2+2(n_j\&CenterDot;b)(n_j\&CenterDot;r_b)\\ -2(b\&CenterDot;r_b)]+\frac{{2\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|\\ j=\mathrm{1,2},...,m\end{array}---\left(9\right)$

式中n_j为太阳系质心SSB到第j颗脉冲星的单位矢量，b是SSB在日心惯性坐标系下的位置矢量，r_b为探测器相对SSB的位置矢量，满足：

r_s＝b+r_b  (10)

D_0j为第j颗脉冲星到日心的距离，m为所用到的脉冲星数量；忽略小项的影响，简化的脉冲星导航测量模型为：

y₃＝h₃(x)＝[Δt₁,Δt₂,…,Δt_m]+ε₃  (11a)

${\mathrm{\&Delta;t}}_j=\frac{n_j\&CenterDot;r_b}{c}+\frac{1}{{2\mathrm{cD}}_{0j}}[{(n_j\&CenterDot;r_b)}^2-{r_b}^2]+\frac{{2\mathrm{\&mu;}}_s}{c^3}|\frac{n_j\&CenterDot;r_b+r_b}{n_j\&CenterDot;b+b}+1|,j=\mathrm{1,2},...,m---\left(11b\right)$

式中，ε₃为测量误差，认为服从高斯分布；在火星最终接近段采用脉冲星测量信息，可以覆盖接近段全过程，满足了自主导航的实时性要求；

对光学测量信息、无线电测量信息及X射线脉冲星测量信息进行组合，式(6)(8)(11)构建了火星最终接近段的自主导航测量模型，可描述为y_k＝h_k(x),k＝1,2,3；

步骤3：基于联邦结构的组合导航滤波算法，解算探测器实时导航状态信息；

根据火星最终接近段状态模型及测量模型y_k＝h_k(x),k＝1,2,3，通过导航滤波计算可以对探测器状态进行估计；由于最终接近过程中，探测器通过光学导航测量，无线电导航测量和X射线脉冲星导航测量获取多源信息，光学导航对垂直光轴方向上的状态估计精度较高，而基于轨道器的无线电导航对径向状态估计精度较高；为了有效融合多源观测信息，采用联邦结构的组合导航滤波算法进行解算，最终输出探测器实时导航状态信息。

3.根据权利要求2所述的一种基于多源信息融合的火星最终接近段自主导航方法，其特征在于：步骤3所述的联邦结构的组合导航滤波算法具体实现方法为，

由于状态模型及测量模型均呈现非线性，故局部滤波器宜选用扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)等任意一种滤波算法；局部滤波器1结合系统动力学模型处理X射线脉冲星测量信息，局部滤波器2结合系统动力学模型处理光学测量信息，局部滤波器3结合系统动力学模型处理无线电测量信息，此外，局部滤波器1、局部滤波器2、局部滤波器3分别滤波输出局部状态估计值和误差协方差阵；信息融合主滤波器对局部滤波器1、局部滤波器2、局部滤波器3的局部滤波结果进行最优融合，得到全局滤波结果，并重置局部滤波器1、局部滤波器2、局部滤波器3；当各敏感器均有观测量输出时，对局部滤波器1、局部滤波器2、局部滤波器3的状态估计值进行最优融合，当光学导航不可用或轨道器不可见时，主滤波器只采用剩余的局部滤波器估计值进行融合或直接输出。

4.根据权利要求3所述的一种基于多源信息融合的火星最终接近段自主导航方法，其特征在于：步骤3所述的局部滤波器1、局部滤波器2、局部滤波器3所用的局部滤波算法为扩展卡尔曼滤波算法，

${\hat{X}}_{i,k/k+1}={\mathrm{\&Phi;}}_k{\hat{X}}_{i,k}---\left(12\right)$

$P_{i,k/k+1}={\mathrm{\&Phi;}}_k{P}_{i,k}{\mathrm{\&Phi;}}_k^T+Q_{i,k}---\left(13\right)$

$K_{i,k+1}=P_{i,k/k+1}{H}_{i,k+1}^T[H_{i,k+1}{P}_{i,k/k+1}{H}_{i,k+1}^T+R_{i,k+1}{]}^{-1}---\left(14\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{i,k/k+1}+K_{i,k+1}[Z_{i,k+1}-H_{i,k+1}{\hat{X}}_{i,k/k+1}]---\left(15\right)$

$P_{i,k+1}=[I-K_{i,k+1}{H}_{i,k+1}]P_{i,k/k+1}---\left(16\right)$

步骤3所述的所述的信息融合主滤波器所用的信息融合算法为，

$P_{g,k+1}=[P_{1,k+1}^{-1}+P_{2,k+1}^{-1}+P_{3,k+1}^{-1}{]}^{-1}---\left(17\right)$

${\hat{X}}_{g,k+1}=P_{g,k+1}[P_{1,k+1}^{-1}{\hat{X}}_{1,k+1}+P_{2,k+1}^{-1}{\hat{X}}_{2,k+1}+P_{3,k+1}^{-1}{\hat{X}}_{3,k+1}]---\left(18\right)$

$\begin{array}{ccc}{\hat{X}}_{i,k+1}={\hat{X}}_{g,k+1},& P_{i,k+1}={\mathrm{\&beta;}}_i^{-1}\&CenterDot;P_{g,k+1},& Q_{i,k+1}{\mathrm{\&beta;}}_i^{-1}{Q}_{k+1}---\left(19\right)\end{array}$

式中，表示系统状态向量，Z为系统观测向量，P表示估计误差方差阵，Φ为根据状态模型得到的状态转移矩阵，H为根据测量模型得到的系统观测矩阵，Q和R分别为系统过程噪声方差阵和观测噪声方差阵；下角标k和k+1表示前一时刻和当前时刻，下角标i表示所属第i个子系统(i＝1,2,3)，下角标g表示全局估计，β_i为信息分配因子，与第i个子系统的误差方差成反比，且满足信息守恒原则， $\underset{i}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_i=1.$

5.根据权利要求3所述的一种基于多源信息融合的火星最终接近段自主导航方法，其特征在于：步骤3所述的局部滤波器1、局部滤波器2、局部滤波器3所用的局部滤波算法为无迹卡尔曼滤波算法，

$M_{i,k}=[{\hat{X}}_{i,k},...{\hat{X}}_{i,k}]+\sqrt{n+{\mathrm{\&lambda;}}_i}\left[\begin{array}{ccc}0& \sqrt{P_{i,k}}& -\sqrt{P_{i,k}}\end{array}\right]---\left(12\right)$

${\hat{M}}_{i,k/k+1}=f(M_{i,k},k)---\left(13\right)$

${\hat{X}}_{k/k+1}={\hat{M}}_{i,k/k+1}{W}_{i,m}---\left(14\right)$

$P_{i,k/k+1}={\hat{M}}_{i,k/k+1}{W}_{i,k}{\hat{M}}_{i,k/k+1}^T+Q_{i,k}---\left(15\right)$

$M_{i,k/k+1}=[{\hat{X}}_{i,k/k+1},\&CenterDot;\&CenterDot;\&CenterDot;,{\hat{X}}_{i,k/k+1}]+\sqrt{n+{\mathrm{\&lambda;}}_i}\left[\begin{array}{ccc}0& \sqrt{P_{i,k/k+1}}& -\sqrt{P_{i,k/k+1}}\end{array}\right]---\left(16\right)$

$Y_{i,k/k+1}=h_i(M_{i,k/k+1},k+1)---\left(17\right)$

μ_i,k+1＝Y_i,k/k+1W_i,m     (18)

$S_{i,k+1}=Y_{i,k/k+1}{W}_{i,c}{Y}_{i,k/k+1}^T+R_{i,k+1}---\left(19\right)$

$C_{i,k+1}=M_{i,k/k+1}{W}_{i,c}{M}_{i,k/k+1}^T---\left(20\right)$

$K_{i,k+1}=C_{i,k+1}{S}_{i,k+1}^{-1}---\left(21\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{i,k/k+1}+K_{i,k+1}[Z_{i,k+1}-{\mathrm{\&mu;}}_{i,k+1}]---\left(22\right)$

$P_{i,k+1}=P_{i,k/k+1}-K_{i,k+1}{S}_{i,k+1}{K}_{i,k+1}^T---\left(23\right)$

步骤3所述的所述的信息融合主滤波器所用的信息融合算法为，

$P_{g,k+1}=[P_{1,k+1}^{-1}+P_{2,k+1}^{-1}+P_{3,k+1}^{-1}{]}^{-1}---\left(24\right)$

${\hat{X}}_{g,k+1}=P_{g,k+1}[P_{1,k+1}^{-1}{\hat{X}}_{1,k+1}+P_{2,k+1}^{-1}{\hat{X}}_{2,k+1}+P_{3,k+1}^{-1}{\hat{X}}_{3,k+1}]---\left(25\right)$

${\hat{X}}_{i,k+1}={\hat{X}}_{g,k+1},P_{i,k+1}{\mathrm{\&beta;}}_i^{-1}\&CenterDot;P_{g.k+1},Q_{i,k+1}={\mathrm{\&beta;}}_i^{-1}{Q}_{k+1}---\left(26\right)$

式中，表示系统状态向量，M为无迹变换的采样点，W_m和W_c为权系数，λ为尺度参数，Z为系统观测向量，P表示估计误差方差阵，Q和R分别为系统过程噪声方差阵和观测噪声方差阵，下角标k和k+1表示前一时刻和当前时刻，下角标i表示所属第i个子系统(i＝1,2,3)，下角标g表示全局估计，β_i为信息分配因子，与第i个子系统的误差方差成反比，且满足信息守恒原则，