传感器数据融合及姿态估计总结

---

本文主要介绍汇总了利用陀螺仪、加速度计和磁力计进行数据融合并由此实现姿态估算的一些方法，主要包括传感器直接结算、陀螺仪积分、互补滤波、Mahony滤波和EKF方法，每一部分的内容都是笔者亲自推导并且利用MATLAB编写代码实现过的，可以保证一定的准确性。在最后面笔者也都会附上自己参考的一些比较好的文章或者博客地址供大家学习，用来对传感器姿态估计入门大致是够用了，更深入的研究可以去翻看一些比较好的论文。本文仅做个人学习记录及分享用途，如果错误请您不吝赐教。

---

超长篇警告！

 

一、准备工作

该部分内容用于进行一些基本内容的普及以及标准统一。过于简单的内容此处不再涉及，直接给出一些参考的博文。

姿态旋转理论基础

陀螺仪、加速度计、磁力计的传感器介绍

接下来是将标准进行统一方便后续理解和表达（主要参考全权老师的《多旋翼飞行器设计与控制》）。

• 规定NED坐标系简称n系，然后机体坐标系表示为b系。
• 绕 X轴 进行的滚转运动，称为Roll ，角度符号用 ϕ 来表示。
• 绕 Y轴 进行的俯仰运动，称为Pitch ，角度符号用 θ 来表示。
• 绕 Z轴 进行的偏航运动，称为Yaw ，角度符号用 ψ 来表示。
• 由此，n系向b系的旋转矩阵$R_n^b$表示为
  
• 四元数规定用$q_0，q_1，q_2，q_3$表示。

 

二、原始数据

本文的原始数据来源：Open source IMU and AHRS algorithms

当然最好还是采用自己的实际数据，我这里图省事就直接用现成的。注意最好还是用的实际的飞控数据，尽管自己用手控制得到的IMU数据也可以用来测试数据融合的算法，但是实际的飞控数据会有很多人手调不出来的抖动及震荡，更利于你对算法的理解及实践。

用MATLAB先把原始数据画出来。

此处图片较小可能有点看不太出来。放大来说，加速度计和磁力计的数据在曲线细节方面显得更“毛糙”，也就是说临近点附近会有很多的抖动及噪声。

加速度数据细节：

磁力计数据细节：

但是陀螺仪对比之下，虽然曲线乍一看貌似有很多抖动的地方，因为陀螺仪还是比较灵敏的。但是你细看这些曲线的时候会发现陀螺仪的输出曲线还是比较平滑的，临近点附近都是很自然的过渡，对比加速度计和磁力计来说噪音会小很多。

此处我们先不展开细说，只需简单了解到陀螺仪和加速度计、磁力计的一个重要区别在于：

• 加速度计和磁力计的数据是足够准确的，但是缺点在于容易受到干扰，数据噪声较大；
• 陀螺仪陀螺仪基本不受外界干扰，并且对于线性加速度（振动）不敏感。

 

三、传感器直接估算姿态

由此我们能不能直接使用传感器的信息来估算姿态呢？答案当然是可以的。主要分为两种方法：（1）加速度计估算Roll和Pitch角，磁力计估算Yaw角；（2）陀螺仪积分得到欧拉角。

以下均假定传感器已经固定在机体上，并且X、Y、Z轴与机体的X、Y、Z轴完全对齐。

3.1 加速度计和磁力计解算姿态角

3.1.1 加速度计解算Roll和Pitch

在机体初始保持水平静止状态时，我们可以认为加速度计测得的加速度与重力加速度保持一致，即 $a=g=(0,0,1{)}^T$。假定机体在旋转过程中机体不出现剧烈运动的情况（加速度不发生剧烈变化），那么实际上在经过旋转$R_n^b$之后得到的加速度计数据满足
$$a=R_n^b\ast g$$

$R_n^b$的形式在之前已经给出了，由此我们推算可知，最后得到的测量值$a$实际上就是$R_n^b$的第三列，即
$$a=[a_x,a_y,a_z{]}^T=[-sin\theta ,sin\varphi cos\theta ,cos\varphi cos\theta {]}^T$$

