CN117232534A - 一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法 - Google Patents

Abstract

本发明涉及一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，包括：根据卫星搭载的星敏感器数量进行星敏感器姿态融合，得到星敏感器融合姿态四元数；以陀螺仪采样时间为基准，对星敏感器融合姿态四元数进行插值处理，实现星敏感器姿态数据与陀螺仪测量数据时间同步，得到插值后融合姿态四元数；根据插值后融合姿态四元数构造系统量测值，以系统量测值和陀螺仪测量数据为输入，采用双向自适应滤波算法计算得到各时刻的卫星姿态估计值。本发明无需精确已知姿态敏感器测量噪声统计特性，在滤波的同时对星敏感器测量噪声方差、陀螺仪常值漂移误差方差及陀螺仪测量噪声方差进行实时统计，并采用双向滤波算法，实现航天器姿态精确测量。

Description

一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法

技术领域

本发明属于遥感卫星定姿技术领域，涉及一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法。

背景技术

姿态测量精度是确保航天器成功完成对地观测、科学探测等在轨飞行任务的重要因素，利用航天器搭载的各类姿态敏感器，可以测量航天器相对于某一参考基准的姿态信息。其中星敏感器通过观测恒星，建立航天器本体与惯性空间的方位联系，实现姿态确定。其具有自主性强，测量精度不随时间变化等优点。但受光学系统尺寸、探测器敏感性等因素限制，星敏感器的姿态测量精度最高仅为角秒级。陀螺仪作为一种惯性测量器件，可实时测量航天器相对惯性空间的角速度，具有输出频率高，瞬时测量精度高等优点，但存在测量结果随时间漂移的问题。为实现航天器高精度姿态确定，通常采用星敏感器与陀螺仪组合的测量模式，通过信息融合算法，集星敏感器不随时间漂移、陀螺仪瞬时精度高的优点为一体，获取航天器姿态信息。其中卡尔曼滤波为普遍采用的信息融合算法。该滤波算法本质上为基于姿态敏感器测量噪声统计特性的加权平均，因此星敏感器测量噪声方差及陀螺仪测量噪声方差的准确性是影响滤波效果的关键因素。在实际工程应用中，星敏感器与陀螺仪的测量噪声统计特性往往难以在实验室精确测定，另外航天器在轨飞行过程中，敏感器的测量特性随时间发生变化，因此需要采用一种自适应滤波算法，根据实际测量数据实时对测量噪声特性进行估计，减小对测量噪声统计特性的依赖性。

发明内容

本发明解决的技术问题是：针对组合定姿算法严重依赖星敏感器与陀螺仪测量误差统计特性的问题，提出了一种基于双向自适应滤波算法的星敏感器陀螺组合定姿方法，实现航天器姿态精确测量。

本发明解决技术的方案是：一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，包括以下步骤：

步骤1、根据卫星搭载的星敏感器数量进行星敏感器姿态融合，得到星敏感器融合姿态四元数；

步骤2、以陀螺仪采样时间为基准，对星敏感器融合姿态四元数进行插值处理，实现星敏感器姿态数据与陀螺仪测量数据时间同步，得到插值后融合姿态四元数；

步骤3、根据插值后融合姿态四元数构造系统量测值，以系统量测值和陀螺仪测量数据为输入，采用双向自适应滤波算法计算得到各时刻的卫星姿态估计值；所述双向自适应滤波算法为：以陀螺仪的离散采样时间顺序为正向，先后执行正向自适应滤波和反向滤波。

进一步的，步骤1中，如果卫星仅搭载一台星敏感器，则直接以该星敏感器姿态四元数作为星敏感器融合姿态四元数。

进一步的，步骤1中，如果卫星搭载两台星敏感器：

首先利用球面插值算法以第一星敏感器测量时间为基准，对第二星敏感器的姿态四元数进行插值计算；

然后利用第一星敏感器的姿态四元数和球面插值后第二星敏感器的姿态四元数，进行双矢量定姿，得到星敏感器融合姿态四元数。

进一步的，所述对第二星敏感器的姿态四元数进行插值计算，具体为：

其中，为球面插值算法的待插值时刻；为第二星敏感器相邻两次测量时刻；为插值后第二星敏感器的姿态四元数，为第二星敏感器时刻测量的姿态四元数，为第二星敏感器t₂ ²时刻测量的姿态四元数。

进一步的，所述进行双矢量定姿，得到星敏感器融合姿态四元数，具体为：

定义则星敏感器融合姿态四元数q_{star_0}表示为：

