1. 三軸地磁資料歸一化
$\left\
norm = \sqrt + m + m} \\
mx = \frac}}\\
my = \frac}}\\
mz = \frac}}
\end \right.$
2. 計算參考方向
用歸一化的資料右乘四元素方向矩陣得到參考方向
$\left[ \begin
hx\\
hy\\
hz\end \right] = \left[ }
& - )}& + )}\\
+ )}&& - )}\\
- )}& + )}&
\end} \right]}\left[ \begin
}\\}\\
}\end \right]$
讓地磁x軸對準北方，則地磁在此座標系下應為$\left[ }&0&\end} \right]$，參考方向要與此座標系下方向一致，則有
$\begin
b = h + h\\
bz = hz
\end$
得到估計方向
$\left[ \begin
wx\\
wy\\
wz\end \right] = \left[ }
&0&\end} \right]}\left[ }
& - )}& + )}\\
+ )}&& - )}\\
- )}& + )}&
\end} \right]}$
3. 用估計方向與三軸方向進行叉乘求取方向誤差
$\begin
exmag = my * wz - mz*wy\\
eymag = mz * wx - mx*wz\\
ezmag = mx * wy - my*wx
\end$
4. 對誤差進行積分比例運算，更新陀螺儀角速度值
餘下步驟請參考姿態解算解析
融合磁力計的Mahony互補濾波演算法
上面部落格有關於磁力計的詳細解釋，不過由於本人資質愚鈍，至今還不是完全理解,不過思想大致和加速度計差不多。結合下面 看效果更好 大致意思如下 加速度計 已知地理座標系的標準重力加速度 0 0 1 然後變換到機體座標系，理論與實際叉乘求出誤差 磁力計 由於標準重力加速度在所有地方都是一樣的，可以由其直...
四元素還是向量？
四元素是描述運動物體姿態的終極 它是由威廉 哈密頓 william rowan hamilton 爵士1843年在愛爾蘭發現的。會中文的數學家 哈密頓生於愛爾蘭，他在十三歲前都受其叔父語言學家詹姆斯照顧。哈密頓很喜歡文學，在大學期間，他不但修讀數學，還有修讀經典文學。因此，他總共精通十二種語言。除了...
四元素還是向量？
ywlj 四元素是描述運動物體姿態的終極 它是由威廉 哈密頓 william rowan hamilton 爵士1843年在愛爾蘭發現的。會中文的數學家 哈密頓生於愛爾蘭，他在十三歲前都受其叔父語言學家詹姆斯照顧。哈密頓很喜歡文學，在大學期間，他不但修讀數學，還有修讀經典文學。因此，他總共精通十二種...