又由于$R_n^b=R(-\varphi ,-\theta ,-\psi )$，那么可以得知Roll和Pitch的角度为
$$\varphi =arcsin(a_x)$$$$\theta =-arctan(\frac{ay}{az})$$

3.1.2 磁力计估算Yaw角

通过加速度计我们解算得到了Roll角和Pitch角，剩余的Yaw角则是通过磁力计数据来进行解算。但是区别在于重力加速度在地球表面任何位置的方向始终都是不变的，因此可以直接从n系转换到b系中，但是地磁场方向和大小并非固定不变。

由于磁力计时固定在机体上的，此时测得的数据是以机体坐标系为基表示的，因此首先需要使用$R_b^n$转换到n系下得到地理坐标的磁力向量，然后在用$R_n^b$转换到b系得到最终值。由于此处笔者也尚未完全搞明白，就先把推导的公式放在这里，公式也已经验证过，大家直接使用即可。

当地磁坐标m系和机体坐标b系重合的时候，磁力计的输出假定为 $M^m=[M_N,0,M_D{]}^T$，在b系下磁力计的输出为$M^b=[M_x^b,M_y^b,M_z^b]$，由此有如下关系：
$$M^b=R_n^b\ast M^m$$$$\left(\begin{array}{c}{M}_x^b\\ M_y^b\\ M_z^b\end{array}\right)=\left(\begin{array}{c}{M}_{N}cos\theta cos\psi -M_{D}sin\theta \\ (sin\varphi sin\theta cos\psi -cos\varphi sin\psi )M_N+sin\varphi cos\theta M_D\\ (cos\varphi sin\theta cos\psi +sin\varphi sin\psi )M_N+cos\varphi cos\theta M_D\end{array}\right)$$

根据上述方程解算出以下内容：
$$M_{N}cos\psi =M_x^{b}cos\theta +M_y^{b}sin\theta sin\psi +M_z^{b}cos\varphi sin\theta$$$$-M_{N}sin\psi =M_y^{b}cos\varphi -M_z^{b}sin\varphi$$

最后可以得到：
$$\psi =-arctan\frac{M_y^{b}cos\varphi -M_z^{b}sin\varphi }{M_x^{b}cos\theta +M_y^{b}sin\theta sin\psi +M_z^{b}cos\varphi sin\theta }$$

根据原始数据编写MATLAB代码解算出欧拉角表示如下，可以看到估算曲线还是包含非常多的噪声的，整条曲线看上去也很“毛躁”，非常符合加速度计和磁力计自身噪音比较多的特征。除此之外，在5S~8S左右的时间Pitch和Yaw角跳变非常剧烈，此时正好是y轴和z轴加速度和磁力均发生剧烈变化的时候。

3.2 陀螺仪积分

之前的推算似乎表明光使用加速度计和磁力计似乎就能够完全得到欧拉角，但是最后解算得到的曲线告诉我们这种结果不太可靠。因此我们此处使用9轴IMU中的另一个传感器进行姿态的估计，陀螺仪的一个巨大优势就是它的精度相比于加速度计和磁力计要高很多，并且对于外界干扰不太敏感。

首先明确一点，陀螺仪输出的是角速度值，那么稍微回顾一下积分的知识，我们就可以确定如下的公式内容：
$$\alpha (t)={\alpha }_0+\int w(t)dt$$

其中，$\alpha (t)$是当前时刻的角度，${\alpha }_0$是初始角度，$w(t)$则是当前角速度。当然，由于整个过程是离散的，因此，实际的当前t时刻的角度表示为，前一t-1时刻的角度加上此次陀螺仪测得的角速度乘以微小时间段。
$${\alpha }_t={\alpha }_{t-1}+w\ast dt$$

对于Roll、Pitch和Yaw角就可以通过陀螺仪测量值的积分得到，当然前提是对初始角度已知。

