CN103344244A - 火星大气进入段消除测量数据中***误差的两步滤波方法 - Google Patents

Abstract

一种火星大气进入段消除测量数据中***误差的两步滤波方法，步骤如下：一、建立基于火星大气进入段飞行器的工程实际方程：二、给定初始值：

和

三、两步滤波方法：（1）、未知的量测***误差估计，（2）、更新：量测更新和时间更新；四、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，至此完成火星大气进入段消除测量数据中***误差的两步滤波方法。该方法不仅可以估计出量测***的测量数据中存在的***误差，而且可以运用估计出来的测量数据中存在的***误差来更好的估计出飞行器的状态。该方法对***的状态可进行精确的估计，因此可以很好的满足未来火星探测任务的着陆误差精度要求。

Description

火星大气进入段消除测量数据中***误差的两步滤波方法

技术领域

本发明涉及火星大气进入段消除测量数据中***误差的两步滤波方法。属于航天导航技术领域。

背景技术

火星大气进入段是整个火星进入、下降、着陆阶段不确定因素最多，状态变化最快，气动环境最复杂、精确着陆任务，火星大气进入段对着陆精度影响最大的阶段，因此要实现飞行器的精确状态估计是必须得到保证的。未来采样返回与载人登陆等火星任务要求着陆误差不超过1km，对火星大气进入段导航精度的要求势必会更高，故有必要对火星大气进入段的导航算法进行研究。然而，决定火星大气进入段导航精度有两个先决条件：

一、火星大气进入段导航滤波的动力学模型必须精确；

二、高精度的传感器和进入导航算法。

滤波方法是非常常见的一种确定飞行器状态估计的方法。传统的滤波方法要求动力学***和量测***的精确已知。但是，在实际工程应用中动力学***和量测***很难精确得到。在火星大气进入段中，由于飞行器初始进入界面的状态具有不确定性，动力学模型中的时变参数具有不确定性，大气密度具有不确定性，飞行器本身特性具有不确定性等等不确定性。这些不确定性导致动力学***很难精确得到，具有未知的偏差输入。另外现有量测手段有限，现有的可以应用于火星大气进入段的量测手段有惯性导航量测方法和无线电测距方法，使得量测***存在未知的量测***误差，例如无线电测距信标误差、无线电信号传递误差以及接收误差。这些动力学***偏差和测量***中的未知测量***误差对确定飞行器的最终状态影响很大，过大的偏差和误差会导致状态误差的增大甚至发散，引起飞行器的导航误差、降低导航精度。

基于以上的实际情况，建立了火星大气进入段消除测量数据中***误差的两步滤波方法，该方法对滤波量测***的未知误差不敏感，可以很好地应用于火星大气进入段飞行器的状态精确估计以满足未来火星的探测任务。

发明内容

1、目的：本发明的目的是提供一种火星大气进入段消除测量数据中***误差的两步滤波方法，以减小飞行器状态估计误差，提高其状态估计精度。

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

本发明一种火星大气进入段消除测量数据中***误差的两步滤波方法，其步骤如下：

步骤一、建立基于火星大气进入段飞行器的工程实际方程：离散时间下的动力学***和量测***

x_k+1=f(x_k)+B_ku_k+w_k   (1)

z_k=h(x_k)+H_kd_k+v_k   (2)

