CN103323009A - 火星大气进入段的非线性三步滤波方法 - Google Patents

Abstract

一种火星大气进入段的非线性三步滤波方法，它包括以下步骤：一、建立工程实际方程；二、给定初始值；三、对状态量x_k进行滤波；四、对动力学系统偏差f_k进行滤波；五、对测量系统中的未知测量系统误差d_k进行滤波；六、更新相关系数、校正状态估计和动力学偏差估计；七、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，即降落伞打开为止；至此完成火星大气进入段的非线性三步滤波方法。本发明统筹考虑了火星实际大气进入过程中，非线性、非高斯随机系统在动力学系统偏差和测量系统中的未知测量系统误差条件下的航天器位置速度估计问题，可以有效保证航天器在火星大气进入段的位置速度估计。

Description

火星大气进入段的非线性三步滤波方法

技术领域

本发明涉及火星大气进入段的非线性三步滤波方法。属于航天导航技术领域。

背景技术

Kalman方法是非常常见的一种确定航天器位置速度方法。它要求动力学系统和量测系统的精确已知。在实际工程中动力学系统和量测系统很难精确得到。在火星大气进入段中，由于飞行器初始进入界面的状态具有不确定性，动力学模型中的时变参数具有不确定性，大气密度具有不确定性，飞行器本身特性具有不确定性。这些不确定性导致动力学系统很难精确得到，产生未知的偏差输入，在火星大气进入阶段地面深空网等测量方法无法利用，使得动力学系统难以校正，具有很大不确定性。另外，现有火星大气进入段量测手段有限，即使在地球上已校准的设备在火星大气进入段中将产生新的未知的系统误差并且难以相互校正，从而使得量测系统存在未知的量测系统误差。这些动力学系统偏差和测量系统中的未知测量系统误差对确定航天器位置速度最终状态影响很大，过大的偏差和误差会导致位置速度误差的增大甚至发散，引起飞行器导航误差、降低导航精度。

现有技术中，可以用于确定航天器位置速度的方法有多种。

现有技术一，基于泰勒展开的扩展Kalman滤波估计方法。该方法忽略了动力学系统偏差和测量系统中的未知测量系统误差。该方法给出了非线性动力学方程和非线性测量方程的泰勒展开加权融合的估算公式。

现有技术二，基于sigma点集（为正态分布采样策略）的无迹Kalman滤波方法。先根据正态分布的均值和方差计算出sigma点集，并确定出各点的权值，再通过动力学方程计算出航天器的位置速度，然后通过量测方程得到的量测数据对航天器的位置速度进行调整修正。

现有技术三，将这些不确定参数扩展为状态量，然后利用Kalman滤波或无迹Kalman滤波方法进行滤波。

现有技术一适用于动力学系统和量测系统精确可知或偏差和量测系统误差对其影响不大的条件下。在超音速强耦合强干扰非线性环境中将动力学展开得到显著的误差，因此不太适用于火星大气进入段。

现有技术二在测量手段有限，测量数据少，难以动力学系统偏差和测量系统中的未知测量系统误差估算和消除。测量设备即使在地面不同环境中产生的系统误差各不相同。即使在地面试验中已经校准的系统误差在新的火星环境（其环境不同与地球）中不再准确，因此量测数据中的系统误差将影响航天器的位置速度进行调整修正，因此不太适用于火星大气进入段。

现有技术三对火星大气进入段的不确定参数很难有效分离，通常认为是常量不太适合实际情况，并且增加了动力学系统的维数加大了计算量。

现有技术一二三对于未知的火星环境，其动力学系统存在动力学系统偏差，其测量系统具有未知测量系统误差。即使这些偏差和误差在地球上已经校准，在火星环境中将难以适用甚至产生新的误差，难以进行校正。因此不太适用于火星大气进入段。

发明内容

1、目的：本发明的目的是提供一种火星大气进入段的非线性三步滤波方法，以减小航天器位置速度误差，提高其精度。

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

本发明一种火星大气进入段的非线性三步滤波方法，它包括以下步骤：

步骤一、建立工程实际方程：离散时间下的动力学系统和量测系统

