CN102116634B - 一种着陆深空天体探测器的降维自主导航方法 - Google Patents

Abstract

本发明属于深空探测器的制导、导航与控制技术领域，具体公开一种着陆深空天体探测器的降维自主导航方法，它包括以下步骤：确定当前时刻探测器相对惯性坐标系的姿态以及位置和速度初值；确定探测器相对天体中心的距离；确定探测器相对惯性坐标系的三维速度；构建导航系统的状态量、状态方程、观测量、观测方程和测量噪声方差阵；测量噪声方差阵的无量纲化处理和能观度的确定；采用分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵；采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离和速度。本发明的方法能够保证自主导航滤波的稳定性和提高关键导航参数的收敛速度和估计精度。

Description

一种着陆深空天体探测器的降维自主导航方法

技术领域

本发明属于深空探测器的制导、导航与控制技术领域，具体涉及一种着陆深空天体探测器的降维自主导航方法。

背景技术

由于深空天体与地球之间的远距离，基于地面深空网的导航很难满足深空着陆GNC系统对探测器状态实时性和精度的要求，因此，自主导航就成为安全和准确地着陆深空天体探测器的一项关键技术。深空着陆探测器自主导航一般采用的方法是：测距仪获取的高度和测速仪获取的本体系速度作为观测量，利用扩展卡尔曼滤波修正惯性导航的位置和速度误差。对于这种导航方法，尽管所有轨道参数都是能观的，但是由于一些轨道参数(除了径向的两维位置)的能观度较低，在导航模型误差和测量噪声特性不确定的影响下，这些能观度较低的轨道参数不但不收敛，还可能发散，影响自主导航算法的稳定性和关键导航参数的估计精度。

发明内容

本发明的目的在于提供一种着陆深空天体探测器的降维自主导航方法，该方法能够保证自主导航滤波的稳定性和提高关键导航参数的收敛速度和估计精度。

实现本发明的技术方案：一种着陆深空天体探测器的降维自主导航方法，它包括以下步骤：

(1)利用陀螺测量的姿态角速度以及加速度计测量的速度增量和轨道初值，确定当前时刻探测器相对惯性坐标系的姿态以及位置和速度初值：

所述的步骤(1)中确定惯性坐标系的探测器位置和速度的具体步骤为：采用探测器上的陀螺测量探测器姿态角速度根据前一时刻t₀的探测器惯性姿态四元素 $\stackrel{\‾}{q}={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right]}^T,$ 确定当前时刻t的探测器本体坐标系与惯性坐标系的姿态转换阵 $C_{\mathrm{bI}}=\left[\begin{array}{ccc}{q}_{t1}^2-q_{t2}^2-q_{t3}^2+q_{t4}^2& 2(q_{t1}{q}_{t2}+q_{t3}{q}_{t4})& 2(q_{t1}{q}_{t3}-q_{t2}{q}_{t4})\\ 2(q_{t1}{q}_{t2}-q_{t3}{q}_{t4})& -q_{t1}^2+q_{t2}^2-q_{t3}^2+q_{t4}^2& 2(q_{t2}{q}_{t3}+q_{t1}{q}_{t4})\\ 2(q_{t1}{q}_{t3}+q_{t2}{q}_{t4})& 2(q_{t2}{q}_{t3}-q_{t1}{q}_{t4})& -q_{t1}^2-q_{t2}^2+q_{t3}^2+q_{t4}^2\end{array}\right],$ 其中， ${\stackrel{\‾}{q}}_t=\stackrel{\‾}{q}+\frac{1}{2}\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ {-q}_2& q_1& q_4\\ -q_1& {-q}_2& -q_3\end{array}\right]\stackrel{\‾}{\mathrm{\ω}}(t-t_0);$

利用加速度计测量速度增量Δv_b和轨道初值，轨道初值包括轨道位置初值r_I0、轨道速度初值v_I0；确定惯性坐标系的速度增量为确定惯性坐标系的探测器位置确定惯性坐标系的探测器速度 $v_I=v_{I0}+\mathrm{\Δ}{v}_I+\mathrm{\Δt}\left(\frac{\mathrm{\μ}{r}_{I0}}{{\left|\right|r_{I0}\left|\right|}^3}\right),$ μ为天体引力常数；