可以看到曲线相比于直接使用加速度和磁力计进行估计要来的平滑许多，并且在5s~8s时间内没有那么剧烈的变化，那么是否意味着我们可以直接使用陀螺仪而不需要其他传感器了呢？当然不是，如果仔细观察，你会发现陀螺仪积分得到的角度随着时间在不断地漂移，在到达最后时间段的时候整条曲线已经具备了非常大的误差。

这就是陀螺仪其中的一个缺陷，陀螺仪获取的数据实际上都带有一定的误差，使得在积分的过程中得到的角度不断飘移，随着时间的累加整个积分误差会变得非常大。

3.3 传感器总结

这里我们已经能够非常明显地看出陀螺仪、加速度计和磁力计这三种传感器的特点。依次来说一下。

首先是加速度计和磁力计，这两个传感器的显著特点就是足够准确，并且不具备任何的漂移误差，可以直接用于对姿态进行测量估计，但是缺点也十分明显，也就是自身的噪音非常大，只要一受到扰动，如机体的震动或者外界其他干扰因素，测量值就会发生剧烈变化，这一点对于磁力计来说更为明显，因此实际工程中会有很多人选择放弃磁力计，即不对Yaw角进行估计，直接开环控制，而重点处理Roll和Pitch这两个姿态角，对于Yaw角就进行速率控制即可。此外，这两个传感器需要在机体保持静止或者稳定运动过程中才能保持测量结果的精准。

然后是陀螺仪，该传感器是IMU中最重要的传感器，因此精度相比于其余两种会更高一些，此外，其对于一些干扰不会太敏感，也就是说能够较小程度地受到外界干扰。但是传感器自身存在一定的漂移，使得其使用时间越长，得到的误差也就会越大。此外，使用陀螺仪数据进行积分的时候必须得知道其初始姿态信息。

 

那么面对这两种类型的传感器，一种噪声较大，但是不存在漂移，另一种基本没有噪声，但是存在漂移，有没有办法能够将这两种结合起来估计机体的姿态呢？这就是我们接下来探讨的数据融合。

 

四、线性互补滤波

将2种传感器的数据进行融合，一种简单的方法就是将各个传感器的数据进行加权，也就是给每一个传感器数据分配一个确信度$k$，当$k$值较大的时候，表明我们更相信这个传感器的数据，反之，表示我们对于这个传感器数据的确信度较低。

由于我们此次只有2种传感器，因此可以得到一个最基本的线性互补滤波公式
$${\alpha }_{t+1}=(1-k)\ast ({\alpha }_t+w_t\ast dt)+k\ast {\alpha }_{measure}$$

等式右边的左半部分的${\alpha }_t+w_t\ast dt$即是当前时刻陀螺仪积分得到的角度值，右半部分的α_{measure}则是通过加速度计或磁力计数据解算出来的角度值，由于此时仅有2种传感器，因此$k$表示对于加速度计/磁力计测量得到的值的确信度，那么$1-k$则表示对于陀螺仪积分得到的角度的确信度。

当然，如果有多个传感器，那么等式即可变为 ${\alpha }_{t+1}=a\ast {\alpha }_1+b\ast {\alpha }_2+c\ast {\alpha }_3+...+n\ast {\alpha }_n$，其中 $a+b+c+...+n=1$。

此处我们将$k=0.1$，也就是说我们相信陀螺仪的积分结果，由此得到如下曲线

对比直接采用传感器的解算方式，线性互补滤波方式在结果上相比于加速度计和磁力计解算结果更为平滑，相比于陀螺仪积分则解决了漂移的问题。但是我们也可以看到在5~8s的时候仍然受到了非常大的干扰，应该是更多地来自于加速度计和磁力计数据方面的问题。尽管通过调节k值大小能够在一定程度上缓解这个问题，但是如果要真正解决这个问题，仍然需要更好的互补滤波算法。

线性滤波算法是在互补滤波中最为基础的，更深入的还有自适应互补滤波，基于模糊方法的互补滤波等等，这些算法会根据不同的情况，自动调节$k$值的大小，来获得更好的姿态解算结果，更为详细的研究请大家再去细看一些论文，此处便不再不多展开。

 

