CN111678512B - 一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法 - Google Patents

Abstract

本发明涉及一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，步骤如下：首先根据星敏感器和陀螺特性，结合四元数方法建立卫星姿态确定的状态方程和量测方程，其次引入因子图方法构建卫星姿态确定的因子图模型，最后引入贝叶斯网络和贝叶斯树理论，设计基于因子图的增量平滑优化算法，从而得到高精度的卫星姿态信息。本发明有效的结合了因子图和四元数方法来解决卫星姿态确定问题，克服了传统滤波方法存在的估计稳定性和实时性等缺点，并且该方法只需建立量测方程，并且可以直接处理非线性系统，简化姿态确定优化过程，获得更高精度的姿态确定结果。适用于高精度姿态确定领域，可为航空航天等研究领域提供一定的技术支持。

Description

一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法

技术领域

本发明涉及一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，可以为卫星姿态确定提供一种新的方法，并且可为航天航空等领域提供一定的技术支持，属于航天器制导、导航与控制技术领域。

背景技术

20世纪中期以来，航天技术快速发展并日趋成熟，提升进入太空与开发利用的能力已成为各航天大国的国家战略。在新的空间应用需求的推动下，小卫星编队飞行已成为航天领域追逐的热点。与传统的大卫星相比，小卫星集群具备研发、维护成本低，射方式灵活，自主性和适应性高等优点，能实现一颗大卫星能完成的，或者无法实现的功能，具有广阔的发展前景。在卫星系统中姿态控制系统是其主要子系统，而姿态确定则是姿态控制系统的核心技术之一，通过姿态自主确定可以保证卫星实现高精准的姿态导航，从而为卫星后续的精确控制提供保障。但是由于小卫星采用低功耗、精度较低的MEMS测量敏感器进行姿态的测量，从而可能导致卫星的姿态确定精度下降，影响卫星的运行性能，则从软件层面解决卫星姿态的精度是必须的。

为了获取卫星的姿态信息，目前常见的卫星姿态确定方法有矢量确定性方法，滤波估计方法等。由于单一传感器可靠性较低，精度差，因此往往采用多传感器组合的方法。采用多传感器融合手段进行姿态确定，可以解决单一导航源姿态确定精度不高的问题，提高姿态确定系统的可靠性和鲁棒性。但事实上，当下卫星系统其单个体正朝着小型化、低成本的方向发展，单个体配置的姿态确定传感器需要进一步低成本化。此外，由于卫星系统中单个体的姿态确定传感器信息更新频率不同，从而大大限制了传统的且对同步性要求高的姿态确定滤波算法在卫星系统中的应用。因此，为了使卫星系统具有高度的自主性和可靠性，需要研究新的基于多源信息融合的卫星姿态确定方法。

现有的关于星敏感器和陀螺组合的卫星姿态确定方法的技术，尤政等人申请发明专利“基于星敏感器的卫星姿态确定方法及系统”，该方法采用通过多个星敏感器和陀螺仪采集的卫星姿态信息，并通过卡尔曼滤波器对有效的卫星姿态信息进行局部状态估计以获得多个局部最优状态估计，但是姿态确定精度一般。现有的关于基于因子图的卫星姿态确定方法的技术，暂无发明专利，而关于因子图导航方法的技术，王慧哲等人申请发明专利“一种基于因子图的多源导航信息融合方法”，张涛等人申请发明专利“一种基于因子图的多源信息融合方法”，这些方法将因子图与多源信息融合算法结合起来，但是优化过程中，计算量过大。

发明内容

为了解决上述背景所提出的技术问题，本发明提供一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，可以为卫星姿态确定提供一种新的方法，能够快速准确的为卫星提供实时姿态信息，并且可为航天航空等领域提供一定的技术支持。

为实现上述目的，本发明提供的一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，包括以下步骤：

步骤(1)、根据星敏感器和陀螺特性，以星敏感器的测量值作为基准，对陀螺的测量信息进行修正，根据星敏感器、陀螺的测量值，结合四元数方法构建卫星姿态估计方程；