其中：

M_R＝[R₁ R₂ R₃]；M_S＝[S₁ S₂ S₃]；

其中：[q₁₁ q₁₂ q₁₃ q₁₄]^T为第一星敏感器的姿态四元数，[q₂₁ q₂₂ q₂₃ q₂₄]^T为插值后第二星敏感器的姿态四元数，M^S1、M^S2分别为第一星敏感器、第二星敏感器相对于卫星本体坐标系的安装矩阵。

进一步的，步骤2所述插值后融合姿态四元数的计算方式为：

t₁＜t_g＜t₂

θ＝cos^-1[q_{star_0}(t₁)·q_{star_0}(t₂)]

其中，t_g为陀螺仪采样时刻；t₁、t₂分别为星敏感器融合姿态在t_g前后的测量时刻；q_star(t_g)为插值后t_g时刻的星敏感器融合姿态四元数，全部采样时刻的插值后融合姿态四元数记为q_star。

进一步的，步骤3具体包括：

步骤3.1、构造卡尔曼滤波系统状态向量如下所示：

X＝[Δq₁ Δq₂ Δq₃ Δb_x Δb_y Δb_z]^T

其中，为误差四元数矢量部分，误差四元数 为四元数估计值，q为四元数真实值；[Δb_x Δb_y Δb_z]^T为陀螺仪常值漂移误差；

步骤3.2、建立离散化系统状态方程如下所示：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

其中，下标k-1、k为陀螺仪离散采样时刻；W_k-1为系统噪声，期望为0，系统噪声方差记为Q_k-1；Φ_k/k-1为k-1时刻至k时刻系统状态转移矩阵；Γ_k/k-1为k-1时刻至k时刻的噪声驱动矩阵；

步骤3.3、以插值后融合姿态四元数q_star为观测量建立系统量测方程：

Z_k＝Z′(1:3)

Z′(1:3)＝[I_3×3 0_3×3]X+V

其中，Z_k为系统量测值；q_star为插值后融合姿态四元数；q_k为利用陀螺仪角速度测量值预测得到的姿态四元数；表示四元数乘法；V为星敏感器测量噪声，期望为0，星敏感器测量噪声方差记为R；

步骤3.4，初始化卫星初始姿态估计值陀螺仪常值漂移估计值系统噪声方差Q₀、星敏感器测量噪声方差R₀、系统状态向量估计初值系统协方差初值P₀：

其中，为第一时刻的插值后融合姿态四元数；σ_b为陀螺仪常值漂移噪声中误差；σ_g为陀螺仪测量噪声中误差；σ_s为星敏感器姿态测量精度；p_q为卫星初始姿态测量方差；p_b为陀螺仪常值漂移方差；

执行正向自适应滤波处理，包括步骤3.5～步骤3.13：

步骤3.5、k从1开始，根据系统状态方程对系统状态向量、系统协方差进行一步预测：

其中，为k-1时刻的系统状态向量估计值，为利用预测得到的k时刻的系统状态向量估计值；P_k/k-1为通过k-1时刻的系统协方差P_k-1预测得到的k时刻的系统协方差；

步骤3.6、根据k-1时刻卫星姿态估计值以及k-1时刻陀螺仪测量角速度[ω_xω_y ω_z]^T计算k时刻卫星姿态预测值

其中，T为k-1时刻至k时刻时间间隔；[ω_x ω_y ω_z]^T为卫星本体坐标系下，陀螺仪测量的卫星三轴角速度；为k-1时刻陀螺仪常值漂移估计值；

步骤3.7、根据k时刻插值后融合姿态四元数q_star(k)以及k时刻卫星姿态预测值计算系统量测值Z_k：

Z_k＝Z′(1:3)

步骤3.8、计算卡尔曼滤波增益矩阵：

其中，H_k＝[I_3×3 0_3×3]；

步骤3.9、判断滤波收敛性，若滤波收敛，执行步骤3.10；若滤波发散，执行步骤3.11；

其中，滤波收敛性判断方法为：

其中，γ≥1为可调系数；Tr表示矩阵求迹运算；

步骤3.10、k时刻星敏感器测量噪声方差R、系统噪声方差Q自适应计算：

其中，R_max、R_min、Q_max、Q_min分别为设置的星敏感器测量噪声方差上下限及系统噪声方差上下限；

步骤3.11、使用强跟踪滤波方法重新计算P_k/k-1：

其中：

其中，为系统量测值预测值，β为弱化因子，ρ为遗忘因子；

