CN112525204B - 一种航天器惯性和太阳多普勒速度组合导航方法 - Google Patents

Abstract

本发明涉及一种航天器惯性和太阳多普勒速度组合导航方法，将惯性导航与多普勒测速方法相结合，首先根据惯性导航***的误差方程建立***状态模型，然后利用光谱仪获得的太阳多普勒速度作为量测量，并建立太阳多普勒速度量测模型，最后使用UKF滤波估计航天器的位置、速度和姿态。本发明属于航天器自主导航领域，可为航天器提供高精度的位置、速度及姿态信息，对航天器导航具有重要的实际意义。

Description

一种航天器惯性和太阳多普勒速度组合导航方法

技术领域

本发明属于航天器自主导航领域，涉及一种航天器惯性和太阳多普勒速度组合导航方法。

背景技术

航天器的发展在国防、信息通讯与科技创新中占有重要地位，而导航***作为航天器上的重要设备，为航天器的操作或控制***提供有关运动参数，从而引导航天器按所需速度和轨迹从起始点运动到目的地，对航天器的飞行任务有着不可忽视的作用。

惯性导航根据惯性测量单元(Inertial Measurement Unit,IMU)中陀螺仪和加速度计的输出，利用积分的方法确定航天器的位置、速度和姿态，具有独立性强、输出频率高、隐蔽性好、较为成熟等特点。由于具有优良性能，被广泛的应用在各类航天器中，但由于惯性测量单元中的陀螺和加速度计不可避免的存在误差，并随时间累积，严重影响导航精度，因此常与其他导航***组合起来用以改善导航性能。天文导航误差不随时间积累，和惯导具有互补性，因此常选用天文导航加以辅助，惯性/天文组合导航成为自主导航中的有效手段。传统的惯性/天文组合导航是利用天文导航中星敏感器提供的姿态信息来修正惯导的姿态误差和陀螺漂移，从而得到高精度姿态，并不能直观修正惯性导航速度与位置误差。而不管在什么领域，速度信息都至关重要，会对导航精度产生巨大影响，因此迫切需要寻找一种合适的天文导航方法来校正惯导的速度信息。研究发现，在航海领域的自主水下航行器导航***中，常常使用多普勒计程仪器来修正惯性导航的速度，能够显著提升航行器的速度精度，据此提出在航天器导航中使用太阳多普勒速度信息辅助惯导，以此来修正速度，提高航天器速度与位置精度。

在深空探测任务中，研究一种全自主、高精度的导航方法具有十分重要的意义。本发明提出的一种航天器惯性和太阳多普勒速度组合导航方法，在传统惯性导航的基础上，辅助太阳多普勒速度导航，能够实现两种导航方法的优势互补，既保留惯导自主性强、输出信息全面的特点，也能够直接提高速度精度，间接修正位置，大幅度提高导航***的准确性与可靠性。考虑到它们在工作原理与信息来源上的互补特性，对于长时间、长距离的航天器自主导航任务，利用太阳多普勒速度信息辅助惯性导航实现航天器自主导航是一种可行且值得深入研究的方法。

发明内容

本发明要解决的技术问题是：传统的惯性/天文组合导航方法，大多利用星敏感器输出的姿态信息来修正惯导的姿态，无法直观修正惯导的速度与位置误差，而速度信息至关重要，会对导航精度产生巨大影响。为解决航天器导航中传统惯性/天文组合导航方法无法直观修正惯导速度的问题，为航天器提供一种将惯性导航与太阳多普勒速度导航结合起来的自主导航方法，利用太阳多普勒速度导航辅助惯导获得高精度速度信息。

本发明解决其技术问题所采用的技术方案为：一种航天器惯性和太阳多普勒速度组合导航方法，实现为：

第一步，根据惯性导航***的误差方程建立***状态模型；

第二步，利用光谱仪获得太阳多普勒速度，并建立太阳多普勒速度量测模型；

第三步，基于第一步中的状态模型和第二步中的量测模型，采用UKF滤波估计航天器的位置、速度和姿态。

具体包括以下步骤：

1、建立基于惯性导航误差方程的***状态模型

