CN105300384A - 一种用于卫星姿态确定的交互式滤波方法 - Google Patents

Abstract

本发明公开了一种用于卫星姿态确定的交互式滤波方法，弥补了optimal-REQUEST算法无法估计陀螺常值漂移的缺陷，提高了算法的适用性。包括以下几个步骤：步骤一：采集传感器量测数据，包括陀螺仪数据和星敏感器数据；步骤二：建立卫星姿态估计系统的状态空间模型，包括构建姿态K矩阵；步骤三：针对上述状态空间模型，已知k时刻的状态估计和陀螺量测，利用CKF算法及k时刻的最优四元数估计k+1时刻的陀螺常值漂移，从而补偿陀螺量测；步骤四：针对上述补偿的陀螺量测，利用optimal-REQUEST算法进行时间更新和量测更新，得到k+1时刻的最优K矩阵，从而得到最优四元数等步骤。本发明有利于提高估计精度。

Description

一种用于卫星姿态确定的交互式滤波方法

技术领域：

本发明涉及一种用于卫星姿态确定的交互式滤波方法，属于利用最优K矩阵方法和非线性滤波技术进行卫星姿态估计的技术领域。

背景技术：

卫星姿态估计技术是航天技术的关键技术之一，由陀螺与星敏感器组成的卫星姿态估计系统由于测姿精度高、可靠性好以及自主性强等优点得到了广泛的应用。K矩阵姿态估计方法是基于Wahba问题的四元数确定方法，具有计算效率高的优点。在QUEST算法的基础上，ItzhackY.Bar-Itzhack提出了利用陀螺量测更新K矩阵的REQUEST算法，进一步提高了QUEST算法的估计精度，但是REQUEST算法具有增益因子不可调节的缺点，无法根据实际的量测误差，自动调节增益因子，使得算法的适用性存在一定问题。在此基础上，D.Choukroun等于2004年提出了optimal-REQUEST算法，在经典Kalman滤波框架的基础下，利用最优增益因子提高了姿态估计精度。但是optimal-REQUEST算法存在不能估计陀螺常值漂移的缺点，而且最优增益因子无法补偿陀螺常值漂移带来的影响，使得算法存在发散现象。

为克服optimal-REQUEST算法无法估计陀螺常值漂移的缺陷，采用CKF算法与optimal-REQUEST算法相结合的方式实现陀螺常值漂移的估计，提高算法的适用性。由于陀螺常值漂移估计为非线性状态空间模型，常用的方法有EKF、UKF和CKF算法，但是EKF算法存在理论上的局限性：模型线性化引入了截断误差，导致滤波精度下降，同时要计算雅克比矩阵，计算复杂。UKF与CKF算法同是基于最优高斯滤波框架，相比于UKF，CKF采用三阶球面相径容积规则来近似非线性函数的均值和方差，可以保证在理论上以三阶多项式逼近任何非线性高斯状态的后验均值和方差，具有实现简单，收敛性好等优点。将optimal-REQUEST算法与CKF算法相结合的滤波方式，可以发挥各自的优势，optimal-REQUEST算法具有线性特征，估计最优四元数，可以避免四元数规范化带来的误差，而采用CKF算法估计陀螺常值漂移，也可以解决量测方程非线性化带来的估计精度差的缺点，使得算法的估计精度得到提高，提高了算法的适用性。

发明内容：

本发明的目的是为了提高姿态估计精度和增强系统的适用性，提出了一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法。发明采用optimal-REQUEST算法估计卫星姿态四元数，采用CKF算法估计陀螺常值漂移，并将optimal-REQUEST算法估计的最优四元数，作为CKF算法的输入，同时CKF算法估计的陀螺常值漂移作为optimal-REQUEST算法的更新补偿，实现两种算法的交互，达到系统的最优性能。

本发明的技术方案具体如下：

本发明的用于一种用于卫星姿态确定的交互式滤波方法，包括以下几个步骤：

步骤1：读取传感器量测数据，所述传感器测量数据包括陀螺仪数据和星敏感器数据；

步骤2：建立卫星姿态估计系统的状态空间模型；

所述的卫星姿态估计系统状态空间模型包括：

(1)陀螺常值漂移估计状态空间模型

β_k+1＝β_k+η_k

$b_{k+1}=A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1}+\mathrm{\δ}{b}_{k+1}$

$\mathrm{\Φ}\left({\mathrm{\ω}}_k\right)=\cos \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)I_4+\sin \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)\mathrm{\Ω}\left({\mathrm{\ω}}_k\right)$

$\mathrm{\Ω}\left({\mathrm{\ω}}_k\right)=\left[\begin{array}{cc}-[{\mathrm{\ω}}_k\×]& {\mathrm{\ω}}_k\\ -{\mathrm{\ω}}_k^T& 0\end{array}\right]$

式中：β_k表示k时刻陀螺常值漂移；β_k+1为k+1时刻陀螺常值漂移；表示k时刻最优估计四元数；Δt表示采样时间；η_k为零均值高斯白噪声，方差为Φ(·)为四元数更新矩阵；ω_k表示k时刻载体转速，[ω_k×]表示向量叉乘矩阵；r_k+1为k+1时刻向量观测器参考向量；b_k+1为k+1时刻向量观测器量测向量；为陀螺仪量测值，量测噪声方差为δb_k+1为零均值高斯白噪声，方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型，对于一般星敏感器，在同一时间往往能够观测多颗有效星，其量测模型可以扩充为：

