互补滤波算法是一种常用于传感器数据融合的算法，特别适合在STM32等嵌入式平台上实现IMU（惯性测量单元）的姿态解算。以下是关于互补滤波在STM32上的实现要点：

1. 互补滤波原理

• 核心思想：结合加速度计（低频信号稳定）和陀螺仪（高频信号动态响应快）的优势，通过加权融合消除噪声。
• 公式：

  角度 = α * (上一角度 + 陀螺仪角速度 * dt) + (1 - α) * 加速度计角度  

  其中 α 是滤波系数（0~1），dt 为采样时间间隔。

2. STM32实现步骤

(1) 硬件准备

• 使用MPU6050、BMI160等IMU传感器，通过I2C或SPI与STM32通信。
• 确保传感器已校准（消除零偏和比例误差）。

(2) 数据采集

// 示例：读取陀螺仪和加速度计原始数据
int16_t gyro_raw[3], accel_raw[3];
MPU6050_Read_Gyro(gyro_raw);  // 陀螺仪角速度（°/s）
MPU6050_Read_Accel(accel_raw); // 加速度计原始数据

(3) 角度计算

• 加速度计角度（俯仰角/横滚角）：

  float accel_angle_pitch = atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ)) * 180/PI;
  float accel_angle_roll = atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/PI;

• 陀螺仪角度：通过积分角速度得到

  gyro_angle_pitch += gyroY * dt;  // dt为采样时间（如0.01s）

(4) 互补滤波融合

#define ALPHA 0.98  // 系数需根据实际调试调整

// 融合计算
float pitch = ALPHA * (prev_pitch + gyroY * dt) + (1 - ALPHA) * accel_angle_pitch;
prev_pitch = pitch;  // 更新历史角度

3. 关键注意事项

1. 采样时间 dt：需通过定时器精确控制（如10ms），推荐使用STM32的硬件定时器中断。
2. 传感器校准：
   • 陀螺仪零偏校准：静止时采集多组数据求平均偏移量。
   • 加速度校准：水平放置时验证重力轴数据。
3. 坐标系对齐：确保加速度计和陀螺仪的X/Y/Z轴方向一致。
4. 浮点优化：若使用无FPU的STM32型号（如Cortex-M3），建议改用定点数运算或查表法加速。

4. 调试技巧

• 通过串口输出原始数据和滤波后的角度，使用上位机（如Serial Plotter、MATLAB）可视化波形。
• 动态调整 α：
  • α 越大，陀螺仪权重越高（适合动态场景，但存在积分漂移）；
  • α 越小，加速度计权重越高（适合静态场景，抗噪声差）。

5. 扩展优化

• 结合磁力计（电子罗盘）实现三轴姿态融合（Yaw角补偿）。
• 对于复杂运动，可升级为卡尔曼滤波或Mahony滤波算法。

互补滤波在STM32上资源占用低，适合实时性要求高的场景（如四轴飞行器平衡控制）。实际代码需根据具体传感器和硬件平台适配。