步骤3.12、系统状态向量、系统协方差更新：

P_k＝(I_6×6-K_kH_k)P_k/k-1

其中，I_6×6为6×6单位矩阵；

步骤3.13、k加1，重复步骤3.5～步骤3.12，完成所有采样时刻测量数据正向滤波，存储每轮滤波得到的Φ_k/k-1、P_k、Pk/k-1及N为采样时刻总数；

步骤3.14，在完成正向滤波后，进行反向滤波：

其中，下标s表示正反向滤波平滑结果；下标f表示正向滤波结果；

步骤3.15、更新k时刻卫星姿态估计值及陀螺仪常值漂移估计值

其中，为正向滤波的k时刻卫星姿态预测值。

进一步的，在步骤3.2中，k-1时刻至k时刻系统状态转移矩阵Φ_k/k-1为：

Φ_k/k-1＝I_3×3+T·F_k

其中，I_3×3为3×3单位矩阵；

k-1时刻至k时刻的噪声驱动矩阵Γ_k/k-1为：

Γ_k/k-1＝T·B_k

进一步的，在步骤3.4中，σ_b、σ_g、σ_s根据陀螺仪、星敏感器测量特性先验值粗略确定；p_q、p_b根据σ_s、σ_b粗略确定：

进一步的，在步骤3.9中，R_max、R_min、Q_max、Q_min具体为：

R_c、Q_c分别表示星敏感器测量噪声方差先验估计值、陀螺仪测量噪声方差。

本发明与现有技术相比的有益效果是：

(1)本发明提供的基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，利用星敏感器姿态测量数据与陀螺仪角速度测量数据，无需精确已知姿态敏感器测量噪声统计特性，在滤波的同时对星敏感器姿态测量噪声方差、陀螺仪常值漂移误差方差及陀螺仪测量噪声方差进行实时统计，并采用双向滤波算法，实现航天器姿态精确测量。

附图说明

图1为本发明实施例基于双向自适应滤波的星敏感器陀螺仪组合定姿的处理流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步阐述。

实施例1

如图1所示，基于双向自适应滤波的星敏感器陀螺仪组合定姿方法的数据处理过程包括：多星敏融合、星敏姿态四元数插值、正向自适应滤波、反向滤波。

详细步骤如下：

步骤1、根据卫星搭载的星敏感器数量进行星敏感器姿态融合，得到星敏感器融合姿态四元数；

步骤2、以陀螺仪采样时间为基准，对星敏感器融合姿态四元数进行插值处理，实现星敏感器姿态数据与陀螺仪测量数据时间同步，得到插值后融合姿态四元数；

步骤3、根据插值后融合姿态四元数构造系统量测值，以系统量测值和陀螺仪测量数据为输入，采用双向自适应滤波算法计算得到各时刻的卫星姿态估计值；所述双向自适应滤波算法为：以陀螺仪的离散采样时间顺序为正向，先后执行正向自适应滤波和反向滤波。

步骤1具体包括以下步骤：

步骤1.1、如果卫星仅搭载一台星敏感器，则直接以该星敏感器姿态四元数作为星敏感器融合姿态四元数。

步骤1.2、如果卫星搭载两台星敏感器，则利用球面插值算法以第一星敏感器测量时间为基准，对第二星敏感器的姿态四元数进行插值计算。

步骤1.2计算公式如下所示：

其中，为球面插值算法的待插值时刻；为第二星敏感器相邻两次测量时刻；为插值后第二星敏感器的姿态四元数，为第二星敏感器时刻测量的姿态四元数，为第二星敏感器时刻测量的姿态四元数。

步骤1.3、利用第一星敏感器的姿态四元数和球面插值后第二星敏感器的姿态四元数，进行双矢量定姿，得到星敏感器融合姿态四元数。

步骤1.3进行双矢量定姿具体为：

定义则星敏感器融合姿态四元数q_{star_0}表示为：

设其中：

M_R＝[R₁ R₂ R₃]；M_S＝[S₁ S₂ S₃]；

其中：[q₁₁ q₁₂ q₁₃ q₁₄]^T为第一星敏感器的姿态四元数，[q₂₁ q₂₂ q₂₃ q₂₄]^T为插值后第二星敏感器的姿态四元数，M^S1、M^S2分别为第一星敏感器、第二星敏感器相对于卫星本体坐标系的安装矩阵。

步骤2、以陀螺仪采样时间为基准，对星敏感器融合姿态四元数q_{star_0}进行插值处理，实现星敏感器姿态数据与陀螺仪测量数据时间同步，得到插值后融合姿态四元数q_star。