五、Mahony

Mahony算法实际上是一种非线性互补滤波，其应该是在飞控姿态估算领域内部网上流传最多的一种方法了。在开始之前希望你对于姿态旋转、矩阵运算以及一阶龙格库塔法具备一定的了解。

它的基本思想如下：首先将加速度计和磁力计的的测量值与根据数学模型推导出来的理论值对比求得误差，然后利用这个误差进行PI运算更新到陀螺仪的数据上，最后利用一阶龙格库塔法对四元数进行更新。具体过程如下：

5.1 加速度计误差计算

该过程较为简单，跟之前通过加速度计数据推算Roll和Pitch角类似，首先已知在n系下重力加速度方向和大小基本固定不变，保持为$g=[0,0,1{]}^T$，由n系向b系旋转的四元数矩阵$C_n^b$表示为
$$C_n^b=\left(\begin{array}{c}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)1-2(q_1^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)1-2(q_1^2+q_2^2)\end{array}\right)$$

那么由此推算得知在当前旋转角度下的理论机体重力为
$$v=\left(\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right)=C_n^b\ast g=\left(\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right)$$

假定此时由加速度计测量得到的三轴上的加速度值为
$$a=\left(\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right)$$

向量之间的误差可以由外积得到，那么就有误差为

$$ea=\left(\begin{array}{c}e{a}_x\\ e{a}_y\\ e{a}_z\end{array}\right)=\left(\begin{array}{c}{a}_y\ast v_z-a_z\ast v_y\\ a_z\ast v_x-a_z\ast v_z\\ a_x\ast v_y-a_y\ast v_x\end{array}\right)$$

5.2 磁力计误差计算

磁力计误差计算对比之下会略显复杂一些。前面已经说过，地磁方向并非和重力一样是固定的，因此由于磁力计是固定在机体上的，此时测得的数据是以机体坐标系为基表示的，因此首先需要使用$C_b^n$转换到n系下得到地理坐标的磁力向量，然后在用$C_n^b$转换到b系得到最终值。由于此处笔者也有一些尚未弄懂，因此仅做一些公式推导和简单说明。

假定磁力计测量得到的数据表示为
$$m=\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)$$

则具体过程如下：

$$h=\left(\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right)=C_b^n\ast m=\left(\begin{array}{c}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)1-2(q_1^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)1-2(q_1^2+q_2^2)\end{array}\right)\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)$$

此时将地理坐标系下的x轴和y轴这两个水平轴的分量$h_x和h_y$合并，$z$轴竖直分量$h_z$保持不变，则有
$$\hat{m}=\left(\begin{array}{c}{b}_x\\ 0\\ b_z\end{array}\right)=\left(\begin{array}{c}\sqrt{h_x^2+h_y^2}\\ 0\\ h_z\end{array}\right)$$

由于此时获得的数据仍然在地理坐标系下，因此还需要转换到机体坐标系下：
$$w=\left(\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right)=C_n^b\ast \hat{m}=\left(\begin{array}{c}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)1-2(q_1^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)1-2(q_1^2+q_2^2)\end{array}\right)\ast \left(\begin{array}{c}{b}_x\\ 0\\ b_z\end{array}\right)$$

$$w=\left(\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right)=\left(\begin{array}{c}{b}_x(1-2(q_2^2+q_3^2))+2b_z(q_1{q}_3-q_0{q}_2)\\ 2b_x(q_1{q}_2-q_0{q}_3)+2b_z(q_2{q}_3+q_0{q}_1)\\ 2b_x(q_1{q}_3+q_0{q}_2)+b_z(1-2(q_1^2+q_2^2))\end{array}\right)$$

然后求解误差：

$$em=\left(\begin{array}{c}e{m}_x\\ e{m}_y\\ e{m}_z\end{array}\right)=\left(\begin{array}{c}{m}_y\ast w_z-m_z\ast w_y\\ m_z\ast w_x-m_z\ast w_z\\ m_x\ast w_y-m_y\ast w_x\end{array}\right)$$

