CN104019817B - 一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法。包括以下几个步骤：采集陀螺与星敏感器的输出数据；确定卫星姿态估计系统的状态变量和量测量；在k时刻进行标准的容积卡尔曼率波时间更新和量测更新，得到一步状态预测方差、一步量测预测方差及互协方差；利用多重次渐消因子对一步状态预测方差进行校正；重新进行容积卡尔曼滤波量测更新，求得k+1时刻的状态估计和状态估计方差；姿态估计非线性离散系统的结束时刻为M，若k+1＝M，则输出k+1时刻的状态估计的姿态四元数及陀螺漂移，完成姿态估计，若k+1＜M，令k＝k+1则重复步骤三至步骤五。本发明具有高估计精度和强鲁棒性的优点。

Description

一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波 方法

技术领域

本发明属于卫星姿态估计的技术领域，特别涉及一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法。

背景技术

卫星姿态估计技术是航天技术的关键技术之一，由陀螺与星敏感器组成的卫星姿态估计系统由于测姿精度高、可靠性好以及自主性强等优点得到了广泛的应用。针对该姿态估计系统，四元数由于计算简单，无三角函数的运算，同时又能避免欧拉角的奇异性问题，因此被作为系统的姿态描述参数。为提高姿态估计的精度以及姿态估计系统的适应能力和鲁棒性，非线性滤波算法提供了强有力的基础保障。扩展卡尔曼滤波(ExtendedKalman Filter,EKF)由于其结构简单、易于实现等优点被广泛应用于姿态估计中。但是，EKF存在理论上的局限性：1)模型线性化引入了截断误差，导致滤波精度下降，同时要计算雅克比矩阵，计算复杂；2)模型失配、未知干扰或状态突变等情况下，鲁棒性差；3)四元数作为状态变量存在范数约束，影响滤波精度。

为了克服EKF算法的局限性，无迹卡尔曼滤波算法(Unscented Kalman Filter,UKF)被提出，该滤波算法的核心思想是利用UT变换(Unscented Transformation，UT)产生一组确定性的Sigma点来近似非线性函数的后验均值和方差，精度能够达到二阶。随着非线性滤波算法的深入研究，在UKF算法的基础上，Ienkaran Arasaratnam和Simon Haykin在2009年提出了一种新的非线性滤波—容积卡尔曼滤波算法(Cubature Kalman filter，CKF)。CKF算法也是基于最优的高斯滤波框架，采用三阶球面相径容积规则来近似非线性函数的均值和方差，可以保证在理论上以三阶多项式逼近任何非线性高斯状态的后验均值和方差，相比较于UKF，具有实现简单，在高维情况下滤波精度高，收敛性好等优点。与EKF类似，CKF算法不足也在于在状态突变或模型不准确的情况下鲁棒性差，跟踪能力差，同时也没有考虑状态变量存在约束的情况。为了提高滤波算法在模型参数变化或系统发生突变时的跟踪能力，一种强跟踪扩展卡尔曼滤波(Strong Tracking EKF)被提出。此后，有学者将强跟踪思想与CKF算法结合，提出了强跟踪CKF(STCKF)算法，但该算法只是通过引入单重次渐消因子对预测误差协方差阵进行调整，虽然具有好的跟踪能力，但是对复杂的多变量系统，无法保证对每个变量都具有好的跟踪能力，同时也没有考虑状态变量存在约束的情况。

发明内容

本发明的目的是提供具有高估计精度和强鲁棒性的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法。

本发明是通过以下技术方案实现的：

一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，包括以下几个步骤：

步骤一：采集陀螺与星敏感器的输出数据；

步骤二：利用输出数据确定卫星姿态估计系统的状态变量和量测量；

步骤三：在k时刻进行标准的容积卡尔曼率波时间更新和量测更新，得到一步状态预测方差、一步量测预测方差及互协方差；

步骤四：利用多重次渐消因子对一步状态预测方差进行校正；

步骤五：利用矫正后的一步状态预测方差，重新进行容积卡尔曼滤波量测更新，求得k+1时刻的状态估计和状态估计方差；

步骤六：姿态估计非线性离散系统的结束时刻为M，若k+1＝M，则输出k+1时刻的状态估计的姿态四元数及陀螺漂移，完成姿态估计，若k+1<M，令k＝k+1则重复步骤三至步骤五。

本发明一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法还可以包括：

1、状态变量 $x_k={\left[\begin{array}{cc}{q}_k^T& {\mathrm{\&beta;}}_k^T\end{array}\right]}^T,$ q_k为姿态四元数，β_k为陀螺漂移，

建立卫星姿态估计系统的状态方程：

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)+w_k$

