CN105300387A - 一种火星大气进入段非线性非高斯秩滤波方法 - Google Patents

Abstract

一种火星大气进入段非线性非高斯秩滤波方法，该方法有四大步骤：步骤一、建立基于火星大气进入段探测器的工程实际方程：步骤二、给定初始值：P₀及系统噪声w_k和v_k的统计特性即均值，方差；步骤三、非线性非高斯秩方法：步骤四、令k＝k+1，返回步骤三继续循环，直到k等于火星大气进入段时间截止所对应的时刻T即超音速降落伞打开所对应的时间为止，至此完成火星大气进入段非线性非高斯秩滤波方法。本发明通过应用非线性非高斯秩滤波方法，探测器在火星大气进入段，可以减少非高斯噪声对系统状态估计的影响，提高状态估计的精度，很好的满足未来火星探测任务对着陆精度的要求。

Description

一种火星大气进入段非线性非高斯秩滤波方法

技术领域

本发明涉及一种火星大气进入段非线性非高斯秩滤波方法。属于航天导航技术领域。

背景技术

火星大气进入段是整个火星进入、下降、着陆阶段历时时间最长，气动环境最复杂、最恶劣，导航误差最大，对着陆精度影响最大的阶段。未来火星探测需要完成采样返回与载人登陆等着陆误差不超过1km或小于100m的任务，这就对火星大气进入段的自主导航精度提出了更高的要求。而影响火星大气进入段自主导航精度的因素主要包括以下三个方面：

一、火星大气进入段精确的动力学模型；

二、火星大气进入段精确的传感器量测模型；

三、火星大气进入自主导航算法。

要实现精确的自主导航，传统的Kalman滤波方法对动力学模型和量测模型要求较为苛刻。然而，在火星大气进入段，气动环境复杂且恶劣，动力学模型具有强非线性，且其噪声的统计特性很难了解，所以要建立精确的动力学模型较为困难。因而，在工程实际中，常将动力学模型的噪声统计特性假设为服从高斯分布的白噪声，因此，这种假设将给实际的火星大气进入段自主导航带来潜在的导航误差。传统的Kalman滤波方法只适用于高斯噪声情况的线性滤波问题；扩展Kalman滤波方法主要适用于弱非线性系统，且其存在线性化导致的截断误差；而无迹Kalman滤波方法虽可用于强非线性系统，但其主要用于高斯噪声系统的非线性滤波。

基于以上的实际情况，建立了一种火星大气进入段非线性非高斯秩滤波方法，该方法适用于非高斯噪声系统的非线性滤波，在火星大气进入段，可以很好地减少非高斯噪声系统对自主导航精度的影响，并提高了火星大气进入段探测器的状态估计精度以满足未来火星精确着陆任务的要求。

发明内容

1、目的：本发明的目的是提供一种火星大气进入段非线性非高斯秩滤波方法，减少非高斯噪声对导航精度的影响，以减小探测器状态估计误差，提高其状态估计的精度。

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

一种火星大气进入段非线性非高斯秩滤波方法，该方法具体步骤如下：

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

x_k+1＝f(x_k,u_k)+w_k(1)

z_k＝h(x_k)+v_k(2)

式中，x_k表示系统状态量；z_k是量测系统测量值；u_k是状态方程的确定性控制项；非线性函数f(·)和h(·)分别为非线性状态转移函数，表示满足探测器动力学原理的函数和非线性量测函数，表示满足量测敏感器测量原理的函数；w_k和v_k分别是动力学系统噪声向量和量测噪声向量，其方差阵分别为Q_k和R_k，且满足

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

式中，δ_kj是克罗内克函数，在数学中，克罗内克函数δ_kj是一个二元函数，克罗内克函数的自变量(输入值)一般是两个整数，如果两者相等，则其输出值为1，否则为0。

步骤二、给定初始值：P₀及系统噪声w_k和v_k的统计特性(均值，方差)其中，为初始状态的估计值，P₀为初始状态的误差方差阵。

步骤三、非线性非高斯秩方法：

(1)、秩采样点集