步骤(2)、根据步骤(1)所构建的卫星姿态估计方程，结合因子图方法，建立星敏感器因子和陀螺因子，从而构建基于因子图的星敏感器和陀螺组合卫星姿态确定系统；

步骤(3)、根据步骤(2)所构建的基于因子图的星敏感器和陀螺组合卫星姿态确定系统，引入贝叶斯网络和贝叶斯树理论，提出了基于因子图的增量平滑算法，从而获得较高精度的卫星姿态信息。

进一步地，步骤(1)的实现过程如下：

(2a)采用星敏感器补偿陀螺漂移，得到卫星姿态确定的状态方程为：

其中，Q_e为姿态误差四元数的矢量部分，

b为陀螺的真值，

为陀螺漂移的估计值。ω＝[ω₁ ω₂ ω₃]^T表示卫星角速度，且

I₃为3×3的单位矩阵，v_g，v_b是不相关零均值高斯白噪声。

(2b)令δq＝Q_e，设姿态确定系统的状态矢量为X_k＝[δq Δb]^T＝[q_e1 q_e2 q_e3 Δb₁Δb₂ Δb₃]^T，卫星姿态估计得测量值由星敏感器的输出来获得，则量测方程为：

Z_k＝H_kX_k+D_kV_k

其中，H_k＝[I_3×3 0_3×3]，V_k＝[v_sc1 v_sc2 v_sc3]表示星敏感器的量测噪声。

进一步地，步骤(2)的实现如下：

(3a)由于陀螺的测量频率高于星敏感器的测量频率，因此以星敏感器建立状态的连接，则k+1时刻和k时刻之间的雅克比矩阵可表示为：

J_t+Δt＝(I+F·Δt)J_t

其中，

雅克比矩阵J_t的初始值为单位矩阵，Δt为陀螺仪测量输出的时间间隔。

(3b)利用雅克比矩阵建立k+1时刻和k时刻之间状态的关系可表示为：

X_k+1＝J_stateX_k

其中，

(3c)由于

则有：

则可以得到陀螺因子f^GYRO为：

(3d)设星敏感器当前时刻的测量值为Q_k+1，则可以建立星敏感器因子f^STAR为：

其中，

为当前时刻的预测值，δq_k+1为当前时刻的状态。

进一步地，步骤(3)的实现如下：

(4a)令Θ＝X_k＝[q_e1 q_e2 q_e3 Δb₁ Δb₂ Δb₃]^T，则根据因子图特性将函数f(Θ)的因子分解定义为：

(4b)姿态确定的目标是找到最大化f(Θ)的变量赋值Θ^*，可表示如下：

(4c)当假设高斯测量模型为：

则最大化式的分解目标函数对应于非线性最小二乘准则，可表示为：

其中，h_i(Θ_i)为测量函数，z_i为测量值，

定义为协方差矩阵Σ的马氏距离的平方。

(4d)假设所有变量的集合为x_k，其初始估计为

高斯-牛顿方法从线性化系统中找到更新增量Δ为：

其中，

是当前线性化点

处的稀疏雅可比矩阵，并且

是给定所有测量到目前为止z_k的残差，雅可比矩阵等效于因子图的线性化版本，其块结构反映了因子图的结构。在求解更新增量Δ之后，将线性化点更新为新估计值

进一步地，步骤(4d)的具体实现如下：

(5a)消除因子。消除算法是一种对任意形式的因子图进行因式分解的方法，可表示为：

f(Θ)＝f(θ₁,...,θ_n)

将其分解成贝叶斯净概率密度为：

其中，S_j表示在所选变量排序θ₁,...,θ_n中与变量θ_j相关联的分隔符S(θ_j)的赋值，分隔符被定义为因子消除后θ_j被调整的变量集。

Π_if_i(Θ_i)因子被转换为条件Π_jP(θ_j|S_j)的等价乘积，表示如下：

