基于容积四元数估计的航天器姿态估计方法.pdf

基于容积四元数估计的航天器姿态估计方法，本发明涉及基于容积四元数估计以及星敏感器与陀螺组合的航天器姿态估计方法。本发明为了解决现有技术存在乘性噪声及噪声相关问题，导致航天器姿态估计精度低的问题。本发明包括：步骤一：建立航天器姿态运动学模型和观测模型；步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声；步骤三：采用容积四元数姿态估计器对航天器姿态进行估计。本发明所提出的CQE??MCNS算法相比CQE算法，角度估计误差减小0.0002°左右，陀螺漂移估计误差减小0.002°/h左右，因此更适用于具有乘性噪声及噪声相关的航天器姿态估计问题。本发明用于航天领域。

1.基于容积四元数估计的航天器姿态估计方法，其特征在于：所述航天器姿态估计方
法的具体过程为：
步骤一：建立航天器姿态运动学模型和观测模型；
步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的
噪声；
步骤三：采用容积四元数姿态估计器对航天器姿态进行估计。
2.根据权利要求1所述基于容积四元数估计的航天器姿态估计方法，其特征在于：所述
步骤一中建立航天器姿态运动学模型和观测模型的具体过程为：
带有乘性噪声的非线性离散系统：
xk+1＝(In×n+ζkΦk)f(xk)+wk (1)
yk＝h(xk)+vk (2)
其中，xk∈Rn是系统k时刻的状态量，yk∈Rm是系统k时刻的观测值；f(xk)和h(xk)是非线
性方程；Φk是已知的常系数矩阵，ζk∈R是乘性噪声，wk∈Rn和vk∈Rm是系统加性噪声和量测
加性噪声；R为实数，Rn为n维实数集，Rm为m维实数集；
ζk和vk是同步相关的，且满足
$E\left\{\left[\begin{array}{c}{\mathrm{\ζ}}_t\\ v_t\end{array}\right]\left[\begin{array}{cc}{\mathrm{\ζ}}_k^T& v_k^T\end{array}\right]\right\}=\left[\begin{array}{cc}{Q}_{\mathrm{\ζ},k}& s_k\\ S_k^T& R_k\end{array}\right]{\mathrm{\δ}}_{t,k}---\left(4\right)$
其中Sk≠0是乘性噪声和加性噪声的互协方差；Qζ,k是ζk的方差，Rk是vk的方差；t＝k时ζk
和vk存在相关性；
采用四元数描述的航天姿态运动学模型，f(xk)具体形式如下：
$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{c}\mathrm{\ω}\\ 0\end{array}\right]\⊗q=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)q---\left(29\right)$
$\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{cc}-\[\mathrm{\ω}\×\]& \mathrm{\ω}\\ -{\mathrm{\ω}}^T& 0\end{array}\right]---\left(30\right)$
其中，q为姿态四元数，是q的导数；ω＝[ω1 ω2 ω3]T为角速度在体坐标系下的表示，
[ω×]表示由ω生成的反对称矩阵表示为：
$\[\mathrm{\ω}\×\]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& -{\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right]---\left(31\right)$
观测模型的建立：
(一)陀螺输出模型
假设陀螺固连在航天器上，并且陀螺的安装方向与航天器本体坐标系重合，测量航天
器的角速度；则陀螺模型表示为：
$\left\{\begin{array}{c}\widetilde{\mathrm{\ω}}=\mathrm{\ω}+\mathrm{\β}+{\mathrm{\η}}_v\\ \mathrm{\β}={\mathrm{\η}}_u\end{array}\right.---\left(32\right)$
其中，表示实际的陀螺输出值，β表示陀螺漂移值，ω表示理想情况下的陀螺输出值，
ηv和ηu分别表示不相关的零均值高斯白噪声，且其协方差分别表示为和将陀螺
输出和陀螺漂移方程离散化后的形式如下：
$\left\{\begin{array}{c}{\widetilde{\mathrm{\ω}}}_{k+1}={\mathrm{\ω}}_k+\frac{1}{2}{({\mathrm{\β}}_{k+1}+{\mathrm{\β}}_k)}^{1/2}+{(\frac{{\mathrm{\σ}}_v^2}{\mathrm{\Δ}t}+\frac{1}{12}{\mathrm{\σ}}_u^2\mathrm{\Δ}t)}^{1/2}{N}_v\\ {\mathrm{\β}}_{k+1}={\mathrm{\β}}_k+{\mathrm{\σ}}_u{\mathrm{\Δt}}^{1/2}{N}_u\end{array}\right.---\left(33\right)$
其中，△t表示步长，Nv和Nu分别表示离散后不相关的零均值高斯白噪声；
(二)星敏感器输出模型
假设星敏感器的安装方向与航天器本体坐标系重合，则星光矢量在航天器本体坐标系
下的观测方程为：
$b=A_I^{b}r+v---\left(34\right)$
其中，r表示星光矢量在惯性系下的单位矢量方向，表示从惯性系到航天器本体坐标
系的变换矩阵；v表示敏感器的观测误差，假设有m个星敏感器同时进行观测，则在第k时刻，
用四元数描述的矢量观测模型为：
$z_k=\left[\begin{array}{c}{b}_1\\ b_2\\ .\\ .\\ .\\ b_m\end{array}\right]{|}_k+\left[\begin{array}{c}{v}_1\\ v_2\\ .\\ .\\ .\\ v_m\end{array}\right]{|}_k=\left[\begin{array}{c}A\left(q\right)r_1\\ A\left(q\right)r_2\\ .\\ .\\ .\\ A\left(q\right)r_m\end{array}\right]{|}_k+v_k---\left(35\right)$
其中，bm和rm表示第m个参考矢量分别在航天器本体坐标系和惯性坐标系下的分量；A
(q)表示姿态转移矩阵，其四元数形式的描述为：
$A\left(q\right)=(q_4^2-|\left|\mathrm{\ρ}\right|{|}^2)I_{3\×3}-2q_4\[\mathrm{\ρ}\×\]+2{\mathrm{\ρ\ρ}}^T---\left(36\right)$
其中，姿态四元数q＝[q1,q2,q3,q4]T，q分解为标量q4和矢量ρ，ρ＝[q1,q2,q3]T；
展开形式是：
$A\left(q\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_2{q}_1-q_4{q}_3)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_3{q}_1+q_4{q}_2)& 2(q_3{q}_2-q_4{q}_1)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(37\right).$
3.根据权利要求1或2所述基于容积四元数估计的航天器姿态估计方法，其特征在于：
所述步骤二中采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中
的噪声的具体过程为：
步骤二一：一步预测；
考虑非线性系统形式为式(1)和(2)，假设后验概率密度函数Pk-1|k-1和状态估计值
是已知的，则xk的一步预测均值和方差形式如下：


其中
$M_{k-1}=E\[{\mathrm{\ζ}}_{k-1}|D_{k-1}\]=S_{k-1}{P}_{\tilde{y}\tilde{y},k-1|k-1}^{-1}(y_{k-1}-h\left({\hat{x}}_{k-1|k-1}\right)\left)\right)---\left(9\right)$
$U_{k-1}=E\[{\mathrm{\ζ}}_t{\mathrm{\ζ}}_k^T|D_{k-1}\]=Q_{\mathrm{\ζ},k-1}-S_{k-1}{P}_{\tilde{y}\tilde{y},k-1|k-1}^{-1}{S}_{k-1}^T---\left(10\right)$

