CN104535080A - 大方位失准角下基于误差四元数的传递对准方法 - Google Patents

Abstract

本发明公开了一种大方位失准角下基于误差四元数的传递对准方法。包括以下几个步骤：将主惯导和子惯导安装在载体上，使主惯导完成自对准过程，建立主惯导载体系；根据获得的主惯导的导航信息作为初始值直接给需要对准的子惯导系统中，使当前子惯导系统完成一次装订粗对准过程；求得子惯导载体系和主惯导载体系之间的安装误差角，将安装误差角等价转换为误差四元数；将安装误差角中的动态变形角视为白噪声过程，得到白噪声过程参数；建立状态方程和状态观测方程，得到滤波模型，进行卡尔曼滤波，得到状态估计；得到子惯导的真实姿态，完成对准。本发明具有估计精度高、估计速度快的优点。

Description

大方位失准角下基于误差四元数的传递对准方法

技术领域

本发明属于一种传递对准方法，尤其涉及一种应用于大方位失准角情况时的，大方位失准角下基于误差四元数的传递对准方法。

背景技术

初始对准技术是惯导系统的关键技术之一，它是惯导系统正常工作的基本条件。而传递对准即是针对于主、子惯导之间的一种初始对准技术。当安装误差角为小角度的情况下，传递对准的模型是线性的，因此可以直接用卡尔曼滤波处理，但是在实际应用当中，不可避免的会出现大方位失准角的问题，从而导致了传递对准模型的非线性。传统的解决办法是采用非线性滤波进行处理，但是这样的办法鲁棒性差，估计速度和估计精度都有很大的改善空间。

现有技术中，《大方位失准角下舰载机快速传递对准技术》，杨功流，中国惯性技术学报，2014年2期，介绍了大方位失准角快速传递对准模型，然后基于实际情况进行仿真实验，但是该方法同样采用的是非线性误差模型。

发明内容

本发明的目的是提供一种具有估计精度高和估计速度快的大方位失准角下基于误差四元数的传递对准方法。

本发明是通过以下技术方案实现的：

大方位失准角下基于误差四元数的传递对准方法，包括以下几个步骤：

步骤一：将主惯导和子惯导安装在载体上，使主惯导完成自对准过程，建立主惯导载体系m，获得主惯导的导航信息以及主惯导载体系m到地理系n的捷联矩阵

步骤二：根据获得的主惯导的导航信息作为初始值直接给需要对准的子惯导系统中，使当前子惯导系统完成一次装订粗对准过程，并且建立子惯导系统载体系s；

步骤三：求得子惯导载体系s和主惯导载体系m之间的安装误差角φ_a＝[φ_axφ_ayφ_az]^T，将安装误差角φ_a等价转换为误差四元数

步骤四：将安装误差角φ_a中的动态变形角θ_λ视为白噪声过程，得到白噪声过程参数η＝[η₀ η₁ η₂ η₃]^T，并且白噪声过程参数满足等式；

步骤五：将误差四元数和白噪声过程参数η作为滤波模型的状态量X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T，建立状态方程：

$\stackrel{\·}{X}=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]X;$

步骤六：根据状态量建立状态观测方程；

步骤七：根据状态方程和状态观测方程建立滤波模型，进行卡尔曼滤波，得到状态估计；

步骤八：将状态估计转换为欧拉角形式得到安装误差角的估计值，得到子惯导的真实姿态，完成对准。

本发明大方位失准角下基于误差四元数的传递对准方法还包括：

1、将安装误差角等价转换为误差四元数的方法为：