则因子消除需要：首先，从因子图中删除所有与变量θ_j相邻的因子f_i(Θ_j)，将分隔符S_j定义为与这些因子相关的所有变量，θ_j除外。其次，将联合密度f_joint(θ_j,S_j)＝Π_if_i(Θ_i)作为各因子的乘积。最后，利用链式法则，对联合密度f_joint(θ_j,S_j)＝P(θ_j,S_j)f_new(S_j)进行因子分解，将条件P(θ_j,S_j)添加到贝叶斯网中，然后将因子f_new(S_j)添加回因子图中。

(5b)创建贝叶斯树。针对每个节点定义一个条件密度P(F_k|S_k)，其中分隔符S_k为簇C_k与其父簇Π_k的交集C_k∩Π_k，正面变量F_k为剩余变量，即

定义C_k＝F_k:S_k，因此关于贝叶斯树定义的变量Θ的联合密度P(Θ)可表示为：

对于贝叶斯网络的每个条件密度P(θ_j,S_j)，按反消除顺序：如果没有父节点(S_j＝{})，则启动一个包含θ_j的新根簇F_r，否则将包含S_j的第一个被消除变量的父簇C_p作为一个正面变量；如果父簇C_p的节点F_p∪S_p等价于条件的分隔节点S_j，则将条件插入到簇C_p中，否则开始新的簇C′作为包含θ_j的C_p的子节点。

(5c)更新贝叶斯树。首先，去除贝叶斯树的顶部，重新构建因子图，对于每一个受新因子影响的变量，删除相应的小团体和所有父节点直至根节点，并存储被移除派系的孤立子树T_orph。然后，将新的因子Φ′添加至重构的因子图中，并重新排序因子图的变量。最后，消除因子图，创建一个新的贝叶斯树，并把孤立子树T_orph转回到新的贝叶斯树。

(5d)增量变量排序。首先，根据卫星姿态确定系统因子图，获得系统的雅克比矩阵。其次，利用COLAMD算法找到理想的列顺序，对雅克比矩阵重排序。最后，将重新排序的雅克比矩阵的行列因子映射到因子图中，进行后续的因子消除。

(5e)部分状态更新。首先，将增量Δ中的变量标记在阈值ξ₁之上：J＝{Δ_j∈Δ||Δ_j|≥ξ₁}。其次，更新被标记变量的线性化点为：

最终，标记所有包含标记变量Θ_J及其父节点的团簇M。

(5f)平滑再线性化。在贝叶斯树T中，从根簇C_r＝F_r:开始，首先，对于当前团簇C_r＝F_r:S_k，从局部条件密度P(F_k|S_k)计算正面变量F_k的更新Δ_k。然后，对于更新Δ_k中变化超过阈值ξ₂的所有变量Δ_kj：递归地处理包含这样一个变量的每一个后代。

本发明与现有技术相比的优点如下：

(1)本发明在解决卫星姿态确定问题时，引入因子图方法，改变了传统的姿态确定系统建模方法，简化了系统建模过程。该方法只针对系统的量测方程进行建模，从而推导出状态方程。

(2)本发明提出了基于因子图的星敏感器和陀螺组合卫星姿态确定方法，利用因子图的二元特性，有效的解决了星敏感器和陀螺组合等传感器数据的异步、变速率等问题。

(3)本发明将四元数方法与因子图方法相结合，建立了基于误差四元数的星敏感器和陀螺组合姿态确定的因子图模型，并利用增量平滑算法进行优化，可以估计出高精度的卫星姿态信息。

(4)本发明提出的基于因子图的增量平滑算法是一种新的基于图形模型的增量稀疏矩阵分解方法，由贝叶斯树数据结构提供。该算法将矩阵更新转化为对贝叶斯树简单的编辑操作，并将条件密度表示其派系，在此基础上，提出了一种增量改变变量排序的新方法，该方法对计算效率有较大的影响。所提出算法的有效性和准确性是建立在平滑再线性化的基础上的，即根据需要选择性再生变量的概念，因此可获得一个完全增量算法，而不需要任何周期性的批处理步骤。

