CN105486312A - 一种星敏感器与高频角位移传感器组合定姿方法及*** - Google Patents

Abstract

本发明提供了一种星敏感器与高频角位移传感器组合定姿方法及***，包括根据角位移传感器测量原理构建其误差测量模型，充分考虑其观测值存在的各种误差源；基于多星敏感器信息融合结果构建***量测方程；根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程；最后采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。本发明可以实现低频星敏感器与高频角位移传感器最优信息融合，得到高频高精度的姿态数据，从而为高分辨率光学影像高精度几何处理奠定基础。

Description

一种星敏感器与高频角位移传感器组合定姿方法及***

技术领域

本发明属于遥感卫星地面预处理领域，特别是涉及到一种星敏感器与高频角位移传感器组合定姿方法及***。

背景技术

高分辨率光学卫星是我国高分辨率对地观测***发展的重要研究领域之一，未来十年我国将发射数颗空间分辨率优于1米的光学卫星(简称亚米级光学卫星)，最高空间分辨率甚至可以达到0.1米左右。但是，更高的空间分辨率并不意味着具有更高的几何相对精度。由于高分辨率光学卫星推扫成像频率为上万赫兹，而传统的星敏感器与陀螺组合定姿方式输出的姿态频率为几赫兹到几十赫兹，很难满足其高精度几何处理需求。本发明针对该问题，提出一种低频星敏感器与高频角位移传感器组合定姿方法。近年来，角位移传感器作为一种新型姿态敏感器逐渐搭载在一系列高分辨率光学卫星上进行在轨使用，由于其可以输出上万赫兹的角增量姿态数据，将其与低频星敏感器进行最优信息融合可以得到高频姿态数据，以满足高分辨率光学影像高精度几何处理要求。

发明内容

本发明针对当前低频姿态数据难以满足高分辨率光学卫星影像几何处理要求的问题，提供了一种基于低频星敏感器与高频角位移传感器高精度组合定姿技术方案。

本发明提供的技术方案为一种星敏感器与高频角位移传感器组合定姿方法，包括以下步骤：

步骤1，构建角位移传感器测量模型如下，

ω_g＝(1+Δ+Λ)ω+b+η_g

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声；

步骤2，基于多星敏感器信息融合结果构建***量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{{}_{9\×9}},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列；

步骤3，根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程，实现如下，

根据姿态运动学方程得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程如下，

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

W(t)＝[-0.5η_gη_b]^T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移；

对以上***状态方程进行线性化离散化得到，

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

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列；

步骤4，基于步骤2和步骤3分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

而且，步骤3中姿态运动学方程表示如下姿态运动学方程表示如下，

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿态四元数与三轴旋转角速度，q＝[q₀q₁q₂q₃]^T，ω＝(ω_x,ω_y,ω_z)。

而且，步骤4实现如下，

(1)基于角位移测量信息进行姿态预测

基于角位移测量信息在t_k-1时刻对下式进行积分，得到卫星姿态四元数预测值