$\begin{array}{c}{z}_k=\left[\begin{array}{c}{z}_{k+\mathrm{1,1}}\\ z_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ z_{k+1,N(k+1)}\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\Φ}({\widetilde{\mathrm{\Φ}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,1}\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,N(k+1)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{b}_{k+\mathrm{1,1}}\\ \mathrm{\δ}{b}_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ \mathrm{\δ}{b}_{k+1,N(k+1)}\end{array}\right]\\ =h\left(x_k\right)+v_k\end{array}.$

式中，z_k+1,i表示第i个有效矢量观测；δb_k+1,i为相应的随机噪声；N(k+1)表示k+1时刻的有效向量个数；x_k表示组合之后的状态参量，此处表示陀螺常值漂移β_k；v_k表示组合之后的零均值高斯白噪声；h(·)表示非线性量测函数；

(2)基于optimal-REQUEST方法的姿态确定状态空间模型

$X_{k+1}={\mathrm{\Φ}}_k{X}_k{\mathrm{\Φ}}_k^T+W_k$

${\mathrm{\Φ}}_k=\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\widehat{\mathrm{\β}}}_{k|k})$

Y_k+1＝X_k+1+V_k+1

$Y_{k+1}=\left[\begin{array}{cc}S-\mathrm{\σ}{I}_3& z\\ z^T& \mathrm{\σ}\end{array}\right]$

$\left\{\begin{array}{cc}S=B+B^T& B=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1,i}{b}_{k+1,i}{r}_{k+1,i}^T\\ z=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1,i}{b}_{k+1,i}\×r_{k+1,i}& \mathrm{\σ}=\mathrm{tr}\left(B\right)\end{array}\right.$

$W_k=\left[\begin{array}{cc}{S}_{\mathrm{\ϵ}}-{\mathrm{\σ}}_{\mathrm{\ϵ}}{I}_3& z_{\mathrm{\ϵ}}\\ z_{\mathrm{\ϵ}}^T& {\mathrm{\σ}}_{\mathrm{\ϵ}}\end{array}\right]\mathrm{\Δt}$