其中x_k表示***状态量，z_k是测量***测量值，u_k是状态方程的确定性控制项,d_k是未知的量测***误差；非线性方程f(·）和h(·)分别是状态转移方程和量测方程并且关于x_k可微；矩阵B_k和H_k具有恰当的维数；w_k和v_k分别是动力学***噪声向量和量测噪声向量，其方差阵分别为Q_k和R_k，它们是不相关的高斯白噪声满足以下式子：

$\left\{\begin{array}{c}E\left[w_k\right]=0\\ \mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0\\ \mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right.;$    (3)

步骤二、给定初始值：

和

其中，

为初始状态的估计值，为初始状态的方差阵；

步骤三、两步滤波方法：

（1）、未知的量测***误差估计

${\tilde{R}}_k{=C}_k{P}_{k/k-1}^x{C}_k^T+R_k$    (4)

$M_k={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{\tilde{R}}_k^{-1}$    (5)

${\hat{d}}_{k/k}=M_k(z_k-C_k{\hat{x}}_{k/k-1})$    (6)

$P_{k/k}^d={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}$    (7)

其中

$C_k={\frac{\∂h\left(x\right)}{\∂x}|}_{x={\hat{x}}_{k/k-1}}$    (8)

式中，为t_k时刻的新息均方误差估计，M_k为t_k时刻的未知的量测***误差的滤波增益，

为t_k时刻的未知的量测***误差的估计，

为状态的一步预测，z_k为t_k时刻的量测数据，

为t_k时刻状态估计的均方误差，

为t_k时刻未知量测***误差估计的均方误差，C_k为量测阵；

（2）、更新

（a）、量测更新

$K_k=P_{k/k-1}^x{C}_k^T{\tilde{R}}_k^{-1}$    (9)

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-C_k{\hat{x}}_{k/k-1}-H_k{\hat{d}}_{k/k})$    (10)

$P_{k/k}^x=P_{k/k-1}^x-K_k\left(\tilde{R}{-H}_k{P}_{k/k}^d{H}_k^T\right)K_k^T$    (11)

$p_{k/k}^{\mathrm{dx}}={\left(P_{k/k}^{\mathrm{xd}}\right)}^T=-{\left(K_k{H}_k{P}_{k/k}^d\right)}^T$    (12)

式中，

为t_k-1时刻到t_k时刻的状态估计的一步预测均方误差，K_k为t_k时刻的状态滤波增益，

为t_k时刻的状态估计，为状态估计与未知量测***误差估计的协方差，为

的转置矩阵；

（b）、时间更新

${\hat{x}}_{k+1/k}=A_k{\hat{x}}_{k/k}+B_k{u}_k+G_k{\hat{d}}_{k/k}$    (13)

$P_{k+1/k}^x=\left[\begin{array}{cc}{A}_k& G_K\end{array}\right]\left[\begin{array}{cc}{P}_{k/k}^x& P_{k/k}^{\mathrm{xd}}\\ P_{k/k}^{\mathrm{dx}}& P_{k/k}^d\end{array}\right]\left[\begin{array}{c}{A}_k^T\\ G_k^T\end{array}\right]+Q_k$    (14)

其中

$A_k={\frac{\∂f\left(x\right)}{\∂x}|}_{x={\hat{x}}_k},G_k=0.$    (15)

式中，A_k为t_k-1时刻到t_k时刻的一步转移矩阵，G_k为常数矩阵，在状态方程中无未知输入的情况下，即在此情况下，G_k取为0矩阵；

步骤四、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时刻T时，至此完成火星大气进入段消除测量数据中***误差的两步滤波方法。

其中，步骤一中要根据实际情况合理建立相应的矩阵B_k和H_k。其中非线性方程f(·)和h(·)关于x_k可微是指，x_k是函数f(·)和h(·)定义域上的一点，且f(x_k)和h(x_k)的导数有定义。且式（3）中δ_kj是克罗内克函数，在数学中，克罗内克函数δ_kj是一个二元函数，克罗内克函数的自变量（输入值）一般是两个整数，如果两者相等，则其输出值为1，否则为0。

其中，在步骤一中所述的“建立基于火星大气进入段飞行器的工程实际方程”，其步骤如下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学***为如下方程：

$\stackrel{\·}{r}=v\sin \mathrm{\γ}$

$\stackrel{\·}{v}=-(D+g_M\sin \mathrm{\γ})$

$\stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g_M}{v})\cos \mathrm{\γ}+\frac{1}{v}L\cos \mathrm{\σ}$

$\stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}$    (16)

$\stackrel{\·}{\mathrm{\λ}}=\frac{v}{r}\cos \mathrm{\γ}\cos \mathrm{\ψ}$

