CN103411627B - 火星动力下降段非线性三步滤波方法 - Google Patents

Abstract

一种火星动力下降段非线性三步滤波方法，其步骤如下：一、建立离散型非线性三步方法的动力学系统和量测系统方程；二、给定初始值；三、状态量滤波；四、动力学系统偏差滤波；五、测量系统中的未知测量系统误差滤波；六、更新相关系数、校正状态估计和动力学偏差估计；七、返回步骤三往下进行，直到等于火星动力下降段时间截止对应的时刻T时，即火星着陆器着陆为止，至此完成火星动力下降段非线性三步滤波方法。本发明统筹考虑了火星实际动力下降过程中，非线性、非高斯随机系统在动力学系统偏差和测量系统中的未知测量系统误差条件下的探测器位置速度估计问题，有效保证探测器在火星动力下降段的位置速度估计。

Description

火星动力下降段非线性三步滤波方法

技术领域

本发明涉及火星动力下降段非线性三步滤波方法。属于航天导航技术领域。

背景技术

火星是距离地球较近的行星，它在很多方面与地球有相似之处，成为人类深空探测的首选目标星体之一。而到现在为止，人类共进行了39次火星探测，其中2/3以失败告终。显然，由于火星恶劣的环境和深空探测技术的不成熟，人类探测火星面临着严峻的挑战。火星探测中，最具意义和最具难度就是探测器的软着陆，而软着陆过程中的进入、下降和着陆过程又是整个探测任务的关键。已实施的15次火星着陆任务中，仅有7次着陆成功。从火星探测器脱离绕飞轨道开始，一般来说，需要经过进入、下降和着陆几个阶段最终到达火星表面，其中进入段和下降段的合理规划和设计是完成精确着陆的必要保障。而在降落的最后一个阶段，即从开启反作用推力器进行有效减速到探测器成功降落到期望着陆点这一过程，称为动力下降段，整个动力下降过程可以划分为三个阶段——动力接近段、常值速度段、常值加速度段。该阶段只持续仅仅几分钟，但却是决定着陆任务成败的最重要阶段之一，如果下降过程不能有效减速或避开障碍，整个探测任务将前功尽弃。因此对于火星动力下降段探测器的状态估计就显得尤为重要，特别是探测器的相对于着陆点的高度和速度。

在火星动力下降段，探测器的挡热板已经抛掉，此时，探测器上携带的除惯性导航系统的其他量测仪器也开始工作，但是在降落伞下降段和动力下降段探测器高度较高，测量精度较差。而且在火星动力下降段探测器还要经受火星大气的不确定性，探测器本体的气动特性的不确定性以及风的漂移等的考验。故要合理利用这些量测仪器对惯性导航系统进行修正。

而目前，应用于火星的工程实际中状态估计滤波方法主要有基于泰勒展开的扩展Kalman滤波估计方法和基于sigma点集（为正态分布采样策略）的无迹Kalman滤波方法。但是扩展Kalman滤波适用于动力学系统和量测系统精确可知的条件下。在火星动力下降过程中，火星重力加速度具有一定的不可估计的误差，因此不太适用于火星动力下降段，虽然这种方法还是目前火星探测实际应用中的方法。无迹Kalman滤波方法虽然可以很好地应用非线性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)$

$y_k=h\left(x_k\right)+F_k^y{f}_k+E_k^y{d}_k+w_k^y---\left(2\right)$

其中x_k表示系统状态量，y_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差。非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_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[w_k^y\right]=0\\ \mathrm{Cov}[w_k^y,w_j^y]=E\left[w_k^y{w}_j^{\mathrm{yT}}\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k^x,w_j^y]=E\left[w_k^x{w}_j^{\mathrm{yT}}\right]=0\end{array}\right.---\left(3\right)$

步骤二、给定初始值：和

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

步骤三、状态量滤波

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

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

$C_{k+1}=S_{k+1}^1{\stackrel{-}{P}}_{k+1}^x(-)S_{k+1}^{1T}+R_{k+1}---\left(6\right)$

${\stackrel{-}{K}}_{k+1}^x={\stackrel{-}{P}}_{k+1}^x(-)S_{k+1}^{1T}{C}_{k+1}^{-1}---\left(7\right)$