其中： $f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=\left[\begin{array}{cc}\cos \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)I_{4\&times;4}+\sin \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)\frac{\mathrm{\&Omega;}({\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k)}{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|}& 0_{4\&times;3}\\ 0_{3\&times;4}& I_{3\&times;3}\end{array}\right]\left[\begin{array}{c}{q}_k\\ {\mathrm{\&beta;}}_k\end{array}\right],$ ω＝[ω₁ ω₂ ω₃]^T，则 $[\mathrm{\&omega;}\&times;]=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_3& {\mathrm{\&omega;}}_2\\ {\mathrm{\&omega;}}_3& 0& -{\mathrm{\&omega;}}_1\\ {-\mathrm{\&omega;}}_2& {\mathrm{\&omega;}}_1& 0\end{array}\right],\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{cc}-[\mathrm{\&omega;}\&times;]& \mathrm{\&omega;}\\ {\mathrm{\&omega;}}^T& 0\end{array}\right],$ 为k时刻陀螺的输出值，Δt为陀螺的采样周期， $w_k=\left[\begin{array}{c}-\frac{\mathrm{\&Delta;t}}{2}\mathrm{\&Xi;}\left(q_k\right){\mathrm{\&eta;}}_v\\ {\mathrm{\&eta;}}_u\end{array}\right]$ 其均值为 $\left[\begin{array}{c}{0}_{4\&times;1}\\ 0_{3\&times;1}\end{array}\right],$ 方差为 $Q_k=\left[\begin{array}{cc}{\mathrm{\&sigma;}}_v^2\frac{\mathrm{\&Delta;t}}{4}[\mathrm{trace}({\hat{q}}_k{\hat{q}}_k^T+P_{q_k})I_{4\&times;4}-({\hat{q}}_k{\hat{q}}_k^T+P_{q_k})]& 0_{4\&times;3}\\ 0_{3\&times;4}& {\mathrm{\&sigma;}}_u^2\mathrm{\&Delta;t}{I}_{3\&times;3}\end{array}\right],$ σ_v为陀螺的测量噪声，σ_u为陀螺漂移的测量噪声，和为k时刻四元数的估计值和方差；

将星敏感器的输出作为量测量：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ z_k^i\end{array}\right]=\left[\begin{array}{c}A\left(q_k\right)r_k^1\\ A\left(q_k\right)r_k^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ A\left(q_k\right)r_k^i\end{array}\right]=\left[\begin{array}{c}{v}_s^1\\ v_s^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ v_s^i\end{array}\right]=h\left(x_k\right)+v_k$

式中，i为星敏感器观测到的恒星个数，为第i个量测量，为对应的参考矢量，为量测噪声，v_k为零均值并且方差为的高斯白噪声，σ_s为星敏感器量测噪声，A(q_k)为四元数描述的载体姿态矩阵，表示为：

$A\left(q_K\right)=(q_4^2-{{\mathrm{\&rho;}}_k}^T{\mathrm{\&rho;}}_k)I_{3\&times;3}+2{\mathrm{\&rho;}}_k{{\mathrm{\&rho;}}_k}^T-2q_4[{\mathrm{\&rho;}}_k\&times;]$

式中， $q_k={\left[\begin{array}{cc}{{\mathrm{\&rho;}}_k}^T& q_4\end{array}\right]}^T.$

3、2、得到一步状态预测方差、一步量测预测方差及互协方差的方法为：

步骤3.1：时间更新，求得一步状态预测方差；

由k时刻的状态估计和状态估计方差P_k|k，求取容积点为：

${\mathrm{\&alpha;}}_i^{\&prime;}=S_{k|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k|k},i=\mathrm{1,2},...,2n$

