火星进入段IMU和甚高频无线电组合导航的UD-SKF方法

1.一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其特征在于：它包括以下四个步骤：
步骤一、建立火星进入段的动力学方程
在火星进入段，将探测器看作是一个无动力的质点，假设进入段的大气相对火星是静止的，同时忽略火星的自转，建立火星进入段的三自由度动力学方程：
式中，r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离即高度、探测器的速度、经度、纬度、飞行路径角和航向角，火星重力加速度g(r)＝μ/r2，μ是火星重力加速度常数，D和L分别为阻力加速度和升力加速度：
式中CD和CL分别为阻力系数和升力系数，S为探测器参考面积，m为探测器质量，为动压，ρ是火星大气密度，此处采用指数大气密度模型：
式中ρ0是标称密度，r0为火星径向基准位置，hs为大气定标高度；
T
取状态向量为X＝[r θ λ v γ ψ]，考虑到系统噪声向量w(t)，则动力学程(1)简写为
式中，f(X(t),t)为系统非线性连续状态转移函数，w(t)为零均值的高斯白噪声；
步骤二、建立火星进入段的量测方程
以基于星间甚高频无线电测量通信的自主导航方案为基础，也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息，建立火星进入段量测方程：
Z＝h(X(t),t)+b+v(t) (6)
式中，
式中，av为速度系下的加速度， 表示双向测距信息，维数m由有效的火星在轨信标和火星表面固定信标数目决定，b为常值偏差向量，v(t)为零均值的高斯白噪声，ηa为IMU测量信息的测量噪声向量，ξR为双向测距的测量噪声向量；
其中，IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度，测量模型为式中，表示各个轴的加速度测量值，a表示真实的加速度，ba加速度计常值漂移，测量噪声ηa为零均值的高斯白噪声；
其中，双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离，表示为:
式中，rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标或火星表面固定信标，bR为双向测距的常值偏差，测距噪声ξR为零均值的高斯白噪声；
步骤三、对上述动力学方程(5)和量测方程(6)进行离散化
Xk+1＝F(Xk)+wk (11)
式中，k＝1,2,3,…，F(Xk)为f(X(t),t)离散后的非线性状态转移函数， 为h(X(t),t)离散后的非线性量测函数，wk和vk互不相关，且其方差矩阵分别为Qk和Rk；
然后将方程(11)和(12)进行线性化，也即将式(11)中的非线性离散函数F(Xk)围绕估计值 展开成泰勒级数，并略去二阶以上项，得线性化之后的动力学方程Xk+1＝Φk+1/kXk+Uk+wk (13)
式中，
然后，再将式(12)中的非线性离散函数 围绕估计值 和 展开成泰勒级
数，并略去二阶以上项，得线性化之后的增量量测方程：
Zk＝HkXk+Yk+vk (16)
式中，
通过上述过程，就得到了线性化后的动力学方程和量测方程，式中Uk和Yk为非随机的外作用项；
步骤四、采用UD-SKF滤波算法，并输出导航信息
考虑式(6)中的常值偏差向量b未能精确建模，故施密特-卡尔曼滤波算法即SKF考虑将其方差融入到算法中，也即是考虑了偏差与状态的互协方差，但是不估计这些偏差，同时，在计算增益矩阵时采用UD分解以确保滤波的数值稳定性；其中步骤四所采用的UD-SKF滤波算法实现步骤为：
步骤4.1.将常值偏差向量b扩维进入状态向量，则动力学方程(13)和量测方程(16)变为
式中，我们考虑的常值偏差向量满足条件：bk＝bk-1。其方差矩阵B0为
B0＝Cov{b0}＝Cov{bk} (21)
和偏差与状态的互协方差矩阵Ck为
并且初始值为C0＝0，式中， 为卡尔曼滤波第k步的状态估计；
同时，相应与动力学方程(19)和量测方程(20)第k步的误差方差阵 为
式中， 为Ck转置矩阵，Pk为状态Xk的误差方差阵
然后，初始化状态向量X0及误差方差阵P0；
步骤4.2.由第k步的状态估计可得，第k+1步的状态一步预测 为
步骤4.3.第k+1步的一步预测误差方差矩阵 为
则得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k
Ck+1|k＝Φk+1|kCk (28)
步骤4.4.第k+1步的滤波增益矩阵 为
其中，由于不需要估计偏差，故强制令偏差项的增益矩阵为零；
则状态 的增益矩阵Kk+1为
由于式(31)中的Ωk+1在滤波过程中无法保持非负定性，此时式(30)中的增益矩阵Kk+1在求逆运算中会产生较大的误差，甚至会发散；为了保持滤波的数值稳定性，采用UD分解来计算增益矩阵Kk+1，会起到很好的效果；UD分解的详细步骤如下：
1)将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式：
Ωk＝UDUT (32)
2)将式(32)代入是式(30)中，得到
3)令 则有
4) 通过以下的迭代过程得到
DX2＝X1(36)
T
UX＝X2
5)最后令 然后即得到Kk；
步骤4.5.第k+1步的状态估计 为
步骤4.6.第k+1步的估计误差方差矩阵 为
则状态 的估计误差方差矩阵Pk+1为
和偏差与状态的互协方差矩阵为
Ck+1＝Ck+|k-Kk+1(Hk+1Ck+1|k+B0) (40)
通过步骤4.1至步骤4.6循环进行即得到探测器的实时状态估计值，包含探测器的高度、速度、经度、纬度、飞行路径角和航向角；
通过上述四个步骤建立IMU和信标无线电组合导航的量测方程，然后利用UD-SKF滤波算法消除测量信息中的误差的影响，并增强滤波的稳定性，达到高效实时估计探测器导航状态的目的。
2.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其特征在于：在步骤一中所述的“建立火星进入段的动力学方程”，是指令得到式(5)。
3.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其特征在于：在步骤二中所述的“建立火星进入段的量测方程”，是指令式(7)中的第一式为
得到式(6)。
4.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其特征在于：在步骤三中所述的“对动力学方程(5)和量测方程(6)进行离散化”，所采用的方法为泰勒级数展开法；泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式：
(n)
式中，n！表示n的阶乘,而f (a)表示函数f(x)在点x＝a处的n阶导数，实际应用中，泰勒级数需要截断，只取有限项，因此会产生相应的截断误差。

