一种基于对偶四元数的惯性/GPS组合导航方法.pdf

本发明公开了一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。该方法首先建立陀螺和加速度计误差模型，并将陀螺及加速度计误差扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；随后，结合GPS的量测信息及对偶四元数捷联惯导算法的计算值，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；最后，对系统状态方程和量测方程进行离散化处理，采用卡尔曼滤波进行闭环估计并对惯导算法中的推力速度对偶四元数、引力速度对偶四元数、位置对偶四元数进行修正，从而得到载体的导航信息。本方法能够有效利用GPS的输出信息对惯导解算误差进行修正，提高惯性导航系统性能，适用于工程应用。

1.一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，包括如下步骤：
步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫
过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；
步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误
差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状
态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；
步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地
理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地
球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；
步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行
闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四
元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。
2.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，
步骤1所述陀螺和加速度计误差模型为：
${\mathrm{\δ\ω}}_{IB}^B={\mathrm{\ϵ}}_b+{\mathrm{\ϵ}}_r+{\mathrm{\ω}}_g,$
δfB＝▽a，
其中，为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，
ωg为白噪声；δfB为加速度计误差，▽a为加速度计一阶马尔科夫过程随机噪声；
对上式中的εb、εr、▽a进行求导后可得到以下数学表达式：
${\stackrel{\·}{\mathrm{\ϵ}}}_b=0$
${\stackrel{\·}{\mathrm{\ϵ}}}_r=-\frac{1}{T_g}{\mathrm{\ϵ}}_r+{\mathrm{\ω}}_r$
${\stackrel{\·}{\▿}}_a=-\frac{1}{T_a}{\▿}_a+{\mathrm{\ω}}_a$
其中，为εb的一阶导数；为εr的一阶导数；为▽a的一阶导数；Tg为陀螺一阶马尔科
夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过
程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。
3.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，
步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：
$\stackrel{\·}{X}=FX+Gw,$
其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1
为系统噪声向量，为X的一阶导数，各矩阵分别表示为：
$X=\left[\begin{array}{c}{\mathrm{\δq}}_{IT}\\ {\mathrm{\δq}}_{IT}^{\′}\\ {\mathrm{\δq}}_{IG}^{\′}\\ {\mathrm{\δq}}_{IU}^{\′}\\ {\mathrm{\ϵ}}_b\\ {\mathrm{\ϵ}}_r\\ {\▿}_a\end{array}\right],w=\left[\begin{array}{c}{\mathrm{\ω}}_g\\ {\mathrm{\ω}}_r\\ {\mathrm{\ω}}_a\end{array}\right],$
$F=\frac{1}{2}\left[\begin{array}{ccccccc}\stackrel{-}{\[{\mathrm{\ω}}_{IB}^B\]}& 0& 0& 0& \stackrel{+}{\[q_{IT}\]}& \stackrel{+}{\[q_{IT}\]}& 0\\ \stackrel{-}{\[f_B\]}& \stackrel{-}{\[{\mathrm{\ω}}_{IB}^B\]}& 0& 0& \stackrel{+}{\[q_{IT}^{\′}\]}& \stackrel{+}{\[q_{IT}^{\′}\]}& \stackrel{+}{\[q_{IT}\]}\\ 0& 0& \stackrel{-}{\[{\mathrm{\ω}}_{IE}^E\]}& 0& 0& 0& 0\\ -2\stackrel{+}{\[q_{IU}\]}\stackrel{+}{\[q_{IT}^{\′}\]}\stackrel{+}{\[{q^{*}}_{IT}\]}\stackrel{-}{\[{q^{*}}_{IT}\]}& 2\stackrel{-}{\[q_{IU}\]}\stackrel{-}{\[{q^{*}}_{IT}\]}& 2I_4& \stackrel{-}{\[{\mathrm{\ω}}_{IE}^E\]}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& M_1& 0\\ 0& 0& 0& 0& 0& 0& M_2\end{array}\right],$
$G=\frac{1}{2}\left[\begin{array}{ccc}\stackrel{+}{\[q_{IT}\]}& 0& 0\\ \stackrel{+}{\[q_{IT}^{\′}\]}& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& M_3& 0\\ 0& 0& M_3\end{array}\right],$
系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义
矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示
为：
$\stackrel{+}{\[q\]}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right],\stackrel{-}{\[q\]}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& q_3& -q_2\\ q_2& -q_3& q_0& q_1\\ q_3& q_2& -q_1& q_0\end{array}\right],$
系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元
数，其中三维向量表示为标量部分为0的四元数；δqIT为推力速度对偶四元数误差的实数部
分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶
部分，δq′IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘
法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，为qIT在四元数乘法中的前乘
矩阵；q′IT为推力对偶四元数的对偶部分，为q′IT在四元数乘法中的前乘矩阵；为地
球自转角速度，为在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q*IT为qIT的共
轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；
系统矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：
$M_1=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& -\frac{1}{T_g}& 0& 0\\ 0& 0& -\frac{1}{T_g}& 0\\ 0& 0& 0& -\frac{1}{T_g}\end{array}\right],M_2=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& -\frac{1}{T_a}& 0& 0\\ 0& 0& -\frac{1}{T_a}& 0\\ 0& 0& 0& -\frac{1}{T_a}\end{array}\right],M_3=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$
4.根据权利要求1所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步
骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：
VG＝Vn+δV，
RG＝Re+δR，
其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误
差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的
位置测量误差，将其建模为白噪声；
利用如下公式将VG转换到惯性系：
$V_{GI}=C_e^i({\mathrm{\ω}}_{ie}\×R_G+C_n^e{V}_G),$
其中，VGI为VG转换到惯性系下的值，为地球系到惯性系的转换矩阵，ωie为四元数
的矢量部分，为地理系到地球系的转换矩阵；
由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：
Z＝HX+v，
其中，为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为
对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
为系统量测噪声阵；
量测系数矩阵H的具体表达式为：
$H=2\left[\begin{array}{ccccccc}-\stackrel{-}{\[{q^{*}}_{IT}\]}\stackrel{+}{\[q_{IT}^{\′}\]}\stackrel{+}{\[{q^{*}}_{IT}\]}& \stackrel{-}{\[{q^{*}}_{IT}\]}& \stackrel{+}{\[{q^{*}}_{IG}\]}& 0& 0& 0& 0\\ 0& 0& 0& \stackrel{-}{\[{q^{*}}_{IU}\]}& 0& 0& 0\end{array}\right],$
其中，qIG为引力速度对偶四元数的实数部分，q*IG为qIG的共轭四元数，为q*IG在四
元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q*IU为qIU的共轭四元数，为
q*IU在四元数乘法中的前乘矩阵，为qI′T在四元数乘法中的前乘矩阵。
5.根据权利要求1所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，所
述步骤4的具体过程为：
(401)将系统状态方程和量测方程离散化处理：
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，
Zk＝HkXk+Vk，
其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时刻系
统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系统的
噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪
声矩阵；
(402)采用卡尔曼滤波对状态量进行闭环估计：
${\hat{X}}_{k,k-1}=0$
$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$
$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$
${\hat{X}}_k=K_k{Z}_k$
$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$
其中，是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,
Qk-1为tk-1时刻系统噪声协方差矩阵,为Φk,k-1的转置，为Γk,k-1的转置，Pk,k-1为
tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时
刻滤波增益矩阵,为Hk的转置，为状态量Xk的卡尔曼滤波估值，I为单位矩阵，为Kk
的转置，Pk为tk时刻滤波状态估计协方差矩阵；
(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中
的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：
${\hat{q}}_{IT}=q_{IT}-\mathrm{\δ}{\hat{q}}_{IT},$
${\hat{q}}_{IT}^{\′}=q_{IT}^{\′}-\mathrm{\δ}{\hat{q}}_{IT}^{\′},$
${\hat{q}}_{IG}^{\′}=q_{IG}^{\′}-\mathrm{\δ}{\hat{q}}_{IG}^{\′},$
${\hat{q}}_{IU}^{\′}=q_{IU}^{\′}-\mathrm{\δ}{\hat{q}}_{IU}^{\′},$
上式中，为δqIT的估计值，为修正后的推力速度对偶四元数的实数部分；为δ
q′IT的估计值，为修正后的推力速度对偶四元数的对偶部分；为δq′IG的估计值，为
修正后的引力速度对偶四元数的对偶部分；为δq′IU的估计值，为修正后的位置对偶
四元数的对偶部分。

一种基于对偶四元数的惯性/GPS组合导航方法

技术领域

本发明涉及一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航
技术领域。

背景技术

近年来，随着高超声速飞行器等高动态飞行器的研制发展，对导航系统性能提出
了更高的要求。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点，但其误
差会随时间累积，需要其他导航手段加以辅助。

GPS全球定位系统是一种高精度的全球三维实时卫星系统，其导航定位的全球性
和高精度使其成为一种先进的导航设备。但是GPS全球定位系统也存在一些不足之处，主要
是GPS在受到遮挡的情况下容易信号丢失，且容易受到人为控制和干扰，因此主要作为一种
辅助导航设备使用。惯性/GPS组合克服了各自缺点，取长补短，使组合后的导航精度高于两
个系统单独工作的精度。

对偶四元数捷联惯性导航算法将载体的旋转和平移统一考虑，以最简洁的形式表
示一般的刚体运动，在高动态环境下，具有比传统捷联惯导算法更高的精度，更能满足高动
态飞行器对高精度导航性能的要求。传统的惯性导航算法误差模型通常是基于数学平台失
准角的线性误差方程，但是该模型仅适用于平台失准角为小量的情况，当载体的姿态角误
差较大时，在组合导航算法中使用该模型会使滤波精度大大降低，收敛时间变长甚至会导
致发散。基于对偶四元数的捷联惯性导航算法可以直接利用对偶四元数建立线性误差模
型，即使在大失准角的情况也同样能具有较快的收敛速度，较好的滤波精度，从而提高了组
合导航系统性能。因此，研究基于对偶四元数的惯性/GPS组合导航算法具有重要的研究意
义。

发明内容

本发明提出了一种基于对偶四元数的惯性/GPS组合导航方法，在飞行器动态飞行
过程中有效利用GPS提供的速度位置信息，对惯性导航解算参数误差进行修正，显著提高惯
性导航系统精度。

本发明为解决其技术问题采用如下技术方案：

一种基于对偶四元数的惯性/GPS组合导航方法，包括如下步骤：

步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔
科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪
声；

步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂
移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系
统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；

步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得
的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度
及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；

步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量
进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对
偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。

步骤1所述陀螺和加速度计误差模型为：

其中，为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机
噪声，ωg为白噪声；δfB为加速度计误差，为加速度计一阶马尔科夫过程随机噪声；

对上式中的εb、εr、进行求导后可得到以下数学表达式：

其中，为εb的一阶导数；为εr的一阶导数；为的一阶导数；Tg为陀螺一阶马
尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科
夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。

步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：

其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈
R12×1为系统噪声向量，为X的一阶导数，各矩阵分别表示为：

系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们
定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可
表示为：

系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为
四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实
数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的
对偶部分，δq′IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元
数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，为qIT在四元数乘法中的
前乘矩阵；q′IT为推力对偶四元数的对偶部分，为q′IT在四元数乘法中的前乘矩阵；
为地球自转角速度，为在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部
分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q*IT为qIT
的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩
阵；

系统矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：

步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：

VG＝Vn+δV，

RG＝Re+δR，

其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测
量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为
GPS的位置测量误差，将其建模为白噪声；

利用如下公式将VG转换到惯性系：

其中，VGI为VG转换到惯性系下的值，为地球系到惯性系的转换矩阵，ωie为四元
数的矢量部分，为地理系到地球系的转换矩阵；

由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：

Z＝HX+v，

其中，为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，
RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
为系统量测噪声阵；

量测系数矩阵H的具体表达式为：

其中，qIG为引力速度对偶四元数的实数部分，为qIG的共轭四元数，为q*IG
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，*IU为qIU的共轭四元数，
为q*IU在四元数乘法中的前乘矩阵，为q′IT在四元数乘法中的前乘矩阵。

所述步骤4的具体过程为：

(401)将系统状态方程和量测方程离散化处理：

Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，

Zk＝HkXk+Vk，

其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时
刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系
统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测
量噪声矩阵；

(402)采用卡尔曼滤波对状态量进行闭环估计：

其中，是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩
阵,Qk-1为tk-1时刻系统噪声协方差矩阵,为Φk,k-1的转置，为Γk,k-1的转置，Pk,k-1
为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk
时刻滤波增益矩阵,为Hk的转置，为状态量Xk的卡尔曼滤波估值，I为单位矩阵，为
Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；

(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算
法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：

上式中，为δqIT的估计值，为修正后的推力速度对偶四元数的实数部分；
为δq′IT的估计值，为修正后的推力速度对偶四元数的对偶部分；为δq′IG的估计
值，为修正后的引力速度对偶四元数的对偶部分；为δq′IU的估计值，为修正后的
位置对偶四元数的对偶部分。

本发明的有益效果如下：

1、基于对偶四元数的惯性/GPS算法可以直接利用对偶四元数建立线性误差模型，
使得该组合导航系统即使在大失准角的情况下也同样能具有较快的滤波收敛速度和较好
的滤波精度，从而提高了组合导航系统性能，适合工程应用。

2、本发明方法给出了组合前GPS输出信息的转换方法，并给出了坐标转化后的系
统量测噪声模型，该模型可以有效提高对偶四元数惯性/GPS组合导航精度，适合工程应用。

附图说明

图1是本发明基于对偶四元数的惯性/GPS组合导航方法的架构图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

如图1所示，本发明所述基于对偶四元数的惯性/GPS组合导航方法的原理是：GPS
输出地理系速度及地球系位置信息，将其输出的地理系速度经坐标转换为惯性系速度。同
时，对偶四元数捷联惯导算法也可以相应得到载体在惯性系下的速度信息及地球系下的位
置信息。利用卡尔曼滤波器融合GPS和对偶四元数捷联惯导算法的地球系位置、惯性系速
度，在线估计得到对偶四元数误差参数，利用该信息对对偶四元数进行补偿修正，以此提高
惯性导航系统性能。

本发明的具体实施方式如下：

1、建立陀螺、加速度计误差模型

陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声及白噪声误差，加速度
计误差建模为一阶马尔科夫过程随机噪声，具体误差模型如下：

其中，为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机
噪声，ωg为白噪声；δfB为加速度计误差，为加速度计一阶马尔科夫过程随机噪声；

对上式中的εb、εr、进行求导后可得到以下数学表达式：

其中，为εb的一阶导数；为εr的一阶导数；为的一阶导数；Tg为陀螺一阶马
尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科
夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。

2、建立基于陀螺、加速度计误差模型的卡尔曼滤波状态方程

状态方程如下所示：

其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈
R12×1为系统噪声向量，为X的一阶导数，各矩阵分别表示为：

系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们
定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可
表示为：

式(7)～(10)中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三
维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为
推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU
为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后
乘矩阵；qIT为推力速度对偶四元数的实数部分，为qIT在四元数乘法中的前乘矩阵；q′IT
为推力对偶四元数的对偶部分，为q′IT在四元数乘法中的前乘矩阵；为地球自转角
速度，为在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，为其
在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q*IT为qIT的共轭四元
数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；

式(9)和式(10)中M1、M2、M3可分别表示为：

3、建立基于GPS测量误差模型的卡尔曼滤波量测方程

(3.1)GPS测量误差模型

GPS测量误差模型为：

VG＝Vn+δV (16)

RG＝Re+δR (17)

其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测
量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为
GPS的位置测量误差，将其建模为白噪声；

利用如下公式将VG转换到惯性系：

其中，VGI为VG转换到惯性系下的值，为地球系到惯性系的转换矩阵，ωie为四元
数的矢量部分，为地理系到地球系的转换矩阵；

(3.2)建立卡尔曼滤波测量方程

量测方程如下所示：

Z＝HX+v (19)

其中，为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，
RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
为系统量测噪声阵；

量测系数矩阵H的具体表达式为：

其中，qIG为引力速度对偶四元数的实数部分，q*IG为qIG的共轭四元数，为q*IG
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q*IU为qIU的共轭四元数，
为q*IU在四元数乘法中的前乘矩阵，为q′IT在四元数乘法中的前乘矩阵。

4、对偶四元数误差在线估计与补偿修正

(4.1)系统状态方程和量测方程离散化处理

将系统状态方程和量测方程进行离散化处理：

Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1 (21)

Zk＝HkXk+Vk (22)

其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时
刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系
统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测
量噪声矩阵；

(4.2)卡尔曼滤波闭环估计

采用卡尔曼滤波对状态量进行闭环估计：

其中，是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩
阵,Qk-1为tk-1时刻系统噪声协方差矩阵,为Φk,k-1的转置，为Γk,k-1的转置，Pk,k-1
为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk
时刻滤波增益矩阵,为Hk的转置，为状态量Xk的卡尔曼滤波估值，I为单位矩阵，为
Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；

(4.3)对偶四元数误差在线补偿修正

由(4.2)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中
的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：

上式中，为δqIT的估计值，为修正后的推力速度对偶四元数的实数部分；
为δq′IT的估计值，为修正后的推力速度对偶四元数的对偶部分；为δq′IG的估计
值，为修正后的引力速度对偶四元数的对偶部分；为δq′IU的估计值，为修正后的
位置对偶四元数的对偶部分。