在4个采样点的情况下，采样策略和对称分布情况的秩采样点集{χ_k-1,i}为

${\mathrm{\χ}}_{k-1,i}=\left\{\begin{array}{cc}{\hat{x}}_{k-1}+u_{p_1}{\left(\sqrt{P_{k-1}}\right)}_i& i=1,...,n\\ {\hat{x}}_{k-1}-u_{p_1}{\left(\sqrt{P_{k-1}}\right)}_{i-n}& i=n+1,...,2n\\ {\hat{x}}_{k-1}+u_{p_2}{\left(\sqrt{P_{k-1}}\right)}_{i-2n}& i=2n+1,...,3n\\ {\hat{x}}_{k-1}-u_{p_2}{\left(\sqrt{P_{k-1}}\right)}_{i-3n}& i=3n+1,...,4n\end{array}\right.---\left(4\right)$

式中，χ_k-1,i为x_k-1的第i个采样点，共有4n个样本点；n为状态向量x_k-1的维数；为P_k-1平方根的第i列向量；为标准正态偏量，用中位秩计算p_j＝(j+2.7)/5.4(也可用平均秩同样计算)，p₁＝0.6852，p₂＝0.8704，

(2)、更新

(a)、时间更新

状态一步预测

${\hat{x}}_{k/k-1}=\frac{1}{4n}\underset{i=1}{\overset{4n}{\Σ}}{x}_{k/k-1,i}---\left(5\right)$

x_k/k-1,i＝f(χ_k-1,i)i＝1,2,…,4n(6)式中，x_k/k-1,i为第i个采样点χ_k-1,i从t_k-1时刻到t_k时刻的一步预测。

一步预测误差的方差阵

$P_{k/k-1}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{(x_{k/k-1,i}-{\hat{x}}_{k/k-1}){(x_{k/k-1,i}-{\hat{x}}_{k/k-1})}^T\right\}+Q_{k-1}---\left(7\right)$

式中，协方差权重系数

$\mathrm{\ω}=2(u_{p_1}^2+u_{p_2}^2)---\left(8\right)$

(b)、量测更新

重新秩采样

${\mathrm{\χ}}_{k/k-1,i}=\left\{\begin{array}{cc}{\hat{x}}_{k/k-1}+u_{p_1}{\left(\sqrt{P_{k/k-1}}\right)}_i& i=1,...,n\\ {\hat{x}}_{k/k-1}-u_{p_1}{\left(\sqrt{P_{k/k-1}}\right)}_{i-n}& i=n+1,...,2n\\ {\hat{x}}_{k/k-1}+u_{p_2}{\left(\sqrt{P_{k/k-1}}\right)}_{i-2n}& i=2n+1,...,3n\\ {\hat{x}}_{k/k-1}-u_{p_2}{\left(\sqrt{P_{k/k-1}}\right)}_{i-3n}& i=3n+1,...,4n\end{array}\right.---\left(9\right)$

状态估计

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})---\left(10\right)$

式中

z_k/k-1,i＝h(χ_k/k-1,i)i＝1,2,…,4n(11)

${\hat{z}}_{k/k-1}=\frac{1}{4n}\underset{i=1}{\overset{4n}{\Σ}}{z}_{k/k-1,i}---\left(12\right)$

而Κ_k为t_k时刻的状态滤波增益，z_k为t_k时刻的量测向量，为t_k-1时刻到t_k时刻的量测一步预测，为t_k时刻的状态估计，z_k/k-1,i为第i个重新采样点χ_k/k-1,i从t_k-1时刻到t_k时刻的量测一步预测。

估计误差的方差阵

$P_k=P_{k/k-1}-K_k{P}_{zz}{K}_k^T---\left(13\right)$

式中，P_zz为系统量测输出变量的方差阵。

滤波增益矩阵

$K_k=P_{xz}{P}_{zz}^{-1}---\left(14\right)$

式中

$P_{zz}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{(z_{k/k-1,i}-{\hat{z}}_{k/k-1}){(z_{k/k-1,i}-{\hat{z}}_{k/k-1})}^T\right\}+R_k---\left(15\right)$