步骤2具体计算方法为：

t₁＜t_g＜t₂

θ＝cos^-1[q_{star_0}(t₁)·q_{star_0}(t₂)]

其中，t_g为陀螺仪采样时刻；t₁、t₂分别为星敏感器融合姿态在t_g前后的测量时刻；q_star(t_g)为插值后t_g时刻的星敏感器融合姿态四元数，全部采样时刻的插值后融合姿态四元数记为q_star。

步骤3具体包括以下步骤：

步骤3.1、构造卡尔曼滤波系统状态向量如下所示：

X＝[Δq₁ Δq₂ Δq₃ Δb_x Δb_y Δb_z]^T

其中，为误差四元数矢量部分，误差四元数 为四元数估计值，q为四元数真实值；[Δb_x Δb_y Δb_z]^T为陀螺仪常值漂移误差。

步骤3.2、建立离散化系统状态方程如下所示：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

其中，下标k-1、k为陀螺仪离散采样时刻；W_k-1为系统噪声，其期望为0，系统噪声方差记为Q_k-1；Φ_k/k-1为k-1时刻至k时刻系统状态转移矩阵，计算方式为：

Φ_k/k-1＝I_3×3+T·F_k

其中，I_3×3为3×3单位矩阵；T为k-1时刻至k时刻时间间隔；[ω_x ω_y ω_z]^T为卫星本体坐标系下，陀螺仪测量的卫星三轴角速度；为陀螺仪常值漂移估计值；

Γ_k/k-1为k-1时刻至k时刻的噪声驱动矩阵，计算方式为：

Γ_k/k-1＝T·B_k

步骤3.3、以插值后融合姿态四元数q_star为观测量建立系统量测方程：

Z_k＝Z′(1:3)＝[I_3×3 0_3×3]X+V

其中，Z_k为系统量测值；q_star为插值后融合姿态四元数；q_k为利用陀螺仪角速度测量值预测得到的姿态四元数；表示四元数乘法；V为星敏感器测量噪声，其期望为0，星敏感器测量噪声方差记为R。

步骤3.4，对卫星初始姿态估计值陀螺仪常值漂移估计值系统噪声方差Q₀、星敏感器测量噪声方差R₀、系统状态向量估计初值系统协方差初值P₀进行初始化：

其中，为第一时刻的插值后融合姿态四元数；σ_b为陀螺仪常值漂移噪声中误差；σ_g为陀螺仪测量噪声中误差；σ_s为星敏感器姿态测量精度；p_q为卫星初始姿态测量方差；p_b为陀螺仪常值漂移方差。

σ_b、σ_g、σ_s可根据陀螺仪、星敏感器测量特性先验值粗略确定，无需精确值。p_q、p_b可根据σ_s、σ_b粗略确定，在一个实施例中，可以为：

执行正向自适应滤波处理，包括步骤3.5～步骤3.13：

步骤3.5、k从1开始，根据系统状态方程对系统状态向量、系统协方差进行一步预测：

其中，为k-1时刻的系统状态向量估计值，为利用预测得到的k时刻的系统状态向量估计值；P_k/k-1为通过k-1时刻的系统协方差P_k-1预测得到的k时刻的系统协方差。

步骤3.6、根据k-1时刻卫星姿态估计值以及k-1时刻陀螺仪测量角速度[ω_xω_y ω_z]^T计算k时刻卫星姿态预测值

其中，T为k-1时刻至k时刻时间间隔；为k-1时刻陀螺仪常值漂移估计值。

步骤3.7、根据k时刻插值后融合姿态四元数q_star(k)以及k时刻卫星姿态预测值计算系统量测值Z_k。

Z_k＝Z′(1:3)

步骤3.8、计算卡尔曼滤波增益矩阵：

其中，H_k＝[I_3×3 0_3×3]。

步骤3.9、判断滤波收敛性，若滤波收敛，执行步骤3.10；若滤波发散，执行步骤3.11。

滤波收敛性判断具体为：

其中，γ≥1为可调系数；Tr表示矩阵求迹运算。

步骤3.10、k时刻星敏感器测量噪声方差R、系统噪声方差Q自适应计算：

其中，R_max、R_min、Q_max、Q_min分别为设置的星敏感器测量噪声方差上下限及系统噪声方差上下限。可按如下公式取值：

R_c、Q_c分别表示星敏感器测量噪声方差先验估计值、陀螺仪测量噪声方差，无需精确值。