$\stackrel{\·}{\mathrm{\ψ}}=\frac{v}{r}\sin \mathrm{\ψ}\cos \mathrm{\γ}\tan \mathrm{\λ}+\frac{L\sin \mathrm{\σ}}{v\cos \mathrm{\γ}}$

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,控制量σ是滚转角。火星引力

GM_Mars=4.28221×10¹³m³/s²。

L，D分别是气动升力和助力定义为式：

其中C_L和C_D为升力系数和阻力系数。火星大气密度ρ近似满足指数表达形式，其表达形式为

而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km。

目前火星进入段的量测方式主要靠加速度计和陀螺仪，另外有学者提出可以考虑现有在轨的国外三颗通信卫星进行测量导航。由于目前状态下无法形成有效的导航网，量测***存在未知的量测***误差。其对应的量测***可以表示为：

${\tilde{a}}^B=a^B+a_a$    (17)

其中

加速度计量测值,a^B加速度真实值,a_a未知的加速计***误差包含偏差随机游走等。

$\tilde{R}=R+a_R$    (18)

$R=\sqrt{{(r-r_i)}^T(r-r_i)}$    (19)

其中是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测***误差。

1)火星大气进入段对应的离散动力学***可以改写为如下形式：

x_k+1=f(x_k)+B_ku_k+w_k   (20)

其中x_k为火星大气进入段动力学***中的状态量包含式（16）中的左边的各分量，飞行器到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ。B_k为对应的动力学***控制量对动力学***的驱动阵，u_k是控制量对应于滚转角σ。

2)对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (21)

其中

$z=\left[\begin{array}{c}\widetilde{a^B}\\ \tilde{R}\end{array}\right]$    (22)

$h\left(x\right)=\left[\begin{array}{c}{a}^B\\ R\end{array}\right]$    (23)

式中，为加速度计的量测值，其具体表达式见式（17）；

为无线电量测值，其具体表达式见式（18）。h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式（17）和式（19）。

其中，步骤二中根据实际情况估计初值。在火星大气进入段之前，飞行器状态估计值和估计均方误差由大气层外飞行器飞行段末端得到。需要特别强调的是，飞行器初始状态估计值要是无偏的。

其中，步骤三中由式（14）可以发现，虽然G_k=0，但是A_k的行数应该和G_k的行数保持一致，而G_k的列数应该和d_k的行数保持一致。

3、优点和功效：

本发明统筹考虑了火星实际大气进入过程中，非线性、非高斯随机***在量测***存在***误差条件下的飞行器状态估计问题。通过应用消除测量数据中***误差的两步滤波方法，飞行器在火星大气进入段，该方法不仅可以估计出量测***的测量数据中存在的***误差，而且可以运用估计出来的测量数据中存在的***误差来更好的估计出飞行器的状态。通过计算机仿真验证，该方法对***的状态可以进行精确的估计，其中飞行器的位置误差可以达到10m，飞行器的速度误差可以达到1m/s。因此该方法可以很好的满足未来火星探测任务的着陆误差精度要求。

附图说明

图1为非线性增量***的概率密度函数图解模型

图2为大气进入段飞行器扩展卡尔曼滤波方法与消除测量数据中***误差的两步方法状态估计误差比较

图3为火星大气进入段的消除测量数据中***误差的两步滤波方法具体实施的流程图

图中的代号、符号说明如下：

EKF为扩展卡尔曼滤波方法。

ERTSF为消除测量数据中***误差的两步滤波方法。

x_k为状态向量。

u_k是状态方程的确定性控制项。

d_k是未知的量测***误差。

非线性方程f(·)和h(·)分别是状态转移方程和量测方程。

z_k为量测向量。

矩阵B_k和H_k分别是控制向量驱动阵和量测阵。。

w_k和v_k分别是动力学***噪声向量和量测噪声向量

为t₀时刻状态估计的均方误差

具体实施方式

见图3，本发明一种火星大气进入段消除测量数据中***误差的两步滤波方法，其具体实施步骤如下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学***为如下方程。