$\left\{\begin{array}{cc}{B}_{\mathrm{\ϵ}}=[\mathrm{\ϵ}\×]B& S_{\mathrm{\ϵ}}=B_{\mathrm{\ϵ}}{B}_{\mathrm{\ϵ}}^T\\ [z_{\mathrm{\ϵ}}\×]=B_{\mathrm{\ϵ}}^T-B_{\mathrm{\ϵ}}& {\mathrm{\σ}}_3=\mathrm{tr}\left(B_{\mathrm{\ϵ}}\right)\end{array}\right.$

式中：X_k表示状态K矩阵；Y_k+1为k+1时刻量测矩阵；V_k+1表示k+1时刻量测噪声；b_k+1,i表示k+1时刻第i个有效观测向量；r_k+1,i表示k+1时刻第i个有效参考向量；tr(·)表示取矩阵的迹；a_k+1,i表示k+1时刻第i个加权系数；W_k为系统过程噪声矩阵；ε为陀螺量测随机噪声向量；

步骤3：针对上述状态空间模型，在已知时刻陀螺常值漂移的状态估计和估计方差的情况下，利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新，得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差；

步骤3.1时间更新；

已知k时刻的状态估计为以及估计协方差P_k|k，求取容积点为：

${\mathrm{\α}}_{i,k|k}=\sqrt{P_{k|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k|k},i=1,...,2n$

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}\sqrt{n}{e}_i,i=1,...,n\\ -\sqrt{n}{e}_{i-n},i=n+1,...,2n\end{array}\right.$

式中，n为状态变量的维数；ξ_i为第i个容积点；e_i表示n维单位列向量在第i个位置为1；P_k|k表示k时刻估计状态协方差矩阵；表示k时刻状态估计；α_i,k|k表示传递容积点；

由于陀螺常值漂移状态方程是线性方程，故容积点经过状态传递之后仍为原值；一步状态预测估计与估计方差：

${\widehat{\mathrm{\β}}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}{\mathrm{\α}}_{i,k|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\widehat{\mathrm{\β}}}_{k+1|k}^T+Q_k$

式中，表示一步预测状态；P_k+1|k表示一步预测状态协方差矩阵；Q_k表示k时刻过程噪声方差阵；

步骤3.2量测更新；

计算更新容积点：

${\mathrm{\λ}}_{i,k+1|k}=\sqrt{P_{k+1|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k+1|k},i=\mathrm{1,2},...,2n$

容积点经过量测非线性函数传递为：

γ_i,k+1|k＝h(λ_i,k+1|k)

式中，h(·)的定义如前向量观测器非线性量测模型；λ_i,k+1|k为计算更新容积点；

量测一步预测、预测方差和互协方差为：

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}$

$P_{\mathrm{zz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}{\mathrm{\γ}}_{i,k+/k}^T-{\hat{z}}_{k+1|k}{\hat{z}}_{k+1|k}^T+R_{k+1}$

$P_{\mathrm{xz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k+1|k}{\mathrm{\γ}}_{i,k+1|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\hat{z}}_{k+1|k}^T$

式中，表示k+1时刻量测一步预测；P_zz,k+1|k表示k+1时刻量测一步预测协方差矩阵；P_xz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵；R_k+1表示k+1时刻量测噪声方差阵；

陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示：

式中，表示k+1时刻容积卡尔曼滤波增益矩阵；表示k+1时刻陀螺常值漂移最优估计；表示k+1时刻向量观测器量测向量；P_k+1|k+1表示k+1时刻估计状态误差协方差矩阵；

步骤4：利用步骤3估计的陀螺常值漂移补偿陀螺量测，然后利用补偿后的陀螺量测构造四元数更新矩阵，在optimal-REQUEST算法更新过程中，修正由于陀螺常值漂移引起的Φ_k不准确问题，在利用optimal-REQUEST算法实现最优K矩阵的求取：

步骤4.1optimal-REQUEST时间更新；

已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差：

${\hat{x}}_{k+1|k}={\mathrm{\Φ}}_k{\hat{X}}_{k|k}{\mathrm{\Φ}}_k^T$

式中，Φ_k表示k时刻四元数更新矩阵；表示k时刻状态K矩阵一步预测；表示k时刻一步预测状态协方差矩阵；表示k时刻过程噪声方差阵；

步骤4.2optimal-REQUEST量测更新

计算最优增益及权值更新：

$m_{k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})m_k+{\mathrm{\ρ}}_{k+1}^{*}\mathrm{\δ}{m}_{k+1}$

式中，表示k+1时刻最优增益；m_k+1表示k+1时刻状态K矩阵归一化权值；δm_k+1表示k+1时刻量测K矩阵归一化权值；

得到k+1时刻的状态估计和估计方差：

${\hat{X}}_{k+1|k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})\frac{m_k}{m_{k+1}}{\hat{X}}_{k+1|k}+{\mathrm{\ρ}}_{k+1}^{*}\frac{\mathrm{\δ}{m}_{k+1}}{m_{k+1}}{Y}_{k+1}$

式中，Y_k+1表示k+1时刻量测K矩阵；表示k+1时刻状态K矩阵最优估计；表示k+1时刻量测噪声方差阵；表示k+1时刻估计状态协方差矩阵；

过程噪声方差阵和量测噪声方差阵计算如下：

$\left\{\begin{array}{c}{\hat{U}}_{k|k}={\hat{B}}_{k|k}({\hat{B}}_{k|k}-{\widehat{\mathrm{\σ}}}_{k|k}{I}_3)\\ [{\hat{y}}_{k|k}\×]={\hat{U}}_{k|k}^T-{\hat{U}}_{k|k}\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；表示陀螺量测噪声方差值；Δt表示系统采样时间；

其中，表示向量观测随机噪声方差值；N(k+1)表示k+1时刻有效向量数；b_k+1,i表示k+1时刻第i个观测向量；r_k+1,i表示k+1时刻第i个参考向量；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

$\frac{1}{2}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i{\left|\right|b_i-A{r}_i\left|\right|}^2=1-q^T\mathrm{Kq}$

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

$\left\{\begin{array}{c}{\hat{y}}_{k|k}^{*}={[(1+{\widehat{\mathrm{\σ}}}_{k|k})I_3-{\hat{B}}_{k|k}^T-{\hat{B}}_{k|k}]}^{-1}{\hat{z}}_{k|k}\\ {\hat{q}}_{k|k}=\frac{1}{\sqrt{1+{\left|\right|{\hat{y}}_{k|k}^{*}\left|\right|}^2}}\left[\begin{array}{c}{\hat{y}}_{k|k}^{*}\\ 1\end{array}\right]\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；表示四元数乘法；表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数；

步骤6：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤3至步骤5，直至姿态估计系统运行结束。

进一步地，所述步骤2中，陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz；星敏感器测量标准差为σ_s＝5”，输出频率为1Hz；陀螺初始漂移β＝[111]^Tdeg/hr，静态情况真实角速率为w＝[000]^Trad/s，动态情况真实角速率为w＝[-0.00230.0023-0.0023]^Trad/s。

进一步地，所述步骤3和步骤4中，初始陀螺常值漂移设置为 ${\widehat{\mathrm{\β}}}_{0|0}={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T\mathrm{deg}/\mathrm{hr},$ 初始方差设置为P_0|0＝diag([(2°/hr)²(2°/hr)²(2°/hr)²])；初始K矩阵设置为初始过程噪声方差为初始权值为m₀＝1。

进一步地，所述步骤6中，M＝5400。

本发明的优点在于：

(1)本发明采用容积卡尔曼滤波对陀螺常值漂移进行估计，不需要对量测非线性方程线性化，避免了雅可比矩阵的计算，有利于提高估计精度。相比于无迹卡尔曼滤波，容积卡尔曼滤波也具有容积点少，计算简便的优点；

(2)本发明采用交互式滤波方法，使得两个滤波过程相互弥补，同时采用optimal-REQUEST与CKF对载体最优四元数和陀螺常值漂移进行估计，并利用最新的估计值带入算法进行下一次的估计，有利于算法快速收敛，提高各参数的估计精度；

(3)本发明采用多重量测构造量测方程，考虑一次观测多颗参考星的方式，更加符合真实情况，同时采用optimal-REQUEST算法估计最优四元数，避免了四元数范数约束带来的估计误差，有利于提高估计精度。

附图说明：

图1是算法流程图；

图2是不含陀螺常值漂移时，QUEST算法、增益为0.1的REQUEST算法、增益为0.01的REQUEST算法、增益为0.001的REQUEST算法及optimal-REQUEST算法姿态估计误差结果图；

图3是静态滤波过程中，不含陀螺漂移时，optimal-REQUEST算法自动调节增益曲线图；

图4是含有陀螺常值漂移时，QUEST算法、增益为0.1的REQUEST算法、增益为0.01的REQUEST算法、增益为0.001的REQUEST算法及optimal-REQUEST算法姿态估计误差结果图；

图5是动态滤波过程中，含陀螺漂移时，optimal-REQUEST算法自动调节增益曲线图；

图6是含有陀螺常值漂移时，optimal-REQUEST与CKF算法、optimal-REQUEST算法及陀螺常值漂移为0时optimal-REQUEST算法姿态估计误差结果图；

图7是CKF算法估计的陀螺常值漂移估计结果图。

具体实施方式：

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

本发明提出的一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法是通过Matlab仿真软件进行仿真实验，与现有滤波算法的估计性能进行比较，如QUEST方法、变因子REQUEST方法及optimal-REQUEST方法。仿真硬件环境均为Intel(R)Core(TM)T9600CPU2.80GHz，4GRAM，Windows7操作系统。如图2和图4所示，在不含有陀螺常值漂移和含有陀螺常值漂移时，几种滤波算法估计误差曲线图，从图中可知，QUEST算法由于不利用陀螺更新，故估计误差只与星敏感器量测误差有关，不受陀螺常值漂移影响，但是在不含陀螺漂移时，估计精度要低于采用陀螺更新的REQUEST与optimal-REQUEST算法。而在不同的增益因子的REQUEST算法与optimal-REQUEST算法对比中，optimal-REQUEST算法的收敛速度与收敛精度都是相对最优的，但是增加陀螺常值漂移之后，由于增益因子的调节作用，使得滤波估计开始发散，降低了估计精度；如图3和图5所示，从静态和动态两种方式对比了增益因子的变化，在静态时增益因子逐渐收敛，表示为估计精度高于量测精度，估计结果在最终结果中占有的权值较高；而动态时，由于载体姿态处于变化过程，增益因子也在不断调节，在匹配估计与量测之间达到算法的优化，得到最优的估计；从图6比较得到，采用CKF对陀螺常值估计，并补偿optimal-REQUEST算法更新方程之后，估计误差能够达到无陀螺常值漂移时的精度，使得算法的适用性得到提高；图7为CKF估计的陀螺常值漂移。

本发明是一种用于卫星姿态确定的optimal-REQUEST与CKF交互式滤波方法，流程如图1所示，包括以下几个步骤：

步骤1：读取传感器量测数据，所述传感器测量数据包括陀螺仪数据和星敏感器数据；

步骤2：建立卫星姿态估计系统的状态空间模型；

所述的卫星姿态估计系统状态空间模型包括：

(1)陀螺常值漂移估计状态空间模型

β_k+1＝β_k+η_k

$b_{k+1}=A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1}+\mathrm{\δ}{b}_{k+1}$

$\mathrm{\Φ}\left({\mathrm{\ω}}_k\right)=\cos \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)I_4+\sin \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)\mathrm{\Ω}\left({\mathrm{\ω}}_k\right)$

$\mathrm{\Ω}\left({\mathrm{\ω}}_k\right)=\left[\begin{array}{cc}-[{\mathrm{\ω}}_k\×]& {\mathrm{\ω}}_k\\ -{\mathrm{\ω}}_k^T& 0\end{array}\right]$

式中：β_k表示k时刻陀螺常值漂移；β_k+1为k+1时刻陀螺常值漂移；表示k时刻最优估计四元数；Δt表示采样时间；η_k为零均值高斯白噪声，方差为Φ(·)为四元数更新矩阵；ω_k表示k时刻载体转速，[ω_k×]表示向量叉乘矩阵；r_k+1为k+1时刻向量观测器参考向量；b_k+1为k+1时刻向量观测器量测向量；为陀螺仪量测值，量测噪声方差为δb_k+1为零均值高斯白噪声，方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型，对于一般星敏感器，在同一时间往往能够观测多颗有效星，其量测模型可以扩充为：

$\begin{array}{c}{z}_k=\left[\begin{array}{c}{z}_{k+\mathrm{1,1}}\\ z_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ z_{k+1,N(k+1)}\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\Φ}({\widetilde{\mathrm{\Φ}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,1}\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,N(k+1)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{b}_{k+\mathrm{1,1}}\\ \mathrm{\δ}{b}_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ \mathrm{\δ}{b}_{k+1,N(k+1)}\end{array}\right]\\ =h\left(x_k\right)+v_k\end{array}.$

式中，z_k+1,i表示第i个有效矢量观测；δb_k+1,i为相应的随机噪声；N(k+1)表示k+1时刻的有效向量个数；x_k表示组合之后的状态参量，此处表示陀螺常值漂移β_k；v_k表示组合之后的零均值高斯白噪声；h(·)表示非线性量测函数；

(2)基于optimal-REQUEST方法的姿态确定状态空间模型

$X_{k+1}={\mathrm{\Φ}}_k{X}_k{\mathrm{\Φ}}_k^T+W_k$

${\mathrm{\Φ}}_k=\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\widehat{\mathrm{\β}}}_{k|k})$

