CN103822633A - 一种基于二阶量测更新的低成本姿态估计方法 - Google Patents

Abstract

本发明提供的是一种基于二阶量测更新的低成本姿态估计方法。本发明通过对三轴微机械陀螺、三轴微机械加速度计和三轴磁强计的输出数据，进行滤波处理得到载体的姿态信息。针对在室内或磁干扰较强场所，磁强计输出会使横摇和纵摇误差变大，传统方法难以解决的问题。本方法在滤波的量测更新阶段，创新性地采用二阶量测更新，即先进行加速度计量测更新再进行磁强计量测更新。以此修正标准量测更新算法从而使磁强计更新只影响方位角。利用本方法可以使用低成本的微惯性测量单元和磁强计进行姿态估计，并且估计精度高、实时性好、适应强磁干扰环境。本方法适用于车辆、无人机和船舰等载体的姿态估计。

Description

一种基于二阶量测更新的低成本姿态估计方法

技术领域

本发明涉及的是一种适用于利用低成本的传感器，对车辆、船舶、无人机载体等进行姿态估计地方法。具体地说是一种基于二阶量测更新的低成本姿态估计方法。

背景技术

在很多工程应用领域中都需要进行姿态估计，例如，机动追踪、无人机和生物应用等方面。如果给定载体初始状态，则可以通过积分陀螺仪计算出估计姿态值。它的估计误差主要由陀螺漂移和算法误差组成。此外，也可以通过加速度计和磁力计的输出计算出姿态值。在这种情况下，由于外部加速度和磁干扰，估计误差可能会变大。因此每种姿态估计算法的基本问题就是如何将陀螺仪、加速度计和磁力计结合起来使用。因此，用微惯性传感器和磁传感器来估计姿态是近年来的流行方法之一。

目前，大多数的姿态估计中观测量都会选取地球磁场，利用磁强计或罗经等设备输出磁力信息。其中磁强计的输出不仅能提供航向信息同时也包括纵摇和横摇的一些信息。但它有一个严重的缺点就是纵摇角和横摇角受磁干扰影响，由于室内存在较强的磁干扰，因此在室内应用该种算法时纵摇误差和横摇误差都有较大的变化。所以较为明智的选择就是利用磁强计输出数据只计算航向角，因此这不适用于传统的四元数间接卡尔曼滤波。

发明内容

本发明的目的在于提供一种通过测量微惯性测量单元和磁强计输出的载体三个轴的角速率、线加速度和磁力数据，利用明改进的四元数卡尔曼滤波处理，得到载体的精确姿态信息的基于二阶量测更新的低成本姿态估计方法。

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

(1)在k=0时刻，即初始时刻由初始对准得到初始姿态φ₀，所述初始姿态包括横摇、纵摇和航向；

(2)在k≥1时刻，利用微机械陀螺输出的载体三轴的角速率数据，计算出姿态角的变化值Δφ，由式

计算出滤波开始前的粗略姿态角；