其中，n为状态变量的维数，是第i个容积点，其产生方式为：n维单位向量为e＝[1 0 … 0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，称为完整全对称点集，[1]_i表示点集中[1]的第i个点；

容积点经过状态方程的传递值为：

${\mathrm{\&gamma;}}_{k+1|k}^i=f({\mathrm{\&alpha;}}_i^{\&prime;},{\widetilde{\mathrm{\&omega;}}}_k)$

一步状态预测和一步状态预测方差为：

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k=J_{k+1}+Q_k;$

步骤3.2：量测更新，得到一步量测预测方差及互协方差；

由一步状态预测方差P_k+1|k，求取更新后的容积点：

${\mathrm{\&eta;}}_i^{\&prime;}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,2n$

其中，一步状态预测方差P_k+1|k分解可得：

容积点经过量测的传递值为：

${\mathrm{\&eta;}}_{k+1|k}^i=h\left({\mathrm{\&eta;}}_i^{\&prime;}\right)$

一步量测预测一步量测预测方差P_zz,k+1|k及互协方差P_xz,k+1|k为：

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^i$

$P_{\mathrm{zz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^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{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{z}}_{k+1|k}^T.$

3、多重次渐消因子满足以下方程：

$\begin{array}{c}{M}_{k+1}=H_{k+1}{\mathrm{\&Lambda;}}_{k+1}{J}_{k+1}{\mathrm{\&Lambda;}}_{k+1}^T{H}_{k+1}^T\\ N_{k+1}=V_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}\\ M_{k+1}=N_{k+1}\end{array}$

其中，Λ_k+1＝diag(λ_1,k+1,λ_2,k+1,…λ_n,k+1)为多重次渐消因子，V_k+1为残差输出序列的协方差阵，残差为ρ为遗忘因子，令 $L_{k+1}={\left(H_{k+1}^T{H}_{k+1}\right)}^{-1}{H}_{k+1}^T{N}_{k+1}{H}_{k+1}{\left(H_{k+1}^T{H}_{k+1}\right)}^{-1},$

多重次渐消因子Λ_k+1中的元素为：

${\mathrm{\&lambda;}}_{i,k+1}=\max \{1,\sqrt{\frac{L_{\mathrm{ii}}}{N_{\mathrm{II}}}}\},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,n$

其中，L_ii和N_ii分别为矩阵L_k+1和N_k+1对角线上的元素，

利用多重次渐消因子，得到矫正后的一步状态预测方差：

$P_{k+1|k}^t={\mathrm{\&Lambda;}}_{k+1}[\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T]{\mathrm{\&Lambda;}}_{k+1}^T+Q_k.$

4、求得k+1时刻的状态估计和状态估计方差的方法为：

步骤5.1：利用矫正后的一步状态预测方差进行量测更新；

更新后的一步量测预测、更新后的一步量测预测方差及更新后的互协方差为：

${\hat{z}}_{k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}$

$P_{\mathrm{zz},k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{z}}_{k+1|k}^t{\left({\hat{z}}_{k+1|k}^t\right)}^T+R_{k+1}$

$P_{\mathrm{xz},k+1|}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{x}}_{k+1|k}{\left({\hat{z}}_{k+1|k}^t\right)}^T$

其中二次更新后的容积点为：

${\mathrm{\&eta;}}_i^{\&prime;\&prime;}=S_{t+1|k}^t{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=\mathrm{1,2},...,2n$

二次更新后的容积点经过量测的传递值为：

${\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}=h\left({\mathrm{\&eta;}}_i^{\&prime;\&prime;}\right);$

步骤5.2：求解满足四元数范数约束条件的滤波增益；

将更新后的互协方差 $P_{\mathrm{xz},k+1|k}^t=\left[\begin{array}{c}{P}_{\mathrm{xz}}^q\\ P_{\mathrm{xz}}^{\mathrm{\&beta;}}\end{array}\right]$ 分为四元数部分和陀螺漂移部分满足四元数范数约束条件的滤波增益K_k+1为：

$K_{k+1}=\left[\begin{array}{c}(P_{\mathrm{xz}}^q-{\mathrm{\&lambda;}}_{k+1}{\hat{q}}_{k+1|k}{r}_{k+1|k}^T){(P_{\mathrm{zz},k+1|k}^t+{\mathrm{\&lambda;}}_{k+1}{r}_{k+1|k}{r}_{k+1|k}^T)}^{-1}\\ P_{\mathrm{xz}}^{\mathrm{\&beta;}}{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}\end{array}\right]$

式中， $r_{k+1|k}=z_{k+1}-{\hat{z}}_{k+1|k}^t,{\mathrm{\&lambda;}}_{k+1}=-\frac{1}{a}+\frac{\left|\right|{\hat{q}}_{k+1|k}+P_{\mathrm{xz}}^q{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}{r}_{k+1|k}\left|\right|}{a},a=r_{k+1|k}^T{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}{r}_{k+1|k},{\hat{q}}_{k+1|k}$ 为一步状态预测估计值中的四元数部分；

步骤5.3：利用滤波增益，求得k+1时刻的状态估计和状态估计方差为：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k+1|k}^t)$

$P_k=P_{k+1|k}^t-K_k{\left(P_{\mathrm{xz},k+1|k}^t\right)}^T-P_{\mathrm{xz},k+1|k}^t{K}_k^T+K_k{P}_{\mathrm{zz},k+1|k}^t{K}_k^T.$