${\stackrel{-}{P}}_{k+1}^x(+)=(I-{\stackrel{-}{K}}_{k+1}^x{S}_{k+1}^1){\stackrel{-}{P}}_{k+1}^x(-)---\left(8\right)$

${\stackrel{-}{\mathrm{\η}}}_{k+1}^x=z_{k+1}-h\left({\stackrel{-}{x}}_{k+1}(-)\right),$ ${\stackrel{-}{x}}_{k+1}(+)={\stackrel{-}{x}}_{k+1}(-)+{\stackrel{-}{K}}_{k+1}^x{\stackrel{-}{\mathrm{\η}}}_{k+1}^x---\left(9\right)$

其中

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

式中：为状态的一步预测，为t_k时刻的状态量，u_k为t_k时刻的控制输入量。为t_k时刻的状态估计均方误差，Φ_k为t_k时刻到t_k+1时刻的一步转移矩阵；Q_k为系统的噪声的方差阵，为一步预测均方误差。为量测阵，C_k+1为量测新息误差阵，为状态增益。I为单位阵，为t_k+1时刻的状态估计均方误差。为量测新息，为状态估计。

步骤四、动力学系统偏差滤波

$U_{k+1}^{12}=F_k^x---\left(11\right)$

$S_{k+1}^2=H_{k+1}{U}_{k+1}^{12}+F_{k+1}^y---\left(12\right)$

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

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

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

为t_k时刻动力学系统偏差对动力学系统驱动阵，为转移矩阵。为t_k+1时刻动力学系统偏差对量测系统量测阵，为t_k+1时刻动力学系统偏差对量测系统校正量测阵。为t_k+1时刻动力学系统偏差估计均方误差，为广义逆矩阵。为动力学系统偏差滤波增益。为t_k+1时刻动力学系统偏差状态估计。

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

$U_{k+1}^{23}=V_k^{23}---\left(16\right)$

$U_{k+1}^{13}=E_k^x+F_k^x{V}_k^{23}---\left(17\right)$

$S_{k+1}^3=H_{k+1}{U}_{k+1}^{13}+F_{k+1}^y{U}_{k+1}^{23}+E_{k+1}^y---\left(18\right)$

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

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

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

为t_k时刻动力学偏差和量测系统误差的相关系数，为转移矩阵。为t_k时刻量测系统误差对动力学系统驱动阵，为变换矩阵。为t_k+1时刻量测系统误差对量测系统量测阵，为t_k+1时刻量测系统误差对量测系统校正量测阵。为t_k+1时刻量测系统误差估计均方误差，为广义逆矩阵。为量测系统误差滤波增益。为t_k+1时刻动力学系统偏差状态估计。

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

