磁力计四元素融合

1. 三轴地磁数据归一化

$\left\{ \begin{array}{l}
norm = \sqrt {m{x^2} + m{y^2} + m{z^2}} \\
mx = \frac{{mx}}{{norm}}\\
my = \frac{{my}}{{norm}}\\
mz = \frac{{mz}}{{norm}}
\end{array} \right.$

 

2. 计算参考方向

用归一化的数据右乘四元素方向矩阵得到参考方向 

$\left[ \begin{array}{l}
hx\\
hy\\
hz
\end{array} \right] = \left[ {\begin{array}{*{20}{c}}
{q_0^2 + q_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})}&{q_0^2 - q_1^2 + q_2^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})}&{q_0^2 - q_1^2 - q_2^2 + q_3^2}
\end{array}} \right]{\rm{ }}\left[ \begin{array}{l}
{\rm{mx}}\\
{\rm{my}}\\
{\rm{mz}}
\end{array} \right]$

让地磁X轴对准北方，则地磁在此坐标系下应为$\left[ {\begin{array}{*{20}{c}}{bx}&0&{bz}\end{array}} \right]$，参考方向要与此坐标系下方向一致，则有

$\begin{array}{l}
b{x^2} = h{x^2} + h{y^2}\\
bz = hz
\end{array}$

得到估计方向

$\left[ \begin{array}{l}
wx\\
wy\\
wz
\end{array} \right] = \left[ {\begin{array}{*{20}{c}}
{bx}&0&{bz}
\end{array}} \right]{\rm{ }}\left[ {\begin{array}{*{20}{c}}
{q_0^2 + q_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})}&{q_0^2 - q_1^2 + q_2^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})}&{q_0^2 - q_1^2 - q_2^2 + q_3^2}
\end{array}} \right]{\rm{ }}$

 

3. 用估计方向与三轴方向进行叉乘求取方向误差

$\begin{array}{l}
exMag = my * wz - mz*wy\\
eyMag = mz * wx - mx*wz\\
ezMag = mx * wy - my*wx
\end{array}$

 

4. 对误差进行积分比例运算，更新陀螺仪角速度值

余下步骤请参考姿态解算解析