[发明专利]一种用于卫星姿态确定的交互式滤波方法

权利要求书

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

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

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

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

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

β_k+1＝β_k+η_k

bk+1=A(Φ(ω~k-βk)q^k|k)rk+1+δbk+1]]>

Φ(ωk)=cos(12||ωk||Δt)I4+sin(12||ωk||Δt)Ω(ωk)]]>

Ω(ωk)=-[ωk×]ωk-ωkT0]]>

式中：β_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(·)表示姿态旋转矩阵；是单向量观测量测模型，对于星敏感器，在同一时间能够观测多颗有效星，其量测模型扩充为：

zk=zk+1,1zk+1,2...zk+1,N(k+1)=A(Φ(ω~k-βk)q^k|k)rk+1,1A(Φ(ω~k-βk)q^k|k)rk+1,2...A(Φ(ω~k-βk)q^k|k)rk+1,N(k+1)+δbk+1,1δbk+1,2...δbk+1,N(k+1)=h(xk)+vk]]>

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

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

Xk+1=ΦkXkΦkT+Wk]]>

Φk=Φ(ω~k-β^k|k)]]>

Y_k+1＝X_k+1+V_k+1

Yk+1=S-σI3zzTσ]]>

S=B+BTB=Σi=1N(k+1)ak+1,ibk+1,irk+1,iTz=Σi=1N(k+1)ak+1,ibk+1,i×rk+1,iσ=tr(B)]]>

Wk=Sϵ-σϵI3zϵzϵTσϵΔt]]>

Bϵ=[ϵ×]BSϵ=Bϵ+BϵT[zϵ×]=BϵT-Bϵσϵ=tr(Bϵ)]]>

式中：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，求取容积点为：

αi,k|k=Pk|kξi+β^k|k,i=1,...,2n]]>

ξi=nei,i=1,...,n-nei-n,i=n+1,...,2n]]>

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

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

β^k+1|k=12nΣi=12nαi,k|k]]>

Pk+1|k=12nΣi=12nαi,k|kαi,k|kT-β^k+1|kβ^k+1|kT+Qk]]>

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

步骤3.2量测更新：

计算更新容积点：

λi,k+1|k=Pk+1|kξi+βk+1|k,i=1,2,...,2n]]>

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

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

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

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

z^k+1|k=12nΣi=12nγi,k+1|k]]>

Pzz,k+1|k=12nΣi=12nγi,k+1|kγi,k+1/kT-z^k+1|kz^k+1|kT+Rk+1]]>

Pxz,k+1|k=12nΣi=12nαi,k|kγi,k+1|kT-β^k+1|kz^k+1|kT]]>

式中，表示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矩阵估计为以及估计协方差得到一步状态预测估计和估计方差：

X^k+1|k=ΦkX^k|kΦkT]]>

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

步骤4.2optimal-REQUEST量测更新：

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

mk+1=(1-ρk+1*)mk+ρk+1*δmk+1]]>

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

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

X^k+1|k+1=(1-ρk+1*)mkmk+1X^k+1|k+ρk+1*δmk+1mk+1Yk+1]]>

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

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

U^k|k=B^k|k(B^k|k-σ^k|kI3)[y^k|k×]=U^k|kT-U^k|k]]>

式中，与为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问题可知：

12Σi=1nai||bi-Ari||2=1-qTKq]]>

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

y^k|k*=[(1+σ^k|k)I3-B^k|kT-B^k|k]-1z^k|kq^k|k=11+||y^k|k*||2y^k|k*1]]>

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；表示由和计算得到的中间变量；表示估计的四元数；由得到，其中，表示的第4行第4列元素；

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

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

δqk|k=qk⊗q^k|k-1δφk|k=2arc cos(δlk|k)]]>

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

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