$x_{k+1}=f(x_k,u_k)+F_k^x{f}_k+E_k^x{d}_k+w_k^x---\left(1\right)$

$z_k=h\left(x_k\right)+F_k^z{f}_k+E_k^z{d}_k+v_k---\left(2\right)$

其中x_k表示系统状态量，z_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差。非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_k微。矩阵

具有恰当的维数。

和v_k分别是动力学系统噪声，它们是不相关的高斯白噪声满足以下式子。

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

步骤二、给定初始值：

和

为初始状态的估计值，

为初始状态估计均方误差，

为动力学偏差和量测系统误差的相关系数。

步骤三、对状态量x_k进行滤波

${\stackrel{-}{x}}_k(-)=f({\hat{x}}_{k-1},u_{k-1})---\left(4\right)$

${\stackrel{-}{P}}_k^x(-)={\mathrm{\Φ}}_{k-1}{\hat{P}}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}---\left(5\right)$

${\stackrel{-}{K}}_k^x={\stackrel{-}{P}}_k^x(-)S_k^{1T}{C}_k^{-1}---\left(6\right)$

${\stackrel{-}{P}}_k^x(+)=(I-{\stackrel{-}{K}}_k^x{S}_k^1){\stackrel{-}{P}}_k^x(-)---\left(7\right)$

${\widehat{\mathrm{\η}}}_k^x=z_k-h\left({\stackrel{-}{x}}_k(-)\right),$ ${\stackrel{-}{x}}_k(+)={\stackrel{-}{x}}_k(-)+{\stackrel{-}{K}}_k^x{\stackrel{-}{\mathrm{\η}}}_k^x---\left(8\right)$

$C_k=S_k^1{\stackrel{-}{P}}_k^x(-)S_k^{1T}+R_k---\left(9\right)$

其中

${\mathrm{\Φ}}_k=\frac{\∂f\left(x\right)}{\∂x}{|}_{x=\hat{x}k(+)}$ $S_k^1=H_k=\frac{\∂h\left(x\right)}{\∂x}{|}_{x={\hat{x}}_k(-)}.---\left(10\right)$

式中：

为t_k-1时刻的状态量，u_k-1为t_k-1时刻的控制输入量。

为状态的一步预测。

为t_k-1时刻的状态估计均方误差，Φ_k-1为t_k-1时刻到t_k时刻的一步转移矩阵；Q_k-1为系统的噪声的方差阵，

为一步预测均方误差。

为量测阵，

为状态增益，C_k为量测新息误差阵。I为单位阵，

为t_k时刻的状态估计均方误差。为量测新息，为状态估计。

步骤四、对动力学系统偏差fk进行滤波

$U_k^{12}=F_{k-1}^x---\left(11\right)$

$S_k^2=H_k{U}_k^{12}+F_k^y---\left(12\right)$

${\stackrel{-}{P}}_k^f(+)={\left(S_k^{2T}{C}_k^{-1}{S}_k^2\right)}^{+}---\left(13\right)$

${\stackrel{-}{K}}_k^f={\stackrel{-}{P}}_k^f(+)S_k^{2T}{C}_k^{-1}---\left(14\right)$

${\stackrel{-}{f}}_k(+)={\stackrel{-}{K}}_k^f{\stackrel{-}{\mathrm{\η}}}_k^x---\left(15\right)$

为t_k-1时刻动力学系统偏差对动力学系统驱动阵，

为t_k时刻动力学系统偏差对量测系统量测阵，为t_k时刻动力学系统偏差对量测系统校正量测阵，为t_k时刻动力学系统偏差估计均方误差，

为

广义逆矩阵，为动力学系统偏差滤波增益。

为t_k时刻动力学系统偏差状态估计。

步骤五、对测量系统中的未知测量系统误差d_k进行滤波

$U_k^{23}=V_{k-1}^{23}---\left(16\right)$

$U_k^{13}=E_{k-1}^x+F_{k-1}^x{V}_{k-1}^{23}---\left(17\right)$

$S_k^3=H_k{U}_k^{13}+F_k^y{U}_k^{23}+E_k^y---\left(18\right)$