建立从主惯导载体系m到子惯导载体系s的方向余弦矩阵将方向余弦矩阵等价转换为误差四元数 $Q_m^s={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T.$

2、根据状态量建立状态观测方程为：

$\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=0_{4\×1}$

$[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]=\left[\begin{array}{ccc}0& -({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& ({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)\\ ({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& 0& -({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)\\ -({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)& ({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)& 0\end{array}\right]$

其中表示为子惯导测得的子惯导载体系s相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量，表示为主惯导测得的主惯导载体系m相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量。

3、根据状态方程和状态观测方程建立滤波模型为：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+V\end{array}\right.$

其中状态量选取为X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T，

并且有：

$\begin{array}{cc}A=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]& B=\left[\begin{array}{c}{I}_{4\×4}\\ 0_{4\×4}\end{array}\right]\end{array}$

$H=\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]$

W＝[w_q0 w_q1 w_q2 w_q3]^T

w_q0,w_q1,w_q2,w_q3为子惯导陀螺系统噪声的四元数形式，观测量Z为4×1的零向量，V为量测噪声；

所述的卡尔曼滤波的基本方程为：

选取状态初值X₀＝[1 0 0 0 1 0 0 0]^T，初始均方误差阵为对角阵P₀，

状态一步预测为：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1},$

一步预测均方误差阵：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{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={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}],$

估计均方误差阵：

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T,$

估计状态归一化处理：

${\hat{X}}_k={\hat{X}}_k/\left|{\hat{X}}_k\right|,$

其中为第k时刻系统状态估计，为根据k-1时刻的状态估计对k时刻状态进行预测得到的状态，Φ_k,k-1为k-1时刻到k时刻的状态转移矩阵，P_k-1为第k-1时刻的均方误差阵，P_k,k-1为根据k-1时刻的均方误差阵对k时刻均方误差阵进行预测得到的矩阵，K_k为第k时刻的滤波增益阵，Z_k为第k时刻的量测值，H_k为第k时刻的量测矩阵，I为单位阵，Γ_k-1为第k-1时刻的系统噪声驱动矩阵，R_k为第k时刻的观测噪声矩阵，Q_k-1为第k-1时刻的系统噪声矩阵，表示为第k时刻系统状态估计的模。

4、将状态估计转换为欧拉角形式方法为：

将状态估计的前四位定义为一组四元数

建立方向余弦矩阵

转换为欧拉角形式：

分别表示为第k时刻东向，北向，航向方向上安装误差角的估计值。

将安装误差角的估计值，与主惯导第k时刻的真实姿态做差，从而得到子惯导此时的真实姿态，以及子惯导的真实载体系s，完成对准。

有益效果：

本发明提出了针对大方位失准角下，传统误差模型为非线性的传递对准问题解决方法，即通过误差四元数建立新的线性模型的方法，并且具有以下优点：

一、通过误差四元数取代欧拉角，从而将误差量的表示形式从三维空间扩展到四维空间并且忽略子惯导的陀螺漂移，建立新的线性模型，从而可以直接用卡尔曼滤波处理，避免了非线性滤波为次优滤波，精度低，鲁棒性，收到本身局限性限制的问题，使得本发明具有估计精度高、估计速度快的优点。

二、本发明可以看成是一种角速度匹配，从而可以应用到高转速的环境中，并且不需要考虑杆臂效应。

三、本发明的观测量为零，相比于传统的传递对准方法，采用观测矩阵内部参数的变化来代替量测，具有滤波模型简单，易于分析的优点。

四、安装误差角越大，误差模型非线性越强，那么用非线性滤波的估计精度和估计速度会随着非线性的强度增大而降低，但是本方法由于建立的模型就是线性的，所以完全不受到安装误差角大小的影响。

附图说明

图1是本发明的流程图；

图2是本发明的方法与传统方法在X轴方向的仿真对比图；

图3是本发明的方法与传统方法在Y轴方向的仿真对比图；

图4是本发明的方法与传统方法在Z轴方向的仿真对比图。

具体实施方式

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

如附图1所示，本发明的具体方法以及相应步骤为：

步骤一、将主、子惯导都安装在载体上，使主惯导完成自对准过程，建立主惯导载体系为m，从而获得主惯导的真实姿态以及主惯导载体系m到地理系n的捷联矩阵

步骤二、将步骤一中已经对准完毕的主惯导系统的速度，位置，姿态等导航信息作为初始值直接复制到需要对准的子惯导系统中，从而使子惯导系统完成一次装订粗对准过程，并且令需要获得的子惯导载体系为s。

步骤三、令子惯导载体系s与主惯导载体系m之间的安装误差角为φ_a＝[φ_ax φ_ay φ_az]^T，然后把从主惯导载体系m到子惯导载体系s的方向余弦矩阵设为并且将其等价转换为误差四元数 表示为的四元数形式。

步骤四、将安装误差角中的动态变形角θ_λ视为白噪声过程，从而有η＝[η₀ η₁ η₂ η₃]^T，为白噪声过程的参数，且有

步骤五、将步骤三中的误差四元数与步骤四中的白噪声过程参数η选为滤波模型的状态量X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T，从而建立状态方程：

$\stackrel{\·}{X}=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]X$

I_4×4为四维单位阵。

步骤六、利用步骤五中的状态量建立状态观测方程如下：

$\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=0_{4\×1}$

$[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]=\left[\begin{array}{ccc}0& -({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& ({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)\\ ({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& 0& -({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)\\ -({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)& ({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)& 0\end{array}\right]$

其中表示为子惯导测得的子惯导载体系s相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量。表示为主惯导测得的主惯导载体系m相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量，0_4×1表示维数为4×1的零向量。

步骤七、利用步骤五中的状态方程以及步骤六中的观测方程建立滤波模型如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+V\end{array}\right.$

其中状态量选取为X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T

并且有：

$\begin{array}{cc}A=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]& B=\left[\begin{array}{c}{I}_{4\×4}\\ 0_{4\×4}\end{array}\right]\end{array}$

$H=\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]$

W＝[w_q0 w_q1 w_q2 w_q3]^T

w_q0,w_q1,w_q2,w_q3为子惯导陀螺系统噪声的四元数形式。观测量Z为4×1的零向量，V为量测噪声。

步骤八、用步骤七的滤波模型进行卡尔曼滤波，选取状态初值X₀＝[1 0 0 0 1 0 0 0]^T，初始均方误差阵为对角阵P₀，P₀内部参数根据情况设定，卡尔曼基本方程如下：

状态一步预测

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

一步预测均方误差阵

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{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={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

估计均方误差阵

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$

估计状态归一化处理

${\hat{X}}_k={\hat{X}}_k/\left|{\hat{X}}_k\right|$

之后再返回状态一步预测，继续迭代，直到设定时间结束为止。

其中为第k时刻系统状态估计，为根据k-1时刻的状态估计对k时刻状态进行预测得到的状态，Φ_k,k-1为k-1时刻到k时刻的状态转移矩阵，P_k-1为第k-1时刻的均方误差阵，P_k,k-1为为根据k-1时刻的均方误差阵对k时刻均方误差阵进行预测得到的矩阵，K_k为第k时刻的滤波增益阵，Z_k为第k时刻的量测值，H_k为第k时刻的量测矩阵，I为单位阵，Γ_k-1为第k-1时刻的系统噪声驱动矩阵，R_k为第k时刻的观测噪声矩阵，Q_k-1为第k-1时刻的系统噪声矩阵。表示为第k时刻系统状态估计的模。

步骤九、将步骤八中得到的状态估计的前四位定义为一组四元数然后建立方向余弦矩阵如下表示:

之后带入到下式，转换为欧拉角形式：

分别表示为第k时刻东向，北向，航向方向上安装误差角的估计值。

步骤十、利用步骤九得到的与主惯导第k时刻的真实姿态做差，从而得到子惯导此时的真实姿态，以及子惯导的真实载体系s，完成对准。

为了进一步说明所述方法的效果，在以下初始条件下分别对传统方法以及本发明的方法进行了仿真，仿真结果如附图2、附图3与附图4所示，并对其进行了分析比较。

初始条件：

地点选为哈尔滨，初始纬度初始经度λ＝126.6705°，设主惯导为高精度惯导系统，没有误差，子惯导陀螺常值漂移为0.01°/h，加速度计零偏为10^-4g₀。载体摇摆幅值选为，纵摇角5°，纵摇摇摆周期为6s，横摇角3°，横摇摇摆周期为9s，航向角2°，航向摇摆周期为10s，并且载体以2m/s的速度匀速北向行驶，采样频率为0.1s，仿真时间为300s，主、子惯导之间的安装误差角设为φ_ax＝1°,φ_ay＝1°,φ_az＝15°。

分析比较:

从图2、图3与图4可以看出，本文提出的新方法在估计速度和估计精度上相比与传统模型下的UKF，CKF方法有着明显的优势，UKF和CKF方法在大约100s左右完成收敛，而本文提出的新方法在10s内就能完成收敛，并且UKF和CKF方法估计精度只有19′左右，而本文提出的新方法能够达到2′的精度，估计精度更优，特别是在水平方向上的估计精度有着明显的提升。分析原因，认识这是由于选择滤波方法的原因，卡尔曼滤波是最优的滤波方法，而UKF和CKF都是衍生于卡尔曼滤波的次优滤波方法，远达不到卡尔曼滤波的估计效果。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.大方位失准角下基于误差四元数的传递对准方法，其特征在于，包括以下几个步骤：

步骤一：将主惯导和子惯导安装在载体上，使主惯导完成自对准过程，建立主惯导载体系m，获得主惯导的导航信息以及主惯导载体系m到地理系n的捷联矩阵

步骤二：根据获得的主惯导的导航信息作为初始值直接给需要对准的子惯导系统中，使当前子惯导系统完成一次装订粗对准过程，并且建立子惯导系统载体系s；

步骤三：求得子惯导载体系s和主惯导载体系m之间的安装误差角φ_a＝[φ_ax φ_ay φ_az]^T，将安装误差角φ_a等价转换为误差四元数

步骤四：将安装误差角φ_a中的动态变形角θ_λ视为白噪声过程，得到白噪声过程参数η＝[η₀ η₁ η₂ η₃]^T，并且白噪声过程参数满足等式；

步骤五：将误差四元数和白噪声过程参数η作为滤波模型的状态量X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T，建立状态方程：

$\stackrel{\·}{X}=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]X;$

步骤六：根据状态量建立状态观测方程；

步骤七：根据状态方程和状态观测方程建立滤波模型，进行卡尔曼滤波，得到状态估计；

步骤八：将状态估计转换为欧拉角形式得到安装误差角的估计值，得到子惯导的真实姿态，完成对准。

2.根据权利要求1所述的大方位失准角下基于误差四元数的传递对准方法，其特征在于：

所述的将安装误差角等价转换为误差四元数的方法为：

建立从主惯导载体系m到子惯导载体系s的方向余弦矩阵将方向余弦矩阵等价转换为误差四元数 $Q_m^s={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T.$

3.根据权利要求1所述的大方位失准角下基于误差四元数的传递对准方法，其特征在于：

所述的根据状态量建立状态观测方程为：

$\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=0_{4\×1}$

$[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]=\left[\begin{array}{ccc}0& -({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& ({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)\\ ({\mathrm{\ω}}_{\mathrm{isz}}^s+{\mathrm{\ω}}_{\mathrm{imz}}^m)& 0& -({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)\\ -({\mathrm{\ω}}_{\mathrm{isy}}^s+{\mathrm{\ω}}_{\mathrm{imy}}^m)& ({\mathrm{\ω}}_{\mathrm{isx}}^s+{\mathrm{\ω}}_{\mathrm{imx}}^m)& 0\end{array}\right]$

其中 ${\mathrm{\ω}}_{\mathrm{is}}^s={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{isx}}^s& {\mathrm{\ω}}_{\mathrm{isy}}^s& {\mathrm{\ω}}_{\mathrm{isz}}^s\end{array}\right]}^T$ 表示为子惯导测得的子惯导载体系s相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量， ${\mathrm{\ω}}_{\mathrm{im}}^m={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{imx}}^m& {\mathrm{\ω}}_{\mathrm{imy}}^m& {\mathrm{\ω}}_{\mathrm{imz}}^m\end{array}\right]}^T$ 表示为主惯导测得的主惯导载体系m相对于地心惯性系i的角速度，其中分别表示xyz轴上的分量。

4.根据权利要求1所述的大方位失准角下基于误差四元数的传递对准方法，其特征在于：

所述的根据状态方程和状态观测方程建立滤波模型为：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+V\end{array}\right.$

其中状态量选取为X＝[q₀ q₁ q₂ q₃ η₀ η₁ η₂ η₃]^T，

并且有：

$\begin{array}{cc}A=\left[\begin{array}{cc}{0}_{4\×4}& I_{4\×4}\\ 0_{4\×4}& 0_{4\×4}\end{array}\right]& B=\left[\begin{array}{c}{I}_{4\×4}\\ 0_{4\×4}\end{array}\right]\end{array}$

$H=\left[\begin{array}{cc}0& -{({\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m)}^T\\ {\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{im}}^m& -[({\mathrm{\ω}}_{\mathrm{is}}^s+{\mathrm{\ω}}_{\mathrm{im}}^m)\×]\end{array}\right]$

W＝[w_q0 w_q1 w_q2 w_q3]^T

w_q0,w_q1,w_q2,w_q3为子惯导陀螺系统噪声的四元数形式，观测量Z为4×1的零向量，V为量测噪声；

所述的卡尔曼滤波的基本方程为：

选取状态初值X₀＝[1 0 0 0 1 0 0 0]^T，初始均方误差阵为对角阵P₀，

状态一步预测为：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1},$

一步预测均方误差阵：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{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={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}],$

估计均方误差阵：

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T,$

估计状态归一化处理：

${\hat{X}}_k={\hat{X}}_k/\left|{\hat{X}}_k\right|,$

其中为第k时刻系统状态估计，为根据k-1时刻的状态估计对k时刻状态进行预测得到的状态，Φ_k,k-1为k-1时刻到k时刻的状态转移矩阵，P_k-1为第k-1时刻的均方误差阵，P_k,k-1为根据k-1时刻的均方误差阵对k时刻均方误差阵进行预测得到的矩阵，K_k为第k时刻的滤波增益阵，Z_k为第k时刻的量测值，H_k为第k时刻的量测矩阵，I为单位阵，Γ_k-1为第k-1时刻的系统噪声驱动矩阵，R_k为第k时刻的观测噪声矩阵，Q_k-1为第k-1时刻的系统噪声矩阵，表示为第k时刻系统状态估计的模。

5.根据权利要求1所述的大方位失准角下基于误差四元数的传递对准方法，其特征在于：

所述的将状态估计转换为欧拉角形式方法为：

将状态估计的前四位定义为一组四元数

建立方向余弦矩阵

转换为欧拉角形式：

分别表示为第k时刻东向，北向，航向方向上安装误差角的估计值。

将安装误差角的估计值，与主惯导第k时刻的真实姿态做差，从而得到子惯导此时的真实姿态，以及子惯导的真实载体系s，完成对准。