姿态解算是飞控的一个基础、重要部分，估计出来的姿态会发布给姿态控制器，控制飞行平稳，是飞行稳定的最重要保障。另外，姿态解算不仅仅用于无人机领域，无人车领域也需要进行姿态解算，用以进行GNSS和IMU、激光点云的融合定位。
传感器基本原理
坐标系描述
姿态的几种表示方式
姿态解算的基本算法
不展开，推荐以下参考：
运载体在三位空间中运动包含六个*度，既有角运动又有线运动。在地球表面附近，运载体的角运动又有线运动。运载体上的惯性传感器（imu）是相对于惯性空间进行测量的，另外，地球绕其自转轴相对于惯性空间以常值角速度旋转，运载体在地球表面上的位置变化会引起其相对于地心的角速度变化。
地心惯性坐标系用oi xi yi zi 表示，原点为地球中心，x轴指向春分点（赤道面与黄道面的交线再与天球相交的交点之一），春分点是天文测量中确定恒星时的起点，z轴为地球自转轴，指向北极，惯性传感器 的输出就是基于该坐标系为参考坐标系的。
地球坐标系用oe xe ye ze 表示，x轴指向本初子午线与赤道面的交点，z轴为地球自转轴，指向北极。e系与地球固连，也称 地心地固坐标系 （Earth-Centered Earth-Fixed,ECEF) ,地球坐标系相对于惯性坐标系的角速度大小为地球自传角速度，w i e = 7.2921151467 ∗ 1 0 − 5 r a d / s w_{ie}=7.2921151467*10^{-5} rad/s w i e = 7 . 2 9 2 1 1 5 1 4 6 7 ∗ 1 0 − 5 r a d / s
WGS-84
导航坐标系用oe xe ye ze ，它是惯性导航系统在求解导航参数时采用的参考坐标系。纯惯导系统的高度通道在原理上是发散的，因而惯导系统多采用当地水平坐标系作为参考坐标系，以实现水平和高度通道的解偶，即利用惯导长时间导航定位时只进行水平定位结算，简单地将高度值设为固定。
X轴：指东
Y轴：指北
Z轴：指天
载体坐标系用ob xb yb zb 表示，原点为载体重心，右-前-上坐标系描述：x轴沿载体横轴向右，y轴沿载体纵轴向前（即载体前进方向），z轴沿载体立轴向上。b系与载体固连，载体坐标系b系与导航坐标系n系的关系可用一组欧拉角表示。
(1)东北天——当地导航坐标系
X轴：指东;
Y轴：指北;
Z轴：指天。
(2)右前上——当地导航坐标系
X轴：指向载体右侧;
Y轴：指向载体前进方向;
Z轴：指天。
欧拉角用来直观表示载体的姿态，如下图
偏航角Yaw $ (\psi) $
俯仰角Pitch $ (\theta)$
翻滚角Roll ( ϕ ) (\phi) ( ϕ )
Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)
泰特-布莱恩角 上图描述了一组欧拉角，主要看右图就好了
(1)原坐标系1如蓝色的x-y-z
(2)坐标系1沿z轴，逆时针旋转ψ ( ψ > 0 ) \psi (\psi >0) ψ ( ψ > 0 ) ψ = Y a w \psi=Yaw ψ = Y a w
(3)将坐标系2沿x(N)轴逆时针旋转θ ( θ > 0 ) \theta(\theta >0) θ ( θ > 0 )
(4)将红色坐标系沿Y轴顺时针旋转( ϕ ) (\phi) ( ϕ )
由于使用欧拉角描述姿态存在万向节死锁的问题，具体参见：万向节死锁 gimbal lock
一个向量的方向（姿态）我们可以用他在参考坐标系（地理坐标系）各个轴向的夹角的余弦来表示（及在各个轴的投影）。C b n = [ c 11 c 12 c 13 c 21 c 22 c 23 c 11 c 12 c 13 ]
\quad
C_b^n=
\begin{bmatrix} c_{11} & c_{12} &c_{13} \\
c_{21} & c_{22} &c_{23} \\
c_{11} & c_{12} &c_{13}
\end{bmatrix}
\quad
C b n = ⎣ ⎡ c 1 1 c 2 1 c 1 1 c 1 2 c 2 2 c 1 2 c 1 3 c 2 3 c 1 3 ⎦ ⎤
一个二维的坐标变换如下：F ( r x 1 , r y 1 ) F(r_{x1},r_{y1}) F ( r x 1 , r y 1 ) F 2 ( r x 2 , r y 2 ) F_{2}(r_{x2},r_{y2}) F 2 ( r x 2 , r y 2 ) r x 2 = r x 1 ∗ cos α + r y 1 ∗ sin α r y 2 = r x 1 ∗ ( − sin α ) + r y 1 ∗ cos α
\begin{aligned}
r_{x2}&=r_{x1}*\cos \alpha+ r_{y1}*\sin \alpha \\
r_{y2}&=r_{x1}*(-\sin \alpha)+r_{y1}*\cos \alpha
\end{aligned}
r x 2 r y 2 = r x 1 ∗ cos α + r y 1 ∗ sin α = r x 1 ∗ ( − sin α ) + r y 1 ∗ cos α [ r x 2 r y 2 r z 2 ] = [ cos α sin α 0 − sin α cos α 0 0 0 1 ] [ r x 1 r y 1 r z 1 ]
\quad
\begin{bmatrix}
r_{x2} \\
r_{y2} \\
r_{z2}
\end{bmatrix}=
\begin{bmatrix} \cos \alpha & \sin \alpha &0 \\
-\sin \alpha & \cos \alpha &0 \\
0 & 0 &1
\end{bmatrix}
\begin{bmatrix}
r_{x1} \\
r_{y1} \\
r_{z1}
\end{bmatrix}
\quad
⎣ ⎡ r x 2 r y 2 r z 2 ⎦ ⎤ = ⎣ ⎡ cos α − sin α 0 sin α cos α 0 0 0 1 ⎦ ⎤ ⎣ ⎡ r x 1 r y 1 r z 1 ⎦ ⎤
第1行表示旋转之后的X2轴 在原坐标系(X1,Y1,Z1)轴下的投影
第2行表示旋转之后的Y2轴 在原坐标系(X1,Y1,Z1)轴下的投影
第3行表示旋转之后的Z2轴 在原坐标系(X1,Y1,Z1)轴下的投影
C n b C_{n}^{b} C n b
假设我们现在有一个东北天坐标系和一个载体坐标系，现需要将东北天坐标系经过3次旋转，使得最终得到的坐标系与载体坐标系重合。
在此之前，需要做出一些规定
旋转的正方向为：从旋转轴看的逆时针方向 旋转的顺序为：Z-X-Y 对应的欧拉角：Yaw-Pitch-Roll
ψ \psi ψ
C n 1 C_{n}^{1} C n 1 C n 1 = [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ]
\quad
C_{n}^{1}=
\begin{bmatrix} \cos \psi & \sin \psi &0 \\
-\sin \psi & \cos \psi &0 \\
0 & 0 &1
\end{bmatrix}
\quad
C n 1 = ⎣ ⎡ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ⎦ ⎤
θ \theta θ
C 1 2 C_{1}^{2} C 1 2 C 1 2 = [ 1 0 0 0 cos θ sin θ 0 − sin θ cos θ ]
\quad
C_{1}^{2}=
\begin{bmatrix} 1 & 0 &0 \\
0 &\cos \theta & \sin \theta \\
0 &-\sin \theta & \cos \theta
\end{bmatrix}
\quad
C 1 2 = ⎣ ⎡ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ⎦ ⎤
ϕ \phi ϕ
C 2 3 C_{2}^{3} C 2 3 C 2 3 = [ cos ϕ 0 − sin ϕ 0 1 0 sin ϕ 0 cos ϕ ]
\quad
C_{2}^{3}=
\begin{bmatrix} \cos \phi & 0 &-\sin \phi \\
0 & 1& 0 \\
\sin \phi & 0& \cos \phi
\end{bmatrix}
\quad
C 2 3 = ⎣ ⎡ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ ⎦ ⎤
C n b C_{n}^{b} C n b
C n b = C 2 3 C 1 2 C n 1 = [ cos ϕ 0 − sin ϕ 0 1 0 sin ϕ 0 cos ϕ ] [ 1 0 0 0 cos θ sin θ 0 − sin θ cos θ ] [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ] = [ cos ψ cos ϕ − sin ψ sin θ sin ϕ sin ψ cos ϕ + cos ψ sin θ sin ϕ − cos θ sin ϕ − sin ψ cos θ cos ψ cos θ sin θ cos ψ sin ϕ + sin ψ sin θ cos ϕ sin ψ sin ϕ − cos ψ sin θ cos ϕ cos θ cos ϕ ] = [ c 11 c 12 c 13 c 21 c 22 c 23 c 11 c 12 c 13 ]
\begin{aligned}
C_{n}^{b}=&C_{2}^{3}C_{1}^{2}C_{n}^{1} \\
=&
\begin{bmatrix} \cos \phi & 0 &-\sin \phi \\
0 & 1& 0 \\
\sin \phi & 0& \cos \phi
\end{bmatrix}
\begin{bmatrix} 1 & 0 &0 \\
0 &\cos \theta & \sin \theta \\
0 &-\sin \theta & \cos \theta
\end{bmatrix}
\begin{bmatrix} \cos \psi & \sin \psi &0 \\
-\sin \psi & \cos \psi &0 \\
0 & 0 &1
\end{bmatrix} \\
=&
\begin{bmatrix}
\cos \psi \cos \phi -\sin \psi \sin \theta \sin \phi
& \sin \psi \cos \phi+\cos \psi \sin \theta \sin \phi & -\cos \theta \sin \phi \\
-\sin \psi \cos \theta & \cos \psi \cos \theta & \sin \theta \\
\cos \psi \sin \phi+ \sin \psi \sin \theta \cos \phi & \sin \psi \sin \phi- \cos \psi \sin \theta \cos \phi & \cos \theta \cos \phi
\end{bmatrix} \\
= &
\begin{bmatrix} c_{11} & c_{12} &c_{13} \\
c_{21} & c_{22} &c_{23} \\
c_{11} & c_{12} &c_{13}
\end{bmatrix}
\end{aligned}
C n b = = = = C 2 3 C 1 2 C n 1 ⎣ ⎡ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ ⎦ ⎤ ⎣ ⎡ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ⎦ ⎤ ⎣ ⎡ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ⎦ ⎤ ⎣ ⎡ cos ψ cos ϕ − sin ψ sin θ sin ϕ − sin ψ cos θ cos ψ sin ϕ + sin ψ sin θ cos ϕ sin ψ cos ϕ + cos ψ sin θ sin ϕ cos ψ cos θ sin ψ sin ϕ − cos ψ sin θ cos ϕ − cos θ sin ϕ sin θ cos θ cos ϕ ⎦ ⎤ ⎣ ⎡ c 1 1 c 2 1 c 1 1 c 1 2 c 2 2 c 1 2 c 1 3 c 2 3 c 1 3 ⎦ ⎤
用欧拉角表示姿态需要指定顺序和方向，用余弦矩阵表示姿态则参数较多，四元数出现了。
三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。
简单来说，四元数的思想就是把方向余弦矩阵的三次旋转表示为只绕一个旋转轴旋转一次完成 ，因此可以用4个数来表示这个过程，其中包括旋转轴向量的长度(θ)和旋转轴单位向量(x,y,z)
[ q 0 q 1 q 2 q 3 ] = [ cos θ 2 x ∗ sin θ 2 y ∗ sin θ 2 z ∗ sin θ 2 ]
\begin{aligned}
\begin{bmatrix}
q_0 \\
q_1 \\
q_2 \\
q_3
\end{bmatrix}
=
\begin{bmatrix}
\cos \frac{\theta}{2} \\
x*\sin \frac{\theta}{2} \\
y*\sin \frac{\theta}{2} \\
z*\sin \frac{\theta}{2} \\
\end{bmatrix}
\end{aligned}
⎣ ⎢ ⎢ ⎡ q 0 q 1 q 2 q 3 ⎦ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎡ cos 2 θ x ∗ sin 2 θ y ∗ sin 2 θ z ∗ sin 2 θ ⎦ ⎥ ⎥ ⎤
q=q0+q1i+q2j+q3k=[s,v]
∣ q ∣ = q 0 2 + q 1 2 + q 2 2 + q 3 2
\begin{aligned}
|q|=\sqrt{q_{0}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}}
\end{aligned}
∣ q ∣ = q 0 2 + q 1 2 + q 2 2 + q 3 2
主要运算
四元数乘法
乘法性质
满足结合律
不满足交换律
乘积的模等于模的乘积
乘积的逆等于各个四元数的逆以相反的顺序相乘
其他运算
*四元数部分参考：旋转矩阵、欧拉角、四元数理论及其转换关系
在进行解算之前，需要完成十分重要的一步(但是很多资料都没展开描述)，将加速度计、陀螺仪、磁力计对其到右前上坐标系。原因是一些集成的imu中三者坐标系并不一致。
车头朝前
向前加速，y轴加速度输出为+
向右加速，x轴加速度输出为+
向上加速，z轴加速度输出为+
车头朝前
从Z+看向Z-(即从上往下看)，逆时针旋转，Z轴角速度输出为+
从X+看向X-(即从车右侧看)，逆时针旋转(车头上翘)，X轴角速度输出为+
从Y+看向Y-(即从车头看)，逆时针旋转，Y轴角速度输出为+
车头朝北，即Y轴指北
磁力计应该输出形如M a g b = [ 0 M N M D ]
\begin{aligned}
Mag_b=
\begin{bmatrix}
0 \\
M_N \\
M_D
\end{bmatrix}
\end{aligned}
M a g b = ⎣ ⎡ 0 M N M D ⎦ ⎤ M D < 0 ， 北 半 球 M D > 0 ， 北 半 球
\begin{aligned}
M_D <0 ，北半球 \\
M_D >0 ，北半球
\end{aligned}
M D < 0 ， 北 半 球 M D > 0 ， 北 半 球
将车头稍微向西偏
检查此时输出，正确的输出形如(主要看之前为0的那一项的变化)：M a g b = [ > 0 a ∗ M N b ∗ M D ]
\begin{aligned}
Mag_b=
\begin{bmatrix}
>0 \\
a*M_N \\
b*M_D
\end{bmatrix}
\end{aligned}
M a g b = ⎣ ⎡ > 0 a ∗ M N b ∗ M D ⎦ ⎤
如果反向了，则需要添加‘-’号
在进行姿态解算前，需要确定初始的姿态，
使用加速度计确定俯仰和翻滚，pitch 和 roll
使用磁力计确定偏航角 yaw
对载体坐标系下的加速度(加速度计的输出)进行归一化A c c = N o r m ( A c c )
\begin{aligned}
Acc=Norm(Acc)
\end{aligned}
A c c = N o r m ( A c c )
计算俯仰pitchp i t c h = a t a n 2 ( a c c y , a c c x 2 + a c c z 2 )
\begin{aligned}
pitch=atan2(acc_y,\sqrt{acc_x^{2}+acc_z^{2}})
\end{aligned}
p i t c h = a t a n 2 ( a c c y , a c c x 2 + a c c z 2 )
计算翻滚rollr o l l = − a t a n 2 ( a c c x , a c c z )
\begin{aligned}
roll=-atan2(acc_x,acc_z)
\end{aligned}
r o l l = − a t a n 2 ( a c c x , a c c z )
实现代码C++
Vector3d getPoseFrom_Acc(Vector3d _acc){
Vector3d acc=_acc;
acc.normalize();
double norm=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Vector3d result;
result<<0,atan2(acc[1],sqrt(acc[0]*acc[0]+acc[2]*acc[2])),-atan2(acc[0],acc[2]);
return result;
}
确定偏航角需要使用磁力计，使用磁力计之前请先按照官方给定校正说明对磁力计进行校正(另外还需要时不时校正，这个东西太弱了))
我们使用ENU和右前上坐标系，因此，当载体坐标系b系与导航坐标系n系重合时，即车头朝北时，正常输出如下：M a g b = M a g n = [ 0 M N M D ]
\begin{aligned}
Mag_b=Mag_n=
\begin{bmatrix}
0 \\
M_N \\
M_D
\end{bmatrix}
\end{aligned}
M a g b = M a g n = ⎣ ⎡ 0 M N M D ⎦ ⎤
两坐标系不重合时，磁力计输出如下：M a g b 1 = [ m a g x b m a g y b m a g z b ]
\begin{aligned}
Mag_{b1}=
\begin{bmatrix}
mag_x^{b} \\
mag_y^{b} \\
mag_z^{b}
\end{bmatrix}
\end{aligned}
M a g b 1 = ⎣ ⎡ m a g x b m a g y b m a g z b ⎦ ⎤
假定有从导航坐标系n系到载体坐标系b系的方向余弦矩阵Cnb，那么将此时磁力计输出变换到导航坐标系下，得到:M a g b 1 = C n b M a g n = C n b [ 0 M N M D ] = [ m a g x b m a g y b m a g z b ] = [ M N ( sin ψ cos ϕ + cos ψ sin θ sin ϕ ) − M D cos θ sin ϕ M N cos ψ cos θ + M D sin θ M N ( sin ψ sin ϕ − cos ψ cos ϕ sin θ ) + M D cos θ cos ϕ ]
\begin{aligned}
Mag_{b1}=C_n^{b}Mag_n=C_n^{b}
\begin{bmatrix}
0 \\
M_N \\
M_D
\end{bmatrix}=
\begin{bmatrix}
mag_x^{b} \\
mag_y^{b} \\
mag_z^{b}
\end{bmatrix}
=&
\begin{bmatrix}
M_N (\sin \psi \cos \phi + \cos \psi \sin \theta \sin \phi)-M_D \cos \theta \sin \phi \\
M_N \cos \psi \cos \theta +M_D \sin \theta \\
M_N (\sin \psi \sin \phi- \cos \psi \cos \phi \sin \theta)+M_D \cos \theta \cos \phi
\end{bmatrix}
\end{aligned}
M a g b 1 = C n b M a g n = C n b ⎣ ⎡ 0 M N M D ⎦ ⎤ = ⎣ ⎡ m a g x b m a g y b m a g z b ⎦ ⎤ = ⎣ ⎡ M N ( sin ψ cos ϕ + cos ψ sin θ sin ϕ ) − M D cos θ sin ϕ M N cos ψ cos θ + M D sin θ M N ( sin ψ sin ϕ − cos ψ cos ϕ sin θ ) + M D cos θ cos ϕ ⎦ ⎤ m y = M N sin ψ = m a g x b cos ϕ + m a g z b sin ϕ m x = M N cos ψ = m a g x b sin ϕ sin θ + m a g y b cos θ − m a g z b cos ϕ sin θ
\begin{aligned}
my=M_N\sin \psi=&mag_x^{b}\cos \phi+ mag_z^{b}\sin \phi \\
mx=M_N\cos \psi=&mag_x^{b}\sin \phi \sin \theta+ mag_y^{b}\cos \theta -mag_z^{b}\cos \phi \sin \theta
\end{aligned}
m y = M N sin ψ = m x = M N cos ψ = m a g x b cos ϕ + m a g z b sin ϕ m a g x b sin ϕ sin θ + m a g y b cos θ − m a g z b cos ϕ sin θ Y a w = ψ = a t a n 2 ( m y , m x )
\begin{aligned}
Yaw=\psi=atan2(my,mx)
\end{aligned}
Y a w = ψ = a t a n 2 ( m y , m x )
数据准备
取初始姿态Yaw,pitch,roll
利用euler_312_To_quaternion()函数，得到初始姿态对应的四元数q
利用C_nb_312_from_qnb()函数，得到初始姿态对应的旋转矩阵Cnb
利用磁力计校正Yaw偏差计算
取当前b系下磁力计输出,并归一化M a g = N o r m ( M a g ) Mag=Norm(Mag) M a g = N o r m ( M a g )
将b系的磁力向量转换到n系下M a g R e f = ( C n b ) T M a g
MagRef=(C_{n}^{b})^TMag
M a g R e f = ( C n b ) T M a g
构建基于b系磁力分量和姿态矩阵的理想磁力计输出M a g R e f _ = [ 0 M a g R e f [ 0 ] 2 + M a g R e f [ 1 ] 2 M a g R e f [ 2 ] ] M a g D i r _ h a t = 1 2 C n b M a g R e f _
\begin{aligned}
MagRef\_= &
\begin{bmatrix}
0 \\
\sqrt{MagRef[0]^2+MagRef[1]^2} \\
MagRef[2]
\end{bmatrix} \\
MagDir\_hat=& \frac{1}{2}C_{n}^{b}MagRef\_
\end{aligned}
M a g R e f _ = M a g D i r _ h a t = ⎣ ⎡ 0 M a g R e f [ 0 ] 2 + M a g R e f [ 1 ] 2 M a g R e f [ 2 ] ⎦ ⎤ 2 1 C n b M a g R e f _
利用叉乘计算实际与理想的偏差M a g _ e r r = M a g × M a g D i r _ h a t
Mag\_err=Mag \times MagDir\_hat
M a g _ e r r = M a g × M a g D i r _ h a t
设置磁力分量偏差的Kp和Ki值
计算磁力分量误差e r r p = k p y a w ∗ M a g _ e r r e r r i = k i y a w ∗ Δ T ∗ M a g _ e r r
\begin{aligned}
err_p=&kp_{yaw}*Mag\_err \\
err_i=&ki_{yaw}*\Delta T*Mag\_err
\end{aligned}
e r r p = e r r i = k p y a w ∗ M a g _ e r r k i y a w ∗ Δ T ∗ M a g _ e r r
利用加速度计校正pitch、roll偏差计算
取当前b系下加速度计输出,并归一化A c c = N o r m ( A c c ) Acc=Norm(Acc) A c c = N o r m ( A c c )
由于标准n系下的加速度计输出应该只有重力分量，即归一化后的理想n系加速度为:A c c n = [ 0 0 1 ] A c c _ h a t = C n b A c c n
\begin{aligned}
Acc_n=
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix} \\
Acc\_hat=C_{n}^{b}Acc_n
\end{aligned}
A c c n = ⎣ ⎡ 0 0 1 ⎦ ⎤ A c c _ h a t = C n b A c c n
利用叉乘计算实际与理想的偏差A c c _ e r r = A c c × A c c _ h a t
Acc\_err=Acc \times Acc\_hat
A c c _ e r r = A c c × A c c _ h a t
设置pitch\roll偏差的Kp和Ki值
计算pitch\roll偏差e r r p = e r r p + k p r o l l p i t c h ∗ A c c _ e r r e r r i = e r r i + k i r o l l p i t c h ∗ Δ T ∗ A c c _ e r r
\begin{aligned}
err_p=&err_p+kp_{rollpitch}*Acc\_err \\
err_i=&err_i+ki_{rollpitch}*\Delta T*Acc\_err
\end{aligned}
e r r p = e r r i = e r r p + k p r o l l p i t c h ∗ A c c _ e r r e r r i + k i r o l l p i t c h ∗ Δ T ∗ A c c _ e r r
对陀螺仪输出角速度进行偏差修正 w = g y r o = [ g y r o x g y r o y g y r o z ] + [ e r r p [ 0 ] e r r p [ 1 ] e r r p [ 2 ] ] + [ e r r i [ 0 ] e r r i [ 1 ] e r r i [ 2 ] ]
\begin{aligned}
w=gyro=
\begin{bmatrix}
gyro_x \\
gyro_y \\
gyro_z
\end{bmatrix}
+
\begin{bmatrix}
err_p[0] \\
err_p[1] \\
err_p[2]
\end{bmatrix}
+
\begin{bmatrix}
err_i[0] \\
err_i[1] \\
err_i[2]
\end{bmatrix}
\end{aligned}
w = g y r o = ⎣ ⎡ g y r o x g y r o y g y r o z ⎦ ⎤ + ⎣ ⎡ e r r p [ 0 ] e r r p [ 1 ] e r r p [ 2 ] ⎦ ⎤ + ⎣ ⎡ e r r i [ 0 ] e r r i [ 1 ] e r r i [ 2 ] ⎦ ⎤
角速度积分，得到角度较小变化量w Δ θ = g y r o ∗ Δ T
\Delta \theta=gyro*\Delta T
Δ θ = g y r o ∗ Δ T
解四元数微分方程
四元数微分方程如下：Q ˙ ( t ) = 1 2 M w ( t ) ′ Q ( t )
\begin{aligned}
\dot{Q}(t)=\frac{1}{2}M'_{w(t)}Q(t)
\end{aligned}
Q ˙ ( t ) = 2 1 M w ( t ) ′ Q ( t )
闭合解为：Q ( t + 1 ) = e 1 2 Δ Θ Q ( t )
\begin{aligned}
Q(t+1)=e^{\frac{1}{2}\Delta\Theta }Q(t)
\end{aligned}
Q ( t + 1 ) = e 2 1 Δ Θ Q ( t ) Δ Θ = ∫ t k t k + 1 [ 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 ] d t ≈ [ 0 − Δ θ x − Δ θ y − Δ θ z Δ θ x 0 Δ θ z − Δ θ y Δ θ y − Δ θ z 0 Δ θ x Δ θ z Δ θ y − Δ θ x 0 ]
\begin{aligned}
\Delta\Theta=\int_{t_k}^{t_k+1}
\begin{bmatrix}
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 \\
\end{bmatrix}
dt
\approx
\begin{bmatrix}
0 &-\Delta\theta_x &-\Delta\theta_y &-\Delta\theta_z \\
\Delta\theta_x &0 &\Delta\theta_z &-\Delta\theta_{y} \\
\Delta\theta_y &-\Delta\theta_z &0 &\Delta\theta_x \\
\Delta\theta_z &\Delta\theta_y &-\Delta\theta_x &0 \\
\end{bmatrix}
\end{aligned}
Δ Θ = ∫ t k t k + 1 ⎣ ⎢ ⎢ ⎡ 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 ⎦ ⎥ ⎥ ⎤ d t ≈ ⎣ ⎢ ⎢ ⎡ 0 Δ θ x Δ θ y Δ θ z − Δ θ x 0 − Δ θ z Δ θ y − Δ θ y Δ θ z 0 − Δ θ x − Δ θ z − Δ θ y Δ θ x 0 ⎦ ⎥ ⎥ ⎤
为了减少运算量，通常采用近似解
毕卡近似解如下：Δ θ 2 = Δ θ x 2 + Δ θ y 2 + Δ θ z 2
\Delta \theta^2=\Delta \theta_x^{2}+\Delta \theta_y^{2}+\Delta \theta_z^{2}
Δ θ 2 = Δ θ x 2 + Δ θ y 2 + Δ θ z 2
代码如下
void mahony(double currT){
//初始化
static double deltaT_prev=0;
static bool init=false;
if(!init){
deltaT_prev=currT;
init=true;
return ;
}
//**数据准备**
Quaterniond q=curr_pose;//euler2quaternion(align_pose);
Matrix3d Cnb=C_nb_312_from_qnb(q);
Cnb.normalize();
//**对陀螺仪输出角速度进行偏差修正**
Vector3d mag=mag_dev.mag;
mag.normalize();
Vector3d magRef=Cnb.transpose()*mag; //得到n系下的磁力计分量
Vector3d magRef_(0,sqrt(magRef[0]*magRef[0]+magRef[1]*magRef[1]),magRef[2]);
//估计磁场方向
Vector3d magDir_hat=0.5*Cnb*magRef_;
//mag 与 方向进行叉积
Vector3d mag_cross_magDir=mag.cross(magDir_hat);
//计算偏差
Vector3d err_p(0,0,0);
Vector3d err_i(0,0,0);
double kp_yaw=1.2;
double ki_yaw=0.0002;
err_p=err_p+kp_yaw*mag_cross_magDir;
err_i=err_i+ki_yaw*(currT-deltaT_prev)*mag_cross_magDir;
//**利用加速度计校正pitch、roll偏差计算**
Vector3d acc;
acc=imu_dev.acc;
acc.normalize();
Vector3d vv=Cnb*Vector3d(0,0,1);
Vector3d acc_cross_vv=acc.cross(vv);
double kp_rollpitch=1.2;
double ki_rollpitch=0.0002;
err_p=err_p+kp_rollpitch*acc_cross_vv;
err_i=err_i+ki_rollpitch*(currT-deltaT_prev)*acc_cross_vv;
//**对陀螺仪输出角速度进行偏差修正**
Vector3d gyro=imu_dev.gyro;
gyro=gyro+err_p+err_i;
//**角速度积分，得到角度较小变化**
if((currT-deltaT_prev)>0){
gyro=gyro*(currT-deltaT_prev);
}
else
return;
//**解四元数微分方程**
Matrix4d gyro_mat;
gyro_mat<< 0, -gyro[0], -gyro[1], -gyro[2],
gyro[0], 0, gyro[2], -gyro[1],
gyro[1], -gyro[2], 0, gyro[0],
gyro[2], gyro[1], -gyro[0], 0;
double gyro_L2=sqrt(gyro[0]*gyro[0]+gyro[1]*gyro[1]+gyro[2]*gyro[2]);
//**毕卡解法**
Vector4d q_curr;
q_curr<<q.w(),q.x(),q.y(),q.z();
Vector4d q_new=((1-gyro_L2*gyro_L2/8)*Matrix4d::Identity()+(0.5-gyro_L2*gyro_L2/48)*gyro_mat)*q_curr;
Quaterniond output;
output.w()=q_new[0];
output.x()=q_new[1];
output.y()=q_new[2];
output.z()=q_new[3];
output.normalize();
curr_pose=output;
deltaT_prev=currT;
}
基于DMC的解法基本思路与Mahony解法相似，这里不赘述，有兴趣的同学可以参考Razor_AHRS的算法
传送门：https://github.com/KristofRobot/razor_imu_9dof
这里不展开卡尔曼、扩展卡尔曼的内容，只是把它当作工具使用
概览部分参考自：四旋翼姿态解算–互补滤波和拓展卡尔曼
X 12 x 1 = [ g y r o ^ 3 x 1 g y r o ˙ ^ 3 x 1 A c c ^ 3 x 1 b M a g ^ 3 x 1 b ] [ b 系 三 轴 角 速 度 估 计 角 加 速 度 估 计 b 系 加 速 度 估 计 b 系 磁 力 分 量 估 计 ]
\begin{aligned}
X_{12x1}=
\begin{bmatrix}
\hat{gyro}_{3x1} \\
\hat{\dot{gyro}}_{3x1} \\
\hat{Acc}_{3x1}^{b} \\
\hat{Mag}_{3x1}^{b}
\end{bmatrix}
\begin{bmatrix}
b系三轴角速度估计 \\
角加速度估计 \\
b系加速度估计 \\
b系磁力分量估计
\end{bmatrix}
\end{aligned}
X 1 2 x 1 = ⎣ ⎢ ⎢ ⎢ ⎡ g y r o ^ 3 x 1 g y r o ˙ ^ 3 x 1 A c c ^ 3 x 1 b M a g ^ 3 x 1 b ⎦ ⎥ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎡ b 系 三 轴 角 速 度 估 计 角 加 速 度 估 计 b 系 加 速 度 估 计 b 系 磁 力 分 量 估 计 ⎦ ⎥ ⎥ ⎤
Z 9 x 1 = [ g y r o 3 x 1 A c c 3 x 1 b M a g 3 x 1 b ] [ b 系 三 轴 角 速 度 测 量 b 系 加 速 度 测 量 b 系 磁 力 分 量 测 量 ]
\begin{aligned}
Z_{9x1}=
\begin{bmatrix}
gyro_{3x1} \\
Acc_{3x1}^{b} \\
Mag_{3x1}^{b}
\end{bmatrix}
\begin{bmatrix}
b系三轴角速度测量 \\
b系加速度测量 \\
b系磁力分量测量
\end{bmatrix}
\end{aligned}
Z 9 x 1 = ⎣ ⎡ g y r o 3 x 1 A c c 3 x 1 b M a g 3 x 1 b ⎦ ⎤ ⎣ ⎡ b 系 三 轴 角 速 度 测 量 b 系 加 速 度 测 量 b 系 磁 力 分 量 测 量 ⎦ ⎤
非线性状态转移（预测）
EKF线性化x k ^ \hat{x_{k}} x k ^ A l i n , k = ∂ f ( x k , w k ) ∂ x k = [ I 3 x 3 I 3 x 3 Δ T 0 0 0 I 3 x 3 0 0 − r a c c Δ T 0 I 3 x 3 + r g y r o Δ T 0 − r m a g Δ T 0 0 I 3 x 3 + r g y r o Δ T ]
\begin{aligned}
A_{lin,k}=\frac{\partial f(x_{k},w_{k})}{\partial x_{k}}=
\begin{bmatrix}
I_{3x3} &I_{3x3}\Delta T & 0 & 0 \\
0 &I_{3x3} &0 &0 \\
-r_{acc}\Delta T &0 &I_{3x3}+r_{gyro}\Delta T &0 \\
-r_{mag}\Delta T &0 &0 &I_{3x3}+r_{gyro}\Delta T
\end{bmatrix}
\end{aligned}
A l i n , k = ∂ x k ∂ f ( x k , w k ) = ⎣ ⎢ ⎢ ⎡ I 3 x 3 0 − r a c c Δ T − r m a g Δ T I 3 x 3 Δ T I 3 x 3 0 0 0 0 I 3 x 3 + r g y r o Δ T 0 0 0 0 I 3 x 3 + r g y r o Δ T ⎦ ⎥ ⎥ ⎤ r a c c r_{acc} r a c c r m a g r_{mag} r m a g r g y r o r_{gyro} r g y r o
计算协方差矩阵估计P ^ = A l i n , k P A l i n , k T + Q
\begin{aligned}
\hat{P}=A_{lin,k}PA_{lin,k}^{T}+Q
\end{aligned}
P ^ = A l i n , k P A l i n , k T + Q
观测矩阵H = [ I 3 x 3 0 0 0 0 0 I 3 x 3 0 0 0 0 I 3 x 3 ]
\begin{aligned}
H=
\begin{bmatrix}
I_{3x3} & 0 &0 &0 \\
0 &0 &I_{3x3} &0\\
0 &0 &0 &I_{3x3}
\end{bmatrix}
\end{aligned}
H = ⎣ ⎡ I 3 x 3 0 0 0 0 0 0 I 3 x 3 0 0 0 I 3 x 3 ⎦ ⎤
观测Y = Z − H X ^
\begin{aligned}
Y=Z-H\hat{X}
\end{aligned}
Y = Z − H X ^
计算卡尔曼增益K = P ^ H T ( H P ^ H T + R k ) − 1
K=\hat{P}H^{T}(H\hat{P}H^{T}+R_{k})^{-1}
K = P ^ H T ( H P ^ H T + R k ) − 1
更新状态变量估计X = X ^ + K Y
X=\hat{X}+KY
X = X ^ + K Y
更新协方差矩阵P = ( I − K H ) P ^
P=(I-KH)\hat{P}
P = ( I − K H ) P ^
代码：待更新
