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

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

式中， 。

3.根据权利要求2所述的一种用于卫星姿态估计的范数约束强跟踪容积卡尔曼滤波方法，其特征在于：所述的得到一步状态预测方差、一步量测预测方差及互协方差的方法为： 

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

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

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

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

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

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

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

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

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

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

。

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

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

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

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

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

。