${\stackrel{-}{P}}_k^d(+)={\left(S_k^{3T}{C}_k^{-1}{S}_k^3\right)}^{+}---\left(19\right)$

${\stackrel{-}{K}}_k^d={\stackrel{-}{P}}_k^d(+)S_k^{3T}{C}_k^{-1}---\left(20\right)$

${\stackrel{-}{d}}_k(+)={\stackrel{-}{K}}_k^d{\stackrel{-}{\mathrm{\η}}}_k^x---\left(21\right)$

为t_k-1时刻量测系统误差对动力学系统驱动阵，

为t_k时刻量测系统误差对量测系统量测阵，为t_k时刻量测系统误差对量测系统校正量测阵，

为t_k时刻量测系统误差估计均方误差，

为

广义逆矩阵，为量测系统误差滤波增益。

为t_k时刻动力学系统偏差状态估计。为t_k-1时刻动力学偏差和量测系统误差的相关系数。

步骤六、更新相关系数、校正状态估计和动力学偏差估计：情况如下

$V_k^{12}=U_k^{12}-{\stackrel{-}{K}}_k^x{S}_k^2,$ $V_k^{13}=U_k^{13}-V_k^{12}{\stackrel{-}{K}}_k^f{S}_k^3-{\stackrel{-}{K}}_k^x{S}_k^3$ $V_k^{23}=V_{k-1}^{23}-{\stackrel{-}{K}}_k^f{S}_k^3---\left(22\right)$

${\hat{x}}_k(+)={\stackrel{-}{x}}_k(+)+V_k^{12}{\stackrel{-}{f}}_k(+)+V_k^{13}{\stackrel{-}{d}}_k(+),$ ${\hat{P}}_k^x(+)={\stackrel{-}{P}}_k^x(+)+V_k^{12}{\stackrel{-}{P}}_k^f(+)V_k^{12T}+V_k^{13}{\stackrel{-}{P}}_k^d(+)V_k^{13T}---\left(23\right)$

${\hat{f}}_k(+)={\stackrel{-}{f}}_k(+)+V_k^{23}{\stackrel{-}{d}}_k(+),$ ${\hat{P}}_k^f(+)={\stackrel{-}{P}}_k^f(+)+V_k^{23}{\stackrel{-}{P}}_k^d(+)V_k^{23T}---\left(24\right)$

为t_k时刻校正后的状态量，

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

为t_k时刻校正后的状态量，

为t_k时刻校正后的状态估计均方误差。

步骤七、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时刻T时，即降落伞打开为止。至此完成火星大气进入段的非线性三步滤波方法。

当离散条件下的扩维动力学系统和量测系统表达形式为

$x_{k+1}={\mathrm{\Φ}}_k{x}_k+B_k{u}_k+F_k^x{f}_k+E_k^x{d}_k+w_k^x---\left(25\right)$

$z_k=H_k{x}_k+F_k^z{f}_k+E_k^z{d}_k+v_k---\left(26\right)$

其扩维Kalman滤波表达形式为

${\hat{x}}_k^a(-)={\mathrm{\Φ}}_{k-1}^a{\hat{x}}_{k-1}^a(+)+B_{k-1}^a{u}_{k-1}---\left(27\right)$

$P_k^a(-)={\mathrm{\Φ}}_{k-1}^a{P}_{k-1}^a(+){\mathrm{\Φ}}_{k-1}^{\mathrm{aT}}+Q_{k-1}^a---\left(28\right)$

$K_k^a=P_k^a(-)H_k^{\mathrm{aT}}{(H_k^a{P}_k^a(-)H_k^{\mathrm{aT}}+R_k)}^{-1}---\left(29\right)$

${\hat{x}}_k^a(+)={\hat{x}}_k^a(-)+K_k^a\widetilde{z_k}={\hat{x}}_{k/k-1}^a+K_k^a(z_k-H_k^a{\hat{x}}_k^a(-))---\left(30\right)$

$P_k^a(+)=(I-K_k^a{H}_k^a)P_k^a(-)---\left(31\right)$