(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据；

(4)设滤波的状态变量为x=[q_e b_g b_a]^T∈R^9×1；式中：q_e表示四元数误差

矢量部分，b_g表示陀螺漂移，b_a表示加速度计零偏；假设四元数误差 ${\tilde{q}}_e={\left[\begin{array}{cc}1& q_e\end{array}\right]}^T,$ 根据四元数相关性质建立状态方程：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_e\\ {\stackrel{\·}{b}}_g\\ {\stackrel{\·}{b}}_a\end{array}\right]=\left[\begin{array}{ccc}-[y_g\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{q}_e\\ b_g\\ b_a\end{array}\right]+\left[\begin{array}{c}-{0.5v}_g\\ v_{b_g}\\ v_{b_a}\end{array}\right]$

式中： $[y_g\×]=\left[\begin{array}{ccc}0& -y_{\mathrm{gz}}& y_{\mathrm{gy}}\\ y_{\mathrm{gz}}& 0& -y_{\mathrm{gx}}\\ -y_{\mathrm{gy}}& y_{\mathrm{gx}}& 0\end{array}\right],$ 下标g代表陀螺、a代表加速度计、m代表磁力计，y_g表示陀螺实际输出。传感器噪声v_a、v_g以及v_m为零均值高斯白噪声；

取观测量为 $z=\left[\begin{array}{c}{y}_a-C\left(\hat{q}\right)\tilde{g}\\ y_m-C\left(\hat{q}\right)\tilde{m}\end{array}\right],$ 由于q_e为小量，再忽略二阶小项，建立量测方程：

$y_a-C\left(\hat{q}\right)\tilde{g}=2[C\left(\hat{q}\right)\tilde{g}\×]q_e+v_a+b_a$

$y_m-C\left(\hat{q}\right)\tilde{m}=2[C\left(\hat{q}\right)\tilde{m}\×]q_e+v_m$

式中：表示由估计四元数组成的姿态矩阵，y_a和y_m分别为加速度计和磁强计输出，定义 $\tilde{g}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$ 和 $\tilde{m}={\left[\begin{array}{ccc}\cos \mathrm{\α}& 0& -\sin \mathrm{\α}\end{array}\right]}^T,$ g为重力加速度，α为磁倾角；

(5)利用步骤(4)中建立的状态方程进行滤波的时间更新，由下式

${\hat{x}}_k^{-}={\mathrm{\Φ}}_k{\hat{x}}_{k-1}$

$P_k^{-}={\mathrm{\Φ}}_k{P}_{k-1}{{\mathrm{\Φ}}_k}^T+Q_k$

分别计算出一步预测量

和状态误差协方差阵

式中：Φ_k为***状态转移矩阵，Q_k为***噪声协方差阵；

(6)在滤波的量测更新阶段采用二阶量测更新，先进行加速度计量测更新；取此阶段观测量

根据建立的量测方程有其更新过程如下：

先计算滤波增益： $K_{a,k}=P_k^{-}{H}_{a,k}^{\′}{(H_{a,k}{P}_k^{-}{H}_{a,k}^{\′}+R_a)}^{-1};$

然后计算状态估计量： ${\hat{x}}_{a,k}={\hat{x}}_k^{-}+K_{a,k}(z_{a,k}-H_{a,k}{\hat{x}}_k^{-});$

再计算状态误差协方差阵： $P_{a,k}=(I-K_{a,k}{H}_{a,k})P_k^{-}{(I-K_{a,k}{H}_{a,k})}^{\′}+K_{a,k}{R}_a{K}_{a,k}^{\′};$

式中：下标a表示加速度计量测更新阶段，R_a为加速度计噪声协方差阵

此时

的9个状态都会更新，利用

更新用

代表已更新状态，取即前三个变量值，利用式

进行校正，然后进行四元数规范化：

$\hat{q}\⇐\frac{\hat{q}}{\left|\right|\hat{q}\left|\right|}$

加速度计更新结束后，将

的q_e部分设置为零；

(7)利用步骤(6)中得到的

进行磁力计量测更新；取此阶段观测量为 $z_{m,k}\≡y_{m,k}-C\left({\hat{q}}_k\right)\tilde{m},$ 则得 $H_{m,k}\≡\left[\begin{array}{ccc}2[C\left(\hat{q}\right)\hat{m}\×]& 0& 0\end{array}\right],$ 其更新过程如下：

取此时状态误差协方差阵为

$P_{m,k}^{-}=\left[\begin{array}{cc}{P}_{a,k}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

由此计算滤波增益： $K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^{\′}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]P_{m,k}^{-}{H}_{m,k}^{\′}{(H_{m,k}{P}_{m,k}^{-}{H}_{m,k}^{\′}+R_m)}^{-1};$

然后计算状态估计量： $x_k^{\′}={\hat{x}}_{a,k}+K_{m,k}(z_{m,k}-H_{m,k}{\hat{x}}_{a,k});$

再计算状态误差协方差阵：P_k=(I-K_m,kH_m,k)P_a,k(I-K_m,kH_m,k)′+K_m,kR_mK_′m,k；

式中：下标m表示磁力计量测更新阶段，P_a,k(1:3,1:3)表示由P_a,k前三行前三列所组成的矩阵，0_mn为m行n列的零矩阵并且

(8)根据滤波计算出的四元数误差，校正步骤(2)中的粗略姿态角，得到精确的姿态角。

此外，在所述步骤(7)磁力计量测更新中修正了标准量测更新算法，从而使磁强计量测更新只影响方位角，其原理如下：

磁力计量测更新前的转换矩阵为

更新后的转换矩阵为由矩阵定理可知，当满足下面条件，q_e只影响

的航向角：

$(C\left({\stackrel{\‾}{q}}_e\right)C\left(\hat{q}\right)-C\left(\hat{q}\right))\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=0$

根据四元数和姿态矩阵的关系，可得

$C\left({\tilde{q}}_e\right)=\left[\begin{array}{ccc}1+2q_{e,1}^2& {2q}_{e,3}& -2q_{e,2}\\ -2q_{e,3}& 1+2q_{e,2}^2& 2q_{e,1}\\ +2q_{e,2}& -2q_{e,1}& 1+q_{e,3}^2\end{array}\right]=I-2[q_e\×]$

由上述条件可推出q_e×r₃=0，即当q_e与r₃平行时，q_e只影响航向角。为了使磁力计更新满足q_e=βr₃,β∈R，需要对K_m,k(1:3,1:3)进行约束。由于在加速度计更新结束时将因此磁力计量测更新方程为

$q_3={\hat{x}}_k(1:3)=K_{m,k}(1:\mathrm{3,1}:3)z_{m,k}$

为使任意的z_m,k都满足q_e=βr₃,β∈R，K_m,k(1:3,1:3)需要具有以下结构K_m,k(1:3,1:3)=r₃l′，当l使Tr P_k(1:3,1:3)取最小值时，l=r₃。因此满足约束的最优滤波增益为 $K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^{\′}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]P_{m,k}^{-}{H}_{m,k}^{\′}{(H_{m,k}{P}_{m,k}^{-}{H}_{m,k}^{\′}+R_m)}^{-1}.$