$\stackrel{\·}{r}=v\sin \mathrm{\γ}$

$\stackrel{\·}{v}=-(D+g_M\sin \mathrm{\γ})$

$\stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g_M}{v})\cos \mathrm{\γ}+\frac{1}{v}L\cos \mathrm{\σ}$

$\stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}$    (24)

$\stackrel{\·}{\mathrm{\λ}}=\frac{v}{r}\cos \mathrm{\γ}\cos \mathrm{\ψ}$

$\stackrel{\·}{\mathrm{\ψ}}=\frac{v}{r}\sin \mathrm{\ψ}\cos \mathrm{\γ}\tan \mathrm{\λ}+\frac{L\sin \mathrm{\σ}}{v\cos \mathrm{\γ}}$

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,控制量σ是滚转角。火星引力

GM_Mars=4.28221×10¹³m³/s²。

L，D分别是气动升力和助力定义为式：

其中C_L和C_D为升力系数和阻力系数。火星大气密度ρ近似满足指数表达形式，其表达形式为

而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km。

目前火星进入段的量测方式主要靠加速度计和陀螺仪，另外有学者提出可以考虑现有在轨的国外三颗通信卫星（见图1）进行测量导航。由于目前状态下无法形成有效的导航网，量测***存在未知的量测***误差。其对应的量测***可以表示为：

${\tilde{a}}^B=a^B+a_a$    (25)

其中

加速度计量测值,a^B加速度真实值,a_a未知的加速计***误差包含偏差随机游走等。

$\tilde{R}=R+a_R$    (26)

$R=\sqrt{{(r-r_i)}^T(r-r_i)}$    (27)

其中是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测***误差。

步骤一：建立工程实际方程：火星大气进入段对应的离散动力学***可以改写为如下形式：

x_k+1=f(x_k)+B_ku_k+w_k   (28)

其中x_k为火星大气进入段动力学***中的状态量包含式（24）中的左边的各分量，飞行器到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ。B_k为对应的动力学***控制量对动力学***的驱动阵，u_k是控制量对应于滚转角σ。

对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (29)

其中

$z=\left[\begin{array}{c}\widetilde{a^B}\\ \tilde{R}\end{array}\right]$    (30)

$h\left(x\right)=\left[\begin{array}{c}{a}^B\\ R\end{array}\right]$    (31)

式中，

为加速度计的量测值，其具体表达式见式（25）；

为无线电量测值，其具体表达式见式（26）。h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式（25）和式（27）。

步骤二、给定初始值：

初始值为飞行器在大气外飞行末端（即大气进入界面）状态估计得到，如表一

表一火星大气进入的初始量及真实量

其中真实量为提前规划的火星大气进入点。实际上也存在一定的不确定性。初始状态估计均方误差 $P_0^x={10}^6\left[\begin{array}{cccccc}10& & & & & \\ & 0.1& & & & \\ & & {10}^{-10}& & & \\ & & & {10}^{-10}& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right].$

三个通信信标的初始位置如表二，而惯性导航和无线电测距的未知量测***误差如表三。

表二火星表面通信信标的初始位置

表三未知量测***误差

步骤三、两步滤波方法：

（1）、未知的量测***误差估计

其中动力学***噪声方差阵为 $R_k=\left[\begin{array}{cccccc}{10}^{-10}& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 10& & \\ & & & & 10& \\ & & & & & 10\end{array}\right],$ 量测噪声方差阵为 $Q=\left[\begin{array}{cccccc}10& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 0.1& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right].$ 并且根据实际的量测***选取相应的矩阵。当三个加速度计***误差一样，三颗卫星到飞行器的无线电测量***误差一样，则得到相应矩阵 $G={\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]}^T,$ $H={\left[\begin{array}{cccccc}1& 1& 1& 0& 0& 0\\ 0& 0& 0& 1& 1& 1\end{array}\right]}^T.$ 对于每个加速度计、无线电测量***误差可以重新设置相应的矩阵。

按照公式（4）-（8）进行滤波，对火星大气进入段未知的量测***误差以及未知量测***误差估计的均方误差进行估计。