5、陀螺测量噪声为σ_v＝0.35°/h，陀螺漂移的两侧噪声为星敏感器测量噪声为σ_s＝18″，初始陀螺漂移β＝[1 1 1]^T°/h，初始状态估计值为 ${\hat{x}}_{0|0}={\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T,$ 初始方差阵设为P_0|0＝diag([(0.2°)² (0.2°)² (0.2°)² (0.2°)² (1.2°/h)² (1.2°/h)²(1.2°/h)²]。

本发明的有益效果为：

(1)本发明采用容积数值积分理论近似非线性函数的均值与方差，不需要对非线性函数进行线性化，避免了雅克比矩阵的计算，有利于提高估计精度；

(2)本发明用多重次渐消因子代替单一的渐消因子，不仅使得各个滤波通道具有不同的调节能力，同时也能保证预测误差协方差阵的对称性，使得滤波器具有在系统发生突变情况下的鲁棒性，提高了姿态估计系统的跟踪能力。

(3)本发明利用四元数范数约束条件，在最小均方误差意义下对滤波增益进行校正，保证了状态估计中的四元数成分满是范数约束条件，有利于提高姿态估计的精度。

附图说明

图1是本发明的方法流程图。

图2是现有的MEKF算法的一次独立实验姿态误差和陀螺漂移误差结果图；

图3是现有的STCKF算法的一次独立实验姿态误差和陀螺漂移误差结果图；

图4是本发明的NSTCKF算法的一次独立实验姿态误差和陀螺漂移误差结果图；

图5是本发明的NSTCKF算法与MEKF，STCKF算法的姿态角均方根误差结果对比图；

图6是本发明的NSTCKF算法与MEKF，STCKF算法的陀螺漂移均方根误差结果对比图。

具体实施方式

下面结合附图对本发明做进一步详细说明。

本发明提出了一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法(Norm-constrained Strong Tracking Cubature Kalman Filter,NSTCKF)。本发明采用容积数值积分理论近似非线性函数的均值与方差，并通过引入两个多重次渐消因子对预测误差协方差阵进行调整，使得不同的滤波通道具有不同的调节能力，保证预测误差协方差阵的对称性，从而实现滤波算法强跟踪性，同时根据四元数范数约束条件，设计最小均方误差意义下的最优滤波增益。

本发明是一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，流程如图1所示，包括以下几个步骤：

步骤一：采集陀螺与星敏感器的输出数据，并将其作为量测量；

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

将姿态四元数q_k和陀螺漂移β_k组成状态变量 $x_k={\left[\begin{array}{cc}{q}_k^T& {\mathrm{\&beta;}}_k^T\end{array}\right]}^T,$ 建立基于四元数的卫星姿态估计系统离散状态方程：

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)+w_k---\left(1\right)$

式中： $f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=\left[\begin{array}{cc}\cos \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)I_{4\&times;4}+\sin \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)\frac{\mathrm{\&Omega;}({\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k)}{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|}& 0_{4\&times;3}\\ 0_{3\&times;4}& I_{3\&times;3}\end{array}\right]\left[\begin{array}{c}{q}_k\\ {\mathrm{\&beta;}}_k\end{array}\right];$ 若ω＝[ω₁ ω₂ ω₃]^T，则 $[\mathrm{\&omega;}\&times;]=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_3& {\mathrm{\&omega;}}_2\\ {\mathrm{\&omega;}}_3& 0& -{\mathrm{\&omega;}}_1\\ {-\mathrm{\&omega;}}_2& {\mathrm{\&omega;}}_1& 0\end{array}\right],\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{cc}-[\mathrm{\&omega;}\&times;]& \mathrm{\&omega;}\\ {\mathrm{\&omega;}}^T& 0\end{array}\right];$ 为k时刻陀螺的输出值；Δt为陀螺的采样周期； $w_k=\left[\begin{array}{c}-\frac{\mathrm{\&Delta;t}}{2}\mathrm{\&Xi;}\left(q_k\right){\mathrm{\&eta;}}_v\\ {\mathrm{\&eta;}}_u\end{array}\right],$ 其均值为 $\left[\begin{array}{c}{0}_{4\&times;1}\\ 0_{3\&times;1}\end{array}\right],$ 方差为 $Q_k=\left[\begin{array}{cc}{\mathrm{\&sigma;}}_v^2\frac{\mathrm{\&Delta;t}}{4}[\mathrm{trace}({\hat{q}}_k{\hat{q}}_k^T+P_{q_k})I_{4\&times;4}-({\hat{q}}_k{\hat{q}}_k^T+P_{q_k})]& 0_{4\&times;3}\\ 0_{3\&times;4}& {\mathrm{\&sigma;}}_u^2\mathrm{\&Delta;t}{I}_{3\&times;3}\end{array}\right];$ σ_v为陀螺的测量噪声；σ_u为陀螺漂移的测量噪声；和为k时刻四元数的估计值和方差；