5.3 误差补偿

根据以上结果得到最终误差为
$$e=ea+em=\left(\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right)$$

根据误差大小，采用PI控制器来调节补偿量的大小，表示为
$$gyro\_compensation=K_p\ast e+K_i\int e\ast dt$$

PID各项的作用就不展开细说了，之前为什么说Mahony算法也是一个互补滤波器呢，因为此处$K_p$的值即表示了对于加速度计和磁力计的确信度。

然后将补偿值叠加到陀螺仪测量得到的角速度量上面即可得到更为准确的角速度值
$$gyro\_data=gyro\_data+gyro\_compensation$$

5.4 一阶龙格库塔法迭代四元数

这里先简单介绍一下什么是一阶龙格库塔法。简单用如下公式表示，相信你一看就清楚了（其中h为步长）
$$y^{\prime }=\frac{dy}{dx}=f(x,y)$$$$K_i=f(x_i,y_i)$$$$y_{i+1}=y_i+h\ast K_i$$

四元数的微分方程此处不再作推导，有兴趣的可以自行搜索，直接给出公式如下：
$$\frac{dq}{dt}=\frac{1}{2}\left(\begin{array}{c}0-w_x-w_y-w_z\\ w_{x}0w_z-w_y\\ w_y-w_{z}0w_x\\ w_z{w}_y-w_{x}0\end{array}\right)\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$$

那么利用一阶龙格库塔法对其进行更新之后得到的公式如下：
$$q_{t+△t}=q_t+\frac{dq}{dt}\ast △t$$

5.5 解算欧拉角

最后根据四元数及欧拉角的转换公式进行解算
$$\varphi =arctan\frac{2(q_0{q}_1+q_2{q}_3)}{1-2(q_1^2+q_2^2)}$$$$\theta =arcsin(2(q_0{q}_2-q_1{q}_3))$$$$\psi =arctan\frac{2(q_0{q}_3+q_1{q}_2)}{1-2(q_2^2+q_3^2)}$$

得到结果如下，可以看到对比以上三种方法效果要好上不少，既没有过多的噪声，也没有漂移，曲线也足够平滑。

 

六、EKF

终于来到了扩展卡尔曼滤波滤波（以下简称EKF）。EKF是一种能够较为准确地进行姿态估计的非线性方法，并且由于其增益是动态调整的，因此往往估算得到的姿态都较好。但是由于计算时间偏长，因此在实际项目中（特别是针对一些微小型飞行器）会更多地使用非线性互补滤波或者Mahony算法。但是学习姿态解算，EKF是必须要了解其原理及实现过程的。

如果你尚未对卡尔曼滤波（以下简称KF）有所了解，建议你先学习完KF之后再来看这部分内容，基本上只要你对KF最够了解，那么EKF的学习将会顺利许多。此处推荐一些文章及博客，都是我觉得都对卡尔曼滤波器进行了深入浅出介绍的较好的干货。
【传感器融合】扩展卡尔曼滤波的逐步理解与实现（上）
零基础读懂“扩展卡尔曼滤波”——上篇
An Introduction to the Kalman Filter

基本上，谈到EKF的学习都避免不了提及这一篇论文：A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU。此处还是建议大家仔细看一下，后续也将会按照这篇论文的思路进行推导和实现。

6.1 KF和EKF

6.1.1 卡尔曼滤波器

此处不再对KF公式进行推导，有兴趣的可以看我大概很久以前的一篇文章：卡尔曼滤波通俗易懂的解释。

已知KF的公式表示如下：

预测模型

