技術特征：

1.基于容積四元數估計的航天器姿態(tài)估計方法，其特征在于：所述航天器姿態(tài)估計方法的具體過程為：

步驟一：建立航天器姿態(tài)運動學模型和觀測模型；

步驟二：采用高斯濾波算法去除步驟一建立的航天器姿態(tài)運動學模型和觀測模型中的噪聲；

步驟三：采用容積四元數姿態(tài)估計器對航天器姿態(tài)進行估計。

2.根據權利要求1所述基于容積四元數估計的航天器姿態(tài)估計方法，其特征在于：所述步驟一中建立航天器姿態(tài)運動學模型和觀測模型的具體過程為：

帶有乘性噪聲的非線性離散系統(tǒng)：

x_k+1＝(I_n×n+ζ_kΦ_k)f(x_k)+w_k (1)

y_k＝h(x_k)+v_k (2)

其中，x_k∈Rⁿ是系統(tǒng)k時刻的狀態(tài)量，y_k∈R^m是系統(tǒng)k時刻的觀測值；f(x_k)和h(x_k)是非線性方程；Φ_k是已知的常系數矩陣，ζ_k∈R是乘性噪聲，w_k∈Rⁿ和v_k∈R^m是系統(tǒng)加性噪聲和量測加性噪聲；R為實數，Rⁿ為n維實數集，R^m為m維實數集；

ζ_k和v_k是同步相關的，且滿足

$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)$

其中S_k≠0是乘性噪聲和加性噪聲的互協方差；Q_ζ,k是ζ_k的方差，R_k是v_k的方差；t＝k時ζ_k和v_k存在相關性；

采用四元數描述的航天姿態(tài)運動學模型，f(x_k)具體形式如下：

$\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為姿態(tài)四元數，是q的導數；ω＝[ω₁ ω₂ ω₃]^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表示步長，N_v和N_u分別表示離散后不相關的零均值高斯白噪聲；

(二)星敏感器輸出模型

假設星敏感器的安裝方向與航天器本體坐標系重合，則星光矢量在航天器本體坐標系下的觀測方程為：

$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)$

其中，b_m和r_m表示第m個參考矢量分別在航天器本體坐標系和慣性坐標系下的分量；A(q)表示姿態(tài)轉移矩陣，其四元數形式的描述為：

$A\left(q\right)=(q_4^2-|\left|\mathrm{\ρ}\right|{|}^2)I_{3\×3}-2q_4\[\mathrm{\ρ}\×\]+2{\mathrm{\ρ\ρ}}^T---\left(36\right)$

其中，姿態(tài)四元數q＝[q₁,q₂,q₃,q₄]^T，q分解為標量q₄和矢量ρ，ρ＝[q₁,q₂,q₃]^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所述基于容積四元數估計的航天器姿態(tài)估計方法，其特征在于：所述步驟二中采用高斯濾波算法去除步驟一建立的航天器姿態(tài)運動學模型和觀測模型中的噪聲的具體過程為：

步驟二一：一步預測；

考慮非線性系統(tǒng)形式為式(1)和(2)，假設后驗概率密度函數P_k-1|k-1和狀態(tài)估計值是已知的，則x_k的一步預測均值和方差形式如下：

其中

$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)$

其中是x_k的一步預測值，I_n×n為n×n的單位矩陣，P_k|k-1為x_k的一步預測方差，為y_k-1的方差，M_k-1表示ζ_k-1的條件均值，U_k-1表示ζ_k-1的條件方差，D_k-1表示測量值的集合；

步驟二二：量測更新；

考慮非線性系統(tǒng)形式為式(1)和(2)，假設狀態(tài)量x_k的預測均值和方差已經得到，則它的后驗均值和方差P_k|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)$

其中為y_k的一步預測值，K_k為濾波增益，為x_k和y_k的一步預測協方差，為y_k的一步預測方差。

4.根據權利要求3所述基于容積四元數估計的航天器姿態(tài)估計方法，其特征在于：所述步驟三中采用容積四元數姿態(tài)估計器對航天器姿態(tài)進行估計的具體過程為：

將狀態(tài)向量選為x＝[δp^T β^T]^T，其中δp為與誤差四元數對應的修正羅德里格斯參數，形式如下：

$\mathrm{\δ}p=b\frac{\mathrm{\δ}\mathrm{\ρ}}{a+{\mathrm{\δq}}_4}---\left(38\right)$

其中0≤a≤1，b為尺度參數；

(一)時間預測

已知濾波器的初值q₀，β₀，P₀，由k-1時刻的狀態(tài)估計和協方差陣估計P_k-1獲得k時刻的容積點為：

$X_{i,k-1}=\sqrt{P_{k-1}}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1},i=1,2,...,n---\left(39\right)$

其中，δp_i,k-1和β_i,k-1分別表示姿態(tài)角誤差和陀螺漂移，ξ_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\}$

e_i表示單位向量，且該單位向量中第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時刻誤差四元數δq_i,k-1的矢量部分，表示第k-1時刻誤差四元數δq_i,k-1的標量部分；

由容積誤差四元數獲得一步預測估計的容積四元數點集為：

${\hat{q}}_{i,k|k-1}={\mathrm{\δq}}_{i,k-1}\⊗{\hat{q}}_{k-1},i=1,2,...,n---\left(41\right)$

容積四元數由姿態(tài)運動學方程(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)$

容積點誤差一步預測均值為和方差陣P_k|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)$

星敏感器生成觀測容積估計點Z_i,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)$

求取容積濾波增益K_k：

$K_k=P_{xz,k}{P}_{\tilde{z}\tilde{z},k}^{-1}---\left(53\right)$

其中，協方差陣和互協方差陣P_xz,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)得到狀態(tài)量的量測更新為：

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

其中，

計算更新容積誤差四元數和陀螺漂移；容積誤差四元數δq_k更新為：

$\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)$

協方差陣P_k更新為：

$P_k=P_{k|k-1}-K_k{P}_{\tilde{z}\tilde{z},k}{K}_k^T---\left(59\right).$