Y_k+1＝X_k+1+V_k+1

$Y_{k+1}=\left[\begin{array}{cc}S-\mathrm{\σ}{I}_3& z\\ z^T& \mathrm{\σ}\end{array}\right]$

$\left\{\begin{array}{cc}S=B+B^T& B=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1,i}{b}_{k+1,i}{r}_{k+1,i}^T\\ z=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1,i}{b}_{k+1,i}\×r_{k+1,i}& \mathrm{\σ}=\mathrm{tr}\left(B\right)\end{array}\right.$

$W_k=\left[\begin{array}{cc}{S}_{\mathrm{\ϵ}}-{\mathrm{\σ}}_{\mathrm{\ϵ}}{I}_3& z_{\mathrm{\ϵ}}\\ z_{\mathrm{\ϵ}}^T& {\mathrm{\σ}}_{\mathrm{\ϵ}}\end{array}\right]\mathrm{\Δt}$

$\left\{\begin{array}{cc}{B}_{\mathrm{\ϵ}}=[\mathrm{\ϵ}\×]B& S_{\mathrm{\ϵ}}=B_{\mathrm{\ϵ}}{B}_{\mathrm{\ϵ}}^T\\ [z_{\mathrm{\ϵ}}\×]=B_{\mathrm{\ϵ}}^T-B_{\mathrm{\ϵ}}& {\mathrm{\σ}}_3=\mathrm{tr}\left(B_{\mathrm{\ϵ}}\right)\end{array}\right.$

式中：X_k表示状态K矩阵；Y_k+1为k+1时刻量测矩阵；V_k+1表示k+1时刻量测噪声；b_k+1,i表示k+1时刻第i个有效观测向量；r_k+1,i表示k+1时刻第i个有效参考向量；tr(·)表示取矩阵的迹；a_k+1,i表示k+1时刻第i个加权系数；W_k为系统过程噪声矩阵；ε为陀螺量测随机噪声向量；

步骤3：针对上述状态空间模型，在已知时刻陀螺常值漂移的状态估计和估计方差的情况下，利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新，得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差；

步骤3.1时间更新；

已知k时刻的状态估计为以及估计协方差P_k|k，求取容积点为：

${\mathrm{\α}}_{i,k|k}=\sqrt{P_{k|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k|k},i=1,...,2n$

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}\sqrt{n}{e}_i,i=1,...,n\\ -\sqrt{n}{e}_{i-n},i=n+1,...,2n\end{array}\right.$

式中，n为状态变量的维数；ξ_i为第i个容积点；e_i表示n维单位列向量在第i个位置为1；P_k|k表示k时刻估计状态协方差矩阵；表示k时刻状态估计；α_i,k|k表示传递容积点；

由于陀螺常值漂移状态方程是线性方程，故容积点经过状态传递之后仍为原值；一步状态预测估计与估计方差：

${\widehat{\mathrm{\β}}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}{\mathrm{\α}}_{i,k|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\widehat{\mathrm{\β}}}_{k+1|k}^T+Q_k$

式中，表示一步预测状态；P_k+1|k表示一步预测状态协方差矩阵；Q_k表示k时刻过程噪声方差阵；

步骤3.2量测更新；

计算更新容积点：

${\mathrm{\λ}}_{i,k+1|k}=\sqrt{P_{k+1|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k+1|k},i=\mathrm{1,2},...,2n$

容积点经过量测非线性函数传递为：

γ_i,k+1|k＝h(λ_i,k+1|k)

式中，h(·)的定义如前向量观测器非线性量测模型；λ_i,k+1|k为计算更新容积点；

量测一步预测、预测方差和互协方差为：

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}$

$P_{\mathrm{zz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}{\mathrm{\γ}}_{i,k+/k}^T-{\hat{z}}_{k+1|k}{\hat{z}}_{k+1|k}^T+R_{k+1}$

$P_{\mathrm{xz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k+1|k}{\mathrm{\γ}}_{i,k+1|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\hat{z}}_{k+1|k}^T$

式中，表示k+1时刻量测一步预测；P_zz,k+1|k表示k+1时刻量测一步预测协方差矩阵；P_xz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵；R_k+1表示k+1时刻量测噪声方差阵；

陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示：

式中，表示k+1时刻容积卡尔曼滤波增益矩阵；表示k+1时刻陀螺常值漂移最优估计；表示k+1时刻向量观测器量测向量；P_k+1|k+1表示k+1时刻估计状态误差协方差矩阵；

步骤4：利用步骤3估计的陀螺常值漂移补偿陀螺量测，然后利用补偿后的陀螺量测构造四元数更新矩阵，在optimal-REQUEST算法更新过程中，修正由于陀螺常值漂移引起的Φ_k不准确问题，在利用optimal-REQUEST算法实现最优K矩阵的求取：

步骤4.1optimal-REQUEST时间更新；

已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差：

${\hat{x}}_{k+1|k}={\mathrm{\Φ}}_k{\hat{X}}_{k|k}{\mathrm{\Φ}}_k^T$

式中，Φ_k表示k时刻四元数更新矩阵；表示k时刻状态K矩阵一步预测；表示k时刻一步预测状态协方差矩阵；表示k时刻过程噪声方差阵；

步骤4.2optimal-REQUEST量测更新

计算最优增益及权值更新：

$m_{k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})m_k+{\mathrm{\ρ}}_{k+1}^{*}\mathrm{\δ}{m}_{k+1}$

式中，表示k+1时刻最优增益；m_k+1表示k+1时刻状态K矩阵归一化权值；δm_k+1表示k+1时刻量测K矩阵归一化权值；

得到k+1时刻的状态估计和估计方差：

${\hat{X}}_{k+1|k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})\frac{m_k}{m_{k+1}}{\hat{X}}_{k+1|k}+{\mathrm{\ρ}}_{k+1}^{*}\frac{\mathrm{\δ}{m}_{k+1}}{m_{k+1}}{Y}_{k+1}$

式中，Y_k+1表示k+1时刻量测K矩阵；表示k+1时刻状态K矩阵最优估计；表示k+1时刻量测噪声方差阵；表示k+1时刻估计状态协方差矩阵；

过程噪声方差阵和量测噪声方差阵计算如下：

$\left\{\begin{array}{c}{\hat{U}}_{k|k}={\hat{B}}_{k|k}({\hat{B}}_{k|k}-{\widehat{\mathrm{\σ}}}_{k|k}{I}_3)\\ [{\hat{y}}_{k|k}\×]={\hat{U}}_{k|k}^T-{\hat{U}}_{k|k}\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；表示陀螺量测噪声方差值；Δt表示系统采样时间；

其中，表示向量观测随机噪声方差值；N(k+1)表示k+1时刻有效向量数；b_k+1,i表示k+1时刻第i个观测向量；r_k+1,i表示k+1时刻第i个参考向量；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

$\frac{1}{2}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i{\left|\right|b_i-A{r}_i\left|\right|}^2=1-q^T\mathrm{Kq}$

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

$\left\{\begin{array}{c}{\hat{y}}_{k|k}^{*}={[(1+{\widehat{\mathrm{\σ}}}_{k|k})I_3-{\hat{B}}_{k|k}^T-{\hat{B}}_{k|k}]}^{-1}{\hat{z}}_{k|k}\\ {\hat{q}}_{k|k}=\frac{1}{\sqrt{1+{\left|\right|{\hat{y}}_{k|k}^{*}\left|\right|}^2}}\left[\begin{array}{c}{\hat{y}}_{k|k}^{*}\\ 1\end{array}\right]\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；表示四元数乘法；表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数。

对本发明的有益效果说明如下：

MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：

陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz；星敏感器测量标准差为σ_s＝5”，输出频率为1Hz；陀螺初始漂移β＝[111]^Tdeg/hr；静态情况真实角速率为w＝[000]^Trad/s，动态情况真实角速率为w＝[-0.00230.0023-0.0023]^Trad/s；初始陀螺常值漂移设置为初始方差设置为P_0|0＝diag([(2°/hr)²(2°/hr)²(2°/hr)²])；初始K矩阵设置为初始过程噪声方差为初始权值为m₀＝1，步骤六中，M＝5400。

Claims (4)

1.一种用于卫星姿态确定的交互式滤波方法，其特征在于，包括以下几个步骤：

步骤1：读取传感器量测数据，所述传感器测量数据包括陀螺仪数据和星敏感器数据；

步骤2：建立卫星姿态估计系统的状态空间模型；

所述的卫星姿态估计系统状态空间模型包括：

(1)陀螺常值漂移估计状态空间模型

β_k+1＝β_k+η_k

$b_{k+1}=A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1}+{\mathrm{\δb}}_{k+1}$

$\mathrm{\Φ}\left({\mathrm{\ω}}_k\right)=\cos \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)I_4+\sin \left(\frac{1}{2}\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right)\mathrm{\Ω}\left({\mathrm{\ω}}_k\right)$

$\left({\mathrm{\ω}}_k\right)=\left[\begin{array}{cc}-[{\mathrm{\ω}}_k\×]& {\mathrm{\ω}}_k\\ {-\mathrm{\ω}}_k^T& 0\end{array}\right]$

式中：β_k表示k时刻陀螺常值漂移；β_k+1为k+1时刻陀螺常值漂移；表示k时刻最优估计四元数；Δt表示采样时间；η_k为零均值高斯白噪声，方差为Φ(·)为四元数更新矩阵；ω_k表示k时刻载体转速，[ω_k×]表示向量叉乘矩阵；r_k+1为k+1时刻向量观测器参考向量；b_k+1为k+1时刻向量观测器量测向量；为陀螺仪量测值，量测噪声方差为δb_k+1为零均值高斯白噪声，方差为A(·)表示姿态旋转矩阵。以上是单向量观测量测模型，对于一般星敏感器，在同一时间往往能够观测多颗有效星，其量测模型可以扩充为：

$\begin{array}{c}{z}_k=\begin{array}{}\end{array}\left[\begin{array}{c}{z}_{k+\mathrm{1,1}}\\ z_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ z_{k+1,N(k+1)}\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,1}\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ A\left(\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k){\hat{q}}_{k|k}\right)r_{k+1,N(k+1)}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\δb}}_{k+\mathrm{1,1}}\\ {\mathrm{\δb}}_{k+\mathrm{1,2}}\\ .\\ .\\ .\\ {\mathrm{\δb}}_{k+1,N(k+1)}\end{array}\right]\\ =h\left(x_k\right)+v_k\end{array}$