其中

$x_k^a(\·)=\left[\begin{array}{c}{x}_k(\·)\\ f_k(\·)\\ d_k(\·)\end{array}\right],$ $P_k^a(\·)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{ccc}{P}_k^x(\·)& P_k^{\mathrm{xf}}(\·)& P_k^{\mathrm{xd}}(\·)\\ {\left(P_k^{\mathrm{xf}}(\·)\right)}^T& P_k^f(\·)& P_k^{\mathrm{fd}}(\·)\\ {\left(P_k^{\mathrm{xd}}(\·)\right)}^T& {\left(P_k^{\mathrm{fd}}(\·)\right)}^T& P_k^d(\·)\end{array}\right],$ $K_k^a=\left[\begin{array}{c}{K}_k^x\\ K_k^f\\ K_k^d\end{array}\right],$ ${\mathrm{\Φ}}_k^a=\left[\begin{array}{ccc}{\mathrm{\Φ}}_k& F_k^x& E_k^x\\ & A_k^f& \\ & & A_k^d\end{array}\right],$

$H_k^a=\left[\begin{array}{ccc}{H}_k& F_k^z& E_k^z\end{array}\right]$ 和 $Q_k^a=\left[\begin{array}{ccc}{Q}_k^x& 0& 0\\ 0& Q_k^f& 0\\ 0& 0& Q_k^d\end{array}\right]$

将一步预测均方误差和估计均方误差进行非线性三步U-V变换，其表达形式为

$P_{k/k-1}^a=U_k{\stackrel{-}{P}}_{k/k-1}^a{U}_k^T$

$P_{k/k}^a=V_k{\stackrel{-}{P}}_k^a{V}_k^T$

其中矩阵U_k和V_k定义为如下形式：

$U_k=\left[\begin{array}{ccc}I& U_k^{12}& U_k^{13}\\ 0& I& U_k^{23}\\ 0& 0& I\end{array}\right]$

$V_k=\left[\begin{array}{ccc}I& V_k^{12}& V_k^{13}\\ 0& I& V_k^{23}\\ 0& 0& I\end{array}\right]$

其中，步骤一到步骤七中涉及到的相关系数均为矩阵U_k和V_k的分块矩阵表达形式。

其中，步骤一中要根据实际情况合理建立相应的矩阵

其中，步骤二中根据实际情况估计初值，在火星大气进入段之前由飞行器大气层外飞行段末端得到状态估计值和估计均方误差。且动力学系统偏差和测量系统中的未知测量系统误差为不相关量。

其中，步骤四和步骤五可以根据考虑动力学系统偏差和测量系统中的未知测量系统误差的先后关系进行互换。

其中，在步骤一中所述的建立工程实际方程，其步骤如下：

a、分析动力学不确定性之间的相互关系，并进行相应的数值计算分析；

b、得到这些不确定性因素对动力学模型的影响主要是通过速度的一阶微分方程中的加速度的影响进入动力学模型从而引起误差的传播；

c、将动力学系统x_k+1=f(x_k,u_k)改写为考虑动力学系统偏差和量测系统误差的动力学系统

将量测系统z_k=h(x_k)改写为考虑动力学系统偏差和量测系统误差的量测系统 $z_k=h\left(x_k\right)+F_k^z{f}_k+E_k^z{d}_k+v_k.$

3、优点和功效：

本发明统筹考虑了火星实际大气进入过程中，非线性、非高斯随机系统在动力学系统偏差和测量系统中的未知测量系统误差条件下的航天器位置速度估计问题。通过火星大气进入段的非线性三步滤波方法在计算过程中引入了对动力学系统偏差和测量系统中的未知测量系统误差进行估计和补偿，减弱了动力学系统偏差和测量系统中的未知测量系统误差对滤波引起的导航误差。因而本发明提出的算法可以有效保证航天器在火星大气进入段的位置速度估计。

附图说明

图1为各状态的估计值的误差图

图2为本发明所述方法流程图

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

Altitude表示飞行器距离火星表面高度，velocity表示飞行器速度,longitude表示经度，latitude表示纬度,FPA表示飞行路径角,azimuth是航向角.

