RT-Thread 互补滤波器 (STM32 + 6 轴 IMU)

作者：wuhanstudio

原文链接：https://zhuanlan.zhihu.com/p/611568999

最近在看无人驾驶的 Prediction 部分，可以利用 EKF (Extended Kalman Filter) 融合不同传感器的数据，例如 IMU, Lidar 和 GNSS，从而给出更加准确的状态预测。

刚好手边开发板有一个 6 轴的 IMU，本来打算试一下 卡尔曼滤波器 (Kalman Filter)，然而 Kalman Filter 更适合 9 轴的传感器，也就是在 6 轴的基础上（3-axis Accel + 3-axis Gyro）融合 3 轴的磁力计。

对于一个只有 6 轴 IMU 的 MCU，轻量级的 互补滤波器 (Complementary Filter) 更加合适，利用 3 轴陀螺仪和 3 轴加速度计来估计开发板的姿态 (Pitch, Roll, Yaw)。

大致流程：首先用 RT-Thread 的 icm20608 软件包读取 陀螺仪 (Gyroscope) 和 加速度计 (Accelerometer) 的数据，分别计算出估计的角度，再用互补滤波器 (Complementary Filter) 融合两个角度估计、进行校正，其实核心算法的代码就 7 行。最后串口把数据发到电脑上，用 Python + OpenGL 可视化。

Github - STM32 IMU 互补滤波器 (RT-Thread)：https://github.com/wuhanstudio/stm32-imu-filter

IMU 传感器 (Inertial Measurement Unit)

我们先介绍下从 I2C 总线读取出传感器原始数值后，如何处理得到加速度和旋转角速度。

一个六轴的 IMU 可以测量 x, y, z 三个方向的重力加速度，和绕三个轴的旋转角速度。比如，开发板如果静止放置在桌面上，会测量到 z 方向的重力加速度。