式中，z_k+1,i表示第i个有效矢量观测；δb_k+1,i为相应的随机噪声；N(k+1)表示k+1时刻的有效向量个数；x_k表示组合之后的状态参量，此处表示陀螺常值漂移β_k；v_k表示组合之后的零均值高斯白噪声；h(·)表示非线性量测函数；

(2)基于optimal-REQUEST方法的姿态确定状态空间模型

$X_{k+1}={\mathrm{\Φ}}_k{X}_k{\mathrm{\Φ}}_k^T+W_k$

${\mathrm{\Φ}}_k=\mathrm{\Φ}({\widetilde{\mathrm{\ω}}}_k-{\widehat{\mathrm{\β}}}_{k|k})$

Y_k+1＝X_k+1+V_k+1

$Y_{k+1}=\left[\begin{array}{cc}S-\mathrm{\δ}{I}_3& z\\ z^T& \mathrm{\σ}\end{array}\right]$

$\left\{\begin{array}{cc}S=B+B^T& B=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1},b_{k+1},r_{k+1,i}^T\\ z=\underset{i=1}{\overset{N(k+1)}{\mathrm{\Σ}}}{a}_{k+1},b_{k+1,i}\×r_{k+1,i}& \mathrm{\σ}=\mathrm{tr}\left(B\right)\end{array}\right.$