RThSKF为火星大气进入段的非线性三步滤波方法。

EKF为火星大气进入的扩展Kalman滤波方法。

具体实施方式

本发明涉及火星大气进入段的非线性三步滤波方法，其航天器沿飞行轨迹进入火星大气，其对应的简化动力学系统为如下方程：

$\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{\λ}}---\left(32\right)$

$\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为升力系数和阻力系数。火星大气密度ρ近似满足指数表达形式。这些量都具有不确定性，主要通过产生加速度偏差进入动力学系统，影响飞行轨迹。

目前火星进入段的量测方式主要靠加速度计和陀螺仪，另外有学者提出可以考虑现有在轨的国外三颗通信卫星（美国两颗，欧空局一颗，由于对我国的技术封锁估计，很难为我国提高服务。）进行测量导航。由于目前状态下无法形成有效的导航网，量测系统存在未知的量测系统误差。其对应的量测系统可以表示为：

${\tilde{a}}^B=I_{3\×3}{a}^B+b_a+{\mathrm{\η}}_a---\left(33\right)$

其中

加速度计量测值,a^B加速度真实值,b_a未知的加速计系统误差包含偏差随机游走等,η_a加速度计量测白噪声

$\tilde{R}=R+b_R+v_R---\left(34\right)$

$R=\sqrt{{(r-r_i)}^T(r-r_i)}---\left(35\right)$

其中

是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐标系下的位置，r_i是通信卫星在火星中心惯性坐标系下的位置.v_R是量测噪声，b_R是未知的量测系统误差.

本发明一种火星大气进入段的非线性三步滤波方法，见图2所示，其步骤如下:

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

$x_{k+1}=f(x_k,u_k)+F_k^x{f}_k+w_k^x---\left(36\right)$

其中x为火星大气进入段动力学系统中的状态量具体包含式（32）中的左边的各分量，飞行器到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ,航向角ψ。f_k主要为动力学系统中的加速度项偏差，

为对应的动力学系统加速度偏差对动力学系统驱动阵。u_k是控制量为对应的滚转角σ。

对应的离散量测方程为

$z_k=h\left(x_k\right)+E_k^z{d}_k+v_k---\left(37\right)$

其中

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

式中，

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

步骤二、给定初始值：

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

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

其中真实量为提前规划的火星大气进入点。实际上也存在一定的不确定性。初始状态估计均方误差 ${\hat{P}}_0^x={\stackrel{-}{P}}_0^x={10}^6\×\left[\begin{array}{cccccc}10& & & & & \\ & 0.1& & & & \\ & & {10}^{-10}& & & \\ & & & {10}^{-10}& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right],$ 动力学偏差和量测系统误差的相关系数 $V_0^{23}=0.$

步骤三、对状态量x_k进行滤波

按照公式（4）-（10）进行滤波，对火星大气进入段的状态进行估计。其中动力学系统噪声方差阵为 $Q=\left[\begin{array}{cccccc}10& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 0.1& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right],$ 量测噪声方差阵为 $R_k=\left[\begin{array}{cccccc}{10}^{-10}& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 10& & \\ & & & & 10& \\ & & & & & 10\end{array}\right].$

步骤四：对动力学系统偏差f_k进行滤波

按照式（11）-式（15）对动力学系统偏差进行滤估计，其中根据实际动力学系统分析选取相应矩阵 $F_{k-1}^x={\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\end{array}\right]}^T,$ $F_k^y={\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\end{array}\right]}^T.$

步骤五：对测量系统中的未知测量系统误差d_k进行滤波

按照式（16）-式（21）对量测系统中的未知量测系统误差进行滤波估计。其中根据实际的量测系统选取相应的矩阵。当三个加速度计系统误差一样，三颗卫星到飞行器的无线电测量系统误差一样，则的到相应矩阵 $E_{k-1}^x={\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]}^T,$ $E_k^y={\left[\begin{array}{cccccc}1& 1& 1& 0& 0& 0\\ 0& 0& 0& 1& 1& 1\end{array}\right]}^T.$ 对于每个加速度计、无线电测量系统误差可以重新设置相应的矩阵。