$P_{xz}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{({\mathrm{\χ}}_{k/k-1,i}-{\hat{x}}_{k/k-1}){(z_{k/k-1,i}-{\hat{z}}_{k/k-1})}^T\right\}---\left(16\right)$

而P_xz为系统状态一步预测与系统量测输出变量的协方差阵。

步骤四、令k＝k+1，返回步骤三继续循环。直到k等于火星大气进入段时间截止所对应的时刻T(即超音速降落伞打开所对应的时间)为止。至此完成火星大气进入段非线性非高斯秩滤波方法。

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

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

$\begin{array}{c}\stackrel{\·}{r}=v\sin \mathrm{\γ}\\ \stackrel{\·}{v}=-(d+g\sin \mathrm{\γ})\\ \stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g}{v})\cos \mathrm{\γ}+\frac{1}{v}l\cos \mathrm{\σ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}\\ \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{\γ}}\end{array}---\left(17\right)$

其中，r表示探测器到火星中心的距离，v是探测器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,σ是滚转角(是控制量)；火星引力加速度为火星引力常数μ＝4.28221×10¹³m³/s²；气动升力加速度l和阻力加速度d分别为：其中C_L和C_D为升力系数和阻力系数；火星大气密度ρ近似满足指数表达形式，其表达形式为而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km。

目前火星进入段的量测敏感器主要有惯性量测单元中的加速度计和甚高频无线电测距，量测模型分别为

$a^m=C_p^m{C}_v^p{a}^v---\left(18\right)$

其中，a^m为火星固联坐标系的加速度，表示导航坐标系到火星固联坐标系的转换矩阵，表示速度坐标系到导航坐标系的转换矩阵，a^v为惯性量测单元中的加速度计在速度坐标系下输出的加速度，表达式分别为

$C_p^m=\left[\begin{array}{ccc}cos\mathrm{\λ}cos\mathrm{\θ}& -\sin \mathrm{\θ}& -sin\mathrm{\λ}cos\mathrm{\θ}\\ cos\mathrm{\λ}sin\mathrm{\θ}& \cos \mathrm{\θ}& -sin\mathrm{\λ}sin\mathrm{\θ}\\ \sin \mathrm{\λ}& 0& cos\mathrm{\λ}\end{array}\right]---\left(19\right)$

$C_v^p=\left[\begin{array}{ccc}cos\mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}cos\mathrm{\ψ}& cos\mathrm{\γ}\cos \mathrm{\ψ}& -sin\mathrm{\ψ}\\ -\sin \mathrm{\γ}sin\mathrm{\ψ}& cos\mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]---\left(20\right)$

a^v＝[-d-lsinσlcosσ]^T(21)

R_i(i＝1,2,3)为探测器与在轨卫星或火星表面信标通过无线电通信测得二者之间的距离，其表达式为

$R_i=\sqrt{{(r_l-r_{b_i})}^T(r_l-r_{b_i})}---\left(22\right)$

其中，r_l是探测器的位置矢量，表示第i个火星表面导航信标的位置矢量。

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

x_k+1＝f(x_k,u_k)+w_k(23)式中，x_k＝[rvγθλψ]^T为火星大气进入段动力学系统中的状态变量包含式(17)中

左边的各分量，控制量u_k对应于滚转角σ。

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

z_k＝h(x_k)+v_k(24)

式中

h(x_k)＝[a^m,R₁,R₂,R₃]^T(25)

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

3、优点和功效：

本发明统筹考虑了火星实际大气进入过程中，非线性非高斯随机系统的探测器状态估计问题。通过应用非线性非高斯秩滤波方法，探测器在火星大气进入段，可以减少非高斯噪声对系统状态估计的影响，提高状态估计的精度。通过计算机仿真验证，该方法对系统的状态可以进行精确的估计，其中探测器的位置估计误差精度可以达到1.11m，速度估计误差精度可以达到0.01m/s。因此该方法可以很好的满足未来火星探测任务对着陆精度的要求。

附图说明

图1为火星大气进入段导航示意图。

图2a为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法高度估计误差示意图。

