CN104567871A - 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 - Google Patents

Abstract

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。本发明包括：设定初始参数值；采集载体运动过程中陀螺及磁强计的输出数据作为量测量；测量地理系下地磁梯度张量；测量载体系下地磁梯度张量；进行状态更新；估计k时刻的状态；估计k时刻的状态。本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍，而且通过三轴磁通门磁强计测量获取量测值y_k的方法具有价格低廉的潜在优势。

Description

一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

技术领域

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。

背景技术

姿态测量是现代导航、制导和控制等诸多领域的一个重要问题。目前，载体姿态确定的方法主要可以分为两大类，矢量确定法和状态估计法。矢量确定法是利用几何关系直接计算，当参考坐标系中的两个矢量准确已知的情况下，采用矢量匹配方法获得单次观测的最优解。但很小的水平基准误差就会带来很大的航向误差，无法利用***动态信息和历史观测信息，不能通过滤波算法提高姿态估计的精度。另一方面，两个矢量准确已知也是很难做到的。这种方法在很大程度上受到观测矢量的精度限制，无法克服观测矢量的不确定性。状态估计法是通过建立动力学或者运动学模型，得到被估计量动态变化模型和测量模型，采用递推的方法计算估计姿态参数和误差参数。它是一种基于统计特性的方法，能够有效克服传感器误差项引起的参考矢量不确定性，得到统计意义上状态量的最优估计值，提高姿态确定的精度。此外该方法可结合载体动态信息和历史观测信息实现递推估计，在提高测量精度的同时还可同步估计姿态速率信息。常见的状态估计法有扩展卡尔曼滤波(EKF)和非线性卡尔曼滤波(UKF)、递推四元数估计(REQUEST)、扩展四元数估计(EQE)等，还有最小模型误差、自适应滤波、预测滤波和鲁棒滤波等用于估计姿态参数。EKF方法是基于对非线性方程的线性化，当***具有强非线性时，线性化可能引起大的误差甚至造成滤波器的不稳定；UKF可以克服EKF的缺点，但对随机变量非线性变换后概率分布的逼近精度相对较低；REQUEST是一种类EKF方法，它在每一步中应用QUEST算法，可以得到比传统的EKF更高的精度，但却难于扩展估计其它参量，EQE结合了REQUEST和EKF的优点，但它同样不能避免线性化带来的问题。

相比于磁场矢量测量，磁场梯度张量测量具有磁矢量测量的优势，但没有磁矢量测量对地磁场方向敏感的缺点，相比于磁异常测量，磁场梯度张量测量能够测量更多关于实测地点的信息，容易实现对测量数据的三维反演；磁场梯度张量作为观测量，根据载体姿态估计的过程方程和观测方程具有强非线性的特性，提出的一种新的四元数卡尔曼滤波(CubatureKalman Filter：CKF)算法，算法本身内蕴了对四元数单位长度的限制，不再需要对四元数估计值进行归一化处理；在高维***中，CKF(Cubature Kalman Filter)的估计精度优于UKF。

本发明提出的一种基于磁场梯度张量测量的新的姿态测量优化递推估计算法思路新，未见文献报道。

发明内容

本发明的目的在于提供一种能够提高估计精度的基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。

本发明的目的是这样实现的：

(1)设定初始参数值：

由惯性测量单元输出数据确定初始时刻***状态值和状态协方差P₀；

(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量；

(3)测量地理系下地磁梯度张量Gⁿ；

根据惯性/地磁组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gⁿ的5个独立分量和

(4)测量载体系下地磁梯度张量G^b；

根据步骤(2)中磁强计的输出数据测量载体系下磁场分量h₁～h₁₀，计算载体系下地磁梯度张量G^b的5个分量；

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x}\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.;$

L_x和L_y分别为x_b和y_b方向上梯度测量基线长；

(5)k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新：

过程噪声和观测噪声都是加性的，状态空间形式的离散非线性***为：

x_k＝f(x_k-1)+w_k-1

y_k＝h(x_k)+v_k

x_k∈Rⁿ和y_k∈R^m分别为状态向量和量测向量；f(·)和h(·)分别为***非线性四元数状态方程量测方程；

姿态估计***的非线性四元数状态方程：

单位四元数：

$q=\left[\begin{array}{c}{q}_0\\ \tilde{q}\end{array}\right]$

载体姿态的单位四元数为 $q={\left(\begin{array}{cc}{q}_0& \tilde{q}\end{array}\right)}^T={\left(\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right)}^T,$ 载体坐标系中的角速度矢量为 ${\mathrm{\ω}}^b=\left(\begin{array}{ccc}{\mathrm{\ω}}_x^b& {\mathrm{\ω}}_y^b& {\mathrm{\ω}}_z^b\end{array}\right),$