步骤3.11、使用强跟踪滤波方法重新计算P_k/k-1：

其中：

其中，为系统量测值预测值，β为弱化因子，满足β≥1；ρ为遗忘因子，满足0≤ρ≤1，一般取为0.95。

步骤3.12、系统状态向量、系统协方差更新：

P_k＝(I_6×6-K_kH_k)P_k/k-1

其中，I_6×6为6×6单位矩阵。

步骤3.13、k加1，重复步骤3.5～步骤3.12，完成所有采样时刻测量数据正向滤波，存储每轮滤波得到的Φ_k/k-1、P_k、Pk/k-1及N为采样时刻总数。

步骤3.14，在完成正向滤波后，进行反向滤波，正反向滤波公式为：

其中，下标s表示正反向滤波平滑结果；下标f表示正向滤波结果。

通过正反向滤波实现数据平滑，提高了滤波精度。

步骤3.15、更新k时刻卫星姿态估计值及陀螺仪常值漂移估计值

其中，为正向滤波的k时刻卫星姿态预测值。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，包括以下步骤：

步骤1、根据卫星搭载的星敏感器数量进行星敏感器姿态融合，得到星敏感器融合姿态四元数；

步骤2、以陀螺仪采样时间为基准，对星敏感器融合姿态四元数进行插值处理，实现星敏感器姿态数据与陀螺仪测量数据时间同步，得到插值后融合姿态四元数；

步骤3、根据插值后融合姿态四元数构造系统量测值，以系统量测值和陀螺仪测量数据为输入，采用双向自适应滤波算法计算得到各时刻的卫星姿态估计值；所述双向自适应滤波算法为：以陀螺仪的离散采样时间顺序为正向，先后执行正向自适应滤波和反向滤波。

2.根据权利要求1所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，步骤1中，如果卫星仅搭载一台星敏感器，则直接以该星敏感器姿态四元数作为星敏感器融合姿态四元数。

3.根据权利要求1所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，步骤1中，如果卫星搭载两台星敏感器：

首先利用球面插值算法以第一星敏感器测量时间为基准，对第二星敏感器的姿态四元数进行插值计算；

然后利用第一星敏感器的姿态四元数和球面插值后第二星敏感器的姿态四元数，进行双矢量定姿，得到星敏感器融合姿态四元数。

4.根据权利要求3所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，所述对第二星敏感器的姿态四元数进行插值计算，具体为：

其中，为球面插值算法的待插值时刻；为第二星敏感器相邻两次测量时刻；为插值后第二星敏感器的姿态四元数，为第二星敏感器时刻测量的姿态四元数，为第二星敏感器时刻测量的姿态四元数。

5.根据权利要求4所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，所述进行双矢量定姿，得到星敏感器融合姿态四元数，具体为：

定义则星敏感器融合姿态四元数q_{star_0}表示为：

其中：

M_R＝[R₁ R₂ R₃]；M_S＝[S₁ S₂ S₃]；

其中：[q₁₁ q₁₂ q₁₃ q₁₄]^T为第一星敏感器的姿态四元数，[q₂₁ q₂₂ q₂₃ q₂₄]^T为插值后第二星敏感器的姿态四元数，M^S1、M^S2分别为第一星敏感器、第二星敏感器相对于卫星本体坐标系的安装矩阵。

6.根据权利要求5所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，步骤2所述插值后融合姿态四元数的计算方式为：

t₁＜t_g＜t₂

θ＝cos^-1[q_{star_0}(t₁)·q_{star_0}(t₂)]

其中，t_g为陀螺仪采样时刻；t₁、t₂分别为星敏感器融合姿态在t_g前后的测量时刻；q_star(t_g)为插值后t_g时刻的星敏感器融合姿态四元数，全部采样时刻的插值后融合姿态四元数记为q_star。

7.根据权利要求6所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，步骤3具体包括：

步骤3.1、构造卡尔曼滤波系统状态向量如下所示：

X＝[Δq₁ Δq₂ Δq₃ Δb_x Δb_y Δb_z]^T

其中，为误差四元数矢量部分，误差四元数 为四元数估计值，q为四元数真实值；[Δb_x Δb_y Δb_z]^T为陀螺仪常值漂移误差；

步骤3.2、建立离散化系统状态方程如下所示：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

其中，下标k-1、k为陀螺仪离散采样时刻；W_k-1为系统噪声，期望为0，系统噪声方差记为Q_k-1；Φ_k/k-1为k-1时刻至k时刻系统状态转移矩阵；Γ_k/k-1为k-1时刻至k时刻的噪声驱动矩阵；