图2b为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法速度估计误差示意图。

图2c为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法飞行路径角估计误差示意图。

图2d为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法经度估计误差示意图。

图2e为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法纬度估计误差示意图。

图2f为火星大气进入段无迹Kalman滤波方法与非线性非高斯秩滤波方法航向角估计误差示意图。

图3为火星大气进入段非线性非高斯秩滤波方法具体实施的流程图。

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

UKF为无迹Kalman滤波方法。

RF为非线性非高斯秩滤波方法。

x_k为状态向量。

z_k是量测系统测量值。

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

f(·)和h(·)分别为非线性状态转移函数，表示满足探测器动力学原理的函数和非线性量测函数，表示满足量测敏感器测量原理的函数z_k为量测向量。

w_k和v_k分别是动力学系统噪声向量和量测噪声向量。

为初始状态的估计值。

P₀为初始状态的误差方差阵。

具体实施方式

见图1-图3，本发明涉及一种火星大气进入段非线性非高斯秩滤波方法，具体实施步骤如下：

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

$\begin{array}{c}\stackrel{\·}{r}=v\sin \mathrm{\γ}\\ \stackrel{\·}{v}=-(d+g\sin \mathrm{\γ})\\ \stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g}{v})\cos \mathrm{\γ}+\frac{1}{v}l\cos \mathrm{\σ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}\\ \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{\γ}}\end{array}---\left(26\right)$

其中，r表示探测器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,σ是滚转角(是控制量)；火星引力加速度为火星引力常数μ＝4.28221×10¹³m³/s²；气动升力加速度l和阻力加速度d分别为：其中C_L和C_D为升力系数和阻力系数；火星大气密度ρ近似满足指数表达形式，其表达形式为而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km。

目前火星进入段的量测敏感器主要有惯性量测单元中的加速度计和甚高频无线电测距(考虑如图1所示的三颗火星表面信标)，量测模型分别为

$a^m=C_p^m{C}_v^p{a}^v---\left(27\right)$

其中，a^m为火星固联坐标系的加速度，表示导航坐标系到火星固联坐标系的转换矩阵，表示速度坐标系到导航坐标系的转换矩阵，a^v为惯性量测单元中的加速度计在速度坐标系下输出的加速度，表达式分别为

$C_p^m=\left[\begin{array}{ccc}cos\mathrm{\λ}cos\mathrm{\θ}& -sin\mathrm{\θ}& -sin\mathrm{\λ}cos\mathrm{\θ}\\ cos\mathrm{\λ}sin\mathrm{\θ}& \cos \mathrm{\θ}& -sin\mathrm{\λ}sin\mathrm{\θ}\\ \sin \mathrm{\λ}& 0& cos\mathrm{\λ}\end{array}\right]---\left(28\right)$

$C_v^p=\left[\begin{array}{ccc}cos\mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -sin\mathrm{\γ}cos\mathrm{\ψ}& cos\mathrm{\γ}\cos \mathrm{\ψ}& -sin\mathrm{\ψ}\\ -sin\mathrm{\γ}sin\mathrm{\ψ}& cos\mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]---\left(29\right)$

a^v＝[-d-lsinσlcosσ]^T(30)

R_i(i＝1,2,3)为探测器与在轨卫星或火星表面信标的无线电通信测得二者之间的距离

$R_i=\sqrt{{(r_l-r_{b_i})}^T(r_l-r_{h_i})}---\left(31\right)$

其中，r_l是探测器的位置矢量，表示第i个火星表面导航信标的位置矢量。

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

x_k+1＝f(x_k,u_k)+w_k(32)

式中，x_k＝[rvγθλψ]^T为火星大气进入段动力学系统中的状态变量包含式(26)中左边的各分量，控制量u_k对应于滚转角σ。

对应的离散量测方程为

z_k＝h(x_k)+v_k(33)

式中

h(x_k)＝[a^m,R₁,R₂,R₃]^T(34)

步骤二、给定初始值：

初始状态真实值和估计值，如表一所示

表一火星大气进入段探测器状态初始估计值及真实值