假设星敏感器观测到多颗恒星，以星敏感器的输出作为量测量，卫星姿态估计系统的离散量测方程为：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ z_k^i\end{array}\right]=\left[\begin{array}{c}A\left(q_k\right)r_k^1\\ A\left(q_k\right)r_k^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ A\left(q_k\right)r_k^i\end{array}\right]=\left[\begin{array}{c}{v}_s^1\\ v_s^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ v_s^i\end{array}\right]=h\left(x_k\right)+v_k---\left(2\right)$

式中，i为星敏感器观测到的恒星个数；为第i个测量矢量；为对应的参考矢量；为量测噪声；v_k为零均值，方差为的高斯白噪声；σ_s为星敏感器量测噪声；A(q_k)为四元数描述的载体姿态矩阵，可表示为：

$A\left(q_K\right)=(q_4^2-{{\mathrm{\&rho;}}_k}^T{\mathrm{\&rho;}}_k)I_{3\&times;3}+2{\mathrm{\&rho;}}_k{{\mathrm{\&rho;}}_k}^T-2q_4[{\mathrm{\&rho;}}_k\&times;]---\left(3\right)$

式中， $q_k={\left[\begin{array}{cc}{{\mathrm{\&rho;}}_k}^T& q_4\end{array}\right]}^T;$

由式(1)和(2)就构成了卫星姿态估计系统的状态空间模型；

步骤三：针对上述状态空间模型，在已知k时刻的状态估计和估计方差情况下，利用标准的容积卡尔曼滤波进行时间更新和量测更新，得到未引入渐消因子前的一步状态预测方差、量测预测方差以及互协方差；

步骤3.1时间更新；

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

${\mathrm{\&alpha;}}_i^{\&prime;}=S_{k|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k|k},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,2n---\left(4\right)$

式中，n为状态变量的维数；是第i个容积点，其产生方式为：设n维单位向量为e＝[1 0 … 0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，称为完整全对称点集，[1]_i表示点集中[1]的第i个点；

容积点经过状态非线性函数传递为：

${\mathrm{\&gamma;}}_{k+1|k}^i=f({\mathrm{\&alpha;}}_i^{\&prime;},{\widetilde{\mathrm{\&omega;}}}_k)---\left(5\right)$

一步状态预测的估计与估计方差为：

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k---\left(6\right)}^i$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k=J_{k+1}+Q_k---\left(7\right)$

步骤3.2量测更新；

将状态预测估计方差P_k+1|k，进行Cholesky分解可得：相应的容积点可以求取为：

${\mathrm{\&eta;}}_i^{\&prime;}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,2n---\left(8\right)$

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

${\mathrm{\&eta;}}_{k+1|k}^i=h\left({\mathrm{\&eta;}}_i^{\&prime;}\right)---\left(9\right)$

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

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}---\left(10\right)$