(5)本发明所提出的基于因子图的增量平滑算法适用范围很广，可以将其扩展至整个导航系统甚至其他优化过程，不仅可以提高优化速率，还可以提高估计精度。

附图说明

图1为本发明基于因子图的星敏感器和陀螺组合卫星姿态确定方法流程图；

图2为本发明实例的基于因子图的星敏感器和陀螺组合姿态测量系统；

图3为本发明实例的基于因子图的增量平滑算法关系图；

图4为本发明实例所获得的卫星三轴姿态角误差。

具体实施方式

下面结合附图，对本发明做进一步的描述。显然，所列举的实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，本发明所述的一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，包括如下步骤：

1、根据星敏感器和陀螺特性，以星敏感器的测量值作为基准，对陀螺的测量信息进行修正，根据星敏感器、陀螺的测量值，结合四元数方法构建卫星姿态估计方程；

首先，采用星敏感器补偿陀螺漂移，得到卫星姿态确定的状态方程为：

其中，Q_e为姿态误差四元数的矢量部分，

b为陀螺的真值，

为陀螺漂移的估计值。ω＝[ω₁ ω₂ ω₃]^T表示卫星角速度，且

I₃为3×3的单位矩阵，v_g，v_b是不相关零均值高斯白噪声。

然后，令δq＝Q_e，设姿态确定系统的状态矢量为X_k＝[δq Δb]^T＝[q_e1 q_e2 q_e3 Δb₁Δb₂ Δb₃]^T，卫星姿态估计得测量值由星敏感器的输出来获得，则量测方程为：

Z_k＝H_kX_k+D_kV_k

其中，H_k＝[I_3×3 0_3×3]，V_k＝[v_sc1 v_sc2 v_sc3]表示星敏感器的量测噪声。

2、根据所构建的卫星姿态估计方程，结合因子图方法，建立星敏感器因子和陀螺因子，从而构建基于因子图的星敏感器和陀螺组合卫星姿态确定系统；

首先，由于陀螺的测量频率高于星敏感器的测量频率，因此以星敏感器建立状态的连接，则k+1时刻和k时刻之间的雅克比矩阵可表示为：

J_t+Δt＝(I+F·Δt)J_t

其中，

雅克比矩阵J_t的初始值为单位矩阵，Δt为陀螺仪测量输出的时间间隔。

然后，利用雅克比矩阵建立k+1时刻和k时刻之间状态的关系可表示为：

X_k+1＝J_stateX_k

其中，

最终，由于

则有：

则可以得到陀螺因子f^GYRO为：

与此同时，设星敏感器当前时刻的测量值为Q_k+1，则可以建立星敏感器因子f^STAR为：

其中，

为当前时刻的预测值，δq_k+1为当前时刻的状态。

本发明实例所选取的星敏感器测量频率为1Hz，陀螺测量频率为50Hz，因此所构建的卫星姿态确定系统的因子图框架如图2所示。

3、根据所构建的基于因子图的星敏感器和陀螺组合卫星姿态确定系统，引入贝叶斯网络和贝叶斯树理论，提出了基于因子图的增量平滑算法，从而获得较高精度的卫星姿态信息。

首先，令Θ＝X_k＝[q_e1 q_e2 q_e3 Δb₁ Δb₂ Δb₃]^T，则根据因子图特性将函数f(Θ)的因子分解定义为：

则姿态确定的目标是找到最大化f(Θ)的变量赋值Θ^*，可表示如下：

然后，假设高斯测量模型为：

则最大化式的分解目标函数对应于非线性最小二乘准则，可表示为：

其中，h_i(Θ_i)为测量函数，z_i为测量值，

定义为协方差矩阵Σ的马氏距离的平方。

最终，设所有变量的集合为x_k，其初始估计为

