技術(shù)特征：

1.一種基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，其特征在于，包括如下步驟：

步驟1：建立聯(lián)合標(biāo)定問題的狀態(tài)空間模型；

步驟2：采用序貫濾波方法估計(jì)出狀態(tài)空間模型的狀態(tài)。

2.根據(jù)權(quán)利要求1所述的基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，其特征在于，所述步驟1中聯(lián)合標(biāo)定問題的狀態(tài)空間模型如下：

系統(tǒng)方程：

${\stackrel{\·}{C}}_b^i=C_b^i({\mathrm{\ω}}_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

其中：陀螺/加速度計(jì)的體坐標(biāo)系表示為b系，慣性坐標(biāo)系表示為i系，即初始時刻的b系；

式中：表示慣性坐標(biāo)系相對于體坐標(biāo)系的姿態(tài)矩陣，表示的時間導(dǎo)數(shù)，表示陀螺測量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的時間導(dǎo)數(shù)，n_g表示陀螺測量的高斯噪聲，n_ε表示陀螺零偏的高斯噪聲，矩陣S表示磁力儀參數(shù)矩陣，表示矩陣S的時間導(dǎo)數(shù)，向量h表示磁力儀零偏，表示h的時間導(dǎo)數(shù)，mⁱ為i系下表示的恒定磁場向量，表示mⁱ的時間導(dǎo)數(shù)，n_mi表示因地球自轉(zhuǎn)引起的磁場向量誤差；其中：運(yùn)算a×表示由一個三維向量a＝[a₁ a₂ a₃]^T構(gòu)成的叉乘矩陣，具體地，a×的展開式如下：

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

觀測方程：

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

式中：y_m表示磁力儀的測量值，為姿態(tài)矩陣的轉(zhuǎn)置，即上標(biāo)T表示矩陣或向量的轉(zhuǎn)置運(yùn)算，n_m表示磁力儀測量值的高斯噪聲。

3.根據(jù)權(quán)利要求1所述的基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，其特征在于，步驟1中的狀態(tài)空間模型的狀態(tài)包括姿態(tài)、磁力儀內(nèi)部參數(shù)、磁力儀外部參數(shù)、陀螺零偏和磁場向量；磁場向量mⁱ的初始值包括：磁力儀的首次測量值；磁力儀參數(shù)矩陣S的初始值包括：單位矩陣或事先給定；陀螺零偏ε和磁力儀零偏h的初始值選為零；因狀態(tài)可觀性的關(guān)系，姿態(tài)的初始值確定為單位矩陣，對應(yīng)的初始方差矩陣為零。

4.根據(jù)權(quán)利要求3所述的基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，其特征在于，所述步驟2包括：采用序貫濾波方法估計(jì)出狀態(tài)空間模型的狀態(tài)，所述序貫濾波方法包括：擴(kuò)展卡爾曼濾波，或者粒子濾波法；具體地，采用擴(kuò)展卡爾曼濾波時，包括如下步驟：

步驟2.1：定義狀態(tài)誤差δx為估計(jì)值減去真值x，即δ表示對應(yīng)狀態(tài)的誤差，而姿態(tài)估計(jì)值與姿態(tài)真值及姿態(tài)誤差ψ的關(guān)系定義為則相應(yīng)的狀態(tài)誤差向量表示為δx≡[ψ^T δε^T vec^T(δS) δh^T δm^iT]^T，vec(·)表示將矩陣按照列的順序拼接起來的運(yùn)算；ψ^T表示姿態(tài)誤差ψ的轉(zhuǎn)置，ε^T表示陀螺零偏向量ε的轉(zhuǎn)置，h^T、m^iT分別表示h、mⁱ的轉(zhuǎn)置；

狀態(tài)誤差的近似線性狀態(tài)空間模型按如下給出

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ {\mathrm{\δy}}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

其中：噪聲

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

式中：表示δx的時間導(dǎo)數(shù)，矩陣F、G、H_m分別表示系統(tǒng)矩陣、系統(tǒng)輸入矩陣和觀測矩陣；I表示單位矩陣，I的下標(biāo)表示單位矩陣的階數(shù)；0表示零矩陣，0的下標(biāo)表示零矩陣的行列數(shù)；符號表示矩陣與矩陣的Kronecker乘積；

步驟2.2：假定當(dāng)?shù)卮艌鱿蛄康拇笮?，對擴(kuò)展卡爾曼濾波的結(jié)果進(jìn)行如下尺度調(diào)整；調(diào)整公式如下：

S_rs＝||mⁱ||S，

mⁱ_rs＝mⁱ/||mⁱ||；

式中：S_rs表示尺度調(diào)整后的磁力儀矩陣，mⁱ_rs表示尺度調(diào)整后的磁場向量，||·||表示取模運(yùn)算。