步骤3.3、以插值后融合姿态四元数q_star为观测量建立系统量测方程：

Z_k＝Z′(1:3)

Z′(1:3)＝[I_3×3 0_3×3]X+V

其中，Z_k为系统量测值；q_star为插值后融合姿态四元数；q_k为利用陀螺仪角速度测量值预测得到的姿态四元数；表示四元数乘法；V为星敏感器测量噪声，期望为0，星敏感器测量噪声方差记为R；

步骤3.4，初始化卫星初始姿态估计值陀螺仪常值漂移估计值系统噪声方差Q₀、星敏感器测量噪声方差R₀、系统状态向量估计初值系统协方差初值P₀：

其中，为第一时刻的插值后融合姿态四元数；σ_b为陀螺仪常值漂移噪声中误差；σ_g为陀螺仪测量噪声中误差；σ_s为星敏感器姿态测量精度；p_q为卫星初始姿态测量方差；p_b为陀螺仪常值漂移方差；

执行正向自适应滤波处理，包括步骤3.5～步骤3.13：

步骤3.5、k从1开始，根据系统状态方程对系统状态向量、系统协方差进行一步预测：

其中，为k-1时刻的系统状态向量估计值，为利用预测得到的k时刻的系统状态向量估计值；P_k/k-1为通过k-1时刻的系统协方差P_k-1预测得到的k时刻的系统协方差；

步骤3.6、根据k-1时刻卫星姿态估计值以及k-1时刻陀螺仪测量角速度[ω_x ω_yω_z]^T计算_k时刻卫星姿态预测值

其中，T为k-1时刻至_k时刻时间间隔；[ω_x ω_y ω_z]^T为卫星本体坐标系下，陀螺仪测量的卫星三轴角速度；为k-1时刻陀螺仪常值漂移估计值；

步骤3.7、根据k时刻插值后融合姿态四元数q_star(k)以及k时刻卫星姿态预测值计算系统量测值Z_k：

步骤3.8、计算卡尔曼滤波增益矩阵：

其中，H_k＝[I_3×3 0_3×3]；

步骤3.9、判断滤波收敛性，若滤波收敛，执行步骤3.10；若滤波发散，执行步骤3.11；

其中，滤波收敛性判断方法为：

其中，γ≥1为可调系数；Tr表示矩阵求迹运算；

步骤3.10、k时刻星敏感器测量噪声方差R、系统噪声方差Q自适应计算：

其中，R_max、R_min、Q_max、Q_min分别为设置的星敏感器测量噪声方差上下限及系统噪声方差上下限；

步骤3.11、使用强跟踪滤波方法重新计算P_k/k-1：

其中：

其中，为系统量测值预测值，β为弱化因子，ρ为遗忘因子；

步骤3.12、系统状态向量、系统协方差更新：

P_k＝(I_6×6-K_kH_k)P_k/k-1

其中，I_6×6为6×6单位矩阵；

步骤3.13、k加1，重复步骤3.5～步骤3.12，完成所有采样时刻测量数据正向滤波，存储每轮滤波得到的Φ_k/k-1、P_k、P_k/k-1及k＝1，2，…，N，N为采样时刻总数；

步骤3.14，在完成正向滤波后，进行反向滤波：

其中，下标s表示正反向滤波平滑结果；下标f表示正向滤波结果；

步骤3.15、更新k时刻卫星姿态估计值及陀螺仪常值漂移估计值

其中，为正向滤波的k时刻卫星姿态预测值。

8.根据权利要求7所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，在步骤3.2中，k-1时刻至k时刻系统状态转移矩阵Φ_k/k-1为：

Φ_k/k-1＝I_3×3+T·F_k

其中，I_3×3为3×3单位矩阵；

k-1时刻至k时刻的噪声驱动矩阵Γ_k/k-1为：

Γ_k/k-1＝T·B_k

9.根据权利要求7所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，在步骤3.4中，σ_b、σ_g、σ_s根据陀螺仪、星敏感器测量特性先验值粗略确定；p_q、p_b根据σ_s、σ_b粗略确定：

10.根据权利要求7所述的一种基于双向自适应滤波的星敏感器陀螺仪组合定姿方法，其特征在于，在步骤3.9中，R_max、R_min、Q_max、Q_min具体为：

R_c、Q_c分别表示星敏感器测量噪声方差先验估计值、陀螺仪测量噪声方差。