惯性导航通过惯性测量单元IMU测量航天器相对惯性空间的角速率和加速度信息，利用牛顿运动定律自动推算航天器的瞬时速度和位置信息。在捷联惯性导航***下，IMU通常由三个正交加速度计和三个正交陀螺仪组成，整个组件直接安装在航天器本体上。根据惯性导航原理，***的状态模型为：

其中，φ＝[φ_E φ_N φ_U]^T是姿态误差角，φ_E、φ_N、φ_U分别表示n系内东、北、天向的姿态误差；

是航天器速度，v_E、v_N、v_U分别表示n系内东、北、天向的速度，/>

是航天器速度误差，δv_E、δv_N、δv_U分别表示n系内东、北、天向的速度误差；r_n＝[L λ h]^T是航天器位置，L、λ、h分别表示n系下纬度、经度及高度，δr_n＝[δL δλδh]^T是航天器位置误差，δL、δλ、δh分别表示n系下纬度误差、经度误差及高度误差；fⁿ是加速度计的输出在n系中的投影；/>

表示航天器在n系下的地球自转角速率，/>

是航天器在n系下w_ie的误差；

是n系相对e系的旋转角速率在n系中的表示，其中R_x和R_y分别为卯酉圈和子午圈的主曲率半径，

是n系下w_en的误差；/>

表示n系相对i系的转动角速度在n系中的表示，/>

是n系下ω_in的误差；ε＝(ε_x ε_y ε_z)^T是惯性导航***x、y、z三个方向的陀螺仪常值漂移；/>

是惯性导航***x、y、z三个方向加速度计的常值偏置；/>

所述n系为地理坐标系，e系是地球坐标系，i系为地心惯性坐标系，b系为航天器本体坐标系；

上式状态模型写为：

X_k＝F(X_k-1,k-1)+W_k-1 (2)

其中，状态量为

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，X_k，X_k-1分别为k时刻和k-1下的状态量，F(X_k-1,k-1)为航天器惯性/太阳多普勒速度组合导航***非线性转移函数，W_k-1为过程噪声。

2、建立太阳多普勒速度量测模型

利用光谱仪获得光谱频移，并根据频移获得航天器相对太阳的径向速度v_r，表示为：

v_r＝c((f_rs-f_es)/f_es) (3)

其中，c为光速，f_rs为航天器所接受到的太阳发出的光谱频率，f_es为太阳发出的光谱频率。

以航天器相对于太阳的多普勒径向速度此作为量测量，利用与航天器位置间的数学联系建立太阳多普勒速度量测模型：

其中，v_r表示航天器相对太阳的径向速度量测量，r_ps,v_ps分别表示航天器相对太阳的位置与速度矢量，r_ps＝||r_ps||表示位置矢量的大小，v_m表示量测噪声。

利用坐标变换将航天器相对太阳的位置矢量与INS中的状态量建立联系。

如图3所示，在地心惯性坐标系O-xyz中，r_ps、v_ps分别为地心惯性系下航天器相对于太阳的位置、速度矢量，r_pe、v_pe分别为航天器相对于地球的位置、速度矢量，则有：

其中，

r_n、v_n为n系内航天器相对于地球的位置、速度矢量，δr_n、δv_n为位置和速度误差，由INS获得；r_se与v_se分别为太阳相对地球的位置和速度矢量，可以由STK工具获得；/>

是从n系到i系的转换矩阵。由此，将量测模型中的状态量与状态模型联系起来。则太阳多普勒速度量测模型表示为：

离散化后太阳多普勒速度量测模型表示为：

Z_k＝H(X_k,k)+V_k (7)

其中，H(·)表示太阳多普勒速度的非线性连续量测函数，V_k表示k时刻太阳多普勒速度的量测误差。

3、进行UKF滤波获得航天器的位置、速度及姿态估计

离散后航天器惯性和太阳多普勒速度组合导航***的状态模型与量测模型为：

其中，F(X_k-1,k-1)为组合导航***非线性转移函数，H(X_k,k)为非线性量测函数，W_k-1及V_k分别表示过程与量测噪声。对***模型式(8)通过UKF进行滤波，获得航天器的后验状态估计

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，以及后验误差协方差/>

将/>

及/>

输出，同时将k时刻状态量与误差协方差的估计值返回UKF滤波器，用于获得k+1时刻的输出。