步骤六、更新相关系数、校正状态估计和动力学偏差估计：情况如下

按照式（22）-式（24）对步骤三中的状态及其均误差进行校正，同时对步骤四中的动力学偏差及其对应的均方误差进行校正。

步骤七、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时刻T时，至超音速降落伞打开为止。至此完成火星大气进入段的非线性三步滤波方法。

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

通过火星大气进入段的非线性三步滤波方法得到各状态的估计值的误差见图1，图1采用了上诉方法和传统的扩展Kalman滤波方法来进行估计。火星大气进入段的非线性三步滤波方法不但能够成功消除量测系统中未知的系统误差，而且还可以消除动力学系统中的偏差，有效地提高导航的精度。

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

Claims (2)

1.一种火星大气进入段的非线性三步滤波方法，其特征在于：它包括以下步骤：

步骤一、建立工程实际方程：离散时间下的动力学系统和量测系统

$x_{k+1}=f(x_k,u_k)+F_k^x{f}_k+E_k^x{d}_k+w_k^x---\left(1\right)$

$z_k=h\left(x_k\right)+F_k^z{f}_k+E_k^z{d}_k+v_k---\left(2\right)$

其中x_k表示系统状态量，z_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_k微；矩阵

具有恰当的维数；

和v_k分别是动力学系统噪声，它们是不相关的高斯白噪声满足以下式子：

$\left\{\begin{array}{c}E\left[w_k^x\right]=0\\ \mathrm{Cov}[w_k^x,w_j^x]=E\left[w_k^x{w}_j^{\mathrm{xT}}\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^x,v_j]=E\left[w_k^x{v}_j^T\right]=0\end{array}\right.---\left(3\right)$

步骤二、给定初始值：

和

为初始状态的估计值，为初始状态估计均方误差，为动力学偏差和量测系统误差的相关系数；

步骤三、对状态量x_k进行滤波

${\stackrel{-}{x}}_k(-)=f({\hat{x}}_{k-1},u_{k-1})---\left(4\right)$

${\stackrel{-}{P}}_k^x(-)={\mathrm{\Φ}}_{k-1}{\hat{P}}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}---\left(5\right)$

${\stackrel{-}{K}}_k^x={\stackrel{-}{P}}_k^x(-)S_k^{1T}{C}_k^{-1}---\left(6\right)$

${\stackrel{-}{P}}_k^x(+)=(I-{\stackrel{-}{K}}_k^x{S}_k^1){\stackrel{-}{P}}_k^x(-)---\left(7\right)$

${\stackrel{-}{\mathrm{\η}}}_k^x=z_k-h\left({\stackrel{-}{x}}_k(-)\right),$ ${\stackrel{-}{x}}_k(+)={\stackrel{-}{x}}_k(-)+{\stackrel{-}{K}}_k^x{\stackrel{-}{\mathrm{\η}}}_k^x---\left(8\right)$

$C_k=S_k^1{\stackrel{-}{P}}_k^x(-)S_k^{1T}+R_k---\left(9\right)$

其中

${\mathrm{\Φ}}_k=\frac{\∂f\left(x\right)}{\∂x}{|}_{x={\hat{x}}_k(+)}$ $S_k^1=H_k=\frac{\∂h\left(x\right)}{\∂x}{|}_{x={\stackrel{-}{x}}_k(-)}.---\left(10\right)$

式中：

为t_k-1时刻的状态量，u_k-1为t_k-1时刻的控制输入量；

为状态的一步预测；为t_k-1时刻的状态估计均方误差，Φ_k-1为t_k-1时刻到t_k时刻的一步转移矩阵；Q_k-1为系统的噪声的方差阵，为一步预测均方误差；

为量测阵，

为状态增益，C_k为量测新息误差阵；I为单位阵，

为t_k时刻的状态估计均方误差；为量测新息，

为状态估计；

步骤四、对动力学系统偏差f_k进行滤波

$U_k^{12}=F_{k-1}^x---\left(11\right)$

$S_k^2=H_k{U}_k^{12}+F_k^y---\left(12\right)$

