用四元素法進行姿態解算，其步驟如下：
1. 四元素初始化（第一次解算時計算）
①靜止狀態下由加速度，磁力計值求取初始$}\gamma $、$}\theta $、$}\psi $：
$\gamma = }\frac}}}}}}$
$\theta = }\frac}}}}}}$
$\psi = }\frac}}}}}}$
②初始化四元素
$\left[ \begin
}_}}\\
\\\\
\end \right] = \left[ \begin
\cos \frac\cos \frac\cos \frac + \sin \frac\sin \frac\sin \frac\\
\sin \frac\cos \frac\cos \frac - \cos \frac\sin \frac\sin \frac\\
\cos \frac\sin \frac\cos \frac + \sin \frac\cos \frac\sin \frac\\
\cos \frac\cos \frac\sin \frac - \sin \frac\sin \frac\cos \frac
\end \right]$
例程：
void margahrsinit(float ax, float ay, float az, float mx, float my, float mz)2. 加速度歸一化
$\left\
norm = \sqrt + a + a} \\
ax = \frac}}\\
ay = \frac}}\\
az = \frac}}
\end \right.$
3. 提取四元素等效余弦矩陣中的重力分量
四元素等效矩陣
$m = \left[ }
& - )}& + )}\\
+ )}&& - )}\\
- )}& + )}&
\end} \right]$
重力矩陣$\left[ }0&0&1\end} \right]$左乘上面矩陣$m$即可提取出重力分量
$\left\
vx = 2( - )\\
vy = 2( + )\\
vz = 1 - 2(q_1^2 + q_2^2) = q_0^2 - q_1^2 - q_2^2 + q_3^2
\end \right.$
4. 向量叉積求取姿態誤差
$\mathop a\limits^ \to \times \mathop b\limits^ \to = ||a||}||b||}\theta $
向量的叉積可以用來判斷兩個向量是否平行，當兩個向量都為單位向量的時候，它們之間的叉積就代表了它們之間的平行度，若平行則叉積為0，若垂直則叉積為1，兩向量的方向差越小，叉積也越小，因此可以用叉積來表示兩歸一化向量的方向誤差
根據向量的叉乘規則：
若$} = \left[ \begin\\\\\end \right]$，
$b = \left[ \begin
\\\\
\end \right]$，
$c = \left[ \begin
\\\\
\end \right]$；
$a = b \times c$
則$\begin
= - \\
= - \\
= -
\end$
可得$\begin
ex = vy * az - vz*ay\\
ey = vz * ax - vx*az\\
ez = vx * ay - vy*ax
\end$
5. 對誤差進行比例積分運算，更新陀螺儀角速度值
$\begin
exint = exint + ex * ki\\
eyint = eyint + ey * ki\\
ezint = ezint + ez * ki
\end$
$\begin
gx = gx + ex * kp + exint\\
gy = gy + ey * kp + eyint\\
gz = gz + ez * kp + ezint
\end$
6．一階龍格庫塔法求解四元素$\\
\\\\
\end \right)_} =
\\\\
\\\end \right)_t} + \frac}\left( \begin
- . - . - .\\
+ . - . + .\\
+ . + . - .\\
- . + . + .
\end \right)$
$$、$$、$$為經過資料融合後的三軸角速度，本例中即為上一步計算出的$gx$、$gy$、$gz$
因為引入了誤差，使得變換出來的四元素模不再等於1，即失去了規範性，因此在下一步計算前要進行規範化
7．四元素反正切求取尤拉角
反正切解算出$}\gamma $、$}\theta $、$}\psi $：
$\left\
\gamma = }\frac + } \right)}} - - + }}\\
\theta = \arcsin \left[ - )} \right]\\
\psi = \arctan \frac + } \right)}} + - - }}
\end \right.$
例程如下：
磁力計部分請參考磁力計四元素融合
void margahrsupdate(float gx, float gy, float gz,float ax, float ay, float az,
float mx, float my, float mz,
uint8_t magdataupdate, float dt)
//-------------------------------------------
if (margahrsinitialized == true)
}//-------------------------------------------
norm = sqrt(sqr(mx) + sqr(my) + sqr(mz));
if ((magdataupdate == true) && (norm != 0.0f))
}//-------------------------------------------
// integrate quaternion rate
q0i = (-q1 * gx - q2 * gy - q3 * gz) * halft;
q1i = (q0 * gx + q2 * gz - q3 * gy) * halft;
q2i = (q0 * gy - q1 * gz + q3 * gx) * halft;
q3i = (q0 * gz + q1 * gy - q2 * gx) * halft;
q0 += q0i;
q1 += q1i;
q2 += q2i;
q3 += q3i;
// normalise quaternion
normr = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normr;
q1 *= normr;
q2 *= normr;
q3 *= normr;
// auxiliary variables to reduce number of repeated operations
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
sensors.margattitude500hz[roll ] = atan2f(2.0f * (q0q1 + q2q3), q0q0 - q1q1 - q2q2 + q3q3);
sensors.margattitude500hz[pitch] = -asinf(2.0f * (q1q3 - q0q2));
sensors.margattitude500hz[yaw ] = atan2f(2.0f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);}}
LPMS IMU姿態解算
參考文章 ahrs姿態解算說明 加速度 陀螺儀 磁力計原理及原始資料分析 ahrs俗稱航姿參考系統，ahrs由加速度計，磁場計，陀螺儀構成，ahrs的真正參考來自於地球的重力場和地球的磁場 他的靜態終精度取決於對磁場的測量精度和對重力的測量精度 而則陀螺決定了他的動態效能。如何用加速度計和陀螺儀的資...
飛行姿態解算（三）
繼之前研究了一些飛行姿態理論方面的問題後，又找到了之前很流行的一段外國大神寫的 來分析分析。第二篇文章的最後，講到了文章中的演算法在實際使用中有重大缺陷。大家都知道，分析演算法理論的時候很多情況下我們沒有考慮太多外界干擾的情況，原因是很多情況下，感測器的精度以及受到的干擾並不會特別大，而顯著的影響到...
Pixhawk學習6 2 姿態解算
在px4中，採用三軸陀螺，三軸加計，三軸磁力計採用ekf進行姿態解算。現在在examples attitude estimator ekf裡面，在cmake裡面並沒有開啟，nuttx px4fmu v2 default裡面用的是ekf2 該演算法中估計的是機體系下的量。定義機體座標系 前右下 導航座...