$\stackrel{\·}{q}=q\⊗\left[\begin{array}{c}0\\ {\mathrm{\ω}}^b\end{array}\right]=\frac{1}{2}{\mathrm{\Ω}}^{b}q$

其中， ${\mathrm{\Ω}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x^b& -{\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_z^b\\ {\mathrm{\ω}}_x^b& 0& {\mathrm{\ω}}_z^b& -{\mathrm{\ω}}_y^b\\ {\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_z^b& 0& {\mathrm{\ω}}_x^b\\ {\mathrm{\ω}}_z^b& {\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_x^b& 0\end{array}\right],$ 表示四元数乘运算；

将陀螺测量坐标系与载体质心本体坐标系重合，陀螺角速度输出采用经典模型：

$\left\{\begin{array}{c}\widetilde{\mathrm{\ω}}\left(t\right)=\mathrm{\ω}\left(t\right)+\mathrm{\β}\left(t\right)+{\mathrm{\η}}_v\left(t\right)\\ \stackrel{\·}{\mathrm{\β}}\left(t\right)={\mathrm{\η}}_u\left(t\right)\end{array}\right.$

为陀螺输出；β(t)为陀螺漂移；η_v(t)和η_u(t)分别为随机游走和漂移斜坡噪声；选取单位四元数q(t)和陀螺漂移β(t)作为***的状态向量，即X＝[q(t)^T β(t)^T]^T代表***的状态向量；

姿态估计***的非线性四元数量测方程；选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量，建立***的观测方程为：

$y=\left[\begin{array}{c}{g}_{\mathrm{xx}}^b\\ g_{\mathrm{yy}}^b\\ g_{\mathrm{xy}}^b\\ g_{\mathrm{yz}}^b\\ g_{\mathrm{xz}}^b\end{array}\right]=\left[\begin{array}{c}{g}_{\mathrm{xx}}^n{T}_{11}^2+g_{\mathrm{yy}}^n{T}_{21}^2+2g_{\mathrm{xy}}^n{T}_{11}{T}_{21}+2g_{\mathrm{yz}}^n{T}_{21}{T}_{31}+2g_{\mathrm{zx}}^n{T}_{11}{T}_{31}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}^2\\ g_{\mathrm{xx}}^n{T}_{12}^2+g_{\mathrm{yy}}^n{T}_{22}^2+2g_{\mathrm{xy}}^n{T}_{12}{T}_{22}+2g_{\mathrm{yz}}^n{T}_{22}{T}_{32}+2g_{\mathrm{zx}}^n{T}_{12}{T}_{32}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}^2\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{12}+g_{\mathrm{yy}}^n{T}_{21}{T}_{22}+g_{\mathrm{xy}}^n(T_{12}{T}_{21}+T_{11}{T}_{22})+g_{\mathrm{yz}}^n(T_{22}{T}_{31}+T_{21}{T}_{32})\\ +g_{\mathrm{zx}}^n(T_{12}{T}_{31}+T_{11}{T}_{32})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{32}\\ g_{\mathrm{xx}}^n{T}_{12}{T}_{13}+g_{\mathrm{yy}}^n{T}_{22}{T}_{23}+g_{\mathrm{xy}}^n(T_{13}{T}_{22}+T_{12}{T}_{23})+g_{\mathrm{yz}}^n(T_{23}{T}_{32}+T_{22}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{32}+T_{12}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}{T}_{33}\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{13}+g_{\mathrm{yy}}^n{T}_{21}{T}_{23}+g_{\mathrm{xy}}^n(T_{11}{T}_{23}+T_{13}{T}_{21})+g_{\mathrm{yz}}^n(T_{23}{T}_{31}+T_{21}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{31}+T_{11}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{33}\end{array}\right]+v=h\left(X\right)+v$

Gⁿ和G^b分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示，和是G^b的5个独立分量，和是Gⁿ的5个独立分量，T_ij＝T_ij(q₀,q₁,q₂,q₃),i＝1,2,3；j＝1,2,3为矩阵的元素，且有

$C_n^b=\begin{array}{c}\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψc\; os\θ}& \mathrm{s\; in\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γs\; in\ψsi\; n\θ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]\\ =\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\end{array}$

h(X)是与状态有关的非线性函数，观测噪声向量v为协方差为R的零均值白噪声；

(5.1)k-1时刻利用四元数对数反对数切换的时间更新；

利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差，采用对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差，算法采用对数变换的Cubature卡尔曼滤波；

(5.1.1)初始化：q＝q₀；

(5.1.2)主循环：对于i＝1,2,…2n，计算x_i＝log_q(q_i)，令其中w_i为CKF中的加权系数；

(5.1.3)如果足够小或超过最大迭代次数，终止循环，输出q，否则，继续循环；

(5.1.4)利用最后一次循环的x_i计算协方差矩阵

(5.2)k-1时刻利用CKF的时间更新；

利用标准CKF算法估计X_i,k|k-1中除四元数以外参量的均值，即陀螺漂移部分的均值；

(5.3)将步骤(5.1)利用LTCKF算法得到的四元数部分均值和步骤(5.2)利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值P_k|k-1为：

$P_{k|k-1}=\underset{k=1}{\overset{2n}{\mathrm{\Σ}}}{w}_k\left\{\right[X_{i,k|k-1}{\hat{x}}_{k|k-1}\left]{[X_{i,k|k-1}-{\hat{x}}_{k|k-1}]}^T\right\}+Q_{k-1}$

其中w_k为***噪声，Q_k-1为***噪声协方差；

(6)利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差P_k|k；

根据步骤5.3得到的误差协方差P_k|k-1按照CKF标准算法确定Cubature点集X_i,k|k-1；

将得到的点集X_i,k|k-1通过与状态有关的非线性函数h(X_i,k|k-1)传播Cubature点得到点集Y_i,k|k-1：

Y_i,k|k-1＝h(X_i,k|k-1)

计算k时刻的量测预测值：

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}$

计算自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1：

$P_{\mathrm{yy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}{T}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T+R_k$

$P_{\mathrm{xy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{y}}_{k|k-1}^T$

R_k是量测噪声协方差，再根据自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1计算卡尔曼增益K_k：

$K_k=P_{\mathrm{xy},k|k-1}{P}_{\mathrm{yy},k|k-1}^{-1}$

利用k时刻通过步骤(2)的得到的新的量测值y_k对该时刻预测值进行校正，求出状态估计和状态协方差矩阵P_k|k：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{yy},k|k-1}{K}_k^T$

k＝k+1，转至步骤(5)；

(7)对姿态及陀螺漂移进行校正：

利用状态估计值，确定四元数估计值以及陀螺漂移估计值，获得修正后k时刻姿态及陀螺漂移；

四元数估计值用和表示，对k时刻姿态角进行修正：

其中式中，为载体偏航角估计值，θ为俯仰角估计值，为横滚角估计值， ${\hat{T}}_{\mathrm{ij}}={\hat{T}}_{\mathrm{ij}}({\hat{q}}_0,{\hat{q}}_1,{\hat{q}}_2,{\hat{q}}_3),i=\mathrm{1,2,3};j=\mathrm{1,2,3};$

(8)姿态估计***的运行时间为N,若k＝N输出姿态及陀螺漂移结果，若k<N,重复步骤(5)-(7)，直到姿态估计***运行结束。

本发明的有益效果在于：本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍，而且通过三轴磁通门磁强计测量获取量测值y_k的方法具有价格低廉的潜在优势。

附图说明

图1是算法流程图；

图2是地磁梯度张量测量配置图；

图3是偏航角误差滤波算法对应的欧拉角误差曲线；

图4是俯仰角误差滤波算法对应的欧拉角误差曲线；

图5是横滚角误差滤波算法对应的欧拉角误差曲线。

具体实施方式

下面结合附图对本发明做进一步描述：

图1给出了本发明的具体流程图。下面结合附图对本发明的具体实施方式进行详细描述：

设过程噪声和观测噪声都是加性的，状态空间形式的离散非线性***为

x_k＝f(x_k-1)+w_k-1    (1)

y_k＝h(x_k)+v_k    (2)

式中，x_k∈Rⁿ和y_k∈R^m分别为状态向量和量测向量；f(·)和h(·)为***非线性状态方程和量测方程；过程噪声w_k和量测噪声v_k为互不相关的高斯白噪声，且均值和协方差矩阵分别为：

$\left\{\begin{array}{c}E\left[w_k\right]=0,E\left[w_k{w}_j^T\right]=Q{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,E\left[v_k{v}_j^T\right]=R{\mathrm{\&delta;}}_{\mathrm{kj}}\end{array}\right.---\left(3\right)$

其中δ_kj为Kronecker-delta函数，Q是非负定对称矩阵，R是正定对称矩阵。

载体姿态估计的状态方程和量测方程具有强非线性的特性，线性化可能引起大的误差甚至造成滤波器的不稳定。提出了一种新的基于地磁梯度张量的四元数Cubature卡尔曼滤波算法，其具体流程如下：

步骤1、设定初始参数值

由惯性测量单元输出数据确定初始时刻***状态值和状态协方差P₀。

步骤2、采集载体运动过程中陀螺及磁强计的输出数据作为量测量

步骤3、建立姿态估计的非线性四元数模型

步骤3.1建立姿态估计***的非线性四元数状态方程单位四元数

$q=\left[\begin{array}{c}{q}_0\\ \tilde{q}\end{array}\right]---\left(4\right)$

设表示载体姿态的单位四元数为 $q={\left(\begin{array}{cc}{q}_0& \tilde{q}\end{array}\right)}^T={\left(\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right)}^T,$ 载体坐标系中的角速度矢量为 ${\mathrm{\&omega;}}^b=\left(\begin{array}{ccc}{\mathrm{\&omega;}}_x^b& {\mathrm{\&omega;}}_y^b& {\mathrm{\&omega;}}_z^b\end{array}\right),$ 则有

$\stackrel{\&CenterDot;}{q}=q\&CircleTimes;\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}^b\end{array}\right]=\frac{1}{2}{\mathrm{\&Omega;}}^{b}q---\left(5\right)$

其中， ${\mathrm{\&Omega;}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x^b& -{\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_z^b\\ {\mathrm{\&omega;}}_x^b& 0& {\mathrm{\&omega;}}_z^b& -{\mathrm{\&omega;}}_y^b\\ {\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_z^b& 0& {\mathrm{\&omega;}}_x^b\\ {\mathrm{\&omega;}}_z^b& {\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_x^b& 0\end{array}\right],$ 表示四元数乘运算。

为简单起见，假设陀螺测量坐标系与载体质心本体坐标系重合，陀螺角速度输出采用经典模型：

$\left\{\begin{array}{c}\widetilde{\mathrm{\&omega;}}\left(t\right)=\mathrm{\&omega;}\left(t\right)+\mathrm{\&beta;}\left(t\right)+{\mathrm{\&eta;}}_v\left(t\right)\\ \stackrel{\&CenterDot;}{\mathrm{\&beta;}}\left(t\right)={\mathrm{\&eta;}}_u\left(t\right)\end{array}\right.---\left(6\right)$

式中，为陀螺输出；β(t)为陀螺漂移；η_v(t)和η_u(t)分别为随机游走和漂移斜坡噪声。

选取单位四元数q(t)和陀螺漂移β(t)作为***的状态向量，即X＝[q(t)^T β(t)^T]^T代表***的状态向量。

由方程(5)和(6)建立载体姿态估计***的非线性四元数状态方程

步骤3.2建立姿态估计***的非线性四元数量测方程

选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量，由式建立***的观测方程为

$y=\left[\begin{array}{c}{g}_{\mathrm{xx}}^b\\ g_{\mathrm{yy}}^b\\ g_{\mathrm{xy}}^b\\ g_{\mathrm{yz}}^b\\ g_{\mathrm{xz}}^b\end{array}\right]=\left[\begin{array}{c}{g}_{\mathrm{xx}}^n{T}_{11}^2+g_{\mathrm{yy}}^n{T}_{21}^2+2g_{\mathrm{xy}}^n{T}_{11}{T}_{21}+2g_{\mathrm{yz}}^n{T}_{21}{T}_{31}+2g_{\mathrm{zx}}^n{T}_{11}{T}_{31}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}^2\\ g_{\mathrm{xx}}^n{T}_{12}^2+g_{\mathrm{yy}}^n{T}_{22}^2+2g_{\mathrm{xy}}^n{T}_{12}{T}_{22}+2g_{\mathrm{yz}}^n{T}_{22}{T}_{32}+2g_{\mathrm{zx}}^n{T}_{12}{T}_{32}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}^2\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{12}+g_{\mathrm{yy}}^n{T}_{21}{T}_{22}+g_{\mathrm{xy}}^n(T_{12}{T}_{21}+T_{11}{T}_{22})+g_{\mathrm{yz}}^n(T_{22}{T}_{31}+T_{21}{T}_{32})\\ +g_{\mathrm{zx}}^n(T_{12}{T}_{31}+T_{11}{T}_{32})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{32}\\ g_{\mathrm{xx}}^n{T}_{12}{T}_{13}+g_{\mathrm{yy}}^n{T}_{22}{T}_{23}+g_{\mathrm{xy}}^n(T_{13}{T}_{22}+T_{12}{T}_{23})+g_{\mathrm{yz}}^n(T_{23}{T}_{32}+T_{22}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{32}+T_{12}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}{T}_{33}\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{13}+g_{\mathrm{yy}}^n{T}_{21}{T}_{23}+g_{\mathrm{xy}}^n(T_{11}{T}_{23}+T_{13}{T}_{21})+g_{\mathrm{yz}}^n(T_{23}{T}_{31}+T_{21}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{31}+T_{11}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{33}\end{array}\right]+v=h\left(X\right)+v---\left(7\right)$

Gⁿ和G^b分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示，和是G^b的5个独立分量，和是Gⁿ的5个独立分量。T_ij＝T_ij(q₀,q₁,q₂,q₃),i＝1,2,3；j＝1,2,3为矩阵的元素，且有

$C_n^b=\begin{array}{c}\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;cos\&theta;}& \mathrm{s\; in\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;sin\&psi;sin\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;c\; os}\mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]\\ =\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\end{array}---\left(8\right)$

h(X)是与状态有关的非线性函数，观测噪声向量v为协方差为R的零均值白噪声。

步骤4测量地理系下地磁梯度张量Gⁿ

根据惯性/地磁组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gⁿ的5个独立分量和

步骤5测量载体系下地磁梯度张量G^b

根据步骤2中磁强计(配置方式如图2所示)的输出数据测量载体系下磁场分量h₁～h₁₀，按(9)式计算载体系下地磁梯度张量G^b的5个分量；

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x}\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.---\left(9\right)$

L_x和L_y分别为x_b和y_b方向上梯度测量基线长。

步骤6k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新

步骤6.1k-1时刻利用四元数对数反对数切换的时间更新

利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差。

本发明采用如下对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差，算法定义为对数变换的Cubature卡尔曼滤波(LTCKF)，具体描述如下：

为叙述方便，对于s,q∈H₁，u∈R³，定义如下变换

logq≡αn＝u    (10)

$\exp \left(u\right)=\exp \left(\mathrm{\&alpha;n}\right)\&equiv;\left[\begin{array}{c}\cos (\mathrm{\&alpha;}/2)\\ n\sin (\mathrm{\&alpha;}/2)\end{array}\right]=q---\left(11\right)$

记

${\log }_q\left(s\right)=\log (s\&CircleTimes;q^{-1})---\left(12\right)$

${\exp }_q\left(u\right)=\exp \left(u\right)\&CircleTimes;q---\left(13\right)$

对k-1时刻状态估计中的四元数部分利用(10)定义的对数函数变换为R³中的向量后与其他状态组成新的根据乔里斯基因子S_k-1|k-1和按公式(14)计算确定Cubature点集

$X_{i,,k-1|k-1}^{*}=S_{k-1|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k-1|k-1}^{*}---\left(14\right)$

记(n-1)维单位列向量e＝[1,0,0…,0]^T,符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点。

将点集中对应于四元数的部分利用公式(11)变换为四元数后构成χ_k-1，将χ_k-1与中与陀螺漂移β(t)对应的部分构成新的X_i,k-1|k-1，然后通过***的状态方程f(X_i,k-1|k-1)根据公式(15)传播Cubature点得到点集X_i,k|k-1。

X_i,k|k-1＝f(X_i,k-1|k-1)    (15)

对于X_i,k|k-1中的四元数部分，利用公式(10)～(13)计算其加权均值，即均值q和协方差矩阵P_q：

具体算法如下：

①初始化：q＝q₀；

②主循环：对于i＝1,2,…2n，计算x_i＝log_q(q_i)，令其中w_i为CKF中的加权系数；

③如果足够小或超过最大迭代次数，终止循环，输出q，否则，继续循环。

④利用最后一次循环的x_i计算协方差矩阵

步骤6.2k-1时刻利用CKF的时间更新

利用标准CKF算法估计X_i,k|k-1中除四元数以外参量的均值，即陀螺漂移部分的均值。

步骤6.3将步骤6.1利用LTCKF算法得到的四元数部分均值和步骤6.2利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值P_k|k-1按下式计算：

$P_{k|k-1}=\underset{k=1}{\overset{2n}{\mathrm{\&Sigma;}}}{w}_k\left\{\right[X_{i,k|k-1}{\hat{x}}_{k|k-1}\left]{[X_{i,k|k-1}-{\hat{x}}_{k|k-1}]}^T\right\}+Q_{k-1}---\left(16\right)$

其中w_k为***噪声，Q_k-1为***噪声协方差。

完成时间更新。

步骤7利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差P_k|k

根据步骤6.3得到的误差协方差P_k|k-1按照CKF标准算法确定Cubature点集X_i,k|k-1。

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(17\right)$

$X_{i,k-1|k-1}=S_{k|k-1}{\mathrm{\&xi;}}_i+{\hat{x}}_{k|k-1}---\left(18\right)$

将得到的点集X_i,k|k-1通过步骤3.2获得的与状态有关的非线性函数h(X_i,k|k-1)传播Cubature点得到点集Y_i,k|k-1。

Y_i,k|k-1＝h(X_i,k|k-1)    (19)

将Y_i,k|k-1代入公式(20)计算k时刻的量测预测值

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{Y}_{i,k|k-1}---\left(20\right)$

将以上得到的结果分别代入公式(21)和(22)计算自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1

$P_{\mathrm{yy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{Y}_{i,k|k-1}{T}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T+R_k---\left(21\right)$

$P_{\mathrm{xy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{y}}_{k|k-1}^T---\left(22\right)$

R_k是量测噪声协方差。再根据自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1计算卡尔曼增益K_k

$K_k=P_{\mathrm{xy},k|k-1}{P}_{\mathrm{yy},k|k-1}^{-1}---\left(23\right)$

利用k时刻通过步骤2的得到的新的量测值y_k对该时刻预测值进行校正，即根据公式(24)和(25)求出状态估计和状态协方差矩阵P_k|k。

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})---\left(24\right)$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{yy},k|k-1}{K}_k^T---\left(25\right)$

k＝k+1，转至步骤6。

步骤8对姿态及陀螺漂移进行校正

利用状态估计值，确定四元数估计值及陀螺漂移估计值，获得修正后k时刻姿态及陀螺漂移。

四元数估计值用和表示，根据下式对k时刻姿态角进行修正。

其中式中，为载体偏航角估计值，θ为俯仰角估计值，为横滚角估计值。 ${\hat{T}}_{\mathrm{ij}}={\hat{T}}_{\mathrm{ij}}({\hat{q}}_0,{\hat{q}}_1,{\hat{q}}_2,{\hat{q}}_3),i=\mathrm{1,2,3};j=\mathrm{1,2,3}.$

步骤9姿态估计***的运行时间为N,若k＝N输出姿态及陀螺漂移结果，若k<N，重复步骤6-8，直到姿态估计***运行结束。

分别采用扩展卡尔曼滤波(EKF)、CKF和LTCKF算法同步处理完全相同的仿真数据，初始状态值及其协方差也完全相同，滤波估计结果如图3、4、5所示，图3、4和5分别表示的是每一步估计值对应的偏航角、俯仰角和横滚角误差。从图3、4、5可以看出，对数变换的Cubature卡尔曼滤波算法对姿态角的估计精度比Cubature卡尔曼滤波算法高出数倍，而Cubature卡尔曼滤波算法对姿态角的估计精度比EKF算法也高出数倍。这是因为姿态估计***的状态方程及观测方程均具有强的非线性，EKF算法存在较大的线性化误差，而Cubature卡尔曼滤波算法是一种非线性滤波，避免了线性化带来的误差及滤波器的不稳定。另一方面，Cubature卡尔曼滤波算法存在四元数加权均值规范化问题及四元数协方差计算奇异性问题，而对数变换的Cubature卡尔曼滤波算法(LTCKF)采用了四元数对数与指数函数进行四元数的均值和方差计算，因此在理论上严格保证了四元数估计值具有单位长度，避免了四元数非归一化带来的计算误差。

本发明的有益效果通过下面仿真实验进行说明

采用欧拉角设定载体的角运动分别为：

$\left\{\begin{array}{c}\mathrm{\&psi;}=\frac{\mathrm{\&pi;}}{3}\sin \left(\frac{\mathrm{\&pi;}}{30}t\right)\\ \mathrm{\&theta;}=\frac{\mathrm{\&pi;}}{8}\sin (\frac{\mathrm{\&pi;}}{10}t+\frac{\mathrm{\&pi;}}{4})\\ \mathrm{\&gamma;}=\frac{\mathrm{\&pi;}}{4}\sin (\frac{\mathrm{\&pi;}}{15}t+\frac{\mathrm{\&pi;}}{2})\end{array}\right.---\left(15\right)$

式中，ψ为偏航角，θ为俯仰角，γ为横滚角。

仍用一个磁偶极子模型模拟地磁异常，磁偶极子磁矩分量分别为m_x＝10⁹A·m²，m_y＝2×10⁸A·m²和m_z＝10⁸A·m²，地磁梯度测量装置相对于磁偶极子的位置分量分别为x＝100m，y＝50m和z＝20m，磁场梯度测量基线长Δx＝Δy＝1m，，磁场梯度张量测量精度设为5nT/m。仿真时间为180s，采样频率为100Hz。随机游走方差σ_v＝0.03°/s，漂移斜坡噪声方差σ_u＝0.003°/s。分别采用扩展卡尔曼滤波(EKF)、CKF和LTCKF算法同步处理完全相同的仿真数据，初始状态值及其协方差也完全相同，为了便于进一步比较，将估计误差的标准差进行了统计运算，如表1所示。

表1 欧拉角估计误差标准差

从表1可以看出，对数变换的Cubature卡尔曼滤波算法对姿态角的估计精度比Cubature卡尔曼滤波算法高出数倍，而Cubature卡尔曼滤波算法对姿态角的估计精度比EKF算法也高出数倍。这是因为姿态估计***的状态方程及观测方程均具有强的非线性，EKF算法存在较大的线性化误差，而Cubature卡尔曼滤波算法是一种非线性滤波，避免了线性化带来的误差及滤波器的不稳定。另一方面，Cubature卡尔曼滤波算法存在四元数加权均值规范化问题及四元数协方差计算奇异性问题，而对数变换的Cubature卡尔曼滤波算法(LTCKF)采用了四元数对数与指数函数进行四元数的均值和方差计算，因此在理论上严格保证了四元数估计值具有单位长度，避免了四元数非归一化带来的计算误差。

Claims (1)

1.一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法，其特征在于，包括如下步骤：

(1)设定初始参数值：

由惯性测量单元输出数据确定初始时刻***状态值和状态协方差P₀；

(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量；

(3)测量地理系下地磁梯度张量Gⁿ；

根据惯性/地磁组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gⁿ的5个独立分量和

(4)测量载体系下地磁梯度张量G^b；

根据步骤(2)中磁强计的输出数据测量载体系下磁场分量h₁～h₁₀，计算载体系下地磁梯度张量G^b的5个分量；

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x}\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.;$

L_x和L_y分别为x_b和y_b方向上梯度测量基线长；

(5)k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新：

过程噪声和观测噪声都是加性的，状态空间形式的离散非线性***为：

x_k＝f(x_k-1)+w_k-1

y_k＝h(x_k)+v_k

x_k∈Rⁿ和y_k∈R^m分别为状态向量和量测向量；f(·)和h(·)分别为***非线性四元数状态方程量测方程；

姿态估计***的非线性四元数状态方程：

单位四元数：

$q=\left[\begin{array}{c}{q}_0\\ \tilde{q}\end{array}\right]$

载体姿态的单位四元数为 $q={\left(\begin{array}{cc}{q}_0& \tilde{q}\end{array}\right)}^T={\left(\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right)}^T,$ 载体坐标系中的角速度矢量为 ${\mathrm{\&omega;}}^b=\left(\begin{array}{ccc}{\mathrm{\&omega;}}_x^b& {\mathrm{\&omega;}}_y^b& {\mathrm{\&omega;}}_z^b\end{array}\right),$

$\stackrel{\&CenterDot;}{q}=q\&CircleTimes;\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}^b\end{array}\right]=\frac{1}{2}{\mathrm{\&Omega;}}^{b}q$

其中， ${\mathrm{\&Omega;}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x^b& -{\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_z^b\\ {\mathrm{\&omega;}}_x^b& 0& {\mathrm{\&omega;}}_z^b& -{\mathrm{\&omega;}}_y^b\\ {\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_z^b& 0& {\mathrm{\&omega;}}_x^b\\ {\mathrm{\&omega;}}_z^b& {\mathrm{\&omega;}}_y^b& -{\mathrm{\&omega;}}_x^b& 0\end{array}\right],$ 表示四元数乘运算；

将陀螺测量坐标系与载体质心本体坐标系重合，陀螺角速度输出采用经典模型：

$\left\{\begin{array}{c}\widetilde{\mathrm{\&omega;}}\left(t\right)=\mathrm{\&omega;}\left(t\right)+\mathrm{\&beta;}\left(t\right)+{\mathrm{\&eta;}}_v\left(t\right)\\ \stackrel{\&CenterDot;}{\mathrm{\&beta;}}\left(t\right)={\mathrm{\&eta;}}_u\left(t\right)\end{array}\right.$

为陀螺输出；β(t)为陀螺漂移；η_v(t)和η_u(t)分别为随机游走和漂移斜坡噪声；选取单位四元数q(t)和陀螺漂移β(t)作为***的状态向量，即X＝[q(t)^T β(t)^T]^T代表***的状态向量；

姿态估计***的非线性四元数量测方程；选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量，建立***的观测方程为

$y=\left[\begin{array}{c}{g}_{\mathrm{xx}}^b\\ g_{\mathrm{yy}}^b\\ g_{\mathrm{xy}}^b\\ g_{\mathrm{yz}}^b\\ g_{\mathrm{xz}}^b\end{array}\right]=\left[\begin{array}{c}{g}_{\mathrm{xx}}^n{T}_{11}^2+g_{\mathrm{yy}}^n{T}_{21}^2+2g_{\mathrm{xy}}^n{T}_{11}{T}_{21}+2g_{\mathrm{yz}}^n{T}_{21}{T}_{31}+2g_{\mathrm{zx}}^n{T}_{11}{T}_{31}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}^2\\ g_{\mathrm{xx}}^n{T}_{12}^2+g_{\mathrm{yy}}^n{T}_{22}^2+2g_{\mathrm{xy}}^n{T}_{12}{T}_{22}+2g_{\mathrm{yz}}^n{T}_{22}{T}_{32}+2g_{\mathrm{zx}}^n{T}_{12}{T}_{32}-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}^2\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{12}+g_{\mathrm{yy}}^n{T}_{21}{T}_{22}+g_{\mathrm{xy}}^n(T_{12}{T}_{21}+T_{11}{T}_{22})+g_{\mathrm{yz}}^n(T_{22}{T}_{31}+T_{21}{T}_{32})\\ +g_{\mathrm{zx}}^n(T_{12}{T}_{31}+T_{11}{T}_{32})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{32}\\ g_{\mathrm{xx}}^n{T}_{12}{T}_{13}+g_{\mathrm{yy}}^n{T}_{22}{T}_{23}+g_{\mathrm{xy}}^n(T_{13}{T}_{22}+T_{12}{T}_{23})+g_{\mathrm{yz}}^n(T_{23}{T}_{32}+T_{22}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{32}+T_{12}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{32}{T}_{33}\\ g_{\mathrm{xx}}^n{T}_{11}{T}_{13}+g_{\mathrm{yy}}^n{T}_{21}{T}_{23}+g_{\mathrm{xy}}^n(T_{11}{T}_{23}+T_{13}{T}_{21})+g_{\mathrm{yz}}^n(T_{23}{T}_{31}+T_{21}{T}_{33})\\ +g_{\mathrm{xz}}^n(T_{13}{T}_{31}+T_{11}{T}_{33})-(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)T_{31}{T}_{33}\end{array}\right]+v=h\left(X\right)+v$

Gⁿ和G^b分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示，和是G^b的5个独立分量，和是Gⁿ的5个独立分量，T_ij＝T_ij(q₀,q₁,q₂,q₃),i＝1,2,3；j＝1,2,3为矩阵的元素，且有

$\begin{array}{c}{C}_n^b=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]\\ =\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\end{array}$

h(X)是与状态有关的非线性函数，观测噪声向量v为协方差为R的零均值白噪声；

(5.1)k-1时刻利用四元数对数反对数切换的时间更新；

利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方差，采用对数指数变换法计算k-1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差，算法采用对数变换的Cubature卡尔曼滤波；

(5.1.1)初始化：q＝q₀；

(5.1.2)主循环：对于i＝1,2,…2n，计算x_i＝log_q(q_i)，令其中w_i为CKF中的加权系数；

(5.1.3)如果足够小或超过最大迭代次数，终止循环，输出q，否则，继续循环；

(5.1.4)利用最后一次循环的x_i计算协方差矩阵

(5.2)k-1时刻利用CKF的时间更新；

利用标准CKF算法估计X_i,k|k-1中除四元数以外参量的均值，即陀螺漂移部分的均值；

(5.3)将步骤(5.1)利用LTCKF算法得到的四元数部分均值和步骤(5.2)利用CKF算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值P_k|k-1为：

其中w_k为***噪声，Q_k-1为***噪声协方差；

(6)利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态和协方差P_k|k；

根据步骤5.3得到的误差协方差P_k|k-1按照CKF标准算法确定Cubature点集X_i,k|k-1；

将得到的点集X_i,k|k-1通过与状态有关的非线性函数h(X_i,k|k-1)传播Cubature点得到点集Y_i,k|k-1：

Y_i,k|k-1＝h(X_i,k|k-1)

计算k时刻的量测预测值：

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{Y}_{i,k|k-1}$

计算自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1：

$P_{\mathrm{yy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{Y}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T+R_k$

$P_{\mathrm{xy},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{y}}_{k|k-1}^T$

R_k是量测噪声协方差，再根据自相关协方差矩阵P_yy,k|k-1和互相关协方差矩阵P_xy,k|k-1计算卡尔曼增益K_k：

$K_k=P_{\mathrm{xy},k|k-1}{P}_{\mathrm{yy},k|k-1}^{-1}$

利用k时刻通过步骤(2)的得到的新的量测值y_k对该时刻预测值进行校正，求出状态估计和状态协方差矩阵P_k|k：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{yy},k|k-1}{K}_k^T$

k＝k+1，转至步骤(5)；

(7)对姿态及陀螺漂移进行校正：

利用状态估计值，确定四元数估计值以及陀螺漂移估计值，获得修正后k时刻姿态及陀螺漂移；

四元数估计值用和表示，对k时刻姿态角进行修正：

其中式中，为载体偏航角估计值，θ为俯仰角估计值，为横滚角估计值， ${\hat{T}}_{\mathrm{ij}}={\hat{T}}_{\mathrm{ij}}({\hat{q}}_0,{\hat{q}}_1,{\hat{q}}_2,{\hat{q}}_3),i=\mathrm{1,2,3};j=\mathrm{1,2,3};$

(8)姿态估计***的运行时间为N,若k＝N输出姿态及陀螺漂移结果，若k<N,重复步骤(5)-(7)，直到姿态估计***运行结束。