(2)利用测距测速仪测量得到的视线距离确定探测器相对天体中心的距离：

所述的步骤(2)中确定探测器相对天体中心的距离的具体步骤为：利用惯性坐标系的探测器位置，确定探测器相对天体中心方向根据步骤(1)确定的姿态转换阵C_bI，确定探测器相对天体中心方向在探测器本体坐标系的指向为

设测距测速仪的一个测距波束在探测器本体坐标系的指向为

确定探测器相对天体中心的方向和测距波束的夹角关系：由测距测速仪测量得到的视线距离ρ，确定探测器相对天体表面的高度h＝ρcos(θ)，确定探测器相对天体中心的距离为r_m＝h+R_c，其中R_c为当地天体表面的参考半径；

(3)利用测距测速仪测量得到的速度以及测距测速仪三个测速波束安装指向确定探测器相对惯性坐标系的三维速度：

所述的步骤(3)中确定探测器相对惯性坐标系的三维速度的具体步骤为：

采用探测器上的测距测速仪测量三个非共面波束的速度v₁，v₂，v₃以及测距测速仪三个波束安装指向确定探测器本体坐标系的三维速度 ${\stackrel{\‾}{v}}_b={\left[\begin{array}{c}{\stackrel{\‾}{l}}_{b1}^T\\ {\stackrel{\‾}{l}}_{b2}^T\\ {\stackrel{\‾}{l}}_{b3}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right];$ 根据速度

以及步骤(1)确定的姿态转换阵C_bI，确定探测器相对惯性坐标系的三维速度

(4)构建导航系统的状态量、状态方程、观测量、观测方程和测量噪声方差阵：

所述的步骤(4)中构建导航系统的状态量、状态方程、观测量、观测方程和测量噪声方差阵的具体步骤为：

选取探测器相对天体中心的径向距离r和惯性坐标系的速度

作为状态量 $X=\left[\begin{array}{cc}r& {\stackrel{\‾}{v}}^T\end{array}\right],$ 建立状态方程为 $\left\{\begin{array}{c}\stackrel{\‾}{r}=\frac{\stackrel{\‾}{r}\·\stackrel{\‾}{v}}{r}\\ \stackrel{\·}{\stackrel{\‾}{v}}=\frac{-\mathrm{\μ}\stackrel{\‾}{r}}{r^3}\end{array}\right.;$

将步骤(2)中得到的探测器相对天体中心的距离r_m和步骤(3)中得到的惯性坐标系的探测器三维速度

作为观测量 $z_m=\left[\begin{array}{c}{r}_m\\ {\stackrel{\‾}{v}}_I\end{array}\right],$ 建立观测方程为 $z=\left[\begin{array}{c}r\\ \stackrel{\‾}{v}\end{array}\right]+\left[\begin{array}{c}{n}_r\\ {\stackrel{\‾}{n}}_v\end{array}\right],$ 其中，n_r为径向距离测量噪声，

为速度测量噪声；测量噪声方差阵为 $R=\left[\begin{array}{cc}{R}_r& R_{\mathrm{rv}}\\ R_{\mathrm{rv}}& R_v\end{array}\right],$ 其中，R_r、R_v、R_rv分别为径向距离测量噪声方差、速度测量噪声方差阵和距离速度测量相关噪声方差阵；于是，确定导航观测矩阵 $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

(5)测量噪声方差阵的无量纲化处理和导航参数能观度的确定：

所述的步骤(5)中测量噪声方差阵的无量纲化处理和导航参数能观度的确定的具体步骤为：

单位距离L定义为天体参考半径，单位速度V定义为以参考半径为半长轴的参考圆轨道上的探测器速度

只需要对测量噪声方差阵R进行无量纲化处理，得到无量纲化的测量噪声方差阵 $R_0=\left[\begin{array}{cc}{R}_r/L& R_{\mathrm{rv}}/\mathrm{LV}\\ R_{\mathrm{rv}}/\mathrm{LV}& R_v/V\end{array}\right];$

定义导航系统能观度矩阵判断能观度矩阵的条件数Cond(G)是否接近1，如果能观度矩阵的条件数接近1，则执行步骤(6)，否则执行步骤(4)；

能观度矩阵的条件数接近于1表明导航参数的能观度高；这里的导航参数包括高度和速度信息；

(6)采用分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵，使之适合采用星上可以实现的基于UD协方差分解的滤波算法：

所述的步骤(6)中分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵的具体步骤为：

测量噪声方差阵R是对称正定矩阵，对其进行UD分解，可以得到R＝BDB^T，R′＝D对角矩阵；

利用变换 $\left\{\begin{array}{c}{z}^{\′}=B^{T}z\\ H^{\′}=B^{T}H\end{array}\right.$ 确定测量噪声不相关的观测方程z′和观测量z′_m及观测矩阵H′， $z^{\′}=B^{T}z=B^T\left[\begin{array}{c}r\\ \stackrel{\‾}{v}\end{array}\right],$ $z_m^{\′}=B^T\left[\begin{array}{c}{r}_m\\ {\stackrel{\‾}{v}}_I\end{array}\right],$ $H^{\′}=B^{T}H=B^T\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=B^T;$

(7)根据步骤(6)确定的观测方程、观测量和观测矩阵，采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离和速度：

所述的步骤(7)中根据上述步骤(6)确定的观测方程z′、观测量z′_m、噪声方差阵R′和观测矩阵H′，采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离

和速度的具体方法为：

$\hat{X}=\tilde{X}+K(z_m^{\′}-z^{\′})$

式中，K为UD协方差分解的扩展卡尔曼滤波增益， $\tilde{X}={\left[\begin{array}{cc}\frac{r_I}{\left|\right|r_I\left|\right|}& v_I^T\end{array}\right]}^T$ 为预估的导航参数；

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器位置 $r_I=\hat{X}\left(1\right)\frac{r_I}{\left|\right|r_I\left|\right|};$

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器速度 $v_I=\hat{X}(2:4).$

本发明的有益技术效果：引入无量纲化处理和能观度矩阵条件数判断导航参数的能观度，通过选取探测器相对天体中心的距离和惯性系的速度作为观测量，选取关键的导航参数高度和速度作为状态量，保证了构建的降维导航方法的稳定性、收敛速度和精度。针对距离和速度测量存在相关噪声的情况，采用分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵，使之适合采用星上可以实现的基于UD协方差分解的滤波方法。采用降维自主导航滤波方法可以提高导航的稳定性和关键导航参数的收敛速度和估计精度，非常适合于深空着陆导航任务的应用。

附图说明

图1为本发明所提供的一种着陆深空天体的降维自主导航方法的流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细说明。

一种着陆深空天体探测器的降维自主导航方法，它包括以下步骤：

(1)利用陀螺测量的姿态角速度

以及加速度计测量的速度增量Δv_b和轨道初值，确定当前时刻探测器相对惯性坐标系的姿态

以及位置r_I和速度v_I初值

采用探测器上的陀螺测量探测器姿态角速度

根据前一时刻t₀的探测器惯性姿态四元素 $\stackrel{\‾}{q}={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right]}^T,$ 初始惯性姿态四元素利用着陆过程开始前的星敏感器测量确定，确定当前时刻t的探测器本体坐标系与惯性坐标系的姿态转换阵 $C_{\mathrm{bI}}=\left[\begin{array}{ccc}{q}_{t1}^2-q_{t2}^2-q_{t3}^2+q_{t4}^2& 2(q_{t1}{q}_{t2}+q_{t3}{q}_{t4})& 2(q_{t1}{q}_{t3}-q_{t2}{q}_{t4})\\ 2(q_{t1}{q}_{t2}-q_{t3}{q}_{t4})& -q_{t1}^2+q_{t2}^2-q_{t3}^2+q_{t4}^2& 2(q_{t2}{q}_{t3}+q_{t1}{q}_{t4})\\ 2(q_{t1}{q}_{t3}+q_{t2}{q}_{t4})& 2(q_{t2}{q}_{t3}-q_{t1}{q}_{t4})& -q_{t1}^2-q_{t2}^2+q_{t3}^2+q_{t4}^2\end{array}\right],$ 其中， ${\stackrel{\‾}{q}}_t=\stackrel{\‾}{q}+\frac{1}{2}\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ {-q}_2& q_1& q_4\\ -q_1& {-q}_2& -q_3\end{array}\right]\stackrel{\‾}{\mathrm{\ω}}(t-t_0).$

利用加速度计测量速度增量Δv_b和轨道初值，轨道初值包括轨道位置初值r_I0、轨道速度初值V_I0。可以确定惯性坐标系的速度增量为确定惯性坐标系的探测器位置

确定惯性坐标系的探测器速度 $v_I=v_{I0}+\mathrm{\Δ}{v}_I+\mathrm{\Δt}\left(\frac{\mathrm{\μ}{r}_{I0}}{{\left|\right|r_{I0}\left|\right|}^3}\right),$ μ为天体引力常数。

(2)利用测距测速仪测量得到的视线距离ρ确定探测器相对天体中心的距离r_m

利用惯性坐标系的探测器位置，确定探测器相对天体中心方向

根据步骤(1)确定的姿态转换阵C_bI，确定探测器相对天体中心方向在探测器本体坐标系的指向为

设测距测速仪的一个测距波束在探测器本体坐标系的指向为

确定探测器相对天体中心的方向和测距波束的夹角关系：

由测距测速仪测量得到的视线距离ρ，确定探测器相对天体表面的高度h＝ρcos(θ)，确定探测器相对天体中心的距离为r_m＝h+R_c，其中Rc为当地天体表面的参考半径。

(3)利用测距测速仪测量得到的速度以及测距测速仪三个测速波束安装指向确定探测器相对惯性坐标系的三维速度

采用探测器上的测距测速仪测量三个非共面波束的速度v₁，v₂，v₃以及测距测速仪三个波束安装指向

确定探测器本体坐标系的三维速度 ${\stackrel{\‾}{v}}_b={\left[\begin{array}{c}{\stackrel{\‾}{l}}_{b1}^T\\ {\stackrel{\‾}{l}}_{b2}^T\\ {\stackrel{\‾}{l}}_{b3}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right].$ 根据速度以及步骤(1)确定的姿态转换阵C_bI，确定探测器相对惯性坐标系的三维速度

(4)构建导航的状态量、状态方程、观测量、观测方程和测量噪声方差阵

选取探测器相对天体中心的径向距离r和惯性坐标系的速度

作为状态量 $X=\left[\begin{array}{cc}r& {\stackrel{\‾}{v}}^T\end{array}\right],$ 建立状态方程为 $\left\{\begin{array}{c}\stackrel{\‾}{r}=\frac{\stackrel{\‾}{r}\·\stackrel{\‾}{v}}{r}\\ \stackrel{\·}{\stackrel{\‾}{v}}=\frac{-\mathrm{\μ}\stackrel{\‾}{r}}{r^3}\end{array}\right..$ 其中，μ为天体引力常数。

将步骤(2)中得到的探测器相对天体中心的距离r_m和步骤(3)中得到的惯性坐标系的探测器三维速度

作为观测量 $z_m=\left[\begin{array}{c}{r}_m\\ {\stackrel{\‾}{v}}_I\end{array}\right],$ 建立观测方程为 $z=\left[\begin{array}{c}r\\ \stackrel{\‾}{v}\end{array}\right]+\left[\begin{array}{c}{n}_r\\ {\stackrel{\‾}{n}}_v\end{array}\right].$ 其中，n_r为径向距离测量噪声，

为速度测量噪声。测量噪声方差阵为 $R=\left[\begin{array}{cc}{R}_r& R_{\mathrm{rv}}\\ R_{\mathrm{rv}}& R_v\end{array}\right].$ 其中，R_r、R_v、R_rv分别为径向距离测量噪声方差、速度测量噪声方差阵和距离速度测量相关噪声方差阵，设 $R=\left[\begin{array}{cccc}10& 0.01& 0.01& 0.01\\ 0.01& 0.01& 0.001& 0.001\\ 0.01& 0.001& 0.01& 0.001\\ 0.01& 0.001& 0.001& 0.01\end{array}\right].$

于是，可以确定导航观测矩阵 $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$ 显然，观测矩阵H的秩为4，因此，导航系统状态量全部可观。

(5)测量噪声方差阵的无量纲化处理和能观度的确定

单位距离L定义为天体参考半径，对于月球L＝1738000m，单位速度V定义为以参考半径为半长轴的参考圆轨道上的探测器速度

对于月球，μ＝4902.75×10⁹m³/s²，V＝1680m/s。只需要对测量噪声方差阵R进行无量纲化处理，得到无量纲化的测量噪声方差阵 $R_0=\left[\begin{array}{cc}{R}_r/L& R_{\mathrm{rv}}/\mathrm{LV}\\ R_{\mathrm{rv}}/\mathrm{LV}& R_v/V\end{array}\right],$ 计算可得 $R_0={10}^{-5}\×\left[\begin{array}{cccc}0.5754& 0& 0& 0\\ 0& 0.5954& 0& 0\\ 0& 0& 0.5954& 0\\ 0& 0& 0& 0.5954\end{array}\right].$

定义能观度矩阵

确定能观度矩阵的条件数Cond(G)＝G＝1.035。

判断能观度矩阵的条件数是否接近1，能观度矩阵的条件数在1±ε范围内说明其接近1，一般可取ε＝0.2。如果能观度矩阵的条件数接近1，则执行步骤(6)，否则执行步骤(4)。

能观度矩阵的条件数接近于1表明导航信息的可观度高。导航信息包括高度和速度信息。

(6)采用分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵，使之适合采用星上可以实现的基于UD协方差分解的滤波方法。

考虑到测量噪声方差阵R是对称正定矩阵，对其进行UD分解，可以得到R＝BDB^T，R′＝D对角矩阵。计算得到 $B=\left[\begin{array}{cccc}-0.99999850& 0.00173412& 0& 0\\ -0.0010011& -0.57734940& 0& -0.81649658\\ -0.0010011& -0.57734940& -0.70710678& -0.40824829\\ -0.0010011& -0.57734940& 0.70710678& -0.40824829\end{array}\right],$ $R^{\′}=D=\left[\begin{array}{cccc}10& 0& 0& 0\\ 0& 0.0012& 0& 0\\ 0& 0& 0.0009& 0\\ 0& 0& 0& 0.0009\end{array}\right]$ 为对角矩阵。

于是利用变换 $\left\{\begin{array}{c}{z}^{\′}=B^{T}z\\ H^{\′}=B^{T}H\end{array}\right.$ 确定测量噪声不相关的观测方程z′和观测量z′_m及观测矩阵H′。 $z^{\′}=B^{T}z=B^T\left[\begin{array}{c}r\\ \stackrel{\‾}{v}\end{array}\right],$ $z_m^{\′}=B^T\left[\begin{array}{c}{r}_m\\ {\stackrel{\‾}{v}}_I\end{array}\right],$ $H^{\′}=B^{T}H=B^T\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=B^T.$

(7)根据上述步骤(6)确定的观测方程z′、观测量z′_m、噪声方差阵R′和观测矩阵H′，采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离

和速度

$\hat{X}=\tilde{X}+K(z_m^{\′}-z^{\′})$

式中，K为UD协方差分解的扩展卡尔曼滤波增益， $\tilde{X}={\left[\begin{array}{cc}\frac{r_I}{\left|\right|r_I\left|\right|}& v_I^T\end{array}\right]}^T$ 为预估的导航参数。

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器位置 $r_I=\hat{X}\left(1\right)\frac{r_I}{\left|\right|r_I\left|\right|}.$

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器速度 $v_I=\hat{X}(2:4).$

本发明说明书中未作详细描述的内容属于本领域专业人员公知的现有技术。

Claims (1)

1.一种着陆深空天体探测器的降维自主导航方法，其特征在于：它包括以下步骤：

(1)利用陀螺测量的姿态角速度以及加速度计测量的速度增量和轨道初值，确定当前时刻探测器相对惯性坐标系的姿态以及位置和速度初值：

所述的步骤(1)中确定惯性坐标系的探测器位置和速度的具体步骤为：采用探测器上的陀螺测量探测器姿态角速度 

根据前一时刻t₀的探测器惯性姿态四元素

确定当前时刻t的探测器本体坐标系与惯性坐标系的姿态转换阵 

其中， 

利用加速度计测量速度增量Δv_b和轨道初值，轨道初值包括轨道位置初值r_I0、轨道速度初值v_I0；确定惯性坐标系的速度增量为 

确定惯性坐标系的探测器位置 

确定惯性坐标系的探测器速度

μ为天体引力常数；

(2)利用测距测速仪测量得到的视线距离确定探测器相对天体中心的距离：

所述的步骤(2)中确定探测器相对天体中心的距离的具体步骤为： 

利用惯性坐标系的探测器位置，确定探测器相对天体中心方向 

根据步骤(1)确定的姿态转换阵C_bI，确定探测器相对天体中心方向在探测器本体坐标系的指向为 

设测距测速仪的一个测距波束在探测器本体坐标系的指向为 

确定探测器相对天体中心的方向和测距波束的夹角关系： 由测距测速仪测量得到的视线距离ρ，确定探测器相对天体表面的高度h＝ρcos(θ)，确定探测器相对天体中心的距离为r_m＝h+R_c，其中R_c为当地天体表面的参考半径；

(3)利用测距测速仪测量得到的速度以及测距测速仪三个测速波束安装指向确定探测器相对惯性坐标系的三维速度：

所述的步骤(3)中确定探测器相对惯性坐标系的三维速度的具体步骤为：

采用探测器上的测距测速仪测量三个非共面波束的速度v₁，v₂，v₃以及测距测速仪三个波束安装指向 

确定探测器本体坐标系的三维速度 

根据速度 

以及步骤(1)确定的姿态转换阵C_bI，确定探测器相对惯性坐标系的三维速度 

(4)构建导航系统的状态量、状态方程、观测量、观测方程和测量噪声方差阵：

所述的步骤(4)中构建导航系统的状态量、状态方程、观测量、观测方程和测量噪声方差阵的具体步骤为：

选取探测器相对天体中心的径向距离r和惯性坐标系的速度 

作为状态 量建立状态方程为

将步骤(2)中得到的探测器相对天体中心的距离r_m和步骤(3)中得到的惯性坐标系的探测器三维速度 

作为观测量

建立观测方程为 

其中，n_r为径向距离测量噪声， 

为速度测量噪声；测量噪声方差阵为

其中，R_r、R_v、R_rv分别为径向距离测量噪声方差、速度测量噪声方差阵和距离速度测量相关噪声方差阵；于是，确定导航观测矩阵

(5)测量噪声方差阵的无量纲化处理和导航参数能观度的确定：

所述的步骤(5)中测量噪声方差阵的无量纲化处理和导航参数能观度的确定的具体步骤为：

单位距离L定义为天体参考半径，单位速度V定义为以参考半径为半长轴的参考圆轨道上的探测器速度 

只需要对测量噪声方差阵R进行无量纲化处理，得到无量纲化的测量噪声方差阵

定义导航系统能观度矩阵 判断能观度矩阵的条件数Cond(G)是否接近1，如果能观度矩阵的条件数接近1，则执行步骤(6)，否则执行步骤(4)；

能观度矩阵的条件数接近于1表明导航参数的能观度高；这里的导航参 数包括高度和速度信息；

(6)采用分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵，使之适合采用星上可以实现的基于UD协方差分解的滤波算法：

所述的步骤(6)中分解变换方法处理测量噪声方差阵、观测方程、观测量和观测矩阵的具体步骤为：

测量噪声方差阵R是对称正定矩阵，对其进行UD分解，可以得到R＝BDB^T，R′＝D对角矩阵；

利用变换

确定测量噪声不相关的观测方程z′和观测量z′_m及观测矩阵H′，

(7)根据步骤(6)确定的观测方程、观测量和观测矩阵，采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离和速度：

所述的步骤(7)中根据上述步骤(6)确定的观测方程z′、观测量z′_m、噪声方差阵R′和观测矩阵H′，采用UD协方差分解的扩展卡尔曼滤波确定探测器相对天体中心的距离 

和速度 

的具体方法为：

式中，K为UD协方差分解的扩展卡尔曼滤波增益，

为预估的导航参数；

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器位置

利用修正后的导航参数修正步骤(1)中得到的惯性坐标系的探测器速 度

Images