其中真实值为提前规划的火星大气进入点；估计值实际上与真实值存在一定误差。初始状态估计均方误差 $P_0=\left[\begin{array}{cccccc}{10}^9& & & & & \\ & 0.1& & & & \\ & & {10}^{-10}& & & \\ & & & {10}^{-10}& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right].$ 而动力学系统噪声服从均值为0，方差阵为 $Q_k=\left[\begin{array}{cccccc}100& & & & & \\ & 1& & & & \\ & & {10}^{-6}& & & \\ & & & {10}^{-6}& & \\ & & & & {10}^{-6}& \\ & & & & & {10}^{-6}\end{array}\right]$ 的极值分布；量测噪声服从均值为0，方差阵为 $R_k=\left[\begin{array}{cccccc}{10}^{-6}& & & & & \\ & {10}^{-6}& & & & \\ & & {10}^{-6}& & & \\ & & & 10& & \\ & & & & 10& \\ & & & & & 10\end{array}\right]$ 的高斯分布。

三个火星表面导航信标的位置如表二所示。

表二火星表面导航信标的位置

步骤三、非线性非高斯秩滤波：

(1)、秩采样点集

按照公式(4)进行秩采样，得到下一步所需的秩采样点集。

(2)、更新

(a)、时间更新

按照式(5)-式(8)对系统状态进行时间更新，得到t_k-1时刻到t_k时刻状态一步预测及一步预测误差的方差阵。

(b)、量测更新

按照式(9)-式(16)对系统进行量测更新，得到t_k时刻状态估计及估计误差的方差阵。

步骤四、令k＝k+1，返回步骤三继续循环。直到k等于火星大气进入段时间截止所对应的时刻T(即超音速降落伞打开所对应的时间)为止。至此完成火星大气进入段非线性非高斯秩滤波方法。

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

从图2a-f可以看出，在相同的初始条件下，非线性非高斯秩滤波方法得到的状态估计精度比无迹Kalman滤波方法的精度高，能够减少系统非高斯噪声对滤波的影响，提高状态估计的精度。由于火星大气进入段探测器与火星表面的高度及速度对于超音速降落伞打开的重要性，所以有必要对火星大气进入段探测器与火星表面的高度及速度的估计精度进行进一步的研究。从图2的子图a和子图b可以看出，非线性非高斯秩滤波方法给出的火星大气进入段探测器高度及速度的估计误差精度相对于由无迹Kalman滤波方法得到的估计误差精度提高了很多。可以进一步看出，非线性非高斯秩滤波方法相对于无迹Kalman滤波方法的优越性。

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

Claims (3)

1.一种火星大气进入段非线性非高斯秩滤波方法，其特征在于：该方法具体步骤如下：

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

x_k+1＝f(x_k,u_k)+w_k(1)

z_k＝h(x_k)+v_k(2)

式中，x_k表示系统状态量；z_k是量测系统测量值；u_k是状态方程的确定性控制项；非线性函数f(·)和h(·)分别为非线性状态转移函数，表示满足探测器动力学原理的函数和非线性量测函数，表示满足量测敏感器测量原理的函数；w_k和v_k分别是动力学系统噪声向量和量测噪声向量，其方差阵分别为Q_k和R_k，且满足

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

式中，δ_kj是克罗内克函数，在数学中，克罗内克函数δ_kj是一个二元函数，克罗内克函数的自变量即输入值是两个整数，如果两者相等，则其输出值为1，否则为0；

步骤二、给定初始值：P₀及系统噪声w_k和v_k的统计特性即均值，方差其中，为初始状态的估计值，P₀为初始状态的误差方差阵；

步骤三、非线性非高斯秩方法：

(1)、秩采样点集

在4个采样点的情况下，采样策略和对称分布情况的秩采样点集{χ_k-1,i}为