本发明的原理是：在惯性导航的基础上，使用天文导航中的多普勒速度信息加以辅助。根据惯性导航***的误差方程建立航天器的状态模型，利用光谱仪实时观测太阳光谱的多普勒速度频移量，直接获取航天器相对太阳的速度，对速度积分间接获取位置信息，并以太阳多普勒速度为量测量，根据多普勒径向速度与航天器位置之间的关系，建立量测模型，将多普勒速度导航与惯性导航组合起来直接修正航天器速度，间接修正位置，最后通过UKF实现对航天器位置、速度等导航参数的估计。

本发明与现有技术相比的优点在于：

(1)现有航天器自主导航技术中，为满足高精度要求与复杂任务，常常采用两种或两种以上导航手段的组合导航***应用于现代军事导航。其中惯性导航以其自主性强、短时间内精度高、输出信息全面连续等特点，成为组合导航中的主要方式，常与其他导航组合起来改善导航精度。常用的导航方式有天文导航、视觉导航、卫星导航等，但考虑到航天器在军事国防方面的应用，视觉导航不适用于远距离导航，GPS导航受制于美国，战时可用性不高，天文导航精度高，且误差不随时间积累，因此常使用天文导航辅助惯性导航完成航天器导航任务。

(2)传统的惯性/天文组合导航，无论深组合、浅组合，都是利用天文导航***提供的高精度姿态信息对惯导***进行校正，并对惯性器件的误差进行补偿，主要用于姿态的修正，不能直接、较好的修正速度与位置信息。本发明提出在惯性导航的基础上，使用太阳多普勒速度信息加以辅助，这种组合导航方法能够实现两种导航方法的优势互补，不仅保留了惯导自主性强、输出信息全面的特点，也能够用太阳多普勒速度导航直接提高速度精度，间接修正位置，大幅度提高导航***的准确性与可靠性。

附图说明

图1为本发明中航天器惯性和太阳多普勒速度组合导航方法流程图；

图2为本发明中航天器惯性和太阳多普勒速度组合导航方法原理图；

图3为本发明中地心惯性坐标系下航天器与太阳的位置关系示意图；

图4为本发明中组合导航坐标系示意图。

具体实施方式

下面结合附图及实例对本发明进行详细说明。

如图1所示，本发明的航天器惯性和太阳多普勒速度组合导航方法具体实施过程如下：

1、建立基于惯性导航误差方程的***状态模型

惯性导航通过惯性测量单元IMU测量航天器相对惯性空间的角速率和加速度信息，利用牛顿运动定律自动推算航天器的瞬时速度和位置信息。在捷联惯性导航***下，IMU通常由三个正交加速度计和三个正交陀螺仪组成，整个组件直接安装在航天器本体上。根据惯性导航原理，***状态模型为：

其中，φ＝[φ_E φ_N φ_U]^T是姿态误差角，φ_E、φ_N、φ_U分别表示n系内东、北、天向的姿态误差；

是航天器速度，v_E、v_N、v_U分别表示n系内东、北、天向的速度，

是航天器速度误差，δv_E、δv_N、δv_U分别表示n系内东、北、天向的速度误差；r_n＝[L λ h]^T是航天器位置，L、λ、h分别表示n系下纬度、经度及高度，δr_n＝[δL δλ δh]^T是航天器位置误差，δL、δλ、δh分别表示n系下纬度误差、经度误差及高度误差；fⁿ是加速度计的输出在n系中的投影；/>

表示航天器在n系下的地球自转角速率，/>

是航天器在n系下w_ie的误差；

是n系相对e系的旋转角速率在n系中的表示，其中R_x和R_y分别为卯酉圈和子午圈的主曲率半径，

是n系下w_en的误差；/>

表示n系相对i系的转动角速度在n系中的表示，/>

是n系下ω_in的误差；ε＝(ε_x ε_y ε_z)^T是惯性导航***x、y、z三个方向的陀螺仪常值漂移；/>

是惯性导航***x、y、z三个方向加速度计的常值偏置；/>

所述n系为地理坐标系，e系是地球坐标系，i系为地心惯性坐标系，b系为航天器本体坐标系；

上式***状态模型写为：

X_k＝F(X_k-1,k-1)+W_k-1 (2)