$W_k=\left[\begin{array}{cc}{S}_{\mathrm{\ϵ}}-{\mathrm{\σ}}_{\mathrm{\ϵ}}{I}_3& z_{\mathrm{\ϵ}}\\ z_{\mathrm{\ϵ}}^T& {\mathrm{\σ}}_{\mathrm{\ϵ}}\end{array}\right]\mathrm{\Δt}$

$\left\{\begin{array}{cc}{B}_{\mathrm{\ϵ}}=[\mathrm{\ϵ}\×]B& S_{\mathrm{\ϵ}}=B_{\mathrm{\ϵ}}+B_{\mathrm{\ϵ}}^T\\ [z_{\mathrm{\ϵ}}\×]=B_{\mathrm{\ϵ}}^T-B_{\mathrm{\ϵ}}& {\mathrm{\σ}}_{\mathrm{\ϵ}}=\mathrm{tr}\left(B_{\mathrm{\ϵ}}\right)\end{array}\right.$

式中：X_k表示状态K矩阵；Y_k+1为k+1时刻量测矩阵；V_k+1表示k+1时刻量测噪声；b_k+1,i表示k+1时刻第i个有效观测向量；r_k+1,i表示k+1时刻第i个有效参考向量；tr(·)表示取矩阵的迹；a_k+1,i表示k+1时刻第i个加权系数；W_k为系统过程噪声矩阵；ε为陀螺量测随机噪声向量；

步骤3：针对上述状态空间模型，在已知时刻陀螺常值漂移的状态估计和估计方差的情况下，利用标准容积卡尔曼滤波进行陀螺常值漂移估计的时间更新和量测更新，得到陀螺常值漂移的一步状态预测方差、量测预测方差以及互协方差；

步骤3.1时间更新；

已知k时刻的状态估计为以及估计协方差P_k|k，求取容积点为：

${\mathrm{\α}}_{i,k|k}=\sqrt{P_{k|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k|k},i=1,...,2n$

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}\sqrt{n}{e}_i,i=1,..,n\\ -\sqrt{n}{e}_{i-n},i=n+1,...,2n\end{array}\right.$

式中，n为状态变量的维数；ξ_i为第i个容积点；e_i表示n维单位列向量在第i个位置为1；P_k|k表示k时刻估计状态协方差矩阵；表示k时刻状态估计；α_i,k|k表示传递容积点；

由于陀螺常值漂移状态方程是线性方程，故容积点经过状态传递之后仍为原值；一步状态预测估计与估计方差：

${\widehat{\mathrm{\β}}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}{\mathrm{\α}}_{i.k|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\widehat{\mathrm{\β}}}_{k+1|k}^T+Q_k$

式中，表示一步预测状态；P_k+1|k表示一步预测状态协方差矩阵；Q_k表示k时刻过程噪声方差阵；

步骤3.2量测更新；

计算更新容积点：

${\mathrm{\λ}}_{i,k+1|k}=\sqrt{P_{k+1|k}}{\mathrm{\ξ}}_i+{\widehat{\mathrm{\β}}}_{k+1|k},i=\mathrm{1,2},...,2n$

容积点经过量测非线性函数传递为：

γ_i,k+1|k＝h(λ_i,k+1|k)

式中，h(·)的定义如前向量观测器非线性量测模型；λ_i,k+1|k为计算更新容积点；

量测一步预测、预测方差和互协方差为：

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}$