$P_{\mathrm{zz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^T-{\hat{z}}_{k+1|k}{\hat{z}}_{k+1|k}^T+R_{k+1}---\left(11\right)$

$P_{\mathrm{xz},k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{z}}_{k+1|k}^T---\left(12\right)$

步骤四：引入多重次渐消因子对上述一步状态预测方差进行校正；

步骤4.1多重次渐消因子的求取；

利用式(7)和式(12)所示的一步状态预测方差和互协方差，求得：

$H_{k+1}=P_{\mathrm{xz},k+1|k}^T{\left(P_{k+1|k}\right)}^{-1}---\left(13\right)$

由强跟踪理论可以得出： $\begin{array}{c}{M}_{k+1}=H_{k+1}{\mathrm{\&Lambda;}}_{k+1}{J}_{k+1}{\mathrm{\&Lambda;}}_{k+1}^T{H}_{k+1}^T\\ N_{k+1}=V_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}\\ M_{k+1}=N_{k+1}\end{array}---\left(14\right)$

式中，Λ_k+1＝diag(λ_1,k+1,λ_2,k+1,…λ_n,k+1)为多重次渐消因子；V_k+1为残差输出序列的协方差阵，令残差为ρ为遗忘因子，通常设为ρ＝0.95，则有：

$V_{k+1}=\left\{\begin{array}{c}{r}_1{r_1}^T,k=0\\ \frac{{\mathrm{\&rho;V}}_k+r_{k+1}{r}_{k+1}^T}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.---\left(15\right)$

根据式(13)-(15)，令 $L_{k+1}={\left(H_{k+1}^T{H}_{k+1}\right)}^{-1}{H}_{k+1}^T{N}_{k+1}{H}_{k+1}{\left(H_{k+1}^T{H}_{k+1}\right)}^{-1},$ 多重次渐消因子Λ_k+1中的元素为：

${\mathrm{\&lambda;}}_{i,k+1}=\max \{1,\sqrt{\frac{L_{\mathrm{ii}}}{N_{\mathrm{II}}}}\},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,n---\left(16\right)$

式中，L_ii和N_ii分别为矩阵L_k+1和N_k+1对角线上的元素；

步骤4.1一步状态预测方差的校正；

利用上述求得的多重次渐消因子，对一步状态预测方差进行校正：

$P_{k+1|k}^t={\mathrm{\&Lambda;}}_{k+1}[\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T]{\mathrm{\&Lambda;}}_{k+1}^T+Q_k.---\left(17\right)$

步骤五：利用上述校正的状态预测方差，重新进行容积卡尔曼滤波量测更新，然后在最小均方误差意义下利用四元数范数约束条件求得滤波增益，最后得到已知k+1时刻的状态估计值和方差；

步骤5.1利用进行量测更新；

将修正的状态预测估计方差进行Cholesky分解可得：相应的容积点可以求取为：

${\mathrm{\&eta;}}_i^{\&prime;\&prime;}=S_{k+1|k}^t{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,2n---\left(18\right)$

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

${\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}=h\left({\mathrm{\&eta;}}_i^{\&prime;\&prime;}\right)---\left(19\right)$

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

${\hat{z}}_{k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}---\left(20\right)$

$P_{\mathrm{zz},k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{z}}_{k+1|k}^t{\left({\hat{z}}_{k+1|k}^t\right)}^T+R_{k+1}---\left(21\right)$

$P_{\mathrm{xz},k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{x}}_{k+1|k}{\left({\hat{z}}_{k+1|k}^t\right)}^T---\left(22\right)$

步骤5.2四元数范数约束条件滤波增益的求解；

将互协方差 $P_{\mathrm{xz},k+1|k}^t=\left[\begin{array}{c}{P}_{\mathrm{xz}}^q\\ P_{\mathrm{xz}}^{\mathrm{\&beta;}}\end{array}\right]$ 分为四元数部分和陀螺漂移部分则满足四元数范数约束条件且使得估计方差最小的滤波增益K_k+1为：

$K_{k+1}=\left[\begin{array}{c}(P_{\mathrm{xz}}^q-{\mathrm{\&lambda;}}_{k+1}{\hat{q}}_{k+1|k}{r}_{k+1|k}^T){(P_{\mathrm{zz},k+1|k}^t+{\mathrm{\&lambda;}}_{k+1}{r}_{k+1|k}{r}_{k+1|k}^T)}^{-1}\\ P_{\mathrm{xz}}^{\mathrm{\&beta;}}{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}\end{array}\right]---\left(23\right)$

式中， $r_{k+1|k}=z_{k+1}-{\hat{z}}_{k+1|k}^t;{\mathrm{\&lambda;}}_{k+1}=-\frac{1}{a}+\frac{\left|\right|{\hat{q}}_{k+1|k}+P_{\mathrm{xz}}^q{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}{r}_{k+1|k}\left|\right|}{a};a=r_{k+1|k}^T{\left(P_{\mathrm{zz},k+1|k}^t\right)}^{-1}{r}_{k+1|k};{\hat{q}}_{k+1|k}$ 为一步状态预测估计值中的四元数部分；

步骤5.3k+1时刻的状态估计值和方差的求取；

利用上述滤波增益，求取k+1时刻的状态估计值和方差为：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k+1|k}^t)---\left(24\right)$

$P_k=P_{k+1|k}^t-K_k{\left(P_{\mathrm{xz},k+1|k}^t\right)}^T-P_{\mathrm{xz},k+1|k}^t{K}_k^T+K_k{P}_{\mathrm{zz},k+1|k}^t{K}_k^T---\left(25\right)$

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

本发明提出的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法是通过Matlab仿真软件进行仿真实验，与现有滤波算法的估计性能进行比较，如乘性扩展卡尔曼滤波(Multiplicative Extended Kalman Filter,MEKF)以及单渐消因子的强跟踪容积卡尔曼滤波(STCKF)。仿真硬件环境均为Intel (R) Core (TM) i5-2410M CPU2.30GHz，4G RAM，Windows7操作系统。如图2到图4所示，在200秒时引入状态突变量，MEKF算法估计的不论是姿态角误差还是陀螺漂移误差的精度都急剧下降，而STCKF算法估计的姿态角误差不受状态突变量的影响，但是陀螺漂移误差的精度急剧下降，相比较于MEKF算法，其收敛速度较快。本文提出的NSTCKF算法估计的姿态角误差和陀螺漂移误差都不受状态突变量的影响，具有很好的鲁棒性和稳定性。从图5到图6所示的均方根误差比较，NSTCKF算法不论是姿态角误差估计还是陀螺漂移误差估计都比MEKF和STCKF算法，具有更高的稳态估计精度。

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

陀螺测量噪声标准差为σ_v＝0.35°/h，陀螺漂移噪声标准差为输出频率为100Hz；星敏感器测量噪声标准差为σ_s＝18″，输出频率为1Hz；初始陀螺漂移β＝[1 1 1]^T°/h；仿真时间400s，在200s处引入状态突变；初始状态估计为 ${\hat{x}}_{0|0}={\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T;$ 初始方差阵为P_0|0＝diag([(0.2°)² (0.2°)² (0.2°)² (0.2°)² (1.2°/h)² (1.2°/h)² (1.2°/h)²]。

Claims (6)

1.一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于，包括以下几个步骤：

步骤一：采集陀螺与星敏感器的输出数据；

步骤二：利用输出数据确定卫星姿态估计系统的状态变量和量测量；

步骤三：在k时刻进行标准的容积卡尔曼滤波时间更新和量测更新，得到一步状态预测方差、一步量测预测方差及互协方差；

步骤四：利用多重次渐消因子对一步状态预测方差进行校正；

步骤五：利用校正后的一步状态预测方差，重新进行容积卡尔曼滤波量测更新，求得k+1时刻的状态估计和状态估计方差；

步骤六：姿态估计非线性离散系统的结束时刻为M，若k+1＝M，则输出k+1时刻的状态估计的姿态四元数及陀螺漂移，完成姿态估计，若k+1＜M，令k＝k+1则重复步骤三至步骤五。

2.根据权利要求1所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于：所述的状态变量q_k为姿态四元数，β_k为陀螺漂移，

建立卫星姿态估计系统的状态方程：

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)+w_k$

其中：ω＝[ω₁ ω₂ ω₃]^T，则 为k时刻陀螺的输出值，Δt为陀螺的采样周期，其均值为方差为σ_v为陀螺的测量噪声，σ_u为陀螺漂移的测量噪声，和P_qk为k时刻四元数的估计值和方差；

将星敏感器的输出作为量测量：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ .\\ .\\ .\\ z_k^i\end{array}\right]=\left[\begin{array}{c}A\left(q_k\right)r_k^1\\ A\left(q_k\right)r_k^2\\ .\\ .\\ .\\ A\left(q_k\right)r_k^i\end{array}\right]+\left[\begin{array}{c}{v}_s^1\\ v_s^2\\ .\\ .\\ .\\ v_s^i\end{array}\right]=h\left(x_k\right)+v_k$

式中，i为星敏感器观测到的恒星个数，为第i个量测量，为对应的参考矢量，为量测噪声，v_k为零均值并且方差为的高斯白噪声，σ_s为星敏感器量测噪声，A(q_k)为四元数描述的载体姿态矩阵，表示为：

$A\left(q_k\right)=(q_4^2-{{\mathrm{\&rho;}}_k}^T{\mathrm{\&rho;}}_k)I_{3\&times;3}+2{\mathrm{\&rho;}}_k{{\mathrm{\&rho;}}_k}^T-2q_4\&lsqb;{\mathrm{\&rho;}}_k\&times;\&rsqb;$

式中，

3.根据权利要求2所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，

其特征在于：所述的得到一步状态预测方差、一步量测预测方差及互协方差的方法为：步骤3.1：时间更新，求得一步状态预测方差；

由k时刻的状态估计和状态估计方差P_k|k，求取容积点为：

${\mathrm{\&alpha;}}_i^{\&prime;}=S_{k|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k|k},i=1,2,...,2n$

其中，n为状态变量的维数，是第i个容积点，其产生方式为：n维单位向量为e＝[1 0 … 0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，称为完整全对称点集，[1]_i表示点集中[1]的第i个点；

容积点经过状态方程的传递值为：

${\mathrm{\&gamma;}}_{k+1|k}^i=f({\mathrm{\&alpha;}}_i^{\&prime;},{\widetilde{\mathrm{\&omega;}}}_k)$

一步状态预测和一步状态预测方差为：

${\hat{x}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&gamma;}}_{k+1|k}^i$

$P_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T+Q_k=J_{k+1}+Q_k;$

步骤3.2：量测更新，得到一步量测预测方差及互协方差；

由一步状态预测方差P_k+1|k，求取更新后的容积点：

${\mathrm{\&eta;}}_i^{\&prime;}=S_{k+1|k}{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=1,2,...,2n$

其中，一步状态预测方差P_k+1|k分解可得：

容积点经过量测的传递值为：

${\mathrm{\&eta;}}_{k+1|k}^i=h\left({\mathrm{\&eta;}}_i^{\&prime;}\right)$

一步量测预测一步量测预测方差P_zz,k+1|k及互协方差P_xz,k+1|k为：

${\hat{z}}_{k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&eta;}}_{k+1|k}^i$

$P_{zz,k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&eta;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^T-{\hat{z}}_{k+1|k}{\hat{z}}_{k+1|k}^T+R_{k+1}$

$P_{xz,k+1|k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{z}}_{k+1|k}^T.$

4.根据权利要求3所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于：所述的多重次渐消因子满足以下方程：

$M_{k+1}=H_{k+1}{\mathrm{\&Lambda;}}_{k+1}{J}_{k+1}{\mathrm{\&Lambda;}}_{k+1}^T{H}_{k+1}^T$

$N_{k+1}=V_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}$

M_k+1＝N_k+1

其中，Λ_k+1＝diag(λ_1,k+1,λ_2,k+1,…λ_n,k+1)为多重次渐消因子，V_k+1为残差输出序列的协方差阵，残差为ρ为遗忘因子，令

多重次渐消因子Λ_k+1中的元素为：

${\mathrm{\&lambda;}}_{i,k+1}=max\{1,\sqrt{\frac{L_{ii}}{N_{ii}}}\},i=1,2,...,n$

其中，L_ii和N_ii分别为矩阵L_k+1和N_k+1对角线上的元素，

利用多重次渐消因子，得到校正后的一步状态预测方差：

$P_{k+1|k}^t={\mathrm{\&Lambda;}}_{k+1}\&lsqb;\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&gamma;}}_{k+1|k}^i\right)}^T-{\hat{x}}_{k+1|k}{\hat{x}}_{k+1|k}^T\&rsqb;{\mathrm{\&Lambda;}}_{k+1}^T+Q_k.$