$D_{k-1}={\left[\begin{array}{cccc}{y}_1^T& y_2^T& \mathrm{...}& y_{k-1}^T\end{array}\right]}^T---\left(12\right)$
其中是xk的一步预测值，In×n为n×n的单位矩阵，Pk|k-1为xk的一步预测方差，
为yk-1的方差，Mk-1表示ζk-1的条件均值，Uk-1表示ζk-1的条件方差，Dk-1表示测量值的
集合；
步骤二二：量测更新；
考虑非线性系统形式为式(1)和(2)，假设状态量xk的预测均值和方差已经得到，则它的
后验均值和方差Pk|k的形式如下：
${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})---\left(18\right)$
$P_{k|k}=P_{k|k-1}-K_k{P}_{\tilde{y}\tilde{y},k|k-1}{K}_k^T---\left(19\right)$
$K_k=P_{\tilde{x}\tilde{y},k|k-1}{P}_{\tilde{y}\tilde{y},k|k-1}^{-1}---\left(20\right)$



其中为yk的一步预测值，Kk为滤波增益，为xk和yk的一步预测协方差，
为yk的一步预测方差。
4.根据权利要求3所述基于容积四元数估计的航天器姿态估计方法，其特征在于：所述
步骤三中采用容积四元数姿态估计器对航天器姿态进行估计的具体过程为：
将状态向量选为x＝[δpT βT]T，其中δp为与误差四元数对应的修正罗德里格斯参数，形
式如下：
$\mathrm{\δ}p=b\frac{\mathrm{\δ}\mathrm{\ρ}}{a+{\mathrm{\δq}}_4}---\left(38\right)$
其中0≤a≤1，b为尺度参数；
(一)时间预测
已知滤波器的初值q0，β0，P0，由k-1时刻的状态估计和协方差阵估计Pk-1获得k时刻
的容积点为：
$X_{i,k-1}=\sqrt{P_{k-1}}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1},i=1,2,...,n---\left(39\right)$
其中，δpi,k-1和βi,k-1分别表示姿态角误差和陀螺漂移，ξi表示
容积点集合{ξi}的第i列向量，容积点集合{ξi}可定义为即
$\left\{{\mathrm{\ξ}}_i\right\}=\sqrt{n}\left\{\left[\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]\left[\begin{array}{c}0\\ 1\\ .\\ .\\ .\\ 0\end{array}\right]\mathrm{...}\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right]\left[\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]\left[\begin{array}{c}0\\ -1\\ .\\ .\\ .\\ 0\end{array}\right]\mathrm{...}\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right]\right\}$
ei表示单位向量，且该单位向量中第i个元素为1；
将误差广义罗德里格参数转换成容积误差四元数为：
$\left\{\begin{array}{c}{\mathrm{\δq}}_{4_{i,k-1}}=\frac{-a\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2+b\sqrt{b^2+(1-a^2)\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2}}{b^2+\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2}\\ {\mathrm{\δ\ρ}}_{i,k-1}=b^{-1}(a+{\mathrm{\δq}}_{4_{i,k-1}}){\mathrm{\δp}}_{i,k-1}\end{array}\right.,i=1,2,\mathrm{...},n---\left(40\right)$
其中，δρi,k-1表示第k-1时刻误差四元数δqi,k-1的矢量部分，表示第k-1时刻误差四
元数δqi,k-1的标量部分；
由容积误差四元数获得一步预测估计的容积四元数点集为：
${\hat{q}}_{i,k|k-1}={\mathrm{\δq}}_{i,k-1}\⊗{\hat{q}}_{k-1},i=1,2,...,n---\left(41\right)$
容积四元数由姿态运动学方程(29)获得，采用解析形式为：
${\hat{q}}_{i,k|k-1}=\left[\begin{array}{cc}cos\left(A\right)I_{3\×3}-\[{\widehat{\mathrm{\ψ}}}_{i,k-1}\×\]& {\widehat{\mathrm{\ψ}}}_{i,k-1}\\ -{\widehat{\mathrm{\ψ}}}_{i,k-1}^T& \cos \left(A\right)\end{array}\right]{\hat{q}}_{i,k-1}---\left(42\right)$
其中：
$A=\frac{1}{2}\left|\right|{\widehat{\mathrm{\ω}}}_{i,k-1}\left|\right|\mathrm{\Δ}t,{\widehat{\mathrm{\ψ}}}_{i,k-1}=\frac{sin\left(A\right){\widehat{\mathrm{\ω}}}_{i,k-1}}{\left|\right|{\widehat{\mathrm{\ω}}}_{i,k-1}\left|\right|}---\left(43\right)$
陀螺的角速度估计为：
${\widehat{\mathrm{\ω}}}_{i,k-1}={\widetilde{\mathrm{\ω}}}_{k-1}-{\mathrm{\β}}_{i,k-1},i=1,2,...,n---\left(44\right)$
其中，i表示取值为1,2,…,n的数，表示第k-1时刻陀螺输出值，βi,k-1表示第k-1时刻
陀螺漂移值；
获得容积四元数一步预测值后，再将其转换为容积误差罗德里格参数；先计算一步预
测容积误差四元数得：
${\mathrm{\δq}}_{i,k|k-1}={\hat{q}}_{i,k|k-1}\⊗{\hat{q}}_{k-1}^{-1},i=1,2,...,n---\left(45\right)$
再将容积误差四元数转换为容积误差罗德里格参数为：
${\mathrm{\δp}}_{i,k|k-1}=b\frac{{\mathrm{\δ\ρ}}_{i,k|k-1}}{a+q_{4_{i,k|k-1}}},i=1,2,...,n---\left(46\right)$
在由步骤二所设计的高斯滤波算法，得到一步预测均值
$\mathrm{\δ}{\hat{p}}_{k|k-1}=\[I_{n\×n}+M_{k-1}{\mathrm{\Φ}}_{k-1}\]\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\δp}}_{i,k|k-1}---\left(47\right)$
容积点误差一步预测均值为和方差阵Pk|k-1为：
${\hat{x}}_{k|k-1}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{p}}_{k|k-1}^T& {\widehat{\mathrm{\β}}}_{k-1}^T\end{array}\right]}^T---\left(48\right)$
$P_{k|k-1}=(I_{n\×n}+2M_{k-1}{\mathrm{\Φ}}_{k-1}+U_{k-1}{\mathrm{\Φ}}_{k-1}{\mathrm{\Φ}}_{k-1}^T)\×\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{X}_{i,k}^{*}{\left(X_{i,k}^{*}\right)}^T-{\hat{x}}_{k|k-1}{\left({\hat{x}}_{k|k-1}\right)}^T+Q_{k-1}---\left(49\right)$
其中，是容积点传播值；
(二)量测更新
计算容积点
$X_{i,k}=\sqrt{P_{k|k-1}}{\mathrm{\ζ}}_i+{\hat{x}}_{k|k-1},i=1,...,2n---\left(50\right)$
星敏感器生成观测容积估计点Zi,k为：
$Z_{i,k}=A\left({\hat{q}}_{i,k|k-1}\right)r,i=1,2,...,n---\left(51\right)$
通过以上容积点得到量测容积点均值
${\hat{z}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{i,k}---\left(52\right)$
求取容积滤波增益Kk：
$K_k=P_{xz,k}{P}_{\tilde{z}\tilde{z},k}^{-1}---\left(53\right)$
其中，协方差阵和互协方差阵Pxz,k分别表示如下：
$P_{\tilde{z}\tilde{z},k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{i,k}{Z}_{i,k}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k---\left(54\right)$
$P_{xz,k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{X}_{i,k}{Z}_{i,k}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(55\right)$
再由式(49)得到状态量的量测更新为：
${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(56\right)$
其中，
计算更新容积误差四元数和陀螺漂移；容积误差四元数δqk更新为：
$\left\{\begin{array}{c}{\mathrm{\δq}}_{4_k}=\frac{-a\left|\right|{\mathrm{\δp}}_k^T|{|}^2+b\sqrt{b^2+(1-a^2)\left|\right|{\mathrm{\δp}}_k^T|{|}^2}}{b^2+\left|\right|{\mathrm{\δp}}_k^T|{|}^2}\\ {\mathrm{\δ\ρ}}_k=b^{-1}(a+{\mathrm{\δq}}_{4_k}){\mathrm{\δp}}_k^T\end{array}\right.---\left(57\right)$
得到容积点四元数
${\hat{q}}_k={\mathrm{\δq}}_k\⊗{\hat{q}}_{k|k-1}---\left(58\right)$
协方差阵Pk更新为：
$P_k=P_{k|k-1}-K_k{P}_{\tilde{z}\tilde{z},k}{K}_k^T---\left(59\right).$

基于容积四元数估计的航天器姿态估计方法

技术领域

本发明涉及基于容积四元数估计以及星敏感器与陀螺组合的航天器姿态估计方
法。

背景技术

随着航天器和空间系统复杂性的不断加大，需要对航天器的姿态进行更加准确和
鲁棒的估计。对每个航天器的功能需求正在增加，而航天器本身的尺寸却越来越小，且硬件
之间是相互独立的，这就导致系统的计算能力受到很大限制。这样的系统常常被称是操作
响应空间(ORS)。ORS系统的主要目标是为了航天器使用模块化的类型来建设加速任务概念
推出的时间。这种模块化的系统，定制的接口和软件必须管理传送到中央处理器的所有数
据。然而，潜在的危险情况是数据传播的时引入未知扰动，或部分数据在传输时丢失。

实际中，航天器系统姿态模型是一个非线性模型。在近些年中，利用一系列含有噪
声的观测值对非线性系统的最优估计问题，吸引了众多学者们的广泛关注和研究。不仅在
航天领域，在其他领域内对非线性系统的状态最优估计问题也取得了许多重大的成果。现
有的大部分滤波理论是基于贝叶斯方法的，贝叶斯方法为状态估计问题提供了一个通用框
架。贝叶斯方法是利用对在所有观测量作为条件下的系统状态的条件概率密度函数进行递
归计算。在目前的应用过程中，通常将系统假设为一种特殊的情形式，即所有的概率密度函
数为高斯概率密度函数,这种系统称为高斯非线性系统。针对高斯非线性系统，学者们提出
了多种滤波方法，应用最为广泛的是扩展卡尔曼滤波算法(EKF)，EKF具有运算速度快，稳定
性可证等优势，但是该算法存在着一定的缺陷。由于是对非线性系统通过泰勒展开进行线
性逼近，所以只适用于弱非线性系统。另外，需要求解泰勒展开中的雅克比矩阵，但是有些
系统的雅克比矩阵很难求解，有些甚至不存在雅克比矩阵，因此EKF应用存在一定的局限
性。因此，高斯滤波框架作为一种估计高斯非线性系统的通用方法框架得到了广泛应用。高
斯滤波框架通过高斯加权积分的方式代替对贝叶斯框架下概率密度函数求解。许多滤波算
法都是建立在高斯滤波框架的基础上，利用不同的对高斯加权积分的近似求解方式实现
的，例如高斯厄尔米特滤波器(GHQF)、无迹卡尔曼滤波算法(UKF)、差分滤波器(DDF)、容积
卡尔曼滤波(CKF)。

由于航天器上的传感元件在数据传输过程中，存在着传输时滞、丢包和多种未知
外来扰动的问题，其中存在乘性噪声就是一种常见的问题。例如系统参数的不确定性可由
乘性噪声描述，量化作用和随机产生的传感器饱和问题可以在建模过程中由乘性噪声描
述。在文献(F.Yang,Z.Wang,and Y.Hung.Robust Kalman filtering for discrete time-
varying uncertain systems with multiplicative noises,IEEE Trans.on Automatic
Control,47(7):1179-1183,2002)(J.Jimenez,T.Ozaki,Local linearization filters
for non-linear continuous-discrete state space models with multiplicative
noise,International Journal of Control,76(12):1159-1170,2003)(F.Carravetta,
A.Germani,and N.Raimondi,Polynomial filtering of discrete-time stochastic
linear systems with multiplicative state noise,IEEE Trans.on Automatic
Control,42(8):1106-1126,1997)中，最小方差滤波器和多项式滤波器可以用来解决具有
乘性噪声的线性系统。对于具有乘性噪声的非线性系统，文献(D.Spinello,D.Stilwell,
Nonlinear Estimation With State-Dependent Gaussian Observation Noise,IEEE
Trans.on Automatic Control,55(6):1358-1366,2010)中设计了使滤波误差的协方差最
小的递归EKF算法。文献(X.Hu,Y.Hu,and B.Xu,Generalised Kalman filter tracking
with multiplicative measurement noise in a wireless sensor network,IET Signal
Processing,8(5):467-474,2014)提出了推广EKF(GEKF)和推广UKF(GUKF)算法，该算法不
需要迭代非线性优化过程，利用条件高斯分布的性质去提高传统EKF和UKF的性能。但是所
有算法都基于系统的乘性噪声和相应的加性噪声不具有相关性的前提下。

在实际的应用中，噪声相关也是一种常见的问题。例如，动态观测过程在相同的噪
声环境中，由于受到的扰动影响相同，所以产生的误差也具有相关性。在近期的相关研究
中，针对噪声相关的问题学者们提出了一些理论方法，其中通过解耦重新构建系统方程的
方法，在解决该问题上应用的最多，文献(G.Chang,Marginal unscented kalman filter
for cross-correlated process and observation noise at the same epoch,IET
Radar Sonar Navigation,8(1):54-64,2014)(A.Hermosocarazo,J.Linarespérez,
Unscented Filtering from Delayed Observations with Correlated Noises,
Mathematical Problems in Engineering,2009(1024-123X):266-287,2009)就是利用解
耦的方法。在文献(X.Wang,Y.Liang,Q.Pan,and F.Yang,A Gaussian approximation
recursive filter for nonlinear systems with correlated noises,Automatica,48
(48):2290–2297,2012)中，利用两步预测的方法去更新后验概率密度函数，成功避开了量
测噪声和系统噪声耦合的问题，提出了一种新型的高斯滤波框架。但在上述文献中，都是考
虑系统加性噪声和量测加性噪声的耦合情况，没有考虑乘性噪声和加性噪声之间的耦合情
况。

发明内容

本发明的目的是为了解决现有技术存在乘性噪声及噪声相关问题，导致航天器姿
态估计精度低的缺点，而提的基于容积四元数估计的航天器姿态估计方法。

基于容积四元数估计的航天器姿态估计方法包括以下步骤：

步骤一：建立航天器姿态运动学模型和观测模型；

步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型
中的噪声；

步骤三：采用容积四元数姿态估计器对航天器姿态进行估计。

本发明的有益效果为：

针对现有技术各种情况的考虑，本发明针对具有乘性噪声的非线性系统且系统中
乘性噪声与加性噪声相关，提出一种改进的高斯滤波框架，作为对该类系统进行最优估计
的统一框架。该框架将对噪声的相关性及条件概率密度函数进行充分利用，从而对系统状
态的估计精度更高。

本发明针对航天器的姿态估计时，可能出现系统乘性噪声，及乘性噪声和观测噪
声相关的问题，设计了一种基于高斯滤波算法的改进高斯滤波框架。同时采用CQE作为改进
高斯滤波算法的特殊实现形式，提出了CQE-MCNS算法。该算法主要针对航天器的姿态估计
问题，继承了CQE算法的优势避免四元数规范约束和奇异的发生，同时又能处理乘性噪声及
噪声相关的问题。通过与CQE算法进行仿真实验对比，结果表明，本发明所提出的CQE-MCNS
算法相比CQE算法，角度估计误差减小0.0002°左右，陀螺漂移估计误差减小0.002°/h左右，
因此更适用于具有乘性噪声及噪声相关的航天器姿态估计问题。此外，本发明所提出的改
进高斯滤波框架可以作为解决具有乘性噪声及噪声相关的系统状态估计问题的通用框架
应用于其他领域。

附图说明

图1为CQE角度估计误差2-范数图；

图2为CQE滚动轴角度估计误差图；

图3为CQE俯仰轴角度估计误差图；

图4为CQE偏移轴角度估计误差图；

图5为CQE陀螺漂移量估计误差2-范数图；

图6为CQE x轴陀螺漂移量估计误差图；

图7为CQE y轴陀螺漂移量估计误差图；

图8为CQE z轴陀螺漂移量估计误差图；

图9为CQE-MCNS角度估计误差2-范数图；

图10为CQE-MCNS滚动轴角度估计误差图；

图11为CQE-MCNS俯仰轴角度估计误差图；

图12为CQE-MCNS偏移轴角度估计误差图；

图13为CQE-MCNS陀螺漂移量估计误差2-范数图；

图14为CQE-MCNS x轴陀螺漂移量估计误差图；

图15为CQE-MCNS y轴陀螺漂移量估计误差图；

图16为CQE-MCNS z轴陀螺漂移量估计误差图。

具体实施方式

具体实施方式一：基于容积四元数估计的航天器姿态估计方法的具体过程为：

步骤一：建立航天器姿态运动学模型和观测模型；

步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型
中的噪声；

步骤三：采用容积四元数姿态估计器对航天器姿态进行估计。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立航
天器姿态运动学模型和观测模型的具体过程为：

考虑带有乘性噪声的非线性离散系统：

xk+1＝(In×n+ζkΦk)f(xk)+wk (1)

yk＝h(xk)+vk (2)

其中，xk∈Rn是系统k时刻的状态量，yk∈Rm是系统k时刻的观测值；f(xk)和h(xk)是
非线性方程；Φk是已知的常系数矩阵，ζk∈R是乘性噪声，wk∈Rn和vk∈Rm是系统加性噪声和
量测加性噪声；R为实数，Rn为n维实数集，Rm为m维实数集；

假设1：wk和vk都是白噪声，且满足

其中δt,k是克罗内克函数，即当t＝k时，值为1，其他情况值为0。

假设2：ζk和vk是同步相关的，且满足

其中Sk≠0是乘性噪声和加性噪声的互协方差；Qζ,k是ζk的方差，Rk是vk的方差；另
外，由于是同步相关，所以只有在t＝k时ζk和vk才存在相关性；

假设3：初始值x0与噪声ζk、wk和vk都是不相关的，且E[x0]＝0,

在上述的假设条件下传统的高斯滤波框架是无法实用的，例如EKF、UKF和CKF。所
以本文的目的是设计一种改进的高斯滤波框架，可以有效地对上述系统进行状态最优估
计。注意1:从式(4)中可以看出，由于相关性的存在，乘性噪声和量测噪声的统计特性已经
发生变化。

采用四元数描述的航天姿态运动学模型，f(xk)具体形式如下：

其中，q为姿态四元数，是q的导数；ω＝[ω1 ω2 ω3]T为角速度在体坐标系下的
表示，[ω×]表示由ω生成的反对称矩阵表示为：

观测模型(包括陀螺输出模型和星敏感器输出模型)的建立：

(一)陀螺输出模型

假设陀螺固连在航天器上，并且陀螺的安装方向与航天器本体坐标系重合，可直
接测量航天器的角速度；则陀螺模型表示为：

其中，表示实际的陀螺输出值，β表示陀螺漂移值，ω表示理想情况下的陀螺输
出值，ηv和ηu分别表示不相关的零均值高斯白噪声，且其协方差分别表示为和
将陀螺输出和陀螺漂移方程离散化后的形式如下：

其中，△t表示步长，Nv和Nu分别表示离散后不相关的零均值高斯白噪声；

(二)星敏感器输出模型

星敏感器是通过观测天体的方向对照星历表来确定航天器的姿态，假设星敏感器
的安装方向与航天器本体坐标系重合，则星光矢量在航天器本体坐标系下的观测方程为：

其中，r表示星光矢量在惯性系下的单位矢量方向，可查询星历表获得，表示从
惯性系到航天器本体坐标系的变换矩阵；v表示敏感器的观测误差，这里认为是高斯白噪
声。假设有m个星敏感器同时进行观测，则在第k时刻，用四元数描述的矢量观测模型为：

其中，bm和rm表示第m个参考矢量分别在航天器本体坐标系和惯性坐标系下的分
量；A(q)表示姿态转移矩阵，其四元数形式的描述为：

其中，姿态四元数q＝[q1,q2,q3,q4]T，q可分解为标量q4和矢量ρ，ρ＝[q1,q2,q3]T；

展开形式是：

q＝[q1,q2,q3,q4]T，ρ＝[q1,q2,q3]T。

注：

惯性坐标系

原点位于地心OI，OIXI轴指向平春分点方向(天球黄道和天球赤道在天球上交叉点
为春分点和秋分点)，OIZI轴平行于平均地球自转轴方向，OIYI轴与另两轴构成右手系。

航天器本体坐标系

航天器本体坐标系与航天器固连，其原点位于航天器质心Ob，三轴与航天器惯量
主轴一致，固联于星体上。ObXb轴指向航天器飞行方向，称为滚动轴，ObZb轴指向地心方向，
称为偏航轴，ObYb轴与另两轴构成右手直角坐标系，称为俯仰轴。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中采
用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声的具体过
程为：

用一种改进的高斯滤波框架处理噪声相关的情况，本文中所设计的滤波算法采用
一步更新后验概率密度。改进的高斯滤波框架与经典高斯滤波算法类似，也分为一步预测
和量测更新两个步骤，分别通过定理1和定理2描述如下。

引理1：假设向量X和Y的联合高斯分布如下

则X在Y＝y情况下的条件概率满足如下高斯分布

步骤二一：一步预测；

考虑非线性系统形式为式(1)和(2)，假设后验概率密度函数Pk-1|k-1和状态估计值
是已知的，则xk的一步预测均值和方差形式如下：

其中

其中是xk的一步预测值，In×n为n×n的单位矩阵，Pk|k-1为xk的一步预测方差，
为变量a的高斯概率密度，b为a的期望，c为a的方差；为yk-1的方差；Mk-1表
示ζk-1的条件均值，Uk-1表示ζk-1的条件方差，Dk-1表示测量值的集合；

证明：首先将表示k-1时刻的式(2)代入协方差的定义中可得式(11)，然后
将式(1)代入一步关于状态量xk的一步预测均值的定义中，可得：

由于乘性噪声ζk-1和Dk-1中的yk-1观测值中的vk-1相关，因此E[ζk-1|Dk-1]的值不为0。
ζk-1和yk-1在以k-1时刻的状态估计值作为条件的联合分布如下：

根据引理1和vk-1与Dk-2相互独立可知式(9)成立，同理可得式(10)也成立。将式(9)
代入式(13)中可以得到式(7)。最后将式(1)代入Pk|k-1定义中，可得到如下

将式(15)的等式右边展开可得式(16)：

由于wk-1和ζk-1不相关，且与状态xk-1中的wk-2也不相关，因此，可得：

最后将式(9)、(10)、(13)和(17)代入式(16)，可得到式(8)。

步骤二二：量测更新；

考虑非线性系统形式为式(1)和(2)，假设状态量xk的预测均值和方差已经得到，
则它的后验均值和方差Pk|k的形式如下：

其中为yk的一步预测值，Kk为滤波增益，为xk和yk的一步预测协方差，
为yk的一步预测方差。

证明：首先将式(2)代入yk的预测均值定义中可得

由于E[vk|Dk-1]＝0，可直接得到式(21)。然后将式(2)代入协方差的定义中
可得：

将式(25)的右侧展开可得

由于式(22)可以直接获得。最后将式(2)代入协方差的
定义中可得：

将式(27)的等式右边展开可得：

由于可知式(23)成立。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三
中采用容积四元数姿态估计器对航天器姿态进行估计的具体过程为：

本发明利用容积卡尔曼滤波器(CKF)作为GF框架的实现形式，与四元数相结合估
计卫星姿态，提出新的算法为乘性相关噪声系统的容积四元数姿态估计器(cubature
quaternion estimator for multiplicative correlated noises system,CQE-MCNS)。为
了避免采用四元数描述姿态时存在的冗余而导致滤波过程中协方差阵出现奇异的状况，将
状态向量选为x＝[δpT βT]T，其中δp为与误差四元数对应的修正罗德里格斯参数，形式如
下：

其中0≤a≤1，b为尺度参数；当a＝0,b＝1式(38)退化为罗德里格参数，a＝1,b＝1
时式(38)退化为修正的罗德里格参数，广义罗德里格参数的奇异点在180°到360°间。

(一)时间预测

已知滤波器的初值q0，β0，P0，由k-1时刻(前一时刻)的状态估计和协方差阵估
计Pk-1获得k时刻(下一时刻)的容积点为：

其中，δpi,k-1和βi,k-1分别表示姿态角误差和陀螺漂移，ξi
表示容积点集合{ξi}的第i列向量。容积点集合{ξi}可定义为即

ei表示单位向量，且该单位向量中第i个元素为1。

在滤波算法更新过程中，为了避免误差广义罗德里格参数描述姿态发生的奇异现
象，将误差广义罗德里格参数转换成容积误差四元数为：

其中，δρi,k-1表示第k-1时刻误差四元数δqi,k-1的矢量部分，表示第k-1时刻误
差四元数δqi,k-1的标量部分。

由容积误差四元数获得一步预测估计的容积四元数点集为：

容积四元数由姿态运动学方程(29)获得，采用常用的解析形式为：

其中：

陀螺的角速度估计为：

其中，i表示取值为1,2,…,n的数，表示第k-1时刻陀螺输出值，βi,k-1表示第k-
1时刻陀螺漂移值。

获得容积四元数一步预测值后，再将其转换为容积误差罗德里格参数；先计算一
步预测容积误差四元数得：

再将容积误差四元数转换为容积误差罗德里格参数为：

在由步骤二所设计的高斯滤波算法，得到一步预测均值

容积点误差一步预测均值为和方差阵Pk|k-1为：

其中，是容积点传播值。

(二)量测更新

计算容积点

星敏感器生成观测容积估计点Zi,k为：

通过以上容积点得到量测容积点均值

求取容积滤波增益Kk：

其中，协方差阵和互协方差阵Pxz,k分别表示如下：

再由式(49)得到状态量的量测更新为：

其中，

计算更新容积误差四元数和陀螺漂移；容积误差四元数δqk更新为：

得到容积点四元数

协方差阵Pk更新为：

其它步骤及参数与具体实施方式一至三之一相同。

实施例一：

在本发明中，通过对近地轨道航天器的姿态估计来验证所设计的CQE-MCNS算法的
有效性。假设航天器的本体坐标系和轨道坐标系重合，这样卫星体系相对于惯性系角速度
与轨道坐标系相对于惯性系角速度相等。同时，假设航天器上装配有星敏感器，用于测量角
度矢量。角速度的测量利用三轴陀螺。星敏感器对姿态矢量测量带有标准差为2角秒的高斯
白噪声。陀螺采样周期为1/s，星敏感器的采样周期为10/s，仿真时间设为3个轨道周期。假
设三轴角度初值有较大估计误差，分别为-45°,45°,155°，陀螺漂移初始值为0，初始方差阵
P0＝[(40°)2I3×3(0.1°/h)2I3×3]T。乘性噪声ζk-1的均值为0，方差为Qζ,k-1＝0.1。相关噪声的
互协方差为Sk＝0.05。下面给出文献(魏喜庆,宋申民.基于容积卡尔曼滤波的卫星姿态估
计[J].宇航学报,2013,34(2):193-200)提出的CQE和所设计的CQE-MCNS的姿态估计结果。

(一)CQE估计结果

图1和图5分别给出的是角度估计误差和陀螺漂移量估计误差的2-范数曲线，图2-
图4和图6-图8则给出三轴角度估计误差和陀螺漂移量估计误差曲线。

从仿真中可以看出，CQE的角度估计误差在0.002°左右，陀螺漂移估计精度达到了
0.01°/h。

(二)CQE-MCNS估计结果

图9和图13分别给出的是角度估计误差和陀螺漂移量估计误差的2-范数曲线，图
10-图12和图14-图16则给出三轴角度估计误差和陀螺漂移量估计误差曲线。

从仿真中可以看出，CQE-MCNS的角度估计误差在0.0005°左右，陀螺漂移估计精度
达到了0.005°/h。在估计精度上，CQE-MCNS相比于CQE有了显著的提高，这是由于乘性噪声
和噪声相关性的存在对CQE的估计精度产生了一定的影响。从而表明本文所设计的CQE-
MCNS算法可以有效估计具有乘性状态噪声及乘性噪声与观测噪声相关的系统状，且有较高
的估计精度。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域
技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于
本发明所附的权利要求的保护范围。