（2）、更新

（a）、量测更新

按照式（9）-式（12）对***进行量测估计，包括对状态估计，状态估计的均方误差以及状态估计与未知量测***误差估计的协方差进行更新。

（b）、时间更新

按照式（13）-式（15）对***进行时间更新，包括对状态估计的一步预测均方误差和状态的一步预测进行更新。

步骤四、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时刻T时，至超音速降落伞打开为止。至此完成火星大气进入段消除测量数据中***误差的两步滤波方法。

其中截止时间主要取决于飞行器与火星表面的高度及速度，能否满足超音速降落伞打开。

从图2可以看出，在相同的初始条件下，消除测量数据中***误差的两步滤波方法得到的状态估计比扩展卡尔曼滤波的精度高，能够消除***误差对滤波的影响，提高滤波的稳定性。由于火星大气进入段飞行器与火星表面的高度及速度对于超音速降落伞打开的重要性，所以有必要对火星大气进入段飞行器与火星表面的高度及速度的估计精度进行进一步的研究。从图2的子图1和子图2可以看出，消除测量数据中***误差的两步滤波方法给出的火星大气进入段飞行器与火星表面的高度及速度的估计误差精度相对于由扩展卡尔曼滤波方法给出的估计误差精度提高了很多。可以进一步看出，消除测量数据中***误差的两步滤波方法相对于扩展卡尔曼滤波的优越性。

以上所述仅为本发明较佳的实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化和替换都应涵盖在本发明的保护范围之内，另外本发明提供的方法可以集成到火星大气进入飞行器位置速度估计软件中。

Claims (2)

1.一种火星大气进入段消除测量数据中***误差的两步滤波方法，其特征在于：其步骤如下：

步骤一、建立基于火星大气进入段飞行器的工程实际方程：离散时间下的动力学***和量测***

x_k+1=f(x_k)+B_ku_k+w_k   (1)

z_k=h(x_k)+H_kd_k+v_k   (2)

其中x_k表示***状态量，z_k是测量***测量值，u_k是状态方程的确定性控制项,d_k是未知的量测***误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于x_k可微；矩阵B_k和H_k具有恰当的维数；w_k和v_k分别是动力学***噪声向量和量测噪声向量，其方差阵分别为Q_k和R_k，它们是不相关的高斯白噪声满足以下式子：