本发明中修正了标准的四元数卡尔曼滤波，使得磁强计输出仅用于航向估计误差补偿。这样做的好处是，使得了这种姿态估计算法可以很好的应用在室内或磁干扰较强的场所。

此外，为了配合这种姿态估计算法，改变了传统的量测更新方式。在传统的滤波方法中不同的观测量同步进行量测更新。但为了适用于本发明的方法采用二阶量测更新，即先进行加速度计量测更新，将得到的状态变量中四元数误差量设为零，然后再进行磁强计量测更新。这样做的目的是使得磁强计的输出仅用于航向估计误差补偿。

本发明与现有的技术相比的优点在于：改变了传统的量测更新方式，解决了在室内或磁干扰较强的场所磁强计输出易受干扰，导致纵摇和横摇误差较大的问题。通过本发明的二阶量测更新方法，磁强计输出只用于航向估计误差补偿，因此避免了磁干扰的影响，提高了滤波在此情况下的精度，实现了对载体姿态信息的准确获取。

对本发明的有益效果说明如下：

采用本发明的基于二阶量测更新的低成本姿态估计方法，假设三个姿态角的初始误差角分别为：0.5°、0.5°、10°，为了模拟实际捷联惯导***三个姿态角的变化，假设惯导***在静基座上以基座原点为中心做如下摇摆：

ψ=10°+6°sin(2πt/4+π/4)

θ=8°sin(2πt/5+π/5)

η=10°sin(2πt/6+π/6)

为了证明本方法不受磁干扰影响，将磁强计的输出噪声设为较大值。仿真实验结果：图3为横摇失准角误差曲线，图4为纵摇失准角误差曲线，图5为航向失准角误差曲线。从图中可以看出采用本方法可以在强磁干扰的环境下，依然可以获得很高的姿态估计精度。

利用本发明方法可以使用低成本的微惯性测量单元和磁强计进行姿态估计，并且估计精度高、实时性好、适应强磁干扰环境。本方法适用于车辆、无人机和船舰等载体的姿态估计。

附图说明

图1为本发明流程图。