${\stackrel{-}{P}}_k^f(+)={\left(S_k^{2T}{C}_k^{-1}{S}_k^2\right)}^{+}---\left(13\right)$

${\stackrel{-}{K}}_k^f={\stackrel{-}{P}}_k^f(+)S_k^{2T}{C}_k^{-1}---\left(14\right)$

${\stackrel{-}{f}}_k(+)={\stackrel{-}{K}}_k^f{\stackrel{-}{\mathrm{\η}}}_k^x---\left(15\right)$

为t_k-1时刻动力学系统偏差对动力学系统驱动阵，

为t_k时刻动力学系统偏差对量测系统量测阵，为t_k时刻动力学系统偏差对量测系统校正量测阵，

为t_k时刻动力学系统偏差估计均方误差，为广义逆矩阵，

为动力学系统偏差滤波增益；

为t_k时刻动力学系统偏差状态估计；

步骤五、对测量系统中的未知测量系统误差d_k进行滤波

$U_k^{23}=V_{k-1}^{23}---\left(16\right)$

$U_k^{13}=E_{k-1}^x+F_{k-1}^x{V}_{k-1}^{23}---\left(17\right)$

$S_k^3=H_k{U}_k^{13}+F_k^y{U}_k^{23}+E_k^y---\left(18\right)$

${\stackrel{-}{P}}_k^a(+)={\left(S_k^{3T}{C}_k^{-1}{S}_k^3\right)}^{+}---\left(19\right)$

${\stackrel{-}{K}}_k^d={\stackrel{-}{P}}_k^d(+)S_k^{3T}{C}_k^{-1}---\left(20\right)$

${\stackrel{-}{d}}_k(+)={\stackrel{-}{K}}_k^d{\stackrel{-}{\mathrm{\η}}}_k^x---\left(21\right)$

为t_k-1时刻量测系统误差对动力学系统驱动阵，

为t_k时刻量测系统误差对量测系统量测阵，

为t_k时刻量测系统误差对量测系统校正量测阵，

为t_k时刻量测系统误差估计均方误差，

为

广义逆矩阵，

为量测系统误差滤波增益；

为t_k时刻动力学系统偏差状态估计；

为t_k-1时刻动力学偏差和量测系统误差的相关系数；

步骤六、更新相关系数、校正状态估计和动力学偏差估计：情况如下

$V_k^{12}=U_k^{12}-{\stackrel{-}{K}}_k^x{S}_k^2,$ $V_k^{13}=U_k^{13}-V_k^{12}{\stackrel{-}{K}}_k^f{S}_k^3-{\stackrel{-}{K}}_k^x{S}_k^3$ $V_k^{23}=V_{k-1}^{23}-{\stackrel{-}{K}}_k^f{S}_k^3---\left(22\right)$

${\hat{x}}_k(+)={\stackrel{-}{x}}_k(+)+V_k^{12}{\stackrel{-}{f}}_k(+)+V_k^{13}{\stackrel{-}{d}}_k(+),$ ${\hat{P}}_k^x(+)={\stackrel{-}{P}}_k^x(+)+V_k^{12}{\stackrel{-}{P}}_k^f(+)V_k^{12T}+V_k^{13}{\stackrel{-}{P}}_k^d(+)V_k^{13T}---\left(23\right)$

${\hat{f}}_k(+)={\stackrel{-}{f}}_k(+)+V_k^{23}{\stackrel{-}{d}}_k(+),$ ${\hat{P}}_k^f(+)={\stackrel{-}{P}}_k^f(+)+V_k^{23}{\stackrel{-}{P}}_k^d(+)V_k^{23T}---\left(24\right)$

为t_k时刻校正后的状态量，

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

为t_k时刻校正后的状态量，

为t_k时刻校正后的状态估计均方误差；

步骤七、令k=k+1，返回步骤三往下进行，直到k等于火星大气进入时间截止对应的时刻T时，即降落伞打开为止；至此完成火星大气进入段的非线性三步滤波方法；

当离散条件下的扩维动力学系统和量测系统表达形式为