高斯-牛顿方法从线性化系统中找到更新增量Δ为：

其中，

是当前线性化点

处的稀疏雅可比矩阵，并且

是给定所有测量到目前为止z_k的残差，雅可比矩阵等效于因子图的线性化版本，其块结构反映了因子图的结构。在求解更新增量Δ之后，将线性化点更新为新估计值

如图3所示，为上述所述的求解更新增量算法的详细关系图，其具体实现步骤如下：

第一步，消除因子。消除算法是一种对任意形式的因子图进行因式分解的方法，可表示为：

f(Θ)＝f(θ₁,...,θ_n)

将其分解成贝叶斯净概率密度为：

其中，S_j表示在所选变量排序θ₁,...,θ_n中与变量θ_j相关联的分隔符S(θ_j)的赋值，分隔符被定义为因子消除后θ_j被调整的变量集。

Π_if_i(Θ_i)因子被转换为条件Π_jP(θ_j|S_j)的等价乘积，表示如下：

则因子消除需要：首先，从因子图中删除所有与变量θ_j相邻的因子f_i(Θ_j)，将分隔符S_j定义为与这些因子相关的所有变量，θ_j除外。其次，将联合密度f_joint(θ_j,S_j)＝Π_if_i(Θ_i)作为各因子的乘积。最后，利用链式法则，对联合密度f_joint(θ_j,S_j)＝P(θ_j,S_j)f_new(S_j)进行因子分解，将条件P(θ_j,S_j)添加到贝叶斯网中，然后将因子f_new(S_j)添加回因子图中。

对上述所涉及的因子图消除算法进行补充说明。提出增量变量排序算法。首先，根据卫星姿态确定系统因子图，获得系统的雅克比矩阵。其次，利用COLAMD算法找到理想的列顺序，对雅克比矩阵重排序。最后，将重新排序的雅克比矩阵的行列因子映射到因子图中，进行后续的因子消除。

第二步，创建贝叶斯树。针对每个节点定义一个条件密度P(F_k|S_k)，其中分隔符S_k为簇C_k与其父簇Π_k的交集C_k∩Π_k，正面变量F_k为剩余变量，即

定义C_k＝F_k:S_k，因此关于贝叶斯树定义的变量Θ的联合密度P(Θ)可表示为：

对于贝叶斯网络的每个条件密度P(θ_j,S_j)，按反消除顺序：如果没有父节点(S_j＝{})，则启动一个包含θ_j的新根簇F_r，否则将包含S_j的第一个被消除变量的父簇C_p作为一个正面变量；如果父簇C_p的节点F_p∪S_p等价于条件的分隔节点S_j，则将条件插入到簇C_p中，否则开始新的簇C′作为包含θ_j的C_p的子节点。

第三步，更新贝叶斯树。首先，去除贝叶斯树的顶部，重新构建因子图，对于每一个受新因子影响的变量，删除相应的小团体和所有父节点直至根节点，并存储被移除派系的孤立子树T_orph。然后，将新的因子Φ′添加至重构的因子图中，并重新排序因子图的变量。最后，消除因子图，创建一个新的贝叶斯树，并把孤立子树T_orph转回到新的贝叶斯树。

第四步，部分状态更新。首先，将增量Δ中的变量标记在阈值ξ₁之上：J＝{Δ_j∈Δ||Δ_j|≥ξ₁}。其次，更新被标记变量的线性化点为：

最终，标记所有包含标记变量Θ_J及其父节点的团簇M。

第五步，平滑再线性化。在贝叶斯树T中，从根簇C_r＝F_r:开始，首先，对于当前团簇C_r＝F_r:S_k，从局部条件密度P(F_k|S_k)计算正面变量F_k的更新Δ_k。然后，对于更新Δ_k中变化超过阈值ξ₂的所有变量Δ_kj：递归地处理包含这样一个变量的每一个后代。