5.根据权利要求4所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于：所述的求得k+1时刻的状态估计和状态估计方差的方法为：

步骤5.1：利用校正后的一步状态预测方差进行量测更新；

更新后的一步量测预测、更新后的一步量测预测方差及更新后的互协方差为：

${\hat{z}}_{k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}$

$P_{zz,k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{z}}_{k+1|k}^t{\left({\hat{z}}_{k+1|k}^t\right)}^T+R_{k+1}$

$P_{xz,k+1|k}^t=\frac{1}{2n}\underset{i=1}{\overset{2n}{\&Sigma;}}{\mathrm{\&gamma;}}_{k+1|k}^i{\left({\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}\right)}^T-{\hat{x}}_{k+1|k}{\left({\hat{z}}_{k+1|k}^t\right)}^T$

其中二次更新后的容积点为：

${\mathrm{\&eta;}}_i^{\&prime;\&prime;}=S_{k+1|k}^t{\mathrm{\&xi;}}_i+{\hat{x}}_{k+1|k},i=1,2,...,2n$

二次更新后的容积点经过量测的传递值为：

${\mathrm{\&eta;}}_{k+1|k}^{\&prime;i}=h\left({\mathrm{\&eta;}}_i^{\&prime;\&prime;}\right);$

步骤5.2：求解满足四元数范数约束条件的滤波增益；