$x_{k+1}={\mathrm{\Φ}}_k{x}_k+B_k{u}_k+F_k^x{f}_k+E_k^x{d}_k+w_k^x---\left(25\right)$

$z_k=H_k{x}_k+F_k^z{f}_k+E_k^z{d}_k+v_k---\left(26\right)$

其扩维Kalman滤波表达形式为

${\hat{x}}_k^a(-)={\mathrm{\Φ}}_{k-1}^a{\hat{x}}_{k-1}^a(+)+B_{k-1}^a{u}_{k-1}---\left(27\right)$

$P_k^a(-)={\mathrm{\Φ}}_{k-1}^a{P}_{k-1}^a(+){\mathrm{\Φ}}_{k-1}^{\mathrm{aT}}+Q_{k-1}^a---\left(28\right)$

$K_k^a=P_k^a(-)H_k^{\mathrm{aT}}{(H_k^a{P}_k^a(-)H_k^{\mathrm{aT}}+R_k)}^{-1}---\left(29\right)$

${\hat{x}}_k^a(+)={\hat{x}}_k^a(-)+K_k^a{\tilde{z}}_k={\hat{x}}_{k/k-1}^a+K_k^a(z_k-H_k^a{\hat{x}}_k^a(-))---\left(30\right)$

$P_k^a(+)=(I-K_k^a{H}_k^a)P_k^a(-)---\left(31\right)$

其中

$x_k^a(\·)=\left[\begin{array}{c}{x}_k(\·)\\ f_k(\·)\\ d_k(\·)\end{array}\right],$ $P_k^a(\·)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{ccc}{P}_k^x(\·)& P_k^{\mathrm{xf}}(\·)& P_k^{\mathrm{xd}}(\·)\\ {\left(P_k^{\mathrm{xf}}(\·)\right)}^T& P_k^f(\·)& P_k^{\mathrm{fd}}(\·)\\ {\left(P_k^{\mathrm{xd}}(\·)\right)}^T& {\left(P_k^{\mathrm{fd}}(\·)\right)}^T& P_k^d(\·)\end{array}\right],$ $K_k^a=\left[\begin{array}{c}{K}_k^x\\ K_k^f\\ K_k^d\end{array}\right],$ ${\mathrm{\Φ}}_k^a=\left[\begin{array}{ccc}{\mathrm{\Φ}}_k& F_k^x& E_k^x\\ & A_k^f& \\ & & A_k^d\end{array}\right],$

$H_k^a=\left[\begin{array}{ccc}{H}_k& F_k^z& E_k^z\end{array}\right]$ 和 $Q_k^a=\left[\begin{array}{ccc}{Q}_k^x& 0& 0\\ 0& Q_k^f& 0\\ 0& 0& Q_k^d\end{array}\right]$

将一步预测均方误差和估计均方误差进行非线性三步U-V变换，其表达形式为

$P_{k/k-1}^a=U_k{\stackrel{-}{P}}_{k/k-1}^a{U}_k^T$

$P_{k/k}^a=V_k{\stackrel{-}{P}}_k^a{V}_k^T$

其中矩阵U_k和V_k定义为如下形式：

$U_k=\left[\begin{array}{ccc}I& U_k^{12}& U_k^{13}\\ 0& I& U_k^{23}\\ 0& 0& I\end{array}\right]$

$V_k=\left[\begin{array}{ccc}I& V_k^{12}& V_k^{13}\\ 0& I& V_k^{23}\\ 0& 0& I\end{array}\right]$ 。

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

a、分析动力学不确定性之间的相互关系，并进行相应的数值计算分析；

b、得到这些不确定性因素对动力学模型的影响主要是通过速度的一阶微分方程中的加速度的影响进入动力学模型从而引起误差的传播；

c、将动力学系统x_k+1=f(x_k,u_k)改写为考虑动力学系统偏差和量测系统误差的动力学系统将量测系统z_k=h(x_k)改写为考虑动力学系统偏差和量测系统误差的量测系统 $z_k=h\left(x_k\right)+F_k^z{f}_k+E_k^z{d}_k+v_k.$

Images