$\left\{\begin{array}{c}E\left[w_k\right]=0\\ \mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0\\ \mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right.$    (3)

步骤二、给定初始值：

和

其中，

为初始状态的估计值，

为初始状态的方差阵；

步骤三、两步滤波方法：

（1）、未知的量测***误差估计

${\tilde{R}}_k{=C}_k{P}_{k/k-1}^x{C}_k^T+R_k$    (4)

$M_k={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{\tilde{R}}_k^{-1}$    (5)

${\hat{d}}_{k/k}=M_k(z_k-C_k{\hat{x}}_{k/k-1})$    (6)

$P_{k/k}^d={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}$    (7)

其中，

$C_k={\frac{\∂h\left(x\right)}{\∂x}|}_{x={\hat{x}}_{k/k-1}}$    (8)

式中，

为t_k时刻的新息均方误差估计，M_k为t_k时刻的未知的量测***误差的滤波增益，

为t_k时刻的未知的量测***误差的估计，

为状态的一步预测，z_k为t_k时刻的量测数据，

为t_k时刻状态估计的均方误差，

为t_k时刻未知量测***误差估计的均方误差，C_k为量测阵；

（2）、更新

（a）、量测更新

$K_k=P_{k/k-1}^x{C}_k^T{\tilde{R}}_k^{-1}$    (9)

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-C_k{\hat{x}}_{k/k-1}-H_k{\hat{d}}_{k/k})$    (10)

$P_{k/k}^x=P_{k/k-1}^x-K_k\left(\tilde{R}{-H}_k{P}_{k/k}^d{H}_k^T\right)K_k^T$    (11)

$p_{k/k}^{\mathrm{dx}}={\left(P_{k/k}^{\mathrm{xd}}\right)}^T=-{\left(K_k{H}_k{P}_{k/k}^d\right)}^T$    (12)

式中，

为t_k-1时刻到t_k时刻的状态估计的一步预测均方误差，K_k为t_k时刻的状态滤波增益，为t_k时刻的状态估计，

为状态估计与未知量测***误差估计的协方差，

为

的转置矩阵；

（b）、时间更新

${\hat{x}}_{k+1/k}=A_k{\hat{x}}_{k/k}+B_k{u}_k+G_k{\hat{d}}_{k/k}$    (13)

$P_{k+1/k}^x=\left[\begin{array}{cc}{A}_k& G_K\end{array}\right]\left[\begin{array}{cc}{P}_{k/k}^x& P_{k/k}^{\mathrm{xd}}\\ P_{k/k}^{\mathrm{dx}}& P_{k/k}^d\end{array}\right]\left[\begin{array}{c}{A}_k^T\\ G_k^T\end{array}\right]+Q_k$    (14)

其中

$A_k={\frac{\∂f\left(x\right)}{\∂x}|}_{x={\hat{x}}_k},G_k=0.$    (15)

式中，A_k为t_k-1时刻到t_k时刻的一步转移矩阵，G_k为常数矩阵，在状态方程中无未知输入的情况下，即在此情况下，G_k取为0矩阵；

步骤四、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，至此完成火星大气进入段消除测量数据中***误差的两步滤波方法。

2.根据权利要求1所述的一种火星大气进入段消除测量数据中***误差的两步滤波方法，其特征在于：在在步骤一中所述的“建立基于火星大气进入段飞行器的工程实际方程”，其步骤如下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学***为如下方程：

$\stackrel{\·}{r}=v\sin \mathrm{\γ}$

$\stackrel{\·}{v}=-(D+g_M\sin \mathrm{\γ})$

$\stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g_M}{v})\cos \mathrm{\γ}+\frac{1}{v}L\cos \mathrm{\σ}$

$\stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}$    (16)

$\stackrel{\·}{\mathrm{\λ}}=\frac{v}{r}\cos \mathrm{\γ}\cos \mathrm{\ψ}$

$\stackrel{\·}{\mathrm{\ψ}}=\frac{v}{r}\sin \mathrm{\ψ}\cos \mathrm{\γ}\tan \mathrm{\λ}+\frac{L\sin \mathrm{\σ}}{v\cos \mathrm{\γ}}$

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,控制量σ是滚转角，火星引力

GM_Mars=4.28221×10¹³m³/s²；

L，D分别是气动升力和助力定义为式：其中C_L和C_D为升力系数和阻力系数；火星大气密度ρ近似满足指数表达形式，其表达形式为

而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km；

由于目前状态下无法形成有效的导航网，量测***存在未知的量测***误差，其对应的量测***表示为：

${\tilde{a}}^B=a^B+a_a$    (17)

其中

加速度计量测值,a^B加速度真实值,a_a未知的加速计***误差包含偏差随机游走；

$\tilde{R}=R+a_R$    (18)

$R=\sqrt{{(r-r_i)}^T(r-r_i)}$    (19)

其中

是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测***误差；

1)火星大气进入段对应的离散动力学***改写为如下形式：

x_k+1=f(x_k)+B_ku_k+w_k   (20)

其中x_k为火星大气进入段动力学***中的状态量包含式（16）中的左边的各分量，飞行器到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ，B_k为对应的动力学***控制量对动力学***的驱动阵，u_k是控制量对应于滚转角σ；

2)对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (21)

其中

$z=\left[\begin{array}{c}\widetilde{a^B}\\ \tilde{R}\end{array}\right]$    (22)

$h\left(x\right)=\left[\begin{array}{c}{a}^B\\ R\end{array}\right]$    (23)

式中，

为加速度计的量测值，其具体表达式见式（17）；

为无线电量测值，其具体表达式见式（18）；h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式（17）和式（19）。

Images