姿态解算是飞行控制器的核心算法之一，其任务是将IMU传感器（加速度计和陀螺仪）的原始数据融合处理，得到准确、稳定的飞行器姿态估计。本章将介绍从数据预处理到Mahony滤波算法的完整实现。

13.1 姿态解算各模块代码实现

前面我们是根据需求一步一步反推出来需要的数据，接下来我们根据如图的步骤进行程序的编写

1. 重力加速度归一化

c

/* Step 1: normalize accelerometer vector. */

norm = InvSqrt(ax * ax + ay * ay + az * az);

ax *= norm;

ay *= norm;

az *= norm;

2. 重力分量提取

c

/* Step 2: estimate gravity direction from current quaternion. */

vx = 2.0f * (q1 * q3 - q0 * q2);

vy = 2.0f * (q0 * q1 + q2 * q3);

vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

3. 求出姿态误差

c

/* Step 3: cross product gives attitude error between measured and estimated gravity. */

ex = ay * vz - az * vy;

ey = az * vx - ax * vz;

ez = ax * vy - ay * vx;

4. 误差积分

c

/* Step 4: PI feedback corrects gyro drift. */

integral_ex += MAHONY_KI * ex * dt;

integral_ey += MAHONY_KI * ey * dt;

integral_ez += MAHONY_KI * ez * dt;

5. 互补滤波

c

gx += MAHONY_KP * ex + integral_ex;

gy += MAHONY_KP * ey + integral_ey;

gz += MAHONY_KP * ez + integral_ez;

6. 解四元数微分方程

c

/* Step 5: integrate quaternion differential equation. */

q->q0 += (-q1 * gx - q2 * gy - q3 * gz) * half_dt;

q->q1 += ( q0 * gx + q2 * gz - q3 * gy) * half_dt;

q->q2 += ( q0 * gy - q1 * gz + q3 * gx) * half_dt;

q->q3 += ( q0 * gz + q1 * gy - q2 * gx) * half_dt;

7. 四元数归一化

c

/* Step 6: normalize quaternion. */

norm = InvSqrt(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);

q->q0 *= norm;

q->q1 *= norm;

q->q2 *= norm;

q->q3 *= norm;

8. 四元数求欧拉角

c

float sinr_cosp = 2.0f * (q->q0 * q->q1 + q->q2 * q->q3);

float cosr_cosp = 1.0f - 2.0f * (q->q1 * q->q1 + q->q2 * q->q2);

float sinp = 2.0f * (q->q0 * q->q2 - q->q3 * q->q1);

float siny_cosp = 2.0f * (q->q0 * q->q3 + q->q1 * q->q2);

float cosy_cosp = 1.0f - 2.0f * (q->q2 * q->q2 + q->q3 * q->q3);

angle->roll = atan2f(sinr_cosp, cosr_cosp) * RAD_TO_DEG;

if(sinp > 1.0f) {

angle->pitch = 90.0f;

} else if(sinp < -1.0f) {

angle->pitch = -90.0f;

} else {

angle->pitch = asinf(sinp) * RAD_TO_DEG;

}

angle->yaw = atan2f(siny_cosp, cosy_cosp) * RAD_TO_DEG;

技巧 如果后续需要添加磁力计来改善偏航稳定性，推荐使用HMC5883L或QMC5883L等I2C接口磁力计，安装位置尽量远离电机，并使用硬铁和软铁校准算法消除干扰。

详细推导链接可以参考：【【深度教学】姿态解算理论及应用（持续更新）】https://www.bilibili.com/video/BV1gS4y1771h?vd_source=25a9a74c1b7b11015f9943b60f3dd1db

---

学习资源

🎬 B站系列视频教程：手把手教你从零组装无人机

📦 百度网盘资料包（SDK、教材PDF、完整源码）：点击下载 提取码: JZS8

---

本系列持续更新，欢迎收藏关注。如有问题欢迎在评论区交流。