将更新后的互协方差分为四元数部分和陀螺漂移部分满足四元数范数约束条件的滤波增益K_k+1为：

$K_{k+1}=\left[\begin{array}{c}(P_{xz}^q-{\mathrm{\&lambda;}}_{k+1}{\hat{q}}_{k+1|k}{r}_{k+1|k}^T){(P_{zz,k+1|k}^t+{\mathrm{\&lambda;}}_{k+1}{r}_{k+1|k}{r}_{k+1|k}^T)}^{-1}\\ P_{xz}^{\mathrm{\&beta;}}{\left(P_{zz,k+1|k}^t\right)}^{-1}\end{array}\right]$

式中， 为一步状态预测估计值中的四元数部分；

步骤5.3：利用滤波增益，求得k+1时刻的状态估计和状态估计方差为：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k+1|k}^t)$

$P_k=P_{k+1|k}^t-K_k{\left(P_{xz,k+1|k}^t\right)}^T-P_{xz,k+1|k}^t{K}_k^T+K_k{P}_{zz,k+1|k}^t{K}_k^T.$

6.根据权利要求5所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于：所述的陀螺测量噪声为σ_v＝0.35°/h，陀螺漂移的两侧噪声为星敏感器测量噪声为σ_s＝18”，初始陀螺漂移β＝[1 1 1]^T°/h，初始状态估计值为初始方差阵设为P_0|0＝diag([(0.2°)² (0.2°)²(0.2°)² (0.2°)² (1.2°/h)² (1.2°/h)² (1.2°/h)²]。