其中，状态量为

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，X_k，X_k-1分别为k时刻和k-1下的状态量，F(X_k-1,k-1)为航天器惯性/太阳多普勒速度组合导航***非线性转移函数，W_k-1为过程噪声。

2、建立太阳多普勒速度量测模型

利用光谱仪获得光谱频移，并根据频移获得航天器相对太阳的径向速度v_r，表示为

v_r＝c((f_rs-f_es)/f_es) (3)

其中，c为光速，f_rs为航天器所接受到的太阳发出的光谱频率，f_es为太阳发出的光谱频率。

以航天器相对于太阳的多普勒径向速度此作为量测量，利用与航天器位置间的数学联系建立太阳多普勒速度量测模型：

其中，v_r表示航天器相对太阳的径向速度量测量，r_ps,v_ps分别表示航天器相对太阳的位置与速度矢量，r_ps＝||r_ps||表示位置矢量的大小，v_m表示量测噪声。

利用坐标变换将航天器相对太阳的位置矢量与INS中的状态量建立联系。

如图3所示，在地心惯性坐标系O-xyz中，r_ps、v_ps分别为地心惯性系下航天器相对于太阳的位置、速度矢量，r_pe、v_pe分别为航天器相对于地球的位置、速度矢量，则有：

其中，

r_n、v_n为n系内航天器相对于地球的位置、速度矢量，δr_n、δv_n为位置和速度误差，由INS获得；r_se与v_se分别为太阳相对地球的位置和速度矢量，可以由STK工具获得；/>

是从n系到i系的转换矩阵。由此，将量测模型中的状态量与状态模型联系起来。则太阳多普勒速度量测模型表示为：

离散化后太阳多普勒速度量测模型表示为：

Z_k＝H(X_k,k)+V_k (7)

其中，H(·)表示太阳多普勒速度的非线性连续量测函数，V_k表示k时刻太阳多普勒速度的量测误差。

3、进行UKF滤波获得航天器的位置、速度及姿态估计

离散后航天器惯性/太阳多普勒速度组合导航***的状态模型与量测模型为：

其中，F(X_k-1,k-1)为组合导航***非线性转移函数，H(X_k,k)为非线性量测函数，W_k-1及V_k分别表示过程与量测噪声。对***模型式通过UKF进行滤波，获得航天器的后验状态估计

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，以及后验误差协方差/>

将/>

及/>

输出。

具体步骤如下：

A.初始化状态量

和状态误差方差阵P₀

式中，

是第0时刻(初始时刻)航天器状态量估计值，X₀是第0时刻航天器状态量真实值。

B.选取sigma采样点

在

附近选取一系列采样点，这些样本点的均值和协方差分别为/>

和/>

状态变量为15×1维，那么选取31个样本点/>

及其权重w₀,w₁…,w₃₀分别如下：

其中τ表示缩放参数，

表示取平方根矩阵的第i行或列。

C.传递sigma采样点并获得先验估计及先验误差协方差

每个采样点的一步预测

为：

合并所有

获得先验状态估计/>

为：

先验误差协方差

为：

式中，Q_k为k时刻状态模型噪声协方差阵。

D.量测更新

根据量测方程，计算每个采样点

的预测量测量/>

合并所有

获得预测量测Y_k为：

计算预测量测协方差P_yy,k及互协方差P_xy,k：

其中R_k为k时刻***的量测噪声协方差阵。计算滤波增益K_k为：

计算后验状态估计

计算后验误差协方差

将

及/>

输出，同时将这些估计值返回滤波器，用于获得k+1时刻的输出。

图2给出了航天器惯性和太阳多普勒速度组合导航方法原理图，介绍了各个导航***的基本原理。

①惯性导航***

惯导***主要由惯性测量单元(IMU)和相应的惯导机械化组成，以牛顿力学定律为基础。捷联惯导中，IMU通常由三个正交加速度计和三个正交陀螺仪组成，整个组件直接安装在航天器本体上。陀螺仪和加速度计分别获得航天器相对惯性坐标系的角速度、加计非引力加速度，将测量数据转换到n系，就能够确定航天器在导航坐标系中的位置、速度与姿态信息。

②多普勒速度导航***

