28 lines
693 B
Markdown
28 lines
693 B
Markdown
# **Mahony姿态解算**
|
||
|
||
## 一、Mahony算法中载体系的重力投影到地理系
|
||
|
||
常见代码的写法如下图所示：
|
||
|
||
```C
|
||
// 写法1
|
||
float halfvx = q1 * q3 - q0 * q2;
|
||
float halfvy = q0 * q1 + q2 * q3;
|
||
float halfvz = q0 * q0 - 0.5f + q3 * q3;
|
||
// 写法2
|
||
float vx = 2.0f*(q1*q3 - q0*q2);
|
||
float vy = 2.0f*(q0*q1 + q2*q3);
|
||
float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
||
```
|
||
|
||
比较有差异的主要在于vz的写法，这两种的写法其实是一样的，推导如下所示
|
||
|
||
$$
|
||
\begin{align*}
|
||
vz &= q_0^2-0.5+q_3^2 \\
|
||
2vz &= 2q_0^2-1+2q_3^2 \\
|
||
&= 2q_0^2+2q_3^2-q_0^2-q_1^2-q_2^2-q_3^2 \\
|
||
&= q_0^2-q_1^2-q_2^2+q_3^2 \\
|
||
\end{align*}
|
||
$$
|