状态预测公式：${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$

协方差预测公式：$P_k^{-}=A{P}_{k-1}{A}^T+Q$

状态更新模型

卡尔曼系数：$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$

状态估计： ${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

噪声协方差矩阵更新：$P_k=(I-K_{k}H)P_k^{-}$

各个公式主要由矩阵运算组成，其中 $x$表示状态量，$A$ 表示状态转移矩阵，$B$ 表示控制矩阵，$P$ 表示噪声协方差矩阵，$Q$ 表示过程激励噪声协方差矩阵（与陀螺仪相关），$H$表示观测矩阵，$R$表示观测噪声协方差矩阵（传感器噪声），$z_k$表示观测量的真值（即传感器测量值）。

然而KF主要适用于线性模型，对于多轴的无人机非线性模型不太适用。

6.1.2 线性与非线性

那么如何让原本仅能应用于线性模型的KF，扩展为能够面向非线性模型的EKF呢？首先从函数说起。

我们知道，所谓的线性函数，说得简单一些，就是函数成一条直线，如y = 1，y = x，y = 2x+1等等，也就是两个变量之间成一次函数关系。非线性函数，如y = x^2+1等，函数主要呈现为一条曲线。

而学过微积分的同学们肯定知道，一条曲线，当你将其分割，分割到足够小的时候，那一段即表现为直线。我们此处将可导和可微不做区分，写作公式表现为

也就是，在f(x)在x0处连续，并且h足够小的时候，利用可导性质将其转换为线性模型。

严格来说，实际上现实世界中几乎所有的物品都是非线性模型，然而非线性方程的研究和求解过于复杂，因此我们需要将其线性化，来简化系统分析。而最有效的方法就是将其在一定范围内转换为非线性模型。

如前所述，当工作点在其附近做小范围变化时，我们将其视作线性，也就是利用其导数将其转换为线性模型，这个思想称为小偏差理论。

6.1.3 雅克比矩阵

前面说到的情况都是针对于函数而言的，那对于矩阵来说如何求得其导数呢？那就是现在要介绍的雅克比矩阵，此处截选百度百科里的介绍。

在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数。

例如，有四元数函数表示为如下矩阵形式

那么其雅克比矩阵（一阶导数）即可表示为

总结一下，只要记住：雅克比矩阵本质上就是导数，是对函数/矩阵的微分。

6.1.4 扩展卡尔曼滤波器

这里直接给出EKF的原理公式，如果你已经对KF具备一定程度的了解，相信你看完公式以及解释之后肯定能够理解其中的含义。更具体的推导和实现我们放在后续的姿态解算过程中。

时间更新方程

状态转移方程：${\hat{x}}_k^{-}=f({\hat{x}}_{k-1},u_{k-1},0)$

协方差预测公式：$P_k^{-}=A_k{P}_{k-1}{A}_k^T+Q_{k-1}$

状态更新模型

卡尔曼系数：$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R_k}$

后验状态更新： ${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-h({\hat{x}}_k^{-},0))$

噪声协方差矩阵更新：$P_k=(I-K_k{H}_k)P_k^{-}$

① 先验的状态量${\hat{x}}_k^{-}$是直接用非线性的状态转移方程$f({\hat{x}}_{k-1},u_{k-1},0)$计算得到的，表示状态量${\hat{x}}_{k-1}$，输入量$u_{k-1}$，噪声$w_{k-1}=0$

② 协方差预测公式中，$P_k$表示的是预测模型的噪声协方差矩阵，$A_k$在这里表示的是非线性方程$f$对$x$偏导的雅克比矩阵，$Q_k$表示$k$时刻的过程激励噪声协方差矩阵（陀螺仪噪声）。

③ 状态更新模型中，$h({\hat{x}}_k,0)$表示的是非线性的观测方程，而$H$则是$h$对于$x$偏导的雅克比矩阵，$R_k$表示$k$时刻的观测噪声协方差矩阵（加速度计\磁力计噪声），$z_k$表示真实的测量值，${\hat{x}}_k$则是后验得到的最终状态量。

6.2 EKF实现姿态解算

按照《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》中的思路，我们来利用一种二阶的扩展卡尔曼滤波器进行姿态解算。

计算过程采用7元素的状态量，即
$$x=(q_0,q_1,q_2,q_3,w_{bx},w_{by},w_{bz}{)}^T$$

6.2.1 初始化欧拉角及噪声

首先获取初始的欧拉角姿态，利用第3章中的公式计算初始欧拉角：
$$\varphi =arcsin(a_x)$$$$\theta =-arctan(\frac{ay}{az})$$$$\psi =-arctan\frac{m_y^{b}cos\varphi -m_z^{b}sin\varphi }{m_x^{b}cos\theta +m_y^{b}sin\theta sin\psi +m_z^{b}cos\varphi sin\theta }$$

由欧拉角解算四元数：
$$q_0=cos\frac{\varphi }{2}cos\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\varphi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}$$$$q_1=sin\frac{\varphi }{2}cos\frac{\theta }{2}cos\frac{\psi }{2}-cos\frac{\varphi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}$$$$q_2=cos\frac{\varphi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\varphi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}$$$$q_3=cos\frac{\varphi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}-sin\frac{\varphi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}$$