${\mathrm{\χ}}_{k-1,i}=\left\{\begin{array}{cc}{\hat{x}}_{k-1}+u_{p_1}{\left(\sqrt{P_{k-1}}\right)}_i& i=1,...,n\\ {\hat{x}}_{k-1}-u_{p_1}{\left(\sqrt{P_{k-1}}\right)}_{i-n}& i=n+1,...,2n\\ {\hat{x}}_{k-1}+u_{p_2}{\left(\sqrt{P_{k-1}}\right)}_{i-2n}& i=2n+1,...,3n\\ {\hat{x}}_{k-1}-u_{p_2}{\left(\sqrt{P_{k-1}}\right)}_{i-3n}& i=3n+1,...,4n\end{array}\right.---\left(4\right)$

式中，χ_k-1,i为x_k-1的第i个采样点，共有4n个样本点；n为状态向量x_k-1的维数；为P_k-1平方根的第i列向量；为标准正态偏量，用中位秩计算p_j＝(j+2.7)/5.4，也能用平均秩同样计算，p₁＝0.6852，u_p1＝0.4822，p₂＝0.8704，u_p2＝1.1281；

(2)、更新

(a)、时间更新

状态一步预测

${\hat{x}}_{k/k-1}=\frac{1}{4n}\underset{i=1}{\overset{4n}{\Σ}}{x}_{k/k-1,i}---\left(5\right)$

x_k/k-1,i＝f(χ_k-1,i)i＝1,2,…,4n(6)

式中，x_k/k-1,i为第i个采样点χ_k-1,i从t_k-1时刻到t_k时刻的一步预测；

一步预测误差的方差阵

$P_{k/k-1}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{(x_{k/k-1,i}-{\hat{x}}_{k/k-1}){(x_{k/k-1,i}-{\hat{x}}_{k/k-1})}^T\right\}+Q_{k-1}---\left(7\right)$

式中，协方差权重系数

$\mathrm{\ω}=2(u_{p_1}^2+u_{p_2}^2)---\left(8\right)$

(b)、量测更新

重新秩采样

${\mathrm{\χ}}_{k/k-1,i}=\left\{\begin{array}{cc}{\hat{x}}_{k/k-1}+u_{p_1}{\left(\sqrt{P_{k/k-1}}\right)}_i& i=1,...,n\\ {\hat{x}}_{k/k-1}-u_{p_1}{\left(\sqrt{P_{k/k-1}}\right)}_{i-n}& i=n+1,...,2n\\ {\hat{x}}_{k/k-1}+u_{p_2}{\left(\sqrt{P_{k/k-1}}\right)}_{i-2n}& i=2n+1,...,3n\\ {\hat{x}}_{k/k-1}-u_{p_2}{\left(\sqrt{P_{k/k-1}}\right)}_{i-3n}& i=3n+1,...,4n\end{array}\right.---\left(9\right)$

状态估计

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(z_k-{\hat{z}}_{k/k-1})---\left(10\right)$

式中

z_k/k-1,i＝h(χ_k/k-1,i)i＝1,2,…,4n(11)

${\hat{z}}_{k/k-1}=\frac{1}{4n}\underset{i=1}{\overset{4n}{\Σ}}{z}_{k/k-1,i}---\left(12\right)$

而K_k为t_k时刻的状态滤波增益，z_k为t_k时刻的量测向量，为t_k-1时刻到t_k时刻的量测一步预测，为t_k时刻的状态估计，z_k/k-1,i为第i个重新采样点χ_k/k-1,i从t_k-1时刻到t_k时刻的量测一步预测；

估计误差的方差阵

$P_k=P_{k/k-1}-K_k{P}_{zz}{K}_k^T---\left(13\right)$

式中，P_zz为系统量测输出变量的方差阵，

滤波增益矩阵

$K_k=P_{xz}{P}_{zz}^{-1}---\left(14\right)$

式中

$P_{zz}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{(z_{k/k-1,i}-{\hat{z}}_{k/k-1}){(z_{k/k-1,i}-{\hat{z}}_{k/k-1})}^T\right\}+R_k---\left(15\right)$

$P_{xz}=\frac{1}{\mathrm{\ω}}\underset{i=1}{\overset{4n}{\Σ}}\left\{({\mathrm{\χ}}_{k/k-1,i}-{\hat{x}}_{k/k-1}){(z_{k/k-1,i}-{\hat{z}}_{k/k-1})}^T\right\}---\left(16\right)$

而P_xz为系统状态一步预测与系统量测输出变量的协方差阵；

步骤四、令k＝k+1，返回步骤三继续循环，直到k等于火星大气进入段时间截止所对应的时刻T即超音速降落伞打开所对应的时间为止，至此完成火星大气进入段非线性非高斯秩滤波方法。

2.根据权利要求1所述的一种火星大气进入段非线性非高斯秩滤波方法，其特征在于：在步骤一中所述的“建立基于火星大气进入段探测器的工程实际方程”，其步骤如下：

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

$\begin{array}{c}\stackrel{\·}{r}=v\sin \mathrm{\γ}\\ \stackrel{\·}{v}=-(d+g\sin \mathrm{\γ})\\ \stackrel{\·}{\mathrm{\γ}}=(\frac{v}{r}-\frac{g}{v})\cos \mathrm{\γ}+\frac{1}{v}l\cos \mathrm{\σ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{v\cos \mathrm{\γ}\sin \mathrm{\ψ}}{r\cos \mathrm{\λ}}\\ \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{\γ}}\end{array}---\left(17\right)$

其中，r表示探测器到火星中心的距离，v是探测器的速度,θ是经度，λ是纬度,γ是飞行路径角,ψ是航向角,σ是滚转角即控制量；火星引力加速度为火星引力常数μ＝4.28221×10¹³m³/s²；气动升力加速度l和阻力加速度d分别为：其中C_L和C_D为升力系数和阻力系数；火星大气密度ρ近似满足指数表达形式，其表达形式为而ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向半径，大小为3437.2km；

目前火星进入段的量测敏感器有惯性量测单元中的加速度计和甚高频无线电测距，量测模型分别为

$a^m=C_p^m{C}_v^p{a}^v---\left(18\right)$

其中，a^m为火星固联坐标系的加速度，表示导航坐标系到火星固联坐标系的转换矩阵，表示速度坐标系到导航坐标系的转换矩阵，a^v为惯性量测单元中的加速度计在速度坐标系下输出的加速度，表达式分别为

$C_p^m=\left[\begin{array}{ccc}cos\mathrm{\λ}cos\mathrm{\θ}& -\sin \mathrm{\θ}& -sin\mathrm{\λ}cos\mathrm{\θ}\\ cos\mathrm{\λ}sin\mathrm{\θ}& \cos \mathrm{\θ}& -sin\mathrm{\λ}sin\mathrm{\θ}\\ \sin \mathrm{\λ}& 0& cos\mathrm{\λ}\end{array}\right]---\left(19\right)$

$C_v^p=\left[\begin{array}{ccc}cos\mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}cos\mathrm{\ψ}& cos\mathrm{\γ}\cos \mathrm{\ψ}& -sin\mathrm{\ψ}\\ -\sin \mathrm{\γ}sin\mathrm{\ψ}& cos\mathrm{\γ}sin\mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]---\left(20\right)$

a^v＝[-d-lsinσlcosσ]^T(21)

R_i(i＝1,2,3)为探测器与在轨卫星或火星表面信标通过无线电通信测得二者之间的距离，其表达式为

$R_i=\sqrt{{(r_l-r_{b_i})}^T(r_l-r_{b_i})}---\left(22\right)$

其中，r_l是探测器的位置矢量，r_bi表示第i个火星表面导航信标的位置矢量；

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

x_k+1＝f(x_k,u_k)+w_k(23)

式中，x_k＝[rvγθλψ]^T为火星大气进入段动力学系统中的状态变量包含式(17)中左边的各分量，控制量u_k对应于滚转角σ；

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

z_k＝h(x_k)+v_k(24)

式中

h(x_k)＝[a^m,R₁,R₂,R₃]^T(25)。

3.根据权利要求1所述的一种火星大气进入段非线性非高斯秩滤波方法，其特征在于：步骤二中根据实际情况估计初值，在火星大气进入段之前，探测器状态估计值和估计均方误差由大气层外探测器飞行段末端得到，强调的是，探测器初始状态估计值要是无偏的。