通过上述所提出的优化算法则可以得到最终的高精度姿态确定信息。本发明实例利用所提出方法进行了数学仿真，并得出了仿真结果如图4所示为卫星三轴姿态角误差，仿真结果表明所提方法可以得到高精度的卫星姿态信息。

以上虽然描述了本发明的具体实施方法，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明原理和实现的前提下，可以对这些实施方案做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。

Claims (3)

1.一种基于因子图的星敏感器和陀螺组合卫星姿态确定方法，其特征在于：包括以下步骤：

步骤(1)、根据星敏感器和陀螺特性，以星敏感器的测量值作为基准，对陀螺的测量信息进行修正，根据星敏感器、陀螺的测量值，结合四元数方法构建卫星姿态估计方程；

步骤(2)、根据步骤(1)所构建的卫星姿态估计方程，结合因子图方法，建立星敏感器因子和陀螺因子，从而构建基于因子图的星敏感器和陀螺组合卫星姿态确定系统；

步骤(3)、根据步骤(2)所构建的基于因子图的星敏感器和陀螺组合卫星姿态确定系统，引入贝叶斯网络和贝叶斯树理论，提出了基于因子图的增量平滑算法，从而获得较高精度的卫星姿态信息；

所述步骤(3)中，引入贝叶斯网络和贝叶斯树理论，设计基于因子图的增量平滑算法的具体实现如下：

(4a)令Θ＝X_k＝[q_e1 q_e2 q_e3 Δb₁ Δb₂ Δb₃]^T，则根据因子图特性将函数f(Θ)的因子分解定义为：

(4b)姿态确定的目标是找到最大化f(Θ)的变量赋值Θ^*，可表示如下：

(4c)当假设高斯测量模型为：

则最大化式的分解目标函数对应于非线性最小二乘准则，可表示为：

其中，h_i(Θ_i)为测量函数，z_i为测量值，

定义为协方差矩阵Σ的马氏距离的平方；

(4d)假设所有变量的集合为x_k，其初始估计为

高斯-牛顿方法从线性化系统中找到更新增量Δ为：

其中，

是当前线性化点

处的稀疏雅可比矩阵，并且

是给定所有测量到目前为止z_k的残差，雅可比矩阵等效于因子图的线性化版本，其块结构反映了因子图的结构，在求解更新增量Δ之后，将线性化点更新为新估计值

步骤(4d)中求解更新增量的算法的具体实现如下：

(5a)消除因子，消除算法是一种对任意形式的因子图进行因式分解的方法，可表示为：

f(Θ)＝f(θ₁,...,θ_n)

将其分解成贝叶斯净概率密度为：

其中，S_j表示在所选变量排序θ₁,...,θ_n中与变量θ_j相关联的分隔符S(θ_j)的赋值，分隔符被定义为因子消除后θ_j被调整的变量集；

∏_if_i(Θ_i)因子被转换为条件∏_jP(θ_j|S_j)的等价乘积，表示如下：

则因子消除需要：首先，从因子图中删除所有与变量θ_j相邻的因子f_i(Θ_j)，将分隔符S_j定义为与这些因子相关的所有变量，θ_j除外；其次，将联合密度f_joint(θ_j,S_j)＝∏_if_i(Θ_i)作为各因子的乘积；最后，利用链式法则，对联合密度f_joint(θ_j,S_j)＝P(θ_j,S_j)f_new(S_j)进行因子分解，将条件P(θ_j,S_j)添加到贝叶斯网中，然后将因子f_new(S_j)添加回因子图中；

(5b)创建贝叶斯树，针对每个节点定义一个条件密度P(F_k|S_k)，其中分隔符S_k为簇C_k与其父簇Π_k的交集C_k∩Π_k，正面变量F_k为剩余变量，即

定义C_k＝F_k:S_k，因此关于贝叶斯树定义的变量Θ的联合密度P(Θ)可表示为：

