姿態解算是飛控的一個基礎、重要部分，估計出來的姿態會發布給姿態控制器，控制飛行平穩，是飛行穩定的最重要保障。另外，姿態解算不僅僅用於無人機領域，無人車領域也需要進行姿態解算，用以進行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 ^
代碼：待更新
