磁力计和加速度计初始姿态解算

磁力计和加速度计初始姿态解算

四旋翼的初始姿态是通过磁力计和加速度计来解算的，缺一不可。学习过程中遇到了不少问题，想通了总结如下：

• 初始姿态解算原理
• 牛顿迭代法
• 迭代初值选取
• 万向节锁
• C++代码示例

初始姿态解算原理

导航坐标系选为东北天，则重力加速度和磁力计在导航坐标系中分别表示为$[0,0,-g]^T$，$[0,m_y^n,m_z^n]^T$。假设机身坐标系下，加速度计和磁力计输出分别为$[a_x,a_y,a_z]^T$，$[m_x^b,m_y^b,m_z^b]^T$。若飞机加速度为0，且加速度计安装位置在飞机重心（原因打算后面的博客再写），则：

$$\begin{equation} \left[ \begin{array}{ccc} a_x\\a_y\\a_z \end{array} \right]= R_n^b \left[ \begin{array}{ccc} 0\\0\\-g \end{array} \right](1) \end{equation}$$
$$\begin{equation} \left[ \begin{array}{ccc} 0\\m_y^n\\m_z^n \end{array} \right]= R_b^n \left[ \begin{array}{ccc} m_x\\m_y\\m_z \end{array} \right](2) \end{equation}$$
其中， $R_b^n=(R_n^b)^T$。 $R_n^b$可以用四元数表示形式为：

⎡⎣⎢⎢q20+q21−q22−q232(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q