$\left\{\begin{array}{c}{V}_{k+1}^{12}=U_{k+1}^{12}-{\stackrel{-}{K}}_{k+1}^x{S}_{k+1}^2\\ V_{k+1}^{13}=U_{k+1}^{13}-V_{k+1}^{12}{\stackrel{-}{K}}_{k+1}^f{S}_{k+1}^3-{\stackrel{-}{K}}_{k+1}^x{S}_{k+1}^3\\ V_{k+1}^{23}=V_k^{23}-{\stackrel{-}{K}}_{k+1}^f{S}_{k+1}^3\end{array}\right.---\left(22\right)$

$\left\{\begin{array}{c}{\hat{x}}_{k+1}(+)={\stackrel{-}{x}}_{k+1}(+)+V_{k+1}^{12}{\stackrel{-}{f}}_{k+1}(+)+V_{k+1}^{13}{\stackrel{-}{d}}_{k+1}(+)\\ {\hat{P}}_{k+1}^x(+)={\stackrel{-}{P}}_{k+1}^x(+)+V_{k+1}^{12}{\stackrel{-}{P}}_{k+1}^f(+)V_{k+1}^{12T}+V_{k+1}^{13}{\stackrel{-}{P}}_{k+1}^d(+)V_{k+1}^{13T}\end{array}\right.---\left(23\right)$

$\left\{\begin{array}{c}{\hat{f}}_{k+1}(+)={\stackrel{-}{f}}_{k+1}(+)+V_{k+1}^{23}{\stackrel{-}{d}}_{k+1}(+)\\ {\hat{P}}_{k+1}^f(+)={\stackrel{-}{P}}_{k+1}^f(+)+V_{k+1}^{23}{\stackrel{-}{P}}_{k+1}^d(+)V_{k+1}^{23T}\end{array}\right.---\left(24\right)$

均为相关系数。为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差。为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差。

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

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

其中，在步骤二中所述的“给定初始值：和”，是根据实际情况估计初值，在火星动力下降段之前，由火星降落伞下降段飞行段末端得到状态估计值和估计均方误差。且动力学系统偏差和测量系统中的未知测量系统误差为不相关量。

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

3、优点和功效：

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

附图说明

图1为火星动力下降段示意图

图2为火星动力下降段非线性三步滤波方法的流程图

具体实施方式

见图2，本发明涉及火星动力下降段非线性三步滤波方法，具体实施步骤如下：

如图1所示，探测器在火星动力下降过程中，其依靠IMU的火星着陆动力学系统如下方程。

$\stackrel{\·}{r}=v$

$\stackrel{\·}{v}=T_m^l(\tilde{a}-b_a-{\mathrm{\ξ}}_a)+g$

$\stackrel{\·}{e}=K(\widetilde{\mathrm{\ω}}-b_{\mathrm{\ω}}-{\mathrm{\ξ}}_{\mathrm{\ω}})$

${\stackrel{\·}{b}}_a={\mathrm{\ξ}}_a---\left(25\right)$

${\stackrel{\·}{b}}_{\mathrm{\ω}}={\mathrm{\ξ}}_{\mathrm{\ω}}$

其中，r=[r₁,r₂,r₃]^T为相对火星着陆点坐标系下的三轴位移，v=[v₁,v₂,v₃]^T为火星探测器相对火星着陆点坐标系下的三轴速度，e=[φ,θ,ψ]^T为三轴Euler姿态角，φ,θ,ψ分别为滚转角、俯仰角、航向角。为加速度计测量值，为陀螺仪测量值，b_a为加速度计偏差，b_ω为陀螺仪偏差，g为火星重力加速度。ξ_a为加速度计测量白噪声，ξ_ω为陀螺仪测量白噪声。为测量仪器坐标系到火星着陆点坐标系的转换矩阵。K为姿态运动学矩阵。

对于未知的火星环境，其动力学系统存在误差，如火星重力加速度误差。即使事先对着陆地点的重力加速度进行估计也还存在一定的误差。特别是在一些地形地貌复杂的具有科学探测的着陆点附近，火星重力加速度将难以进行事先估计校正，故应该在动力学方程中加入如下一项

b_g=b_g+w^b （26）

其中，b_g为火星重力加速度偏差，w^b为火星重力加速度量测白噪声。

目前火星动力下降段，美国喷气推力实验室（JPL）开发了一种集高度计和速度计为一体的小体积、低能耗的微型传感器（MCAV），它采用激光测距测速来实现高度、速度信息的同时输出。其对应的测量系统方程可以如下式来表示：

$y=\left[\begin{array}{c}{r}_3\\ T_l^{m}v\end{array}\right]+\left[\begin{array}{c}{d}_r\\ d_v\end{array}\right]+w^y---\left(27\right)$

其中，r₃为高度方向，为火星着陆点坐标系到测量仪器坐标系的转换矩阵，v=[v₁,v₂,v₃]^T为火星探测器相对火星着陆点坐标系下的三轴速度，即为测量仪器测得速度数据，而d_r和d_v分别为高度测量和速度测量的系统误差，v为量测噪声。

步骤一：根据火星动力下降段的实际情况：火星动力下降段对应的离散动力学系统可以改写为如下形式：

$x_{k+1}=f\left(x_k\right)+F_k^x{b}_{\mathrm{gk}}+w_k^x---\left(28\right)$

其中x为火星动力下降段动力学系统中的状态量具体包含式（25）中的左边的各分量，即 $x={[r^T,v^T,e^T,b_a^T,b_w^T]}_{15\×1}^T.$ 矩阵 $F_k^x={\left[\begin{array}{ccc}{0}_{1\×5}& 1& 0_{1\×9}\end{array}\right]}^T.$

对应的离散量测方程为

$y_k=h\left(x_k\right)+E_k^y{d}_k+w_k^y---\left(29\right)$

其中，

$d=\left[\begin{array}{c}{d}_r\\ d_v\end{array}\right]---\left(30\right)$

$h\left(x\right)=\left[\begin{array}{c}{r}_3\\ T_l^{m}v\end{array}\right]---\left(31\right)$

$E_k^y=I_{4\×4}---\left(32\right)$

步骤二、给定初始值：

初始值为探测器在降落伞下降段末端状态的估计，这个可以根据实际火星动力下降段探测器状态估计得到。初始状态估计均方误差可以分别根据降落伞下降段末端估计得到，而动力学偏差和量测系统误差的相关系数可以根据实际的动力学系统和量测系统状态得到。

步骤三、状态量滤波：

按照公式（4）-（10）进行滤波，对火星大气进入段的状态进行估计。

步骤四：动力学系统偏差滤波：

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

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

按照式（16）-式（21）对量测系统中的未知量测系统误差进行滤波估计。其中根据实际的量测系统选取相应的矩阵

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

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

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

其中截止时间主要取决于探测器与火星表面的高度及速度，能否满足软着陆的条件要求。

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

Claims (1)

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)$

$y_k=h\left(x_k\right)+F_k^y{f}_k+E_k^y{d}_k+w_k^y---\left(2\right)$

其中x_k表示系统状态量，y_k是测量系统测量值,f_k是未知的动力学系统偏差,d_k是未知的量测系统误差；非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于x_k可微；矩阵具有恰当的维数；和分别是动力学系统噪声和量测系统噪声，它们是不相关的高斯白噪声，满足以下式子

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

步骤二、给定初始值：和

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

步骤三、状态量滤波

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

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

$C_{k+1}=S_{k+1}^1{\stackrel{\‾}{P}}_{k+1}^x(-)S_{k+1}^{1T}+R_{k+1}---\left(6\right)$

${\stackrel{\‾}{K}}_{k+1}^x={\stackrel{\‾}{P}}_{k+1}^x(-)S_{k+1}^{1T}{C}_{k+1}^{-1}---\left(7\right)$

${\stackrel{\‾}{P}}_{k+1}^x(+)=(I-{\stackrel{\‾}{K}}_{k+1}^x{S}_{k+1}^1){\stackrel{\‾}{P}}_{k+1}^x(-)---\left(8\right)$

${\stackrel{\‾}{\mathrm{\η}}}_{k+1}^x=z_{k+1}-h\left({\stackrel{\‾}{x}}_{k+1}\right(-\left)\right),{\stackrel{\‾}{x}}_{k+1}(+)={\stackrel{\‾}{x}}_{k+1}(-)+{\stackrel{\‾}{K}}_{k+1}^x{\stackrel{\‾}{\mathrm{\η}}}_{k+1}^x---\left(9\right)$

其中

$\begin{array}{cc}{\mathrm{\Φ}}_{k+1}=\frac{\∂f\left(x\right)}{\∂x}{|}_{x={\hat{x}}_{k+1}(+)}& S_{k+1}^1=H_{k+1}=\frac{\∂h\left(x\right)}{\∂x}{|}_{x={\stackrel{\‾}{x}}_{k+1}(-)}\end{array}.---\left(10\right)$

式中：为状态的一步预测，为t_k时刻的状态量，u_k为t_k时刻的控制输入量；为t_k时刻的状态估计均方误差，Φ_k为t_k时刻到t_k+1时刻的一步转移矩阵；Q_k为系统的噪声的方差阵，为一步预测均方误差；为量测阵，C_k+1为量测新息误差阵，为状态增益；I为单位阵，为t_k+1时刻的状态估计均方误差；为量测新息，为状态估计；

步骤四、动力学系统偏差滤波

$U_{k+1}^{12}=F_k^x---\left(11\right)$

$S_{k+1}^2=S_{k+1}^1{U}_{k+1}^{12}+F_{k+1}^y---\left(12\right)$

${\stackrel{\‾}{P}}_{k+1}^f(+)={\left(S_{k+1}^{2T}{C}_{k+1}^{-1}{S}_{k+1}^2\right)}^{+}---\left(13\right)$

${\stackrel{\‾}{K}}_{k+1}^f={\stackrel{\‾}{P}}_{k+1}^f(+)S_{k+1}^{2T}{C}_{k+1}^{-1}---\left(14\right)$

${\stackrel{\‾}{f}}_{k+1}(+)={\stackrel{\‾}{K}}_{k+1}^f{\stackrel{\‾}{\mathrm{\η}}}_{k+1}^x---\left(15\right)$

为t_k时刻动力学系统偏差对动力学系统驱动阵，为转移矩阵，为t_k+1时刻动力学系统偏差对量测系统量测阵，为t_k+1时刻动力学系统偏差对量测系统校正量测阵；为t_k+1时刻动力学系统偏差估计均方误差，为广义逆矩阵；为动力学系统偏差滤波增益，为t_k+1时刻动力学系统偏差状态估计；其中，根据实际动力学系统分析选取相应矩阵

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

$U_{k+1}^{23}=V_k^{23}---\left(16\right)$

$U_{k+1}^{13}=E_k^x+F_k^x{V}_k^{23}---\left(17\right)$

$S_{k+1}^3=S_{k+1}^1{U}_{k+1}^{13}+F_{k+1}^y{U}_{k+1}^{23}+E_{k+1}^y---\left(18\right)$

${\stackrel{\‾}{P}}_{k+1}^d(+)={\left(S_{k+1}^{3T}{C}_{k+1}^{-1}{S}_{k+1}^3\right)}^{+}---\left(19\right)$

${\stackrel{\‾}{K}}_{k+1}^d={\stackrel{\‾}{P}}_{k+1}^d(+)S_{k+1}^{3T}{C}_{k+1}^{-1}---\left(20\right)$

${\stackrel{\‾}{d}}_{k+1}(+)={\stackrel{\‾}{K}}_{k+1}^d{\stackrel{\‾}{\mathrm{\η}}}_{k+1}^x---\left(21\right)$

为t_k时刻动力学偏差和量测系统误差的相关系数，为转移矩阵；为t_k时刻量测系统误差对动力学系统驱动阵，为变换矩阵；为t_k+1时刻量测系统误差对量测系统量测阵，为t_k+1时刻量测系统误差对量测系统校正量测阵；为t_k+1时刻量测系统误差估计均方误差，为广义逆矩阵；为量测系统误差滤波增益；为t_k+1时刻动力学系统偏差状态估计；其中，根据实际的量测系统选取相应的矩阵

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

$\left\{\begin{array}{c}{V}_{k+1}^{12}=U_{k+1}^{12}-{\stackrel{\‾}{K}}_{k+1}^x{S}_{k+1}^2\\ V_{k+1}^{13}=U_{k+1}^{13}-V_{k+1}^{12}{\stackrel{\‾}{K}}_{k+1}^f{S}_{k+1}^3-{\stackrel{\‾}{K}}_{k+1}^x{S}_{k+1}^3\\ V_{k+1}^{23}=V_k^{23}-{\stackrel{\‾}{K}}_{k+1}^f{S}_{k+1}^3\end{array}\right.---\left(22\right)$

$\left\{\begin{array}{c}{\hat{x}}_{k+1}(+)={\stackrel{\‾}{x}}_{k+1}(+)+V_{k+1}^{12}{\stackrel{\‾}{f}}_{k+1}(+)+V_{k+1}^{13}{\stackrel{\‾}{d}}_{k+1}(+)\\ {\hat{P}}_{k+1}^x(+)={\stackrel{\‾}{P}}_{k+1}^x(+)+V_{k+1}^{12}{\stackrel{\‾}{P}}_{k+1}^f(+)V_{k+1}^{12T}+V_{k+1}^{13}{\stackrel{\‾}{P}}_{k+1}^d(+)V_{k+1}^{13T}\end{array}\right.---\left(23\right)$

$\left\{\begin{array}{c}{\hat{f}}_{k+1}(+)={\stackrel{\‾}{f}}_{k+1}(+)+V_{k+1}^{23}{\stackrel{\‾}{d}}_{k+1}(+)\\ {\hat{P}}_{k+1}^f(+)={\stackrel{\‾}{P}}_{k+1}^f(+)+V_{k+1}^{23}{\stackrel{\‾}{P}}_{k+1}^d(+)V_{k+1}^{23T}\end{array}\right.---\left(24\right)$

均为相关系数，为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差，为t_k+1时刻校正后的状态量，为t_k+1时刻校正后的状态估计均方误差；

步骤七、令k＝k+1，返回步骤三往下进行，直到k等于火星动力下降段时间截止对应的时刻T时，即火星着陆器着陆为止；至此完成火星动力下降段非线性三步滤波方法；

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

其中，在步骤二中所述的“给定初始值：和”，是根据实际情况估计初值，在火星动力下降段之前，由火星降落伞下降段飞行段末端得到状态估计值和估计均方误差，且动力学系统偏差和测量系统中的未知测量系统误差为不相关量；

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

其中，探测器在火星动力下降过程中，其依靠IMU的火星着陆动力学系统如下方程；

$\begin{array}{c}\stackrel{\·}{r}=v\\ \stackrel{\·}{v}=T_m^l(\tilde{a}-b_a-{\mathrm{\ξ}}_a)+g\\ \stackrel{\·}{e}=K(\widetilde{\mathrm{\ω}}-b_{\mathrm{\ω}}-{\mathrm{\ξ}}_{\mathrm{\ω}})\\ {\stackrel{\·}{b}}_a={\mathrm{\ξ}}_a\\ {\stackrel{\·}{b}}_{\mathrm{\ω}}={\mathrm{\ξ}}_{\mathrm{\ω}}\end{array}---\left(25\right)$

其中，r＝[r₁,r₂,r₃]^T为相对火星着陆点坐标系下的三轴位移，v＝[v₁,v₂,v₃]^T为火星探测器相对火星着陆点坐标系下的三轴速度，e＝[φ,θ,ψ]^T为三轴Euler姿态角，φ,θ,ψ分别为滚转角、俯仰角、航向角；为加速度计测量值，为陀螺仪测量值，b_a为加速度计偏差，b_ω为陀螺仪偏差，g为火星重力加速度；ξ_a为加速度计测量白噪声，ξ_ω为陀螺仪测量白噪声；为测量仪器坐标系到火星着陆点坐标系的转换矩阵；K为姿态运动学矩阵；

对于未知的火星环境，其动力学系统存在误差，即使事先对着陆地点的重力加速度进行估计也还存在误差；在一些地形地貌复杂的具有科学探测的着陆点附近，火星重力加速度将难以进行事先估计校正，故应该在动力学方程中加入如下一项

b_g＝b_g+w^b (26)

其中，b_g为火星重力加速度偏差，w^b为火星重力加速度量测白噪声；

目前火星动力下降段，美国喷气推力实验室JPL开发了一种集高度计和速度计为一体的小体积、低能耗的微型传感器MCAV，它采用激光测距测速来实现高度、速度信息的同时输出；其对应的测量系统方程如下式来表示：

$y=\left[\begin{array}{c}{r}_3\\ T_l^{m}v\end{array}\right]+\left[\begin{array}{c}{d}_r\\ d_v\end{array}\right]+w^y---\left(27\right)$

其中，r₃为高度方向，为火星着陆点坐标系到测量仪器坐标系的转换矩阵，v＝[v₁,v₂,v₃]^T为火星探测器相对火星着陆点坐标系下的三轴速度，即为测量仪器测得速度数据，而d_r和d_v分别为高度测量和速度测量的系统误差，v为量测噪声；

其中，根据火星动力下降段的实际情况：火星动力下降段对应的离散动力学系统改写为如下形式：

$x_{k+1}=f\left(x_k\right)+F_k^x{b}_{gk}+w_k^x---\left(28\right)$

其中x为火星动力下降段动力学系统中的状态量具体包含式(25)中的左边的各分量，即矩阵

对应的离散量测方程为

$y_k=h\left(x_k\right)+E_k^y{d}_k+w_k^y---\left(29\right)$

其中，

$d=\left[\begin{array}{c}{d}_r\\ d_v\end{array}\right]---\left(30\right)$

$h\left(x\right)=\left[\begin{array}{c}{r}_3\\ T_l^{m}v\end{array}\right]---\left(31\right)$

$E_k^y=I_{4\×4}.---\left(32\right)$