根据陀螺仪噪声、加速度计噪声和磁力计噪声确定矩阵$Q、R_a、R_m$
$$Q=\left(\begin{array}{c}{w}_{n}0\\ 0w_{bn}\end{array}\right)$$$$R_a=diag(a_n)$$$$R_m=diag(m_n)$$

其中$w_n$表示陀螺仪噪声，$w_{bn}$表示陀螺仪偏置噪声，$a_n$表示加速度计噪声，$m_n$表示磁力计噪声。

6.2.2 Start of “a priori” system estimation

之前在介绍Mahony的时候已经给出了四元数的微分方程如下
$$\frac{dq}{dt}=\frac{1}{2}\left(\begin{array}{c}0-w_x-w_y-w_z\\ w_{x}0w_z-w_y\\ w_y-w_{z}0w_x\\ w_z{w}_y-w_{x}0\end{array}\right)\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$$

令
$${\Omega }_{nb}^n=\left(\begin{array}{c}0-w_x-w_y-w_z\\ w_{x}0w_z-w_y\\ w_y-w_{z}0w_x\\ w_z{w}_y-w_{x}0\end{array}\right)$$

$$A_{TC}=\frac{1}{2}{\Omega }_{nb}^n$$

$$A^1=(I+\frac{1}{2}{\Omega }_{nb}^{n}T)=\frac{1}{2}T\left(\begin{array}{c}1-w_x-w_y-w_z\\ w_{x}1w_z-w_y\\ w_y-w_{z}1w_x\\ w_z{w}_y-w_{x}1\end{array}\right)$$

以上是论文中使用$x=(q_0,q_1,q_2,q_3{)}^T$作为状态量的推导，但是由于此次我们使用的是7元素的状态量，因此我们考虑了陀螺仪偏置$w_{bn}$，上式可以重写为

$$A^1=\frac{1}{2}T\left(\begin{array}{c}1-(w_x-w_{bx})-(w_y-w_{by})-(w_z-w_{bz})\\ (w_x-w_{bx})1(w_z-w_{bz})-(w_y-w_{by})\\ (w_y-w_{by})-(w_z-w_{bz})1(w_x-w_{bx})\\ (w_z-w_{bz})(w_y-w_{by})-(w_x-w_{bx})1\end{array}\right)$$

由此得到了状态转移矩阵
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}$$$$\left(\begin{array}{c}{q}_{k+1}\\ w_{bk+1}\end{array}\right)=\left(\begin{array}{c}{A}^{1}0\\ 0I\end{array}\right)\left(\begin{array}{c}{q}_k\\ w_{bk}\end{array}\right)$$

对这个状态转移矩阵求解关于x的偏导得到其雅克比矩阵表示为
$$A_k=\left(\begin{array}{c}{A}^1{L}_k\\ 0I\end{array}\right)$$$$L_k=\left(\begin{array}{c}{q}_1{q}_2{q}_3\\ -q_0{q}_3-q_2\\ -q_3-q_0-q_1\\ q_2-q_1-q_0\end{array}\right)\frac{T}{2}$$

那么先验估计的噪声协方差矩阵表示为：
$$P_k^{-}=A_k{P}_{k-1}{A}_k^T+Q_{k-1}$$