多普勒频移，指的是光源和移动物体之间的相对运动引起光谱线基于其波长发生相位和频率的变化。当导航天体为太阳时，通过观测航天器与太阳的相对运动引起的多普勒频移测量，可以获得航天器相对于太阳的径向速度，以此作为多普勒速度量测量，积分可获得航天器的位置。

基于惯性导航***的误差方程建立组合导航***状态模型，利用光谱仪实时观测太阳光谱的多普勒速度频移量，直接获取航天器相对太阳的速度，对速度积分间接获取位置信息，并以太阳多普勒速度为量测量，根据多普勒径向速度与航天器位置之间的关系，建立量测模型，将多普勒速度导航与惯性导航组合起来直接修正航天器速度，间接修正位置，最后通过UKF实现对航天器位置、速度等导航参数的估计。

图3给出了地心惯性坐标系下航天器与太阳的位置关系示意图。在地心惯性坐标系下，r_ps、v_ps分别为航天器相对于太阳的位置、速度矢量，r_pe、v_pe分别为航天器相对于地球的位置、速度矢量，r_se与v_se分别为太阳相对地球的位置、速度矢量。根据向量几何关系，可以将r_ps表示为r_pe、r_se之差，v_ps表示为v_pe、v_se之差，从而建立地心系下的航天器多普勒径向速度与航天器速度与位置之间的联系。

图4给出了组合导航坐标系示意图，由于有些测量值是用不同的坐标系表示的，需要进行一些计算和变换，在此介绍了针对航天器惯性和太阳多普勒速度组合导航中涉及的常用坐标系，包括地心惯性坐标系(i系)，地球坐标系(e系)，地理坐标系(n系)，航天器本体坐标系(b系)。

①惯性坐标系(i系，O_ix_iy_iz_i)

惯性坐标系的原点位于地球质心O_i，x_i轴在赤道平面内并指向春分点方向，z_i轴垂直于赤道平面，且与地球自转轴方向一致，y_i轴与x_i轴和z_i轴均垂直，并构成右手直角坐标系。

②地球坐标系(e系，O_ix_ey_ez_e)

地球坐标系的原点位于地球质心O_i，z_e轴垂直于赤道平面，且与地球自转方向一致，x_e轴在赤道平面内并指向本初子午线，y_e轴垂直于x_e轴和z_e轴，并构成右手直角坐标系。坐标系与地球固连，与地球一起旋转。

可获得从地球系转换到地心惯性系的变换矩阵

表示为

其中，ω_ie为地球绕其旋转轴的自转角速率，t是时间参数。

③地理坐标系(n系，O x_ny_nz_n)

地理坐标系为当地东北天坐标系，在此也作为导航坐标系，其原点位于航天器质心O，O x_n轴指向当地水平东向，O y_n轴指向当地水平北向，O z_n轴沿垂线方向指向天顶。

可获得从地理系转换到地球坐标系的变换矩阵

表示为：

其中，L、λ为航天器在地球上的纬度、经度大小。则可获得从地理系转换到地心惯性系的变换矩阵

④航天器本体坐标系(b系，O x_by_bz_b)

航天器本体坐标系就是固定在航天器上面的坐标系，其原点位于航天器质心O，纵轴O y_b为本体纵向指向前，横轴O x_b指向其右，Oz_b垂直于O x_by_b构成右手坐标系。

⑤IMU坐标系

IMU坐标系是固定在IMU上的坐标系，它是专门为惯导***设置的坐标系，三轴指向为IMU敏感轴的方向，与本体坐标系一致。

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

Claims (2)

1.一种航天器惯性和太阳多普勒速度组合导航方法，其特征在于，包括以下步骤：

第一步，根据惯性导航***的误差方程建立***状态模型；

第二步，利用光谱仪获得太阳多普勒速度，并建立太阳多普勒速度量测模型；

第三步，基于第一步的***状态模型和第二步的量测模型，采用UKF滤波估计航天器的位置、速度和姿态，从而完成航天器惯性和太阳多普勒速度组合导航；

所述第一步，根据惯性导航***的误差方程建立***状态模型，包括以下步骤：惯性导航通过惯性测量单元IMU测量航天器相对惯性空间的角速率和加速度信息，利用牛顿运动定律自动推算航天器的瞬时速度和位置信息，在捷联惯性导航***下，IMU由三个正交加速度计和三个正交陀螺仪组成，直接安装在航天器本体上，根据惯性导航原理，***状态模型为：