$P_{\mathrm{zz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,k+1|k}{\mathrm{\γ}}_{i,k+1|k}^T-{\hat{z}}_{k+1|k}{\hat{z}}_{k+1|k}^T+R_{k+1}$

$P_{\mathrm{xz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,k|k}{\mathrm{\γ}}_{i,k+1|k}^T-{\widehat{\mathrm{\β}}}_{k+1|k}{\hat{z}}_{k+1|k}^T$

式中，表示k+1时刻量测一步预测；P_zz,k+1|k表示k+1时刻量测一步预测协方差矩阵；P_xz,k+1|k表示k+1时刻量测与状态一步预测互协方差矩阵；R_k+1表示k+1时刻量测噪声方差阵；

陀螺常值漂移估计状态空间滤波增益、k+1时刻的状态估计及估计方差如下式所示：

式中，表示k+1时刻容积卡尔曼滤波增益矩阵；表示k+1时刻陀螺常值漂移最优估计；表示k+1时刻向量观测器量测向量；P_k+1|k+1表示k+1时刻估计状态误差协方差矩阵；

步骤4：利用步骤3估计的陀螺常值漂移补偿陀螺量测，然后利用补偿后的陀螺量测构造四元数更新矩阵，在optimal-REQUEST算法更新过程中，修正由于陀螺常值漂移引起的Φ_k不准确问题，在利用optimal-REQUEST算法实现最优K矩阵的求取：

步骤4.1optimal-REQUEST时间更新；

已知k时刻的状态K矩阵估计为以及估计协方差得到一步状态预测估计和估计方差：

${\hat{X}}_{k+1|k}={\mathrm{\Φ}}_k{\hat{X}}_{k|k}{\mathrm{\Φ}}_k^T$

式中，Φ_k表示k时刻四元数更新矩阵；表示k时刻状态K矩阵一步预测；表示k时刻一步预测状态协方差矩阵；表示k时刻过程噪声方差阵；

步骤4.2optimal-REQUEST量测更新

计算最优增益及权值更新：

$m_{k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})m_k+{\mathrm{\ρ}}_{k+1}^{*}\mathrm{\δ}{m}_{k+1}$

式中，表示k+1时刻最优增益；m_k+1表示k+1时刻状态K矩阵归一化权值；δm_k+1表示k+1时刻量测K矩阵归一化权值；

得到k+1时刻的状态估计和估计方差：

${\hat{X}}_{k+1|k+1}=(1-{\mathrm{\ρ}}_{k+1}^{*})\frac{m_k}{m_{k+1}}{\hat{X}}_{k+1|k}+{\mathrm{\ρ}}_{k+1}^{*}\frac{{\mathrm{\δm}}_{k+1}}{m_{k+1}}{Y}_{k+1}$

式中，Y_k+1表示k+1时刻量测K矩阵；表示k+1时刻状态K矩阵最优估计；表示k+1时刻量测噪声方差阵；表示k+1时刻估计状态协方差矩阵；

过程噪声方差阵和量测噪声方差阵计算如下：

$\left\{\begin{array}{c}{\hat{U}}_{k|k}={\hat{B}}_{k|k}({\hat{B}}_{k|k}-{\widehat{\mathrm{\σ}}}_{k|k}{I}_3)\\ [{\hat{y}}_{k|k}\×]={\hat{U}}_{k|k}^T-{\hat{U}}_{k|k}\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；表示陀螺量测噪声方差值；Δt表示系统采样时间；

其中，表示向量观测随机噪声方差值；N(k+1)表示k+1时刻有效向量数；b_k+1,i表示k+1时刻第i个观测向量；r_k+1,i表示k+1时刻第i个参考向量；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

$\frac{1}{2}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i{\left|\right|b_i-{\mathrm{Ar}}_i\left|\right|}^2=1-q^T\mathrm{Kq}$

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

$\left\{\begin{array}{c}{\hat{y}}_{k|k}^{*}={[(1+{\widehat{\mathrm{\σ}}}_{k|k})I_3-{\hat{B}}_{k|k}^T-{\hat{B}}_{k|K}]}^{-1}{\hat{z}}_{k|k}\\ {\hat{q}}_{k|k}=\frac{1}{\sqrt{1+{\left|\right|{\hat{y}}_{k|k}^{*}\left|\right|}^2}}\left[\begin{array}{c}{\hat{y}}_{k|k}^{*}\\ 1\end{array}\right]\end{array}\right.$

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

$\left\{\begin{array}{c}{\mathrm{\δq}}_{k|k}=q_k\⊗{\hat{q}}_{k|k}^{-1}\\ {\mathrm{\δ\φ}}_{k|k}=2\arccos \left({\mathrm{\δl}}_{k|k}\right)\end{array}\right.$

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；表示四元数乘法；δl_k|k表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数；

步骤6：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤3至步骤5，直至姿态估计系统运行结束。

2.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法，其特征在于，步骤2中，陀螺量测噪声标准差为陀螺常值漂移随机噪声标准差为输出频率为1Hz；星敏感器测量标准差为σ_s＝5”，输出频率为1Hz；陀螺初始漂移β＝[111]^Tdeg/hr，静态情况真实角速率为w＝[000]^Trad/s，动态情况真实角速率为w＝[-0.00230.0023-0.0023]^Trad/s。

3.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法，其特征在于，步骤3和步骤4中，初始陀螺常值漂移设置为 ${\widehat{\mathrm{\β}}}_{0|0}={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T\mathrm{deg}/\mathrm{hr},$ 初始方差设置为P_0|0＝diag([(2°/hr)²(2°/hr)²(2°/hr)²])；初始K矩阵设置为初始过程噪声方差为初始权值为m₀＝1。

4.根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法，其特征在于，步骤6中，M＝5400。