嵌入式实战：基于STM32的MPU6050卡尔曼滤波姿态解算（附源码）

1. MPU6050传感器与姿态解算基础

MPU6050这款六轴传感器在嵌入式圈子里可以说是老熟人了，它集成了三轴加速度计和三轴陀螺仪，能同时测量线性加速度和角速度。我最早接触这个传感器是在做四轴飞行器项目时，当时为了获取稳定的飞行姿态数据，没少和它打交道。

传感器输出的原始数据其实挺"脏"的。加速度计在静态时很准，但一动起来就受运动加速度干扰；陀螺仪短期响应快，但积分会累积误差。这就好比用两种有缺陷的尺子量东西——一把尺子静止时准但动起来会变形，另一把尺子量得快但会越量越歪。姿态解算的核心就是要融合这两把"尺子"的测量结果。

在实际项目中，我试过好几种融合算法。互补滤波简单但效果一般，Mahony滤波不错但对参数敏感，最后发现还是卡尔曼滤波最稳。不过很多新手看到卡尔曼滤波的数学公式就头大，其实在嵌入式系统里我们用简化版就足够了。

2. 硬件连接与电路设计

2.1 STM32与MPU6050的硬件对接

我用的是STM32F103C8T6最小系统板，和MPU6050模块连接特别简单，就四根线：

• VCC接3.3V（注意MPU6050是3.3V器件）
• GND接地
• SDA接PB7
• SCL接PB6

这里有个坑要注意：如果STM32用的是5V供电，记得加电平转换，我当初直接连把MPU6050烧了。后来学乖了，要么用3.3V供电的STM32，要么加个电平转换模块。

上拉电阻也很关键，一般用4.7kΩ就行。有一次调试时数据老是丢包，后来发现是上拉电阻没焊好。建议直接用开发板上的I2C接口，省事。

2.2 电源与抗干扰设计

MPU6050对电源噪声特别敏感。实测下来，最好在VCC脚加个0.1μF的陶瓷电容。如果要做PCB，记得把MPU6050尽量远离数字电路，我的经验是至少留出5mm间距。

有一次项目出现角度漂移，折腾好久才发现是电机干扰。后来在电源端加了LC滤波就好多了。所以如果系统里有大功率器件，一定要做好电源隔离。

3. 卡尔曼滤波算法实现

3.1 简化版卡尔曼滤波原理

完整的卡尔曼滤波有预测和更新两个阶段，但在STM32上我们可以用简化版。核心思想就是：

1. 用陀螺仪积分预测角度
2. 用加速度计测量来修正预测
3. 根据两者可信度动态调整权重

这就好比在陌生地方走路：陀螺仪是你的步伐计数（会累积误差），加速度计是偶尔看到的路标（可能有偏差）。卡尔曼滤波就是聪明地结合这两种信息。

3.2 代码实现关键点

先定义卡尔曼结构体：

typedef struct {
    float Q_angle;   // 过程噪声协方差
    float