对于贝叶斯网络的每个条件密度P(θ_j,S_j)，按反消除顺序：如果没有父节点(S_j＝{})，则启动一个包含θ_j的新根簇F_r，否则将包含S_j的第一个被消除变量的父簇C_p作为一个正面变量；如果父簇C_p的节点F_p∪S_p等价于条件的分隔节点S_j，则将条件插入到簇C_p中，否则开始新的簇C′作为包含θ_j的C_p的子节点；

(5c)更新贝叶斯树，首先，去除贝叶斯树的顶部，重新构建因子图，对于每一个受新因子影响的变量，删除相应的小团体和所有父节点直至根节点，并存储被移除派系的孤立子树T_orph；然后，将新的因子Φ′添加至重构的因子图中，并重新排序因子图的变量；最后，消除因子图，创建一个新的贝叶斯树，并把孤立子树T_orph转回到新的贝叶斯树；

(5d)增量变量排序，首先，根据卫星姿态确定系统因子图，获得系统的雅克比矩阵；其次，利用COLAMD算法找到理想的列顺序，对雅克比矩阵重排序；最后，将重新排序的雅克比矩阵的行列因子映射到因子图中，进行后续的因子消除；

(5e)部分状态更新，首先，将增量Δ中的变量标记在阈值ξ₁之上：J＝{Δ_j∈Δ||Δ_j|≥ξ₁}；其次，更新被标记变量的线性化点为：Θ_J:＝Θ_J⊕Δ_J；最终，标记所有包含标记变量Θ_J及其父节点的团簇M；

(5f)平滑再线性化，在贝叶斯树T中，从根簇C_r＝F_r:开始，首先，对于当前团簇C_r＝F_r:S_k，从局部条件密度P(F_k|S_k)计算正面变量F_k的更新Δ_k；然后，对于更新Δ_k中变化超过阈值ξ₂的所有变量Δ_kj：递归地处理包含这样一个变量的每一个后代。

2.根据权利要求1所述的基于因子图的星敏感器和陀螺组合卫星姿态确定方法，其特征在于：所述步骤(1)中，构建卫星姿态估计方程实现如下：

(2a)采用星敏感器补偿陀螺漂移，得到卫星姿态确定的状态方程为：

其中，Q_e为姿态误差四元数的矢量部分，

b为陀螺的真值，

为陀螺漂移的估计值，ω＝[ω₁ ω₂ ω₃]^T表示卫星角速度，且

I₃为3×3的单位矩阵，v_g，v_b是不相关零均值高斯白噪声；

(2b)令δq＝Q_e，设姿态确定系统的状态矢量为X_k＝[δq Δb]^T＝[q_e1 q_e2 q_e3 Δb₁ Δb₂Δb₃]^T，卫星姿态估计得测量值由星敏感器的输出来获得，则量测方程为：

Z_k＝H_kX_k+D_kV_k

其中，H_k＝[I_3×3 0_3×3]，V_k＝[v_sc1 v_sc2 v_sc3]表示星敏感器的量测噪声。

3.根据权利要求1所述的基于因子图的星敏感器和陀螺组合卫星姿态确定方法，其特征在于，所述步骤(2)中，构建基于因子图的星敏感器和陀螺组合卫星姿态确定系统的具体实现如下：

(3a)由于陀螺的测量频率高于星敏感器的测量频率，因此以星敏感器建立状态的连接，则k+1时刻和k时刻之间的雅克比矩阵可表示为：

J_t+Δt＝(I+F·Δt)J_t

其中，

雅克比矩阵J_t的初始值为单位矩阵，Δt为陀螺仪测量输出的时间间隔；

(3b)利用雅克比矩阵建立k+1时刻和k时刻之间状态的关系可表示为：

X_k+1＝J_stateX_k

其中，

(3c)由于

则有：

则可以得到陀螺因子f^GYRO为：

(3d)设星敏感器当前时刻的测量值为Q_k+1，则可以建立星敏感器因子f^STAR为：

其中，

为当前时刻的预测值，δq_k+1为当前时刻的状态。

Images