火星进入段IMU和甚高频无线电组合导航的UD-SKF方法\n技术领域\n[0001] 本发明涉及一种火星进入段组合导航的自主导航方法，具体涉及以一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,属于航天自主导航技术领域.\n背景技术\n[0002] 2012年8月，美国的火星探测器“好奇号”成功登陆火星，着陆点距离预定目标点\n2385米。但是，未来的火星探测导航任务要求探测器要达到定点着陆的要求，也即是着陆精度在1000米之内。在着陆之前，火星探测器会经历火星进入段、下降段和着陆段（简称EDL），其中火星进入段的导航对探测器的最终着陆精度影响最为显著。这主要是由于火星进入段气动加热环境恶劣，且探测器包裹在整流罩内，导致很多导航敏感器无法正常工作，只能主要依靠惯性测量单元（简称IMU）提供导航信息；其次由于火星和地球之间的通信大延迟，无法利用深空网实时提供较为准确的导航信息。因此，如何规划火星进入段的自主导航方案和自主导航滤波算法成为火星探测器能否定点着陆的首要问题。\n[0003] 目前，大量学者提出了比较新颖的火星进入段自主导航方案，较著名的是Levesque等提出的基于星间甚高频无线电测量通信的自主导航方案（参见Levesque,J F.Advanced navigation and guidance for high-precision planetary landing on Mars.Ph.D.Thesis,Department of Electrical Engineering,Universite de Sherbrooke,2006.）。此导航方案解决了导航信息不足导致的不可观性的问题，但是初始误差、大气模型的不确定性、探测器参数和气动特性的不确定性、信标的位置偏差和信标与探测器之间的通信偏差等导致此自主导航方案也存在导航精度不一定满足未来火星探测任务的要求。\n[0004] 实时高效的自主导航滤波算法也是解决这类问题的有效途径。目前火星进入段导航方式采用的是对IMU的输出进行积分，递推得到相应的探测器状态，但是此种方法收惯性单元漂移和偏差的影响较大，随着时间推移导航误差积累较大。由于火星进入段动力学具有很强的非线性，因此对探测器进行实时估计的算法必须是非线性滤波算法。工程上应用最多的是扩展卡尔曼滤波（简称EKF），此方法易于工程实现，但是由于线性化产生的截断误差会导致滤波精度和稳定性降低（参见Lightsey,E G,Mogensen,A E,Burkhart,P D,et al.,Real-time navigation for mars missions using the mars network.Journal of Spacecraft and Rockets,2008.45(3):519-533.）。有学者提出了无迹卡尔曼滤波（简称UKF），并应用于火星进入段自主导航中，估计精度较EKF高，但是计算量较EKF大（参见Levesque,J F.Advanced navigation and guidance for high-precision planetary landing on Mars.Ph.D.Thesis,Department of Electrical Engineering,Universite de Sherbrooke,2006.）。但是这两类方法无法有效地处理上述问题的不确定性和偏差，即使采用将这些不确定性或偏差进行状态扩维，然后进行估计，也能取得较好地效果。但是这种处理方式无疑增加了计算量和计算复杂度，不利于对探测器的状态进行实时估计。\n[0005] 本发明在提出的火星进入段IMU和甚高频无线电组合导航的UD-SKF滤波将这些偏差的方差加入状态方差矩阵的计算中，但是不估计这些偏差。此自主导航滤波算法成功地考虑了这些偏差的影响，但是又避免估计它们，有效地提高了导航精度，同时也大量的减少了计算量和计算复杂度，有利于火星进入段对探测器的进入状态进行实时高效地估计。\n发明内容\n[0006] 本发明的目的是提供一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法。针对现有的非线性自主导航滤波算法无法消除这些偏差带来较大误差，甚至导致发散的现象，或者进行状态扩维后，导致计算量和计算复杂度急剧增加的不利情况，本发明所提出的基于UD-SKF滤波算法成功地考虑了这些偏差的影响，但是又避免估计它们，有效地提高了导航精度，同时也大量的减少了计算量和计算复杂度，有利于火星进入段对探测器的进入状态进行实时高效地估计。\n[0007] 本发明涉及一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，它包括以下四个步骤（具体流程图参见图1）：\n[0008] 步骤一、建立火星进入段的动力学方程\n[0009] 在火星进入段，将探测器看作是一个无动力的质点。假设进入段的大气相对火星是静止的，同时忽略火星的自转，建立火星进入段的三自由度动力学方程：\n[0010] \n[0011] \n[0012] \n[0013] \n[0014] \n[0015] \n[0016] 式中，r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离（高度）、探测器\n2\n的速度、经度、纬度、飞行路径角和航向角，火星重力加速度g(r)＝μ/r，μ是火星重力加速度常数，D和L分别为阻力加速度和升力加速度：\n[0017] \n[0018] \n[0019] 式中CD和CL分别为阻力系数和升力系数，S为探测器参考面积，m为探测器质量，为动压，ρ是火星大气密度，此处采用指数大气密度模型：\n[0020] \n[0021] 式中ρ0是标称密度，r0为火星径向基准位置，hs为大气定标高度。\n[0022] 取状态向量为X=[r θ λ v γ ψ]T，考虑到系统噪声向量w(t)，则动力学程（1）可简写为\n[0023] \n[0024] 式中，f(X(t),t)为系统非线性连续状态转移函数，w(t)为零均值的高斯白噪声。\n[0025] 步骤二、建立火星进入段的量测方程\n[0026] 本发明以基于星间甚高频无线电测量通信的自主导航方案为基础，也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息，建立火星进入段量测方程：\n[0027] Z=h(X(t),t)+b+v(t) (6)\n[0028] 式中，\n[0029] \n[0030] av=[-D-D·L/D·sinφ D·L/D·cosφ]T (7)\n[0031] \n[0032] v(t)=[ηa ξR]T\n[0033] 式中，av为速度系下的加速度， 表示双向测距信息，维数m由有效的火星在轨信标和火星表面固定信标数目决定，b为常值偏差向量，v(t)为零均值的高斯白噪声，ηa为IMU测量信息的测量噪声向量，ξR为双向测距的测量噪声向量。\n[0034] 其中，IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度，测量模型为\n[0035] \n[0036] 式中，表示各个轴的加速度测量值，a表示真实的加速度，ba加速度计常值漂移，测量噪声ηa为零均值的高斯白噪声。\n[0037] 其中，双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离，无线电通信双向测距示意图参见图2，可表示为:\n[0038] \n[0039] \n[0040] 式中，rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标(k=o)或火星表面固定信标(k=b)，bR为双向测距的常值偏差，测距噪声ξR为零均值的高斯白噪声。\n[0041] 步骤三、对上述动力学方程（5）和量测方程（6）进行离散化\n[0042] Xk+1=F(Xk)+wk （11）\n[0043] \n[0044] 式中，k=1,2,3,…，F(Xk)为f(X(t),t)离散后的非线性状态转移函数， 为h(X(t),t)离散后的非线性量测函数，wk和vk互不相关，且其方差矩阵分别为Qk和Rk。\n[0045] 然后将方程（11）和（12）进行线性化，也即将式（11）中的非线性离散函数F(Xk)围绕估计值 展开成泰勒级数，并略去二阶以上项，得线性化之后的动力学方程[0046] Xk+1=Φk+1/kXk+Uk+wk （13）\n[0047] 式中，\n[0048] \n[0049] \n[0050] 然后，再将式（12）中的非线性离散函数 围绕估计值 和 展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程\n[0051] Zk=HkXk+Yk+vk （16）\n[0052] 式中，\n[0053] \n[0054] \n[0055] 通过上述过程，就得到了线性化后的动力学方程和量测方程，式中Uk和Yk为非随机的外作用项。\n[0056] 步骤四、采用UD-SKF滤波算法，并输出导航信息。\n[0057] 考虑式（6）中的常值偏差向量b未能精确建模，故施密特-卡尔曼滤波算法（Schmidt Kalman filter,简称SKF）考虑将其方差融入到算法中，也即是考虑了偏差与状态的互协方差，但是不估计这些偏差。同时，在计算增益矩阵时采用UD分解以确保滤波的数值稳定性。\n[0058] 其中步骤四所采用的UD-SKF滤波算法实现步骤为：\n[0059] 1.将常值偏差向量b扩维进入状态向量，则动力学方程（13）和量测方程（16）变为\n[0060] \n[0061] \n[0062] 式中，我们考虑的常值偏差向量满足条件：bk=bk-1。其方差矩阵B0为[0063] B0=Cov{b0}=Cov{bk} (21)\n[0064] 和偏差与状态的互协方差矩阵Ck为\n[0065] \n[0066] 并且初始值为C0=0。式中， 为卡尔曼滤波第k步的状态估计。\n[0067] 同时，相应与动力学方程（19）和量测方程（20）第k步的误差方差阵 为[0068] \n[0069] 式中， 为Ck转置矩阵，Pk为状态Xk的误差方差阵\n[0070] \n[0071] 然后，初始化状态向量X0及误差方差阵P0。\n[0072] 2.由第k步的状态估计可得，第k+1步的状态一步预测 为\n[0073] \n[0074] 3.第k+1步的一步预测误差方差矩阵 为\n[0075] \n[0076] 则可得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k\n[0077] \n[0078] Ck+1|k=Φk+1|kCk (28)\n[0079] 4.第k+1步的滤波增益矩阵 为\n[0080] \n[0081] 其中，由于不需要估计偏差，故强制令偏差项的增益矩阵为零。\n[0082] 则状态 的增益矩阵Kk+1为\n[0083] \n[0084] \n[0085] 由于式（31）中的Ωk+1在滤波过程中无法保持非负定性，此时式（30）中的增益矩阵Kk+1在求逆运算中会产生较大的误差，甚至会发散。为了保持滤波的数值稳定性，本发明采用UD分解来计算增益矩阵Kk+1，会起到很好的效果。UD分解的详细步骤如下：\n[0086] 1）将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式：\n[0087] Ωk=UDUT (32)\n[0088] 2）将式（32）代入是式（30）中，得到\n[0089] \n[0090] \n[0091] 3）令 则有\n[0092] \n[0093] 4） 可以通过以下的迭代过程得到\n[0094] \n[0095] DX2=X1 (36)\nT\n[0096] UX=X2\n[0097] 5）最后令 然后即可得到Kk。\n[0098] 5.第k+1步的状态估计 为\n[0099] \n[0100] 6.第k+1步的估计误差方差矩阵 为\n[0101] \n[0102] \n[0103] 则状态 的估计误差方差矩阵Pk+1为\n[0104] \n[0105] 和偏差与状态的互协方差矩阵为\n[0106] Ck+1=Ck+1|k-Kk+1(Hk+1Ck+1|k+B0) (40)\n[0107] 通过以上6步循环进行即可得到探测器的实时状态估计值，包含探测器的高度、速度、经度、纬度、飞行路径角和航向角。本发明的UD‐SKF滤波算法的原理图参见图3。\n[0108] 本发明共由步骤一、步骤二、步骤三和步骤四四个步骤组成，通过建立IMU和信标无线电组合导航的量测方程，然后利用UD‐SKF滤波算法消除测量信息中的误差的影响，并增强滤波的稳定性，达到高效实时估计探测器导航状态的目的。\n[0109] 本发明的原理是：由于火星进入段气动加热环境恶劣，且探测器包裹在整流罩内，导致很多导航敏感器无法正常工作，只能主要依靠惯性测量单元（简称IMU）提供导航信息，而仅依靠IMU提供的导航信息无法满足完探测器任务精确导航的要求。由于在火星进入段甚高频无线电可以进行通信，因此利用探测器与预先投放的火星在轨信标或火星表面固定信标之间的甚高频无线电双向通信测出探测器与信标之间的距离作为新的测量信息加入量测方程。经可观性理论分析和仿真验证表明，与三个信标进行双向通信测距即可满足完全可观的，且状态估计更加精确。但是由于IMU提供的测量信息带有常值偏差和双向测距存在通信偏差，这些偏差将给导航状态估计带来较大的偏差，甚至可能导致发散。因此，采用UD‐SKF滤波算法将这些偏差的方差融合到对状态的估计中而不去估计这些偏差，同时使用UD分解将来避免增益矩阵可能产生的较多误差，增强滤波过程的稳定性，从而能对探测器进行实时高效地估计导航状态。\n[0110] 其中，在步骤一中所述的“建立火星进入段的动力学方程”，是指令[0111] \n[0112] 得到式（5）。\n[0113] 其中，在步骤二中所述的“建立火星进入段的量测方程”，是指令式（7）中的第一式为\n[0114] \n[0115] 得到式（6）。\n[0116] 其中，在步骤三中所述的“对动力学方程（5）和量测方程（6）进行离散化”，所采用的方法为泰勒级数展开法。泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式：\n[0117] \n[0118] 式中，n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数。实际应用中，泰勒级数需要截断，只取有限项，因此会产生相应的截断误差。\n[0119] 本发明的优点是：本发明所采用的UD‐SKF滤波算法与传统的EKF相比，增加了少量的计算量，不仅将量测方程的偏差信息融合到滤波过程中，极大地改善了滤波效果，提高了导航精度，而且利用UD分解，减少了滤波过程中出现较大误差甚至发散的情况，增强滤波过程的稳定性，从而能对探测器进行实时高效地估计导航状态。\n附图说明\n[0120] 图1为本发明的流程图。\n[0121] 图2为本发明中的测量信息——无线电通信双向测距示意图。\n[0122] 图3为本发明中的滤波方法——UD‐SKF滤波算法的原理图。\n[0123] 图4为本发明的UD‐SKF滤波算法与EKF导航误差对比图。\n[0124] 图中符号说明如下：\n[0125] 图2中“rorbiter”表示在轨信标与火星中心的距离向量，“rlander”表示探测器与火星中心的距离向量，“rbeacon”表示火星表面固定信标与火星中心的距离向量。\n[0126] 图4中“EKF Errors”表示EKF滤波算法的误差，“UD‐SKF Errors”表本发明的UD‐SKF滤波算法的误差，“t axis”表示时间轴，“Altitude Error”表示探测器的高度误差，“Velocity Error”表示探测器的速度误差，“FPA Error”表示探测器的飞行路径角误差，“Longitude Error”表示探测器的经度误差，“Latitude Error”表示探测器的纬度误差，“Azimuth Error”表示探测器的航向角误差。\n具体实施方式\n[0127] 下面结合附图和实施例对本发明作详细说明。\n[0128] 本发明涉及一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其计算流程图如图1所示和UD-SKF算法方法流程图如图2所示，它包括以下四个步骤：\n[0129] 步骤一、建立火星进入段的动力学方程\n[0130] 在火星进入段，将探测器看作是一个无动力的质点。假设进入段的大气相对火星是静止的，同时忽略火星的自转，建立火星进入段的三自由度动力学方程：\n[0131] \n[0132] \n[0133] \n[0134] \n[0135] \n[0136] \n[0137] 式中，r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离（高度）、探测器\n2\n的速度、经度、纬度、飞行路径角和航向角，火星重力加速度g(r)＝μ/r，μ是火星重力加\n9 3 2\n速度常数（μ＝42828.29×10m/s），D和L分别为阻力加速度和升力加速度：\n[0138] \n[0139] \n[0140] 式中CD和CL分别为阻力系数和升力系数，S为探测器参考面积，m为探测器质量，为动压，ρ是火星大气密度，此处采用指数大气密度模型：\n[0141] \n[0142] 式中ρ0是标称密度，r0为火星径向基准位置(r0＝7.5km)，hs为大气定标高度(hs＝7.5km)。\n[0143] 取状态向量为X=[r θ λ v γ ψ]T，考虑到系统噪声向量w(t)，则动力学方程（1）可简写为\n[0144] \n[0145] 式中，f(X(t),t)为系统非线性连续状态转移函数，w(t)为零均值的高斯白噪声。\n[0146] 其中，本发明的仿真验证中的探测器的参考值和估计值如下表所示：\n[0147] \n[0148] 其中，本发明的仿真验证中的弹道系数（B=CDS/m）、升阻比（L/D=CL/CD）和标称大气密度参考值和估计值分别如下表所示：\n[0149] \n[0150] 步骤二、建立火星进入段的量测方程\n[0151] 本发明以基于星间甚高频无线电测量通信的自主导航方案为基础，也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息，建立火星进入段量测方程：\n[0152] Z=h(X(t),t)+b+v(t) (6)\n[0153] 式中，\n[0154] \n[0155] av=[-D-D·L/D·sinφ D·L/D·cosφ]T (7)\n[0156] \nT\n[0157] v(t)=[ηa ξR]\n[0158] 式中，av为速度系下的加速度， 表示双向测距信息，维数m由有效的火星在轨信标和火星表面固定信标数目决定，b为常值偏差向量，v(t)为零均值的高斯白噪声，ηa为IMU测量信息的测量噪声向量，ξR为双向测距的测量噪声向量。\n[0159] 其中，IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度，测量模型为\n[0160] \n[0161] 式中，表示各个轴的加速度测量值，a表示真实的加速度，ba加速度计常值漂移，测量噪声ηa为零均值的高斯白噪声。\n[0162] 其中，双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离，无线电通信双向测距示意图参见图2，可表示为:\n[0163] \n[0164] \n[0165] 式中，rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标(k=o)或火星表面固定信标(k=b)，bR为双向测距的常值偏差，测距噪声ξR为零均值的高斯白噪声。\n[0166] 其中，本发明的仿真验证中选择一个火星在轨信标和两个火星表面固定信标，具体参数参见下表：\n[0167] \n[0168] \n[0169] 其中，本发明的仿真验证中对于测量中IMU的加速度常值偏差取为0.003m/s2和通信偏差针对在轨信标和固定信标分别取为24m和45m。\n[0170] 步骤三、对上述动力学方程（5）和量测方程（6）进行离散化\n[0171] Xk+1=F(Xk)+wk （11）\n[0172] \n[0173] 式中，k=1,2,3,…，F(Xk)为f(X(t),t)离散后的非线性状态转移函数， 为h(X(t),t)离散后的非线性量测函数，wk和vk互不相关，且其方差矩阵分别为Qk和Rk。\n[0174] 然后将方程（11）和（12）进行线性化，也即将式（11）中的非线性离散函数F(Xk)围绕估计值 展开成泰勒级数，并略去二阶以上项，得线性化之后的动力学方程[0175] Xk+1=Φk+1/kXk+Uk+wk （13）\n[0176] 式中，\n[0177] \n[0178] \n[0179] 然后，再将式（12）中的非线性离散函数 围绕估计值 和 展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程\n[0180] Zk=HkXk+Yk+vk （16）\n[0181] 式中，\n[0182] \n[0183] \n[0184] 通过上述过程，就得到了线性化后的动力学方程和量测方程，式中Uk和Yk为非随机的外作用项。\n[0185] 步骤四、采用UD-SKF滤波算法，并输出导航信息。\n[0186] 考虑式（6）中的常值偏差向量b未能精确建模，故施密特-卡尔曼滤波算法（Schmidt Kalman filter,简称SKF）考虑将其方差融入到算法中，也即是考虑了偏差与状态的互协方差，但是不估计这些偏差。同时，在计算增益矩阵时采用UD分解以确保滤波的数值稳定性。\n[0187] 其中步骤四所采用的UD-SKF滤波算法实现步骤为：\n[0188] 7.将常值偏差向量b扩维进入状态向量，则动力学方程（13）和量测方程（16）变为\n[0189] \n[0190] \n[0191] 式中，我们考虑的常值偏差向量满足条件：bk=bk-1。其方差矩阵B0为[0192] B0=Cov{b0}=Cov{bk} (21)\n[0193] 和偏差与状态的互协方差矩阵Ck为\n[0194] \n[0195] 并且初始值为C0=0。式中， 为卡尔曼滤波第k步的状态估计。\n[0196] 同时，相应与动力学方程（19）和量测方程（20）第k步的误差方差阵 为[0197] \n[0198] 式中， 为Ck转置矩阵，Pk为状态Xk的误差方差阵\n[0199] \n[0200] 然后，初始化状态向量X0及误差方差阵P0。\n[0201] 8.由第k步的状态估计可得，第k+1步的状态一步预测 为\n[0202] \n[0203] 9.第k+1步的一步预测误差方差矩阵 为\n[0204] \n[0205] 则可得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k\n[0206] \n[0207] Ck+1|k=Φk+1|kCk (28)\n[0208] 10.第k+1步的滤波增益矩阵 为\n[0209] \n[0210] 其中，由于不需要估计偏差，故强制令偏差项的增益矩阵为零。\n[0211] 则状态 的增益矩阵Kk+1为\n[0212] \n[0213] \n[0214] 由于式（31）中的Ωk+1在滤波过程中无法保持非负定性，此时式（30）中的增益矩阵Kk+1在求逆运算中会产生较大的误差，甚至会发散。为了保持滤波的数值稳定性，本发明采用UD分解来计算增益矩阵Kk+1，会起到很好的效果。UD分解的详细步骤如下：\n[0215] 1）将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式：\n[0216] Ωk=UDUT (32)\n[0217] 2）将式（32）代入是式（30）中，得到\n[0218] \n[0219] \n[0220] 3）令 则有\n[0221] \n[0222] 4） 可以通过以下的迭代过程得到\n[0223] \n[0224] DX2=X1 (36)\n[0225] UTX=X2\n[0226] 5）最后令 然后即可得到Kk。\n[0227] 11.第k+1步的状态估计 为\n[0228] \n[0229] 12.第k+1步的估计误差方差矩阵 为\n[0230] \n[0231] \n[0232] 则状态 的估计误差方差矩阵Pk+1为\n[0233] \n[0234] 和偏差与状态的互协方差矩阵为\n[0235] Ck+1=Ck+1|k-Kk+1(Hk+1Ck+1|k+B0) (40)\n[0236] 通过以上6步循环进行即可得到探测器的实时状态估计值，包含探测器的高度、速度、经度、纬度、飞行路径角和航向角。本发明的UD‐SKF滤波算法的原理图参见图3。\n[0237] 其中，在步骤一中所述的“建立火星进入段的动力学方程”，是指令[0238] \n[0239] 得到式（5）。\n[0240] 其中，在步骤二中所述的“建立火星进入段的量测方程”，是指令式（7）中的第一式为\n[0241] \n[0242] 得到式（6）。由于是具体实施中采用三个信标，故式（42）中只有三个信标的距离信息。\n[0243] 其中，在步骤三中所述的“对动力学方程（5）和量测方程（6）进行离散化”，所采用的方法为泰勒级数展开法。泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式：\n[0244] \n[0245] 式中，n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数。实际应用中，泰勒级数需要截断，只取有限项，因此会产生相应的截断误差。\n[0246] 从图4的UD‐SKF滤波算法与EKF导航误差对比图可以看出，由EKF滤波算法所估计的状态仅有航向角是收敛的，其余的状态都是发散的，而由UD‐SKF滤波算法所估计的状态除了速度的收敛速度较慢之外，其余的状态都很快地收敛到参考值。从二者的对比来看，明显UD‐SKF滤波算法要远远好于EKF滤波算法，且最终的滤波效果也好于EKF滤波算法（见下表）。\n[0247]