${\stackrel{\·}{\hat{q}}}_{k/k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗{\widehat{\mathrm{\ω}}}_{k-1}$

其中，表示t_k-1时刻四元数最优估计值，表示t_k-1时刻角速度最优估计值；

角位移漂移预测值的预测模型如下，

${\hat{b}}_{k/k-1}={\hat{b}}_{k-1}$

其中，表示t_k-1时刻角位移漂移最优估计值；

对于误差协方差阵的预测值解算模型如下，

${\hat{P}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{P}}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，表示t_k-1时刻误差协方差阵估计值，Q_k-1表示***干扰方差阵；

(2)基于星敏感器测量值进行姿态修正

在t_k时刻，根据***量测方程计算观测矩阵H_k，进一步计算滤波增益K_k，

$K_k=P_{k/k-1}{H}_k^T{\[H_k{P}_{k/k-1}{H}_k^T+R_k\]}^{-1}$

其中，P_k/k-1表示误差协方差阵预测值，则相应的***状态变量为，

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

卫星姿态四元数t_k时刻的修正更新值与角位移漂移的修正更新值分别为，

${\hat{q}}_k={\hat{q}}_{k/k-1}\⊗\mathrm{\Δ}{\hat{q}}_k$

${\hat{b}}_k={\hat{b}}_{k/k-1}+\mathrm{\Δ}{\hat{b}}_k$

其中，表示误差四元数估计值，表示误差漂移量估计值；

误差协方差阵P_k的更新计算为，

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

其中，R_k表示星敏感器量测噪声方差阵，I表示单位矩阵，H_k采用修正更新后的状态变量重新进行计算；

根据上述过程(1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反向滤波结果进行整体加权平差。

本发明还提供一种星敏感器与高频角位移传感器组合定姿***，包括以下模块：

第一模块，用于构建角位移传感器测量模型如下，

ω_g＝(1+Δ+Λ)ω+b+η_g

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声；

第二模块，用于基于多星敏感器信息融合结果构建***量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{{}_{9\×9}},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列；

第三模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程，实现如下，

根据姿态运动学方程得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程如下，

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

W(t)＝[-0.5η_gη_b]^T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移；

对以上***状态方程进行线性化离散化得到，

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

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列；

第四模块，用于基于第二模块和第三模块分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

而且，第三模块中姿态运动学方程表示如下姿态运动学方程表示如下，

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿态四元数与三轴旋转角速度，q＝[q₀q₁q₂q₃]^T，ω＝(ω_x,ω_y,ω_z)。

而且，第四模块实现如下，

(1)基于角位移测量信息进行姿态预测

基于角位移测量信息在t_k-1时刻对下式进行积分，得到卫星姿态四元数预测值

${\stackrel{\·}{\hat{q}}}_{k/k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗{\widehat{\mathrm{\ω}}}_{k-1}$

其中，表示t_k-1时刻四元数最优估计值，表示t_k-1时刻角速度最优估计值；

角位移漂移预测值的预测模型如下，

${\hat{b}}_{k/k-1}={\hat{b}}_{k-1}$

其中，表示t_k-1时刻角位移漂移最优估计值；

对于误差协方差阵的预测值解算模型如下，

${\hat{P}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{P}}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，表示t_k-1时刻误差协方差阵估计值，Q_k-1表示***干扰方差阵；

(2)基于星敏感器测量值进行姿态修正

在t_k时刻，根据***量测方程计算观测矩阵H_k，进一步计算滤波增益K_k，

$K_k=P_{k/k-1}{H}_k^T{\[H_k{P}_{k/k-1}{H}_k^T+R_k\]}^{-1}$

其中，P_k/k-1表示误差协方差阵预测值，则相应的***状态变量为，

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

卫星姿态四元数t_k时刻的修正更新值与角位移漂移的修正更新值分别为，

${\hat{q}}_k={\hat{q}}_{k/k-1}\⊗\mathrm{\Δ}{\hat{q}}_k$

${\hat{b}}_k={\hat{b}}_{k/k-1}+\mathrm{\Δ}{\hat{b}}_{k-1}$

其中，表示误差四元数估计值，表示误差漂移量估计值；

误差协方差阵P_k的更新计算为，

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

其中，R_k表示星敏感器量测噪声方差阵，I表示单位矩阵，H_k采用修正更新后的状态变量重新进行计算；

根据上述过程(1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反向滤波结果进行整体加权平差。

本发明提供了一种低频星敏感器与高频角位移传感器组合定姿技术方案，实现了星敏感器高精度定姿优势与角位移传感器高频率角增量输出优势互补，通过本发明提供的技术方案可以得到高频率、高精度的卫星姿态数据，为高分辨率光学卫星高精度几何处理奠定基础。

附图说明

图1为本发明实施例的流程图。

具体实施方式

以下结合附图和实施例详细说明本发明技术方案。

参见图1所示星敏感器与高频角位移传感器组合定姿技术流程图，以下针对实施例流程中的各步骤，对本发明方法做进一步详细描述。

步骤1，根据角位移传感器工作原理，构建合理的误差测量模型，充分考虑其测量值中存在的各种误差源。

角位移传感器作为一种惯性姿态传感器，可以直接输出高频的角增量数据，得到的测量数据含义是该采样时刻相比上一采样时刻时间间隔内所产生的角增量大小。根据上述原理得到：假设角位移传感器数据采样周期为T，某一时刻t的卫星三轴角位移观测值分别为N_X,N_y,N_z，则得到该时刻t的三轴角速度ω_x,ω_y,ω_z，具体展开如下：

${\mathrm{\ω}}_x=\frac{N_X}{T},{\mathrm{\ω}}_y=\frac{N_Y}{T},{\mathrm{\ω}}_z=\frac{N_Z}{T}---\left(1\right)$

由于角位移传感器在轨工作过程中存在的误差项包括安装误差、标度因数误差、漂移误差以及测量噪声等，故进一步得到角位移传感器的误差测量模型如下：

ω_g＝(1+Δ+Λ)ω+b+η_g(2)

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声。这里假设角位移传感器测量噪声为高斯白噪声，即σ_g表示测量噪声中误差。

进一步假设角位移传感器的漂移b由白噪声驱动，表示为：

$\stackrel{\·}{b}={\mathrm{\η}}_b---\left(3\right)$

其中，表示漂移量的微分值，η_b为角位移传感器与时间相关的噪声，满足σ_b表示漂移噪声中误差。

步骤2，基于多星敏感器信息融合结果构建***量测方程。

本发明基于多星敏感器光轴矢量观测值实现多星敏感器最优信息融合，进一步构建***量测方程，具体过程如下：

假设一组正交矢量在本体坐标系、惯性坐标系下表达方式如下：

$\left\{\begin{array}{c}{l}_b^1={\[x_b^1,y_b^1,z_b^1\]}^T\\ l_b^2={\[x_b^2,y_b^2,z_b^2\]}^T\\ l_b^3={\[x_b^3,y_b^3,z_b^3\]}^T\end{array}\right.,\left\{\begin{array}{c}{l}_{mi}^1={\[x_{mi}^1,y_{mi}^1,z_{mi}^1\]}^T\\ l_{mi}^2={\[x_{mi}^2,y_{mi}^2,z_{mi}^2\]}^T\\ l_{mi}^3={\[x_{mi}^3,y_{mi}^3,z_{mi}^3\]}^T\end{array}\right.,\left\{\begin{array}{c}{l}_i^1={\[x_i^1,y_i^1,z_i^1\]}^T\\ l_i^2={\[x_i^2,y_i^2,z_i^2\]}^T\\ l_i^3={\[x_i^3,y_i^3,z_i^3\]}^T\end{array}\right.---\left(4\right)$

其中，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值。

进一步根据推导得到量测方程如下：

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{{}_{9\×9}},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}---\left(5\right)$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列，且满足： $\left\{\begin{array}{c}E\left(V_k\right)=0\\ E\left(V_k{V}_j^T\right)=R_k{\mathrm{\δ}}_{kj}\end{array}\right.,$ E(V_k)表示观测噪声序列平均值，表示不同时刻之间观测噪声序列协方差，表示t_j时刻观测噪声矩阵转置，δ_kj表示狄克莱函数；R_k表示星敏感器量测噪声方差阵。

步骤3，基于姿态运动学方程以及角位移传感器误差测量模型构建***的状态方程。

本发明根据姿态运动学方程以及角位移传感器测量模型进一步构建状态方程，具体过程如下：

姿态运动学方程表示如下：

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]---\left(6\right)$

其中，表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿态四元数与三轴旋转角速度，q＝[q₀q₁q₂q₃]^T，ω＝(ω_x,ω_y,ω_z)。

进一步推导得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程：

$\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)\\ F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\\ W\left(t\right)={\left[\begin{array}{cc}-0.5{\mathrm{\η}}_g& {\mathrm{\η}}_b\end{array}\right]}^T\end{array}---\left(7\right)$

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移，以上得到的状态方程为连续动态***滤波方程，需要进行线性化和离散化，故进行线性化离散化得到：

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1(8)

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列。

步骤4，基于步骤2和步骤3分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

由于星敏感器输出频率为几赫兹到几十赫兹，而角位移传感器输出频率达到上万赫兹，以往的组合定姿滤波器设计已经无法满足实际需要。本发明针对该问题提出一种双向卡尔曼滤波与整体加权平差相结合的模型算法实现多频多种姿态敏感器最优信息融合。

(1)基于角位移测量信息进行姿态预测

基于角位移测量信息在t_k-1时刻对下式进行积分，可以得到卫星姿态四元数预测值

${\stackrel{\·}{\hat{q}}}_{k/k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗{\widehat{\mathrm{\ω}}}_{k-1}---\left(9\right)$

其中，表示t_k-1时刻四元数最优估计值，表示t_k-1时刻角速度最优估计值。

角位移漂移预测值的预测模型如下：

${\hat{b}}_{k/k-1}={\hat{b}}_{k-1}---\left(10\right)$

其中，表示t_k-1时刻角位移漂移最优估计值。

对于误差协方差阵的预测值解算模型如下：

${\hat{P}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{P}}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(11\right)$

其中，表示t_k-1时刻误差协方差阵估计值，Q_k-1表示***干扰方差阵。

(2)基于星敏感器测量值进行姿态修正

在t_k时刻，根据步骤2量测方程计算观测矩阵H_k。进一步计算滤波增益K_k：

$K_k=P_{k/k-1}{H}_k^T{\[H_k{P}_{k/k-1}{H}_k^T+R_k\]}^{-1}---\left(12\right)$

其中，P_k/k-1表示误差协方差阵预测值，则相应的***状态变量为：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(13\right)$

卫星姿态四元数t_k时刻的修正更新值与角位移漂移的修正更新值分别为：

$\begin{array}{c}{\hat{q}}_k={\hat{q}}_{k/k-1}\⊗\mathrm{\Δ}{\hat{q}}_k\\ {\hat{b}}_k={\hat{b}}_{k/k-1}+\mathrm{\Δ}{\hat{b}}_k\end{array}---\left(14\right)$

其中，表示误差四元数估计值，表示误差漂移量估计值。

误差协方差阵P_k的更新计算为：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(15\right)$

其中，R_k表示星敏感器量测噪声方差阵，I表示单位矩阵，H_k采用修正更新后的状态变量重新进行计算，这样做的目的是使滤波过程具有更好的收敛性。在校正了卫星姿态四元数和角位移的漂移后，状态变量需要重新设置为零。

根据上述过程进行初始滤波、反向滤波以及正向滤波，(1)到(2)为初次滤波；(2)到(1)为反向滤波；最后(1)到(2)为正向滤波，每次滤波的初始值为上次滤波的结果；最终基于正向滤波与反向滤波结果进行整体加权平差。

$\begin{array}{c}{\hat{q}}_{fb}\left(k\right)={{\hat{q}}_b}^{-1}\left(k\right)\⊗{\hat{q}}_f\left(k\right)\\ \mathrm{\Δ}{\hat{x}}_{fb}\left(k\right)={\left[\begin{array}{cc}\mathrm{sgn}\left({\hat{q}}_{fb0}\right)\left[\begin{array}{ccc}{\hat{q}}_{fb1}& {\hat{q}}_{fb2}& {\hat{q}}_{fb3}\end{array}\right]& {\left({\hat{b}}_f\right(k)-{\hat{b}}_b(k\left)\right)}^T\end{array}\right]}^T\\ {\hat{P}}_s\left(k\right)={\left({{\hat{P}}_f}^{-1}\right(k)+{{\hat{P}}_b}^{-1}(k\left)\right)}^{-1}\\ \mathrm{\Δ}{\hat{x}}_s\left(k\right)={\left[\begin{array}{cc}\mathrm{\Δ}{{\widehat{\stackrel{\→}{q}}}_s}^T\left(k\right)& \mathrm{\Δ}{{\hat{b}}_s}^T\left(k\right)\end{array}\right]}^T={\hat{P}}_s\left(k\right){{\hat{P}}_f}^{-1}\left(k\right)\mathrm{\Δ}{\hat{x}}_{fb}\left(k\right)\\ \mathrm{\Δ}{\hat{q}}_{s0}\left(k\right)=sqrt(1-\mathrm{\Δ}{{\hat{q}}_{s1}}^T(k\left)\mathrm{\Δ}{{\hat{q}}_{s1}}^T\right(k)-\mathrm{\Δ}{{\hat{q}}_{s2}}^T(k\left)\mathrm{\Δ}{{\hat{q}}_{s2}}^T\right(k)-\mathrm{\Δ}{{\hat{q}}_{s3}}^T(k\left)\mathrm{\Δ}{{\hat{q}}_{s3}}^T\right(k\left)\right)\\ \begin{array}{cc}{\hat{q}}_s\left(k\right)={\hat{q}}_b\left(k\right)\⊗\mathrm{\Δ}{\hat{q}}_s\left(k\right)& {\hat{b}}_s\left(k\right)={\hat{b}}_b\left(k\right)+\mathrm{\Δ}{\hat{b}}_s\left(k\right)\end{array}\end{array}---\left(16\right)$

根符号“f”表示正向滤波结果，根符号“b”表示反向滤波结果，根符号“s”表示整体加权平差结果，

表示t_k时刻正向滤波姿态最优估计值，表示t_k时刻反向滤波姿态最优估计值的逆，表示t_k时刻正反向滤波结果四元数误差； ${\hat{q}}_{fb}\left(k\right)={\left[\begin{array}{cccc}{\hat{q}}_{fb0}& {\hat{q}}_{fb1}& {\hat{q}}_{fb2}& {\hat{q}}_{fb3}\end{array}\right]}^T;$

表示正反向状态量滤波结果误差矩阵，表示返回标量正负号， 表示t_k时刻漂移正向与反向滤波最优估计值，

与分别表示误差协方差阵正反向最优估计值的逆，表示整理加权的误差协方差阵估计值；

表示正反向状态量滤波结果误差矩阵加权平均值，表示误差四元数整体加权平均矢量部分结果，表示误差漂移整体加权平均结果；

表示误差四元数整体加权结果，表示误差漂移整体加权结果；

表示t_k时刻四元数整体加权结果， ${\hat{q}}_s\left(k\right)={\left[\begin{array}{cccc}\mathrm{\Δ}{\hat{q}}_{s0}\left(k\right)& \mathrm{\Δ}{{\hat{q}}_{s1}}^T\left(k\right)& \mathrm{\Δ}{{\hat{q}}_{s2}}^T\left(k\right)& \mathrm{\Δ}{{\hat{q}}_{s3}}^T\left(k\right)\end{array}\right]}^T,$ 表示t_k时刻漂移量整体加权结果。

具体实施时，本发明所提供方法可基于软件技术实现自动运行流程，也可采用模块化方式实现相应***。本发明实施例还提供一种星敏感器与高频角位移传感器组合定姿***，包括以下模块：

第一模块，用于构建角位移传感器测量模型如下，

ω_g＝(1+Δ+Λ)ω+b+η_g

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声；

第二模块，用于基于多星敏感器信息融合结果构建***量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{{}_{9\×9}},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列；

第三模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程，实现如下，

根据姿态运动学方程得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程如下，

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

W(t)＝[-0.5η_gη_b]^T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移；

对以上***状态方程进行线性化离散化得到，

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

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列；

第四模块，用于基于第二模块和第三模块分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

各模块具体实现可参见相应步骤，本发明不予赘述。

本文中所描述的具体实例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (6)

1.一种星敏感器与高频角位移传感器组合定姿方法，其特征在于，包括以下步骤：

步骤1，构建角位移传感器测量模型如下，

ω_g＝(1+Δ+Λ)ω+b+η_g

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声；

步骤2，基于多星敏感器信息融合结果构建***量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{9\×9},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列；

步骤3，根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程，实现如下，

根据姿态运动学方程得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程如下，

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

W(t)＝[-0.5η_gη_b]^T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移；

对以上***状态方程进行线性化离散化得到，

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

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列；

步骤4，基于步骤2和步骤3分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

2.根据权利要求1所述星敏感器与高频角位移传感器组合定姿方法，其特征在于：步骤3中姿态运动学方程表示如下姿态运动学方程表示如下，

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿态四元数与三轴旋转角速度，q＝[q₀q₁q₂q₃]^T，ω＝(ω_x,ω_y,ω_z)。

3.根据权利要求1或2所述星敏感器与高频角位移传感器组合定姿方法，其特征在于：步骤4实现如下，

(1)基于角位移测量信息进行姿态预测

基于角位移测量信息在t_k-1时刻对下式进行积分，得到卫星姿态四元数预测值

${\stackrel{\·}{\hat{q}}}_{k/k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗{\widehat{\mathrm{\ω}}}_{k-1}$

其中，表示t_k-1时刻四元数最优估计值，表示t_k-1时刻角速度最优估计值；

角位移漂移预测值的预测模型如下，

${\hat{b}}_{k/k-1}={\hat{b}}_{k-1}$

其中，表示t_k-1时刻角位移漂移最优估计值；

对于误差协方差阵的预测值解算模型如下，

${\hat{P}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{P}}_{k-1}={\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，表示t_k-1时刻误差协方差阵估计值，Q_k-1表示***干扰方差阵；

(2)基于星敏感器测量值进行姿态修正

在t_k时刻，根据***量测方程计算观测矩阵H_k，进一步计算滤波增益K_k，

$K_k=P_{k/k-1}{H}_k^T{\[H_k{P}_{k/k-1}{H}_k^T+R_k\]}^{-1}$

其中，P_k/k-1表示误差协方差阵预测值，则相应的***状态变量为，

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

卫星姿态四元数t_k时刻的修正更新值与角位移漂移的修正更新值分别为，

${\hat{q}}_k={\hat{q}}_{k/k-1}\⊗\mathrm{\Δ}{\hat{q}}_k$

${\hat{b}}_k={\hat{b}}_{k/k-1}+\mathrm{\Δ}{\hat{b}}_k$

其中，表示误差四元数估计值，表示误差漂移量估计值；

误差协方差阵P_k的更新计算为，

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

其中，R_k表示星敏感器量测噪声方差阵，I表示单位矩阵，H_k采用修正更新后的状态变量重新进行计算；

根据上述过程(1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反向滤波结果进行整体加权平差。

4.一种星敏感器与高频角位移传感器组合定姿***，其特征在于，包括以下模块：

第一模块，用于构建角位移传感器测量模型如下，

ω_g＝(1+Δ+Λ)ω+b+η_g

其中，ω_g为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移传感器漂移大小，η_g为角位移传感器测量噪声；

第二模块，用于基于多星敏感器信息融合结果构建***量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Z_k＝H_kX_k+V_k

$H_k={\left[\begin{array}{cc}-2A_{bi}^T\left(\hat{q}\right)\[l_b^1\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^2\×\]& 0_{3\×3}\\ -2A_{bi}^T\left(\hat{q}\right)\[l_b^3\×\]& 0_{3\×3}\end{array}\right]}_{9\×9},Z_k={\left[\begin{array}{c}{l}_{mi}^1-A_{bi}^T\left(\hat{q}\right)l_b^1\\ l_{mi}^2-A_{bi}^T\left(\hat{q}\right)l_b^2\\ l_{mi}^3-A_{bi}^T\left(\hat{q}\right)l_b^3\end{array}\right]}_{9\×1}$

其中，Z_k表示观测矢量，H_k表示观测矩阵，X_k表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，V_k表示t_k时刻观测噪声序列；

第三模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建***状态方程，实现如下，

根据姿态运动学方程得到基于状态 $X_{6\×1}={\left[\begin{array}{cc}\mathrm{\Δ}{\stackrel{\→}{q}}^T& {\mathrm{\Δb}}^T\end{array}\right]}^T$ 的***状态方程如下，

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{cc}-\[\widehat{\mathrm{\ω}}\×\]& -0.5I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

W(t)＝[-0.5η_gη_b]^T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示***噪声序列，表示角速度矢量叉乘，I_3×3表示单位矩阵，0_3×3表示元素为0的矩阵，表示误差四元数矢量部分，Δb^T表示误差漂移；

对以上***状态方程进行线性化离散化得到，

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

其中，X_k、X_k-1表示t_k时刻、t_k-1时刻状态变量，Φ_k,k-1表示状态转移矩阵，Γ_k-1表示***噪声驱动阵，W_k-1表示***噪声序列；

第四模块，用于基于第二模块和第三模块分别构建的***量测方程和***状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。

5.根据权利要求4所述星敏感器与高频角位移传感器组合定姿方法，其特征在于：第三模块中姿态运动学方程表示如下姿态运动学方程表示如下，

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿态四元数与三轴旋转角速度，q＝[q₀q₁q₂q₃]^T，ω＝(ω_x,ω_y,ω_z)。

6.根据权利要求4或5所述星敏感器与高频角位移传感器组合定姿方法，其特征在于：第四模块实现如下，

(1)基于角位移测量信息进行姿态预测

基于角位移测量信息在t_k-1时刻对下式进行积分，得到卫星姿态四元数预测值

其中，表示t_k-1时刻四元数最优估计值，表示t_k-1时刻角速度最优估计值；

角位移漂移预测值的预测模型如下，

${\hat{b}}_{k/k-1}={\hat{b}}_{k-1}$

其中，表示t_k-1时刻角位移漂移最优估计值；

对于误差协方差阵的预测值解算模型如下，

${\hat{P}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{P}}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，表示t_k-1时刻误差协方差阵估计值，Q_k-1表示***干扰方差阵；

(2)基于星敏感器测量值进行姿态修正

在t_k时刻，根据***量测方程计算观测矩阵H_k，进一步计算滤波增益K_k，

$K_k=P_{k/k-1}{H}_k^T{\[H_k{P}_{k/k-1}{H}_k^T+R_k\]}^{-1}$

其中，P_k/k-1表示误差协方差阵预测值，则相应的***状态变量为，

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

卫星姿态四元数t_k时刻的修正更新值与角位移漂移的修正更新值分别为，

${\hat{q}}_k={\hat{q}}_{k/k-1}\⊗\mathrm{\Δ}{\hat{q}}_k$

${\hat{b}}_k={\hat{b}}_{k/k-1}+\mathrm{\Δ}{\hat{b}}_k$

其中，表示误差四元数估计值，表示误差漂移量估计值；

误差协方差阵P_k的更新计算为，

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

其中，R_k表示星敏感器量测噪声方差阵，I表示单位矩阵，H_k采用修正更新后的状态变量重新进行计算；

根据上述过程(1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反向滤波结果进行整体加权平差。