其中，φ＝[φ_E φ_N φ_U]^T是姿态误差角，φ_E、φ_N、φ_U分别表示n系内东、北、天向的姿态误差；

是航天器速度，v_E、v_N、v_U分别表示n系内东、北、天向的速度，

是航天器速度误差，δv_E、δv_N、δv_U分别表示n系内东、北、天向的速度误差；r_n＝[L λ h]^T是航天器位置，L、λ、h分别表示n系下纬度、经度及高度，δr_n＝[δL δλ δh]^T是航天器位置误差，δL、δλ、δh分别表示n系下纬度误差、经度误差及高度误差；fⁿ是加速度计的输出在n系中的投影；/>

表示航天器在n系下的地球自转角速率，/>

是航天器在n系下w_ie的误差；

是n系相对e系的旋转角速率在n系中的表示，其中R_x和R_y分别为卯酉圈和子午圈的主曲率半径，

是n系下w_en的误差；/>

表示n系相对i系的转动角速度在n系中的表示，/>

是n系下ω_in的误差；ε＝(ε_x ε_y ε_z)^T是惯性导航***x、y、z三个方向的陀螺仪常值漂移；/>

是惯性导航***x、y、z三个方向加速度计的常值偏置；/>

所述n系为地理坐标系，e系是地球坐标系，i系为地心惯性坐标系，b系为航天器本体坐标系；

上式***状态模型写为：

X_k＝F(X_k-1,k-1)+W_k-1 (2)

其中，状态量为

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，X_k，X_k-1分别为k时刻和k-1下的状态量，F(X_k-1,k-1)为航天器惯性和太阳多普勒速度组合导航***非线性转移函数，W_k-1为过程噪声；

所述第二步，建立太阳多普勒速度量测模型步骤为：

利用光谱仪获得光谱频移，并根据频移获得航天器相对太阳的径向速度v_r，表示为：

v_r＝c((f_rs-f_es)/f_es) (3)

其中，c为光速，f_rs为航天器所接受到的太阳发出的光谱频率，f_es为太阳发出的光谱频率；

以航天器相对于太阳的多普勒径向速度作为量测量，利用与航天器位置间的数学联系建立太阳多普勒速度量测模型：

其中，v_r表示航天器相对太阳的径向速度量测量，r_ps,v_ps分别为地心惯性系下航天器相对于太阳的位置、速度矢量，r_ps＝||r_ps||表示位置矢量的大小，v_m表示量测噪声；

利用坐标变换将航天器相对太阳的位置矢量与INS中的状态量建立联系；

在地心惯性坐标系O-xyz中，r_ps、v_ps分别为地心惯性系下航天器相对于太阳的位置、速度矢量，r_pe、v_pe分别为航天器相对于地球的位置、速度矢量，则有

其中，

r_n、v_n为n系内航天器相对于地球的位置、速度矢量，δr_n、δv_n为位置和速度误差，由INS获得；r_se与v_se分别为太阳相对地球的位置和速度矢量，由STK工具获得；/>

是从n系到i系的转换矩阵，由此，将量测模型中的状态量与状态模型联系起来，则太阳多普勒速度量测模型表示为：

离散化后太阳多普勒速度量测模型表示为：

Z_k＝H(X_k,k)+V_k (7)

其中，H(g)表示太阳多普勒速度的非线性连续量测函数，V_k表示k时刻太阳多普勒速度的量测误差。

2.根据权利要求1所述的一种航天器惯性和太阳多普勒速度组合导航方法，其特征在于：所述第三步中，进行UKF滤波获得航天器的位置、速度及姿态估计如下：

离散后航天器惯性和太阳多普勒速度组合导航***的状态模型与量测模型为：

其中，F(X_k-1,k-1)为组合导航***非线性转移函数，H(X_k,k)为非线性量测函数，W_k-1及V_k分别表示过程与量测噪声，对式通过UKF进行滤波，获得航天器的后验状态估计

分别为航天器的姿态误差角，速度、位置误差，陀螺仪的常值漂移与加速度计的常值偏置，以及后验误差协方差/>

将/>

及/>

输出，同时将k时刻状态量与误差协方差的估计值返回UKF滤波器，用于获得k+1时刻的输出。

Images