ref:https://blog.csdn.net/moumde/article/details/111391642
author: moumde
本文主要介绍了陀螺仪、加速度计和磁力计的数据集成，主要包括传感器直接结算、陀螺仪积分、互补滤波器、Mahony滤波和EKF方法的每一部分都是作者moumde亲自推导利用MATLAB编写代码可以保证一定的准确性。最后，作者还将附上一些更好的文章或博客地址供您学习。估计介绍传感器姿势大致足够了。更深入的研究可以阅读一些更好的论文。本文仅为个人学习记录和共享用途。如果你犯了错误，请给我建议。
超长警告！
一、准备工作
这部分用于普及一些基本内容和统一标准。这里不再涉及太简单的内容，直接给出一些参考博客。
理论基础的姿态旋转
传感器介绍陀螺仪、加速度计和磁力计
下一步是统一方便后续理解和表达标准(主要参考全权教师的多旋翼飞机设计与控制)。
规定NED坐标系简称n系，然后机体坐标系表示b系。 绕 X轴 滚动运动称为滚动运动Roll ，角度符号用 ? 来表示。 绕 Y轴 俯仰运动称为俯仰运动Pitch ，角度符号用 θ 来表示。 绕 Z轴 偏航运动称为偏航运动Yaw ，角度符号用 ψ 来表示。 由此，n旋转矩阵系向b系 R n b R_n^b Rnb表示为 插入图片描述 规定使用四元数 q 0 ， q 1 ， q 2 ， q 3 q_0，q_1，q_2，q_3 q0，q1，q2，q3表示。
二、原始数据
原始数据源：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 a=g=(0,0,1)^T a=g=(0,0,1)T。假设身体在旋转过程中没有剧烈运动(加速度没有剧烈变化)，实际上是在旋转 R n b R_n^b Rnb加速度计数据满足后获得的加速度计数据 a = R n b ? g a=R_n^b*g a=Rnb?g
R n b R_n^b Rnb之前已经给出了形式，所以我们可以计算出最终得到的测量值 a a a实际上就是 R n b R_n^b Rnb第三列，即 a = [ a x , a y , a z ] T = [ ? s i n θ , s i n ? c o s θ , c o s ? c o s θ ] T a=[a_x, a_y, a_z]^T=[-sinθ, sin?cosθ, cos?cosθ]^T a=[ax,ay,az]T=[?sinθ,sin?cosθ,cos?cosθ]T
又由于 R n b = R ( ? ? , ? θ , ? ψ ) R_n^b=R(-?,-θ,-ψ) Rnb=R(??,?θ,?ψ)，然后就可以知道了Roll和Pitch的角度为 ? = a r c s i n ( a x ) ?=arcsin(a_x) ?=arcsin(ax) θ = ? a r c t a n ( a y a z ) θ=-arctan(\frac{ay}{az}) θ=?arctan(azay) 3.1.2 磁力计估算Yaw角
我们通过加速度计解算Roll角和Pitch角，剩余的Yaw角度是通过磁计数据来解决的。但不同之处在于，重力加速度在地球表面的任何位置变的，所以它可以直接从n系转换为b系，但地磁场的方向和大小不是固定的。
由于磁力计时固定在机体上，此时测量的数据基于机体坐标系，因此首先需要使用 R b n R_b^n Rbn将地理坐标的磁向量转换为n系，然后使用 R n b R_n^b Rnb最终值转换为b系。因为这里作者还没有完全理解，所以我先把推导公式放在这里，公式已经验证了，我们可以直接使用。
当地磁坐标m系与机体坐标b系重叠时，假设磁力计的输出是 M m = [ M N , 0 , M D ] T M^m=[M_N, 0, M_D]^T Mm=[MN,0,MD]T，B系下磁力计的输出是 M b = [ M x b , M y b , M z b ] M^b=[M^b_x, M^b_y, M^b_z] Mb=[Mxb,Myb,Mzb]，有以下关系： M b = R n b ? M m M^b=R_n^b*M^m Mb=Rnb?Mm ( M x b M y b M z b ) = ( M N c o s θ c o s ψ ? M D s i n θ ( s i n ? s i n θ c o s ψ ? c o s ? s i n ψ ) M N s i n ? c o s θ M D ( c o s ? s i n θ c o s ψ s i n ? s i n ψ ) M N c o s ? c o s θ M D ) ???MbxMbyMbz??? = ???MNcosθcosψ?MDsinθ(sin?sinθcosψ?cos?sinψ)MN sin?cosθMD(cos?sinθcosψ sin?sinψ)MN cos?cosθMD???
??MxbMybMzb??=??MNcosθcosψ?MDsinθ(sin?sinθcosψ?cos?sinψ)MN sin?cosθMD(cos?sinθcosψ sin?sinψ)MN cos?cosθMD??
根据上述方程解算出以下内容： M N c o s ψ = M x b c o s θ M y b s i n θ s i n ψ M z b c o s ? s i n θ M_Ncosψ=M^b_xcosθ M^b_ysinθsinψ M^b_zcos?sinθ MNcosψ=Mxbcosθ Mybsinθsinψ Mzbcos?sinθ ? M N s i n ψ = M y b c o s ? ? M z b s i n ? -M_Nsinψ=M^b_ycos?-M^b_zsin? ?MNsinψ=Mybcos??Mzbsin?
最后可以得到： ψ = ? a r c t a n M y b c o s ? ? M z b s i n ? M x b c o s θ M y b s i n θ s i n ψ M z b c o s ? s i n θ ψ=-arctan\frac{M^b_ycos?-M^b_zsin?}{M^b_xcosθ M^b_ysinθsinψ M^b_zcos?sinθ} ψ=?arctanMxbcosθ Mybsinθsinψ Mzbcos?sinθMybcos??Mzbsin?
根据原始数据编写MATLAB代码解算表示欧拉角如下。可以看出，估计曲线仍然包含大量噪声。整个曲线看起来也很狂躁，这与加速度计和磁力计本身的噪声特性非常一致。此外，在5S~8S左右的时间Pitch和Yaw角跳变和z轴的加速度和磁力都发生剧烈变化时，角跳变非常剧烈。 插入图片描述 3.2 陀螺仪积分
之前的计算似乎表明，使用加速度计和磁力计似乎可以完全获得欧拉角，但最终的曲线告诉我们，结果并不可靠。所以我们在这里使用9轴IMU陀螺仪的另一个巨大优点是其精度远高于加速度计和磁力计，对外部干扰不敏感。
首先要明确的是，陀螺仪输出的是角速度值，所以稍微回顾一下积分的知识，就可以确定以下公式: α ( t ) = α 0 ∫ w ( t ) d t α(t)=α_0 \int{w(t)}dt α(t)=α0 ∫w(t)dt
其中， α ( t ) α(t) α(t)是当前时刻的角度， α 0 α_0 α0是初始角度， w ( t ) w(t) w(t)这是当前的角度速度。当然，由于整个过程是离散的，当前t时刻的实际角度是t-1时刻的角度加上此次陀螺仪测得的角速度乘以微小时间段。 α t = α t ? 1 w ? d t α_t=α_{t-1} w*dt αt=αt?1 w?dt
对于Roll、Pitch和Yaw陀螺可以通过角测量值的积分得到，当然前提是对初始角度已知。 在这里插入图片描述
可以看到曲线相比于直接使用加速度和磁力计进行估计要来的平滑许多，并且在5s~8s时间内没有那么剧烈的变化，那么是否意味着我们可以直接使用陀螺仪而不需要其他传感器了呢？当然不是，如果仔细观察，你会发现陀螺仪积分得到的角度随着时间在不断地漂移，在到达最后时间段的时候整条曲线已经具备了非常大的误差。
这就是陀螺仪其中的一个缺陷，陀螺仪获取的数据实际上都带有一定的误差，使得在积分的过程中得到的角度不断飘移，随着时间的累加整个积分误差会变得非常大。 3.3 传感器总结
这里我们已经能够非常明显地看出陀螺仪、加速度计和磁力计这三种传感器的特点。依次来说一下。
首先是加速度计和磁力计，这两个传感器的显著特点就是足够准确，并且不具备任何的漂移误差，可以直接用于对姿态进行测量估计，但是缺点也十分明显，也就是自身的噪音非常大，只要一受到扰动，如机体的震动或者外界其他干扰因素，测量值就会发生剧烈变化，这一点对于磁力计来说更为明显，因此实际工程中会有很多人选择放弃磁力计，即不对Yaw角进行估计，直接开环控制，而重点处理Roll和Pitch这两个姿态角，对于Yaw角就进行速率控制即可。此外，这两个传感器需要在机体保持静止或者稳定运动过程中才能保持测量结果的精准。
然后是陀螺仪，该传感器是IMU中最重要的传感器，因此精度相比于其余两种会更高一些，此外，其对于一些干扰不会太敏感，也就是说能够较小程度地受到外界干扰。但是传感器自身存在一定的漂移，使得其使用时间越长，得到的误差也就会越大。此外，使用陀螺仪数据进行积分的时候必须得知道其初始姿态信息。
那么面对这两种类型的传感器，一种噪声较大，但是不存在漂移，另一种基本没有噪声，但是存在漂移，有没有办法能够将这两种结合起来估计机体的姿态呢？这就是我们接下来探讨的数据融合。
四、线性互补滤波
将2种传感器的数据进行融合，一种简单的方法就是将各个传感器的数据进行加权，也就是给每一个传感器数据分配一个确信度 k k k，当 k k k值较大的时候，表明我们更相信这个传感器的数据，反之，表示我们对于这个传感器数据的确信度较低。
由于我们此次只有2种传感器，因此可以得到一个最基本的线性互补滤波公式 α t + 1 = ( 1 − k ) ∗ ( α t + w t ∗ d t ) + k ∗ α m e a s u r e α_{t+1}=(1-k)*(α_{t}+w_t*dt)+k*α_{measure} αt+1=(1−k)∗(αt+wt∗dt)+k∗αmeasure
等式右边的左半部分的 α t + w t ∗ d t α_{t}+w_t*dt αt+wt∗dt即是当前时刻陀螺仪积分得到的角度值，右半部分的α_{measure}则是通过加速度计或磁力计数据解算出来的角度值，由于此时仅有2种传感器，因此 k k k表示对于加速度计/磁力计测量得到的值的确信度，那么 1 − k 1-k 1−k则表示对于陀螺仪积分得到的角度的确信度。
当然，如果有多个传感器，那么等式即可变为 α t + 1 = a ∗ α 1 + b ∗ α 2 + c ∗ α 3 + . . . + n ∗ α n α_{t+1}=a*α_{1}+b*α_{2}+c*α_{3}+...+n*α_{n} αt+1=a∗α1+b∗α2+c∗α3+...+n∗αn，其中 a + b + c + . . . + n = 1 a+b+c+...+n=1 a+b+c+...+n=1。
此处我们将 k = 0.1 k=0.1 k=0.1，也就是说我们相信陀螺仪的积分结果，由此得到如下曲线 在这里插入图片描述 对比直接采用传感器的解算方式，线性互补滤波方式在结果上相比于加速度计和磁力计解算结果更为平滑，相比于陀螺仪积分则解决了漂移的问题。但是我们也可以看到在5~8s的时候仍然受到了非常大的干扰，应该是更多地来自于加速度计和磁力计数据方面的问题。尽管通过调节k值大小能够在一定程度上缓解这个问题，但是如果要真正解决这个问题，仍然需要更好的互补滤波算法。
线性滤波算法是在互补滤波中最为基础的，更深入的还有自适应互补滤波，基于模糊方法的互补滤波等等，这些算法会根据不同的情况，自动调节 k k k值的大小，来获得更好的姿态解算结果，更为详细的研究请大家再去细看一些论文，此处便不再不多展开。
五、Mahony
Mahony算法实际上是一种非线性互补滤波，其应该是在飞控姿态估算领域内部网上流传最多的一种方法了。在开始之前希望你对于姿态旋转、矩阵运算以及一阶龙格库塔法具备一定的了解。
它的基本思想如下：首先将加速度计和磁力计的的测量值与根据数学模型推导出来的理论值对比求得误差，然后利用这个误差进行PI运算更新到陀螺仪的数据上，最后利用一阶龙格库塔法对四元数进行更新。具体过程如下： 5.1 加速度计误差计算
该过程较为简单，跟之前通过加速度计数据推算Roll和Pitch角类似，首先已知在n系下重力加速度方向和大小基本固定不变，保持为 g = [ 0 , 0 , 1 ] T g=[0,0,1]^T g=[0,0,1]T，由n系向b系旋转的四元数矩阵 C n b C_n^b Cnb表示为 C n b = ( 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 ) ) C_n^b= ⎛⎝⎜⎜⎜1−2(q22+q23)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q21+q23)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟
Cnb=⎝⎛1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)⎠⎞
那么由此推算得知在当前旋转角度下的理论机体重力为 v = ( v x v y v z ) = C n b ∗ g = ( 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 ) ) v= ⎛⎝⎜⎜⎜vxvyvz⎞⎠⎟⎟⎟ =C_n^b*g= ⎛⎝⎜⎜⎜2(q1q3−q0q2)2(q2q3+q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟
v=⎝⎛vxvyvz⎠⎞=Cnb∗g=⎝⎛2(q1q3−q0q2)2(q2q3+q0q1)1−2(q12+q22)⎠⎞
假定此时由加速度计测量得到的三轴上的加速度值为 a = ( a x a y a z ) a= ⎛⎝⎜⎜⎜axayaz⎞⎠⎟⎟⎟
a=⎝⎛axayaz⎠⎞
向量之间的误差可以由外积得到，那么就有误差为
e a = ( e a x e a y e a z ) = ( a y ∗ v z − a z ∗ v y a z ∗ v x − a z ∗ v z a x ∗ v y − a y ∗ v x ) ea= ⎛⎝⎜⎜⎜eaxeayeaz⎞⎠⎟⎟⎟ = ⎛⎝⎜⎜⎜ay∗vz−az∗vyaz∗vx−az∗vzax∗vy−ay∗vx⎞⎠⎟⎟⎟
ea=⎝⎛eaxeayeaz⎠⎞=⎝⎛ay∗vz−az∗vyaz∗vx−az∗vzax∗vy−ay∗vx⎠⎞ 5.2 磁力计误差计算
磁力计误差计算对比之下会略显复杂一些。前面已经说过，地磁方向并非和重力一样是固定的，因此由于磁力计是固定在机体上的，此时测得的数据是以机体坐标系为基表示的，因此首先需要使用 C b n C_b^n Cbn转换到n系下得到地理坐标的磁力向量，然后在用 C n b C_n^b Cnb转换到b系得到最终值。由于此处笔者也有一些尚未弄懂，因此仅做一些公式推导和简单说明。
假定磁力计测量得到的数据表示为 m = ( m x m y m z ) m= ⎛⎝⎜⎜⎜mxmymz⎞⎠⎟⎟⎟
m=⎝⎛mxmymz⎠⎞
则具体过程如下：
h = ( h x h y h z ) = C b n ∗ m = ( 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 ) ) ( m x m y m z ) h= ⎛⎝⎜⎜⎜hxhyhz⎞⎠⎟⎟⎟ =C_b^n*m= ⎛⎝⎜⎜⎜1−2(q22+q23)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1−2(q21+q23)2(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟ ⎛⎝⎜⎜⎜mxmymz⎞⎠⎟⎟⎟
h=⎝⎛hxhyhz⎠⎞=Cbn∗m=⎝⎛1−2(q22+q32)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1−2(q12+q32)2(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)1−2(q12+q22)⎠⎞⎝⎛mxmymz⎠⎞
此时将地理坐标系下的x轴和y轴这两个水平轴的分量 h x 和 h y h_x和h_y hx和hy合并， z z z轴竖直分量 h z h_z hz保持不变，则有 m ^ = ( b x 0 b z ) = ( h x 2 + h y 2 0 h z ) \hat{m}= ⎛⎝⎜⎜bx0bz⎞⎠⎟⎟ = ⎛⎝⎜⎜⎜h2x+h2y‾‾‾‾‾‾‾√0hz⎞⎠⎟⎟⎟ m^=⎝⎛bx0bz⎠⎞=⎝⎜⎛hx2+hy2
0hz⎠⎟⎞
由于此时获得的数据仍然在地理坐标系下，因此还需要转换到机体坐标系下： w = ( w x w y w z ) = C n b ∗ m ^ = ( 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 ) ) ∗ ( b x 0 b z ) w= ⎛⎝⎜⎜⎜wxwywz⎞⎠⎟⎟⎟ =C_n^b*\hat{m}= ⎛⎝⎜⎜⎜1−2(q22+q23)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q21+q23)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟ * ⎛⎝⎜⎜bx0bz⎞⎠⎟⎟
w=⎝⎛wxwywz⎠⎞=Cnb∗m^=⎝⎛1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)⎠⎞∗⎝⎛bx0bz⎠⎞
w = ( w x w y w z ) = ( b x ( 1 − 2 ( q 2 2 + q 3 2 ) ) + 2 b z ( q 1 q 3 − q 0 q 2 ) 2 b x ( q 1 q 2 − q 0 q 3 ) + 2 b z ( q 2 q 3 + q 0 q 1 ) 2 b x ( q 1 q 3 + q 0 q 2 ) + b z ( 1 − 2 ( q 1 2 + q 2 2 ) ) ) w= ⎛⎝⎜⎜⎜wxwywz⎞⎠⎟⎟⎟ = ⎛⎝⎜⎜⎜bx(1−2(q22+q23))+2bz(q1q3−q0q2)2bx(q1q2−q0q3)+2bz(q2q3+q0q1)2bx(q1q3+q0q2)+bz(1−2(q21+q22))⎞⎠⎟⎟⎟
w=⎝⎛wxwywz⎠⎞=⎝⎛bx(1−2(q22+q32))+2bz(q1q3−q0q2)2bx(q1q2−q0q3)+2bz(q2q3+q0q1)2bx(q1q3+q0q2)+bz(1−2(q12+q22))⎠⎞
然后求解误差：
e m = ( e m x e m y e m z ) = ( m y ∗ w z − m z ∗ w y m z ∗ w x − m z ∗ w z m x ∗ w y − m y ∗ w x ) em= ⎛⎝⎜⎜⎜emxemyemz⎞⎠⎟⎟⎟ = ⎛⎝⎜⎜⎜my∗wz−mz∗wymz∗wx−mz∗wzmx∗wy−my∗wx⎞⎠⎟⎟⎟
em=⎝⎛emxemyemz⎠⎞=⎝⎛my∗wz−mz∗wymz∗wx−mz∗wzmx∗wy−my∗wx⎠⎞ 5.3 误差补偿
根据以上结果得到最终误差为 e = e a + e m = ( e x e y e z ) e=ea+em= ⎛⎝⎜⎜⎜exeyez⎞⎠⎟⎟⎟
e=ea+em=⎝⎛exeyez⎠⎞
根据误差大小，采用PI控制器来调节补偿量的大小，表示为 g y r o _ c o m p e n s a t i o n = K p ∗ e + K i ∫ e ∗ d t gyro\_compensation = K_p*e+K_i\int{e*dt} gyro_compensation=Kp∗e+Ki∫e∗dt
PID各项的作用就不展开细说了，之前为什么说Mahony算法也是一个互补滤波器呢，因为此处 K p K_p Kp的值即表示了对于加速度计和磁力计的确信度。
然后将补偿值叠加到陀螺仪测量得到的角速度量上面即可得到更为准确的角速度值 g y r o _ d a t a = g y r o _ d a t a + g y r o _ c o m p e n s a t i o n gyro\_data=gyro\_data+gyro\_compensation gyro_data=gyro_data+gyro_compensation 5.4 一阶龙格库塔法迭代四元数
这里先简单介绍一下什么是一阶龙格库塔法。简单用如下公式表示，相信你一看就清楚了（其中h为步长） y ′ = d y d x = f ( x , y ) y'=\frac{dy}{dx}=f(x,y) y′=dxdy=f(x,y) K i = f ( x i , y i ) K_i=f(x_i,y_i) Ki=f(xi,yi) y i + 1 = y i + h ∗ K i y_{i+1}=y_i+h*K_i yi+1=yi+h∗Ki
四元数的微分方程此处不再作推导，有兴趣的可以自行搜索，直接给出公式如下： d q d t = 1 2 ( 0 − w x − w y − w z w x 0 w z − w y w y − w z 0 w x w z w y − w x 0 ) ( q 0 q 1 q 2 q 3 ) \frac{dq}{dt}=\frac{1}{2} ⎛⎝⎜⎜⎜⎜0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎞⎠⎟⎟⎟⎟ ⎛⎝⎜⎜⎜⎜q0q1q2q3⎞⎠⎟⎟⎟⎟
dtdq=21⎝⎜⎜⎛0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎠⎟⎟⎞⎝⎜⎜⎛q0q1q2q3⎠⎟⎟⎞
那么利用一阶龙格库塔法对其进行更新之后得到的公式如下： q t + △ t = q t + d q d t ∗ △ t q_{t+△t}=q_t+\frac{dq}{dt}*△t qt+△t=qt+dtdq∗△t 5.5 解算欧拉角
最后根据四元数及欧拉角的转换公式进行解算 ϕ = a r c t a n 2 ( q 0 q 1 + q 2 q 3 ) 1 − 2 ( q 1 2 + q 2 2 ) ϕ=arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)} ϕ=arctan1−2(q12+q22)2(q0q1+q2q3) θ = a r c s i n ( 2 ( q 0 q 2 − q 1 q 3 ) ) θ=arcsin(2(q_0q_2-q_1q_3)) θ=arcsin(2(q0q2−q1q3)) ψ = a r c t a n 2 ( q 0 q 3 + q 1 q 2 ) 1 − 2 ( q 2 2 + q 3 2 ) ψ=arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)} ψ=arctan1−2(q22+q32)2(q0q3+q1q2)
得到结果如下，可以看到对比以上三种方法效果要好上不少，既没有过多的噪声，也没有漂移，曲线也足够平滑。 在这里插入图片描述
六、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的公式表示如下：
预测模型
状态预测公式： x ^ k − = A x ^ k − 1 + B u k − 1 \hat{x}_k^- =A\hat{x}_{k-1} + Bu_{k-1} x^k−=Ax^k−1+Buk−1
协方差预测公式： P k − = A P k − 1 A T + Q P_k^- = AP_{k-1}A^T + Q Pk−=APk−1AT+Q
状态更新模型
卡尔曼系数： K k = P k − H T H P k − H T + R K_k = \frac {P_k^-H^T} {HP_k^-H^T + R} Kk=HPk−HT+RPk−HT
状态估计： x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-) x^k=x^k−+Kk(zk−Hx^k−)
噪声协方差矩阵更新： P k = ( I − K k H ) P k − P_k = (I - K_kH)P_k^- Pk=(I−KkH)Pk−
各个公式主要由矩阵运算组成，其中 x x x表示状态量， A A A 表示状态转移矩阵， B B B 表示控制矩阵， P P P 表示噪声协方差矩阵， Q Q Q 表示过程激励噪声协方差矩阵（与陀螺仪相关）， H H H表示观测矩阵， R R R表示观测噪声协方差矩阵（传感器噪声）， z k z_k zk表示观测量的真值（即传感器测量值）。
然而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具备一定程度的了解，相信你看完公式以及解释之后肯定能够理解其中的含义。更具体的推导和实现我们放在后续的姿态解算过程中。
时间更新方程
状态转移方程： x ^ k − = f ( x ^ k − 1 , u k − 1 , 0 ) \hat{x}_k^- =f(\hat{x}_{k-1} ,u_{k-1},0) x^k−=f(x^k−1,uk−1,0)
协方差预测公式： P k − = A k P k − 1 A k T + Q k − 1 P_k^- = A_kP_{k-1}A_k^T + Q_{k-1} Pk−=AkPk−1AkT+Qk−1
状态更新模型
卡尔曼系数： K k = P k − H T H P k − H T + R k K_k = \frac {P_k^-H^T} {HP_k^-H^T + R_k} Kk=HPk−HT+RkPk−HT
后验状态更新： x ^ k = x ^ k − + K k ( z k − h ( x ^ k − , 0 ) ) \hat{x}_k = \hat{x}_k^- + K_k(z_k - h(\hat{x}_k^-,0)) x^k=x^k−+Kk(zk−h(x^k−,0))
噪声协方差矩阵更新： P k = ( I − K k H k ) P k − P_k = (I - K_kH_k)P_k^- Pk=(I−KkHk)Pk−
① 先验的状态量 x ^ k − \hat{x}_k^- x^k−是直接用非线性的状态转移方程 f ( x ^ k − 1 , u k − 1 , 0 ) f(\hat{x}_{k-1} ,u_{k-1},0) f(x^k−1,uk−1,0)计算得到的，表示状态量 x ^ k − 1 \hat{x}_{k-1} x^k−1，输入量 u k − 1 u_{k-1} uk−1，噪声 w k − 1 = 0 w_{k-1}=0 wk−1=0
② 协方差预测公式中， P k P_k Pk表示的是预测模型的噪声协方差矩阵， A k A_k Ak在这里表示的是非线性方程 f f f对 x x x偏导的雅克比矩阵， Q k Q_k Qk表示 k k k时刻的过程激励噪声协方差矩阵（陀螺仪噪声）。
③ 状态更新模型中， h ( x ^ k , 0 ) h(\hat{x}_{k} ,0) h(x^k,0)表示的是非线性的观测方程，而 H H H则是 h h h对于 x x x偏导的雅克比矩阵， R k R_k Rk表示 k k k时刻的观测噪声协方差矩阵（加速度计\磁力计噪声）， z k z_k zk表示真实的测量值， x ^ k \hat{x}_k 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 b x , w b y , w b z ) T x=(q_0,q_1,q_2,q_3,w_{bx},w_{by},w_{bz})^T x=(q0,q1,q2,q3,wbx,wby,wbz)T 6.2.1 初始化欧拉角及噪声
首先获取初始的欧拉角姿态，利用第3章中的公式计算初始欧拉角： ϕ = a r c s i n ( a x ) ϕ=arcsin(a_x) ϕ=arcsin(ax) θ = − a r c t a n ( a y a z ) θ=-arctan(\frac{ay}{az}) θ=−arctan(azay) ψ = − a r c t a n m y b c o s ϕ − m z b s i n ϕ m x b c o s θ + m y b s i n θ s i n ψ + m z b c o s ϕ s i n θ ψ=-arctan\frac{m^b_ycosϕ-m^b_zsinϕ}{m^b_xcosθ+m^b_ysinθsinψ+m^b_zcosϕsinθ} ψ=−arctanmxbcosθ+mybsinθsinψ+mzbcosϕsinθmybcosϕ−mzbsinϕ
由欧拉角解算四元数： q 0 = c o s ϕ 2 c o s θ 2 c o s ψ 2 + s i n ϕ 2 s i n θ 2 s i n ψ 2 q_0=cos\frac{ϕ}{2}cos\frac{θ}{2}cos\frac{ψ}{2}+sin\frac{ϕ}{2}sin\frac{θ}{2}sin\frac{ψ}{2} q0=cos2ϕcos2θcos2ψ+sin2ϕsin2θsin2ψ q 1 = s i n ϕ 2 c o s θ 2 c o s ψ 2 − c o s ϕ 2 s i n θ 2 s i n ψ 2 q_1=sin\frac{ϕ}{2}cos\frac{θ}{2}cos\frac{ψ}{2}-cos\frac{ϕ}{2}sin\frac{θ}{2}sin\frac{ψ}{2} q1=sin2ϕcos2θcos2ψ−cos2ϕsin2θsin2ψ q 2 = c o s ϕ 2 s i n θ 2 c o s ψ 2 + s i n ϕ 2 c o s θ 2 s i n ψ 2 q_2=cos\frac{ϕ}{2}sin\frac{θ}{2}cos\frac{ψ}{2}+sin\frac{ϕ}{2}cos\frac{θ}{2}sin\frac{ψ}{2} q2=cos2ϕsin2θcos2ψ+sin2ϕcos2θsin2ψ q 3 = c o s ϕ 2 c o s θ 2 s i n ψ 2 − s i n ϕ 2 s i n θ 2 c o s ψ 2 q_3=cos\frac{ϕ}{2}cos\frac{θ}{2}sin\frac{ψ}{2}-sin\frac{ϕ}{2}sin\frac{θ}{2}cos\frac{ψ}{2} q3=cos2ϕcos2θsin2ψ−sin2ϕsin2θcos2ψ
根据陀螺仪噪声、加速度计噪声和磁力计噪声确定矩阵 Q 、 R a 、 R m Q、R_a、R_m Q、Ra、Rm Q = ( w n 0 0 w b n ) Q= (wn00wbn)
Q=(wn00wbn) R a = d i a g ( a n ) R_a=diag(a_n) Ra=diag(an) R m = d i a g ( m n ) R_m=diag(m_n) Rm=diag(mn)
其中 w n w_n wn表示陀螺仪噪声， w b n w_{bn} wbn表示陀螺仪偏置噪声， a n a_n an表示加速度计噪声， m n m_n mn表示磁力计噪声。 6.2.2 Start of “a priori” system estimation
之前在介绍Mahony的时候已经给出了四元数的微分方程如下 d q d t = 1 2 ( 0 − w x − w y − w z w x 0 w z − w y w y − w z 0 w x w z w y − w x 0 ) ( q 0 q 1 q 2 q 3 ) \frac{dq}{dt}=\frac{1}{2} ⎛⎝⎜⎜⎜⎜0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎞⎠⎟⎟⎟⎟ ⎛⎝⎜⎜⎜⎜q0q1q2q3⎞⎠⎟⎟⎟⎟
dtdq=21⎝⎜⎜⎛0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎠⎟⎟⎞⎝⎜⎜⎛q0q1q2q3⎠⎟⎟⎞
令 Ω n b n = ( 0 − w x − w y − w z w x 0 w z − w y w y − w z 0 w x w z w y − w x 0 ) Ω^n_{nb}= ⎛⎝⎜⎜⎜⎜0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎞⎠⎟⎟⎟⎟
Ωnbn=⎝⎜⎜⎛0−wx−wy−wzwx0wz−wywy−wz0wxwzwy−wx0⎠⎟⎟⎞
A T C = 1 2 Ω n b n A_{TC}=\frac{1}{2}Ω^n_{nb} ATC=21Ωnbn
A 1 = ( I + 1 2 Ω n b n T ) = 1 2 T ( 1 − w x − w y − w z w x 1 w z − w y w y − w z 1 w x w z w y − w x 1 ) A^1=(I+\frac{1}{2}Ω^n_{nb}T)=\frac{1}{2}T ⎛⎝⎜⎜⎜⎜1−wx−wy−wzwx1wz−wywy−wz1wxwzwy−wx1⎞⎠⎟⎟⎟⎟
A1=(I+21ΩnbnT)=21T⎝⎜⎜⎛1−wx−wy−wzwx1wz−wywy−wz1wxwzwy−wx1⎠⎟⎟⎞
以上是论文中使用 x = ( q 0 , q 1 , q 2 , q 3 ) T x=(q_0,q_1,q_2,q_3)^T x=(q0,q1,q2,q3)T作为状态量的推导，但是由于此次我们使用的是7元素的状态量，因此我们考虑了陀螺仪偏置 w b n w_{bn} wbn，上式可以重写为
A 1 = 1 2 T ( 1 − ( w x − w b x ) − ( w y − w b y ) − ( w z − w b z ) ( w x − w b x ) 1 ( w z − w b z ) − ( w y − w b y ) ( w y − w b y ) − ( w z − w b z ) 1 ( w x − w b x ) ( w z − w b z ) ( w y − w b y ) − ( w x − w b x ) 1 ) A^1=\frac{1}{2}T ⎛⎝⎜⎜⎜⎜1−(wx−wbx)−(wy−wby)−(wz−wbz)(wx−wbx)1(wz−wbz)−(wy−wby)(wy−wby)−(wz−wbz)1(wx−wbx)(wz−wbz)(wy−wby)−(wx−wbx)1⎞⎠⎟⎟⎟⎟
A1=21T⎝⎜⎜⎛1−(wx−wbx)−(wy−wby)−(wz−wbz)(wx−wbx)1(wz−wbz)−(wy−wby)(wy−wby)−(wz−wbz)1(wx−wbx)(wz−wbz)(wy−wby)−(wx−wbx)1⎠⎟⎟⎞
由此得到了状态转移矩阵 x ^ k − = A x ^ k − 1 \hat{x}_k^- =A\hat{x}_{k-1} x^k−=Ax^k−1 ( q k + 1 w b k + 1 ) = ( A 1 0 0 I ) ( q k w b k ) (qk+1wbk+1) = (A100I) (qkwbk)
(qk+1wbk+1)=(A100I)(qkwbk)
对这个状态转移矩阵求解关于x的偏导得到其雅克比矩阵表示为 A k = ( A 1 L k 0 I ) A_k= (A1Lk0I) Ak=(A1Lk0I) L k = ( q 1 q 2 q 3 − q 0 q 3 − q 2 − q 3 − q 0 − q 1 q 2 − q 1 − q 0 ) T 2 L_k= ⎛⎝⎜⎜⎜⎜q1q2q3−q0q3−q2−q3−q0−q1q2−q1−q0⎞⎠⎟⎟⎟⎟
\frac{T}{2} Lk=⎝⎜⎜⎛q1q2q3−q0q3−q2−q3−q0−q1q2−q1−q0⎠⎟⎟⎞2T
那么先验估计的噪声协方差矩阵表示为： P k − = A k P k − 1 A k T + Q k − 1 P_k^- = A_kP_{k-1}A_k^T + Q_{k-1} Pk−=AkPk−1AkT+Qk−1 6.2.3 Start of the correction stage 1
此处利用加速度计进行先验值的更新。
之前在Mahony算法中已经提到将n系下的重力加速度转换到当前旋转角度的b系下的理论值时，得到如下方程（ g = ( 0 , 0 , 1 ) T g=(0,0,1)^T g=(0,0,1)T） h 1 ( q k ) = C n b ∗ g = ( 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 ) ) h_1(q_k)=C_n^b*g= ⎛⎝⎜⎜⎜2(q1q3−q0q2)2(q2q3+q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟
h1(qk)=Cnb∗g=⎝⎛2(q1q3−q0q2)2(q2q3+q0q1)1−2(q12+q22)⎠⎞
将该非线性方程求解其雅克比矩阵 ∂ h 1 ( q k ) ∂ q k = ( − 2 q 2 2 q 3 − 2 q 0 2 q 1 2 q 1 2 q 0 2 q 3 2 q 2 2 q 0 − 2 q 1 − 2 q 2 2 q 3 ) \frac{∂h_1(q_k)}{∂q_k}= ⎛⎝⎜⎜−2q22q3−2q02q12q12q02q32q22q0−2q1−2q22q3⎞⎠⎟⎟
∂qk∂h1(qk)=⎝⎛−2q22q3−2q02q12q12q02q32q22q0−2q1−2q22q3⎠⎞
由于还要对陀螺仪偏置求偏导，但是其实际值为0，因此完整的雅克比矩阵表示为 H 1 H_1 H1 H k 1 = ( ∂ h 1 ( q k ) ∂ q k 0 ) H_{k1}=(\frac{∂h_1(q_k)}{∂q_k}\quad 0) Hk1=(∂qk∂h1(qk)0)
求解一阶段的卡尔曼增益： K k 1 = P k − H k 1 T H k 1 P k − H k 1 T + R a k K_{k1}= \frac {P_k^-H_{k1}^T} {H_{k1}P_k^-H_{k1}^T + R_{ak}} Kk1=Hk1Pk−Hk1T+RakPk−Hk1T
获取加速度计的真实测量值： z k 1 = [ a x , a y , a z ] T z_{k1}=[a_x,a_y,a_z]^T zk1=[ax,ay,az]T
计算更新量： q ∈ 1 = K k 1 ( z k 1 − h 1 ( q ^ k − ) ) q_{∈1}=K_{k1}(z_{k1}-h_1(\hat{q}_k^-)) q∈1=Kk1(zk1−h1(q^k−))，并且把 q ∈ 1 q_{∈1} q∈1这个四元数中的 q 3 q_3 q3置0防止在更新Roll和Pitch时对于Yaw角的干扰。
后验状态更新： q ^ k 1 = x ^ k − + q ∈ 1 \hat{q}_{k1} = \hat{x}_k^- +q_{∈1} q^k1=x^k−+q∈1，此处只更新四元数。
后验误差协方差矩阵计算： P k 1 = ( I − K k 1 H k 1 ) P k − P_{k1} = (I - K_{k1}H_{k1})P_k^- Pk1=(I−Kk1Hk1)Pk− 6.2.4 Start of the correction stage 2
进行阶段2的磁力计更新过程。
同样，相关理论推导已经在Mahony的磁力计部分实现，此处仅给出公式
假定磁力计测量得到的数据表示为 m = ( m x m y m z ) m= ⎛⎝⎜⎜⎜mxmymz⎞⎠⎟⎟⎟
m=⎝⎛mxmymz⎠⎞
h = ( h x h y h z ) = C b n ∗ m = ( 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 ) ) ( m x m y m z ) h= ⎛⎝⎜⎜⎜hxhyhz⎞⎠⎟⎟⎟ =C_b^n*m= ⎛⎝⎜⎜⎜1−2(q22+q23)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1−2(q21+q23)2(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟ ⎛⎝⎜⎜⎜mxmymz⎞⎠⎟⎟⎟
h=⎝⎛hxhyhz⎠⎞=Cbn∗m=⎝⎛1−2(q22+q32)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1−2(q12+q32)2(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)1−2(q12+q22)⎠⎞⎝⎛mxmymz⎠⎞
m ^ = ( b x 0 b z ) = ( h x 2 + h y 2 0 h z ) \hat{m}= ⎛⎝⎜⎜bx0bz⎞⎠⎟⎟ = ⎛⎝⎜⎜⎜h2x+h2y‾‾‾‾‾‾‾√0hz⎞⎠⎟⎟⎟ m^=⎝⎛bx0bz⎠⎞=⎝⎜⎛hx2+hy2
0hz⎠⎟⎞
h 2 = C n b ∗ m ^ = ( b x ( 1 − 2 ( q 2 2 + q 3 2 ) ) + 2 b z ( q 1 q 3 − q 0 q 2 ) 2 b x ( q 1 q 2 − q 0 q 3 ) + 2 b z ( q 2 q 3 + q 0 q 1 ) 2 b x ( q 1 q 3 + q 0 q 2 ) + b z ( 1 − 2 ( q 1 2 + q 2 2 ) ) ) h_{2}=C_n^b*\hat{m}= ⎛⎝⎜⎜⎜bx(1−2(q22+q23))+2bz(q1q3−q0q2)2bx(q1q2−q0q3)+2bz(q2q3+q0q1)2bx(q1q3+q0q2)+bz(1−2(q21+q22))⎞⎠⎟⎟⎟
h2=Cnb∗m^=⎝⎛bx(1−2(q22+q32))+2bz(q1q3−q0q2)2bx(q1q2−q0q3)+2bz(q2q3+q0q1)2bx(q1q3+q0q2)+bz(1−2(q12+q22))⎠⎞
求解该非线性方程的雅克比矩阵 ∂ h 2 ( q k ) ∂ q k = ( 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 ) \frac{∂h_2(q_k)}{∂q_k}= ⎛⎝⎜⎜bxq0−bzq2bxq1+bzq3−bxq2−bzq0−bxq3+bzq1−bxq3+bzq1bxq2+bzq0bxq1+bzq3−bxq0+bzq2bxq2+bzq0bxq3−bzq1bxq0−bzq2bxq1+bzq3⎞⎠⎟⎟
∂qk∂h2(qk)=⎝⎛bxq0−bzq2bxq1+bzq3−bxq2−bzq0−bxq3+bzq1−bxq3+bzq1bxq2+bzq0bxq1+bzq3−bxq0+bzq2bxq2+bzq0bxq3−bzq1bxq0−bzq2bxq1+bzq3⎠⎞
同理，陀螺仪偏置求偏导为0
H k 2 = ( ∂ h 2 ( q k ) ∂ q k 0 ) H_{k2}=(\frac{∂h_2(q_k)}{∂q_k}\quad 0) Hk2=(∂qk∂h2(qk)0)
求解二阶段的卡尔曼增益： K k 2 = P k − H k 2 T H k 2 P k − H k 2 T + R m k K_{k2}= \frac {P_k^-H_{k2}^T} {H_{k2}P_k^-H_{k2}^T + R_{mk}} Kk2=Hk2Pk−Hk2T+RmkPk−Hk2T
获取磁力计的真实测量值： z k 2 = [ m x , m y , m z ] T z_{k2}=[m_x,m_y,m_z]^T zk2=[mx,my,mz]T
计算更新量： q ∈ 2 = K k 2 ( z k 2 − h 2 ( q ^ k − ) ) q_{∈2}=K_{k2}(z_{k2}-h_2(\hat{q}_k^-)) q∈2=Kk2(zk2−h2(q^k−))，并且把 q ∈ 2 q_{∈2} q∈2这个四元数中的 q 1 、 q 2 q_1、q_2 q1、q2置0防止在更新Yaw角时对于Roll和Pitch的干扰。
后验状态更新： x ^ k = q ^ k 1 + q ∈ 2 \hat{x}_k = \hat{q}_{k1} +q_{∈2} x^k=q^k1+q∈2，此处只更新四元数。
后验误差协方差矩阵计算： P k = ( I − K k 2 H k 2 ) P k 1 P_{k} = (I - K_{k2}H_{k2})P_{k1} Pk=(I−Kk2Hk2)Pk1 6.3 EKF解算结果
以上便是二阶的EKF姿态解算的全部流程，在计算完成之后取出状态量 x x x的前4个元素，也就是四元数q进行欧拉角的计算，最后可以得到结果如下：
在这里插入图片描述
以上便是此次博文全部内容，如有错误请及时留言指出
参考资料
由加速度计解算得到姿态角 加速度计和磁力计与姿态角的关系详解 磁力计如何用来计算姿态（2） 姿态篇：一.初识姿态估计 基于飞控的姿态估计算法详解 详解几种飞控的姿态解算算法 IMU Attitude Estimation Mahony姿态解算算法笔记（一） Mahony姿态解算算法笔记（二） Open source IMU and AHRS algorithms 四元数姿态解算----扩展卡尔曼滤波器设计 基于EKF的姿态解算 基于EKF的IMU姿态结算 【传感器融合】扩展卡尔曼滤波的逐步理解与实现（上） 零基础读懂“扩展卡尔曼滤波”——上篇 零基础读懂“扩展卡尔曼滤波”——中篇 零基础读懂“扩展卡尔曼滤波”——下篇 A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU An Introduction to the Kalman Filter