6.2.3 Start of the correction stage 1

此处利用加速度计进行先验值的更新。

之前在Mahony算法中已经提到将n系下的重力加速度转换到当前旋转角度的b系下的理论值时，得到如下方程（$g=(0,0,1{)}^T$）
$$h_1(q_k)=C_n^b\ast g=\left(\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right)$$

将该非线性方程求解其雅克比矩阵
$$\frac{\partial h_1(q_k)}{\partial q_k}=\left(\begin{array}{c}-2q_{2}2q_3-2q_{0}2q_1\\ 2q_{1}2q_{0}2q_{3}2q_2\\ 2q_0-2q_1-2q_{2}2q_3\end{array}\right)$$

由于还要对陀螺仪偏置求偏导，但是其实际值为0，因此完整的雅克比矩阵表示为$H_1$
$$H_{k1}=(\frac{\partial h_1(q_k)}{\partial q_k}0)$$

求解一阶段的卡尔曼增益：$K_{k1}=\frac{P_k^{-}{H}_{k1}^T}{H_{k1}{P}_k^{-}{H}_{k1}^T+R_{ak}}$

获取加速度计的真实测量值：$z_{k1}=[a_x,a_y,a_z{]}^T$

计算更新量：$q_{\in 1}=K_{k1}(z_{k1}-h_1({\hat{q}}_k^{-}))$，并且把$q_{\in 1}$这个四元数中的$q_3$置0防止在更新Roll和Pitch时对于Yaw角的干扰。

后验状态更新：${\hat{q}}_{k1}={\hat{x}}_k^{-}+q_{\in 1}$，此处只更新四元数。

后验误差协方差矩阵计算：$P_{k1}=(I-K_{k1}{H}_{k1})P_k^{-}$

6.2.4 Start of the correction stage 2

进行阶段2的磁力计更新过程。

同样，相关理论推导已经在Mahony的磁力计部分实现，此处仅给出公式

假定磁力计测量得到的数据表示为
$$m=\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)$$

$$h=\left(\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right)=C_b^n\ast m=\left(\begin{array}{c}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)1-2(q_1^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)1-2(q_1^2+q_2^2)\end{array}\right)\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)$$

$$\hat{m}=\left(\begin{array}{c}{b}_x\\ 0\\ b_z\end{array}\right)=\left(\begin{array}{c}\sqrt{h_x^2+h_y^2}\\ 0\\ h_z\end{array}\right)$$

$$h_2=C_n^b\ast \hat{m}=\left(\begin{array}{c}{b}_x(1-2(q_2^2+q_3^2))+2b_z(q_1{q}_3-q_0{q}_2)\\ 2b_x(q_1{q}_2-q_0{q}_3)+2b_z(q_2{q}_3+q_0{q}_1)\\ 2b_x(q_1{q}_3+q_0{q}_2)+b_z(1-2(q_1^2+q_2^2))\end{array}\right)$$

求解该非线性方程的雅克比矩阵
$$\frac{\partial h_2(q_k)}{\partial q_k}=\left(\begin{array}{c}{b}_x{q}_0-b_z{q}_2{b}_x{q}_1+b_z{q}_3-b_x{q}_2-b_z{q}_0-b_x{q}_3+b_z{q}_1\\ -b_x{q}_3+b_z{q}_1{b}_x{q}_2+b_z{q}_0{b}_x{q}_1+b_z{q}_3-b_x{q}_0+b_z{q}_2\\ b_x{q}_2+b_z{q}_0{b}_x{q}_3-b_z{q}_1{b}_x{q}_0-b_z{q}_2{b}_x{q}_1+b_z{q}_3\end{array}\right)$$

同理，陀螺仪偏置求偏导为0

$$H_{k2}=(\frac{\partial h_2(q_k)}{\partial q_k}0)$$

求解二阶段的卡尔曼增益：$K_{k2}=\frac{P_k^{-}{H}_{k2}^{}}{H_{k2}{P}_k^{-}{H}_{k2}^T+R_{mk}}$