图2为本发明滤波算法流程图。

图3为具体实施方式的横摇失准角误差实验曲线。

图4为具体实施方式的纵摇失准角误差实验曲线。

图5为具体实施方式的航向失准角误差实验曲线。

具体实施方式

(1)结合图1，在初始时刻根据初始对准的结果得到初始姿态φ₀，包括横摇、纵摇和航向，然后计算出初始姿态矩阵C(q₀)；

(2)当k≥1时刻，利用微机械陀螺输出载体三轴角速率数据y_g，计算出姿态角的变化值Δφ，由式计算出滤波开始前的粗略姿态角

得到

(3)采集微机械加速度计和磁力计输出的载体三轴的线加速度和磁力数据；

(4)设滤波的状态变量为x=[q_e b_g b_a]^T∈R^9×1。式中：q_e表示四元数误差

矢量部分，b_g表示陀螺漂移，b_a表示加速度计零偏。本方法中假设四元数误差 ${\tilde{q}}_e={\left[\begin{array}{cc}1& q_e\end{array}\right]}^T,$ 根据四元数相关性质建立状态方程：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_e\\ {\stackrel{\·}{b}}_g\\ {\stackrel{\·}{b}}_a\end{array}\right]=\left[\begin{array}{ccc}-[y_g\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{q}_e\\ b_g\\ b_a\end{array}\right]+\left[\begin{array}{c}-{0.5v}_g\\ v_{b_g}\\ v_{b_a}\end{array}\right]$

式中： $[y_g\×]=\left[\begin{array}{ccc}0& -y_{\mathrm{gz}}& y_{\mathrm{gy}}\\ y_{\mathrm{gz}}& 0& -y_{\mathrm{gx}}\\ -y_{\mathrm{gy}}& y_{\mathrm{gx}}& 0\end{array}\right],$ 下标g代表陀螺、a代表加速度计、m代表磁力计，y_g表示陀螺实际输出。传感器噪声v_a、v_g以及v_m为零均值高斯白噪声。

取观测量为 $z=\left[\begin{array}{c}{y}_a-C\left(\hat{q}\right)\tilde{g}\\ y_m-C\left(\hat{q}\right)\tilde{m}\end{array}\right],$ 由于q_e为小量，再忽略二阶小项，建立量测方程：

$y_a-C\left(\hat{q}\right)\tilde{g}=2[C\left(\hat{q}\right)\tilde{g}\×]q_e+v_a+b_a$

$y_m-C\left(\hat{q}\right)\tilde{m}=2[C\left(\hat{q}\right)\tilde{m}\×]q_e+v_m$

式中：

表示由估计四元数组成的姿态矩阵，y_a和y_m分别为加速度计和磁强计输出，定义 $\tilde{g}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$ 和 $\tilde{m}={\left[\begin{array}{ccc}\cos \mathrm{\α}& 0& -\sin \mathrm{\α}\end{array}\right]}^T,$ g为重力加速度，α为磁倾角；

(5)利用步骤(4)中建立的状态方程进行滤波的时间更新，由下式

${\hat{x}}_k^{-}={\mathrm{\Φ}}_k{\hat{x}}_{k-1}$

$P_k^{-}={\mathrm{\Φ}}_k{P}_{k-1}{{\mathrm{\Φ}}_k}^T+Q_k$

分别计算出一步预测量

和状态误差协方差阵

式中：Φ_k为***状态转移矩阵，Q_k为***噪声协方差阵；

(6)结合图2，在滤波的量测更新阶段采用二阶量测更新，先进行加速度计量测更新。该步骤中，取此阶段观测量

根据建立的量测方程有

其更新过程如下：

先计算滤波增益： $K_{a,k}=P_k^{-}{H}_{a,k}^{\′}{(H_{a,k}{P}_k^{-}{H}_{a,k}^{\′}+R_a)}^{-1};$

然后计算状态估计量： ${\hat{x}}_{a,k}={\hat{x}}_k^{-}+K_{a,k}(z_{a,k}-H_{a,k}{\hat{x}}_k^{-});$

再计算状态误差协方差阵： $P_{a,k}=(I-K_{a,k}{H}_{a,k})P_k^{-}{(I-K_{a,k}{H}_{a,k})}^{\′}+K_{a,k}{R}_a{K}_{a,k}^{\′}.$

式中：下标a表示加速度计量测更新阶段，R_a为加速度计噪声协方差阵

此时

的9个状态都会更新，利用

更新

用代表已更新状态，取

即前三个变量值，利用式进行校正，然后进行四元数规范化：

$\hat{q}\⇐\frac{\hat{q}}{\left|\right|\hat{q}\left|\right|}$

加速度计更新结束后，将

的q_e部分设置为零；

(7)利用步骤(6)中得到的

进行磁力计量测更新。本方法修正了标准量测更新算法从而使磁力计量测更新只影响方位角。由此，计算出四元数的误差量。该步骤中，取此阶段观测量为 $z_{m,k}\≡y_{m,k}-C\left({\hat{q}}_k\right)\tilde{m},$ 则可得 $H_{m,k}\≡\left[\begin{array}{ccc}2[C\left(\hat{q}\right)\hat{m}\×]& 0& 0\end{array}\right],$ 其更新过程如下：

取此时状态误差协方差阵为

$P_{m,k}^{-}=\left[\begin{array}{cc}{P}_{a,k}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

由此计算滤波增益： $K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^{\′}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]P_{m,k}^{-}{H}_{m,k}^{\′}{(H_{m,k}{P}_{m,k}^{-}{H}_{m,k}^{\′}+R_m)}^{-1};$

然后计算状态估计量： $x_k^{\′}={\hat{x}}_{a,k}+K_{m,k}(z_{m,k}-H_{m,k}{\hat{x}}_{a,k});$

再计算状态误差协方差阵：P_k=(I-K_m,kH_m,k)P_a,k(I-K_m,kH_m,k)′+K_m,kR_mK′_m,k；

式中：下标m表示磁力计量测更新阶段，P_a,k(1:3,1:3)表示由P_a,k前三行前三列所组成的矩阵，0_mn为m行n列的零矩阵并且 $r_3=C\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T.$

(8)根据滤波计算出的四元数误差，校正步骤(2)中的粗略姿态角，得到精确的姿态角，重复步骤(5)～(7)直至结束。

Claims (1)

1.一种基于二阶量测更新的低成本姿态估计方法，其特征是：

(1)在k=0时刻，即初始时刻由初始对准得到初始姿态φ₀，所述初始姿态包括横摇、纵摇和航向；

(2)在k≥1时刻，利用微机械陀螺输出的载体三轴的角速率数据，计算出姿态角的变化值Δφ，由式

计算出滤波开始前的粗略姿态角；

(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据；

(4)设滤波的状态变量为x=[q_e b_g b_a]^T∈R^9×1；式中：q_e表示四元数误差

矢量部分，b_g表示陀螺漂移，b_a表示加速度计零偏；假设四元数误差 ${\tilde{q}}_e={\left[\begin{array}{cc}1& q_e\end{array}\right]}^T,$ 根据四元数相关性质建立状态方程：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_e\\ {\stackrel{\·}{b}}_g\\ {\stackrel{\·}{b}}_a\end{array}\right]=\left[\begin{array}{ccc}-[y_g\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{q}_e\\ b_g\\ b_a\end{array}\right]+\left[\begin{array}{c}-{0.5v}_g\\ v_{b_g}\\ v_{b_a}\end{array}\right]$

式中： $[y_g\×]=\left[\begin{array}{ccc}0& -y_{\mathrm{gz}}& y_{\mathrm{gy}}\\ y_{\mathrm{gz}}& 0& -y_{\mathrm{gx}}\\ -y_{\mathrm{gy}}& y_{\mathrm{gx}}& 0\end{array}\right],$ 下标g代表陀螺、a代表加速度计、m代表磁力计，y_g表示陀螺实际输出。传感器噪声v_a、v_g以及v_m为零均值高斯白噪声；

取观测量为 $z=\left[\begin{array}{c}{y}_a-C\left(\hat{q}\right)\tilde{g}\\ y_m-C\left(\hat{q}\right)\tilde{m}\end{array}\right],$ 由于q_e为小量，再忽略二阶小项，建立量测方程：

$y_a-C\left(\hat{q}\right)\tilde{g}=2[C\left(\hat{q}\right)\tilde{g}\×]q_e+v_a+b_a$

$y_m-C\left(\hat{q}\right)\tilde{m}=2[C\left(\hat{q}\right)\tilde{m}\×]q_e+v_m$

式中：

表示由估计四元数组成的姿态矩阵，y_a和y_m分别为加速度计和磁强计输出，定义 $\tilde{g}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$ 和 $\tilde{m}={\left[\begin{array}{ccc}\cos \mathrm{\α}& 0& -\sin \mathrm{\α}\end{array}\right]}^T,$ g为重力加速度，α为磁倾角；

(5)利用步骤(4)中建立的状态方程进行滤波的时间更新，由下式

${\hat{x}}_k^{-}={\mathrm{\Φ}}_k{\hat{x}}_{k-1}$

$P_k^{-}={\mathrm{\Φ}}_k{P}_{k-1}{{\mathrm{\Φ}}_k}^T+Q_k$

分别计算出一步预测量

和状态误差协方差阵

式中：Φ_k为***状态转移矩阵，Q_k为***噪声协方差阵；

(6)在滤波的量测更新阶段采用二阶量测更新，先进行加速度计量测更新；取此阶段观测量根据建立的量测方程有

其更新过程如下：

先计算滤波增益： $K_{a,k}=P_k^{-}{H}_{a,k}^{\′}{(H_{a,k}{P}_k^{-}{H}_{a,k}^{\′}+R_a)}^{-1};$

然后计算状态估计量： ${\hat{x}}_{a,k}={\hat{x}}_k^{-}+K_{a,k}(z_{a,k}-H_{a,k}{\hat{x}}_k^{-});$

再计算状态误差协方差阵： $P_{a,k}=(I-K_{a,k}{H}_{a,k})P_k^{-}{(I-K_{a,k}{H}_{a,k})}^{\′}+K_{a,k}{R}_a{K}_{a,k}^{\′};$

式中：下标a表示加速度计量测更新阶段，R_a为加速度计噪声协方差阵

此时

的9个状态都会更新，利用

更新

用代表已更新状态，取

即前三个变量值，利用式

进行校正，然后进行四元数规范化：

$\hat{q}\⇐\frac{\hat{q}}{\left|\right|\hat{q}\left|\right|}$

加速度计更新结束后，将

的q_e部分设置为零；

(7)利用步骤(6)中得到的

进行磁力计量测更新；取此阶段观测量为 $z_{m,k}\≡y_{m,k}-C\left({\hat{q}}_k\right)\tilde{m},$ 则得 $H_{m,k}\≡\left[\begin{array}{ccc}2[C\left(\hat{q}\right)\hat{m}\×]& 0& 0\end{array}\right],$ 其更新过程如下：

取此时状态误差协方差阵为

$P_{m,k}^{-}=\left[\begin{array}{cc}{P}_{a,k}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

由此计算滤波增益： $K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^{\′}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]P_{m,k}^{-}{H}_{m,k}^{\′}{(H_{m,k}{P}_{m,k}^{-}{H}_{m,k}^{\′}+R_m)}^{-1};$

然后计算状态估计量： $x_k^{\′}={\hat{x}}_{a,k}+K_{m,k}(z_{m,k}-H_{m,k}{\hat{x}}_{a,k});$

再计算状态误差协方差阵：P_k=(I-K_m,kH_m,k)P_a,k(I-K_m,kH_m,k)′+K_m,kR_mK′_m,k；

式中：下标m表示磁力计量测更新阶段，P_a,k(1:3,1:3)表示由P_a,k前三行前三列所组成的矩阵，0_mn为m行n列的零矩阵并且 $r_3=C\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T;$

(8)根据滤波计算出的四元数误差，校正步骤(2)中的粗略姿态角，得到精确的姿态角。

Images