简介这份资源面向嵌入式开发初学者与从事无人机、机器人、运动设备研发的工程师提供基于STM32平台读取MPU6050六轴惯性测量单元数据并完成姿态解算的完整源码重点解决加速度计动态噪声与陀螺仪积分漂移带来的姿态误差问题。压缩包共129个文件约3.64MB以c与h源码文件为核心配合uvproj、uvopt等Keil工程配置、axf与hex可执行产物、map与lst编译映射文件以及若干bak备份与说明文档覆盖驱动、滤波与工程构建各环节。资源已有5129人学习下载热度较高。源码中可看到inv_mpu、inv_mpu_dmp_motion_driver、MPU6050、IOI2C等模块完整呈现I2C初始化、寄存器读写、原始数据预处理与卡尔曼滤波状态预测、测量更新、协方差更新的实现路径便于读者对照理解传感器融合流程并直接移植到自己的嵌入式项目中调试与验证。1. 从一块 MPU6050 说起为什么你的姿态角总在跳如果你玩过平衡车、四轴飞控或者云台大概率绕不开 MPU6050 这颗六轴传感器。它便宜、好买、资料多但真正把它用起来的人都会遇到同一个问题读出来的加速度计和陀螺仪数据直接算角度要么抖得没法看要么漂得离谱。加速度计静态准但动态噪声大陀螺仪动态好但积分会累积漂移单靠任何一个都做不出能用的姿态解算。这份 MPU6050 姿态解算 STM32 源码核心就是用卡尔曼滤波把两路数据融合起来输出稳定的俯仰角、横滚角和偏航角。代码基于 STM32 标准外设库I2C 读取原始数据卡尔曼滤波在 MCU 上实时运行适合做平衡车、云台、姿态记录仪这类需要实时姿态反馈的项目。如果你正在找一份能直接编译、能看懂滤波过程、能改成自己项目的姿态解算代码这份资源值得花时间跑一遍。2. 项目架构与目录结构先看清代码怎么组织2.1 整体分层设计这份源码没有用 HAL 库走的是 STM32 标准外设库的路子代码量不大但分层清晰。整个工程可以分成四层硬件驱动层、数据采集层、算法层和应用层。硬件驱动层负责 I2C 初始化和 MPU6050 寄存器读写数据采集层把原始寄存器值转成物理量算法层就是卡尔曼滤波的核心应用层通过串口把姿态角输出到上位机。这种分层的意义在于你换传感器或者换 MCU 的时候只需要动驱动层算法层几乎不用改。我见过太多人把 I2C 读写和滤波计算揉在一个文件里后面想移植到别的平台改到怀疑人生。2.2 目录结构说明工程目录大致是这样的MPU6050_Kalman_STM32/ ├── CMSIS/ # 内核支持文件 ├── STM32F10x_StdPeriph_Driver/ # 标准外设库 ├── User/ │ ├── main.c # 主循环初始化 姿态输出 │ ├── stm32f10x_it.c # 中断服务函数 │ └── system_stm32f10x.c # 系统时钟配置 ├── Hardware/ │ ├── mpu6050.c # MPU6050 驱动I2C 读写 │ ├── mpu6050.h │ ├── i2c.c # 软件模拟 I2C │ └── i2c.h ├── Algorithm/ │ ├── kalman.c # 卡尔曼滤波实现 │ └── kalman.h └── Output/ # 编译输出Hardware目录放的是和芯片打交道的代码Algorithm目录只放滤波算法User目录是业务逻辑。这种划分在中小型嵌入式项目里很常见好处是算法可以单独拿出去做单元测试不用连着硬件一起跑。2.3 关键数据结构在mpu6050.h里定义了一个结构体来存姿态数据typedef struct { float Accel_X; // X轴加速度单位 g float Accel_Y; float Accel_Z; float Gyro_X; // X轴角速度单位 °/s float Gyro_Y; float Gyro_Z; float Roll; // 横滚角单位 ° float Pitch; // 俯仰角单位 ° float Yaw; // 偏航角单位 ° } MPU6050_Data_t;这个结构体贯穿整个数据流驱动层填充加速度和角速度算法层读取这两组值算出角度应用层直接拿Roll、Pitch、Yaw用。参数单位统一成 g 和 °/s后面做三角函数运算时不用再换算少一层出错的可能。3. 核心模块拆解I2C 读取与卡尔曼滤波实现3.1 MPU6050 初始化与数据读取MPU6050 上电后要先解除休眠、设置量程和采样率这些寄存器配置在MPU6050_Init()里完成void MPU6050_Init(void) { I2C_Init(); // 先初始化 I2C 总线 MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 解除休眠 MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07); // 采样率 1kHz/(17)125Hz MPU6050_WriteReg(MPU6050_CONFIG, 0x06); // 低通滤波 5Hz MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); // 陀螺仪 ±2000°/s MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x10); // 加速度 ±8g }这里有几个参数值得注意。SMPLRT_DIV设成 7采样率就是 125Hz对于姿态解算来说够用了太高反而增加 MCU 负担。CONFIG寄存器设成 0x06开启 5Hz 低通滤波能滤掉一部分电机振动带来的高频噪声。陀螺仪量程选 ±2000°/s是因为平衡车或云台在快速调整时角速度可能很大量程小了会饱和。读取原始数据的函数把六个寄存器值拼成有符号 16 位数void MPU6050_ReadData(MPU6050_Data_t *data) { uint8_t buf[14]; MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, buf, 14); int16_t raw_acc_x (buf[0] 8) | buf[1]; int16_t raw_acc_y (buf[2] 8) | buf[3]; int16_t raw_acc_z (buf[4] 8) | buf[5]; int16_t raw_gyro_x (buf[8] 8) | buf[9]; int16_t raw_gyro_y (buf[10] 8) | buf[11]; int16_t raw_gyro_z (buf[12] 8) | buf[13]; // ±8g 量程对应 4096 LSB/g >float Kalman_Filter(float newAngle, float newRate, float dt) { static float Q_angle 0.001f; // 角度过程噪声 static float Q_bias 0.003f; // 陀螺仪零偏过程噪声 static float R_measure 0.03f; // 测量噪声 static float angle 0.0f; // 最优角度估计 static float bias 0.0f; // 陀螺仪零偏估计 static float P[2][2] {{1, 0}, {0, 1}}; // 误差协方差矩阵 // 预测步用陀螺仪角速度积分 float rate newRate - bias; angle dt * rate; // 更新协方差矩阵 P[0][0] dt * (dt * P[1][1] - P[0][1] - P[1][0] Q_angle); P[0][1] - dt * P[1][1]; P[1][0] - dt * P[1][1]; P[1][1] Q_bias * dt; // 计算卡尔曼增益 float S P[0][0] R_measure; float K[2]; K[0] P[0][0] / S; K[1] P[1][0] / S; // 更新步用加速度计角度修正 float y newAngle - angle; angle K[0] * y; bias K[1] * y; // 更新协方差矩阵 float P00_temp P[0][0]; float P01_temp P[0][1]; P[0][0] - K[0] * P00_temp; P[0][1] - K[0] * P01_temp; P[1][0] - K[1] * P00_temp; P[1][1] - K[1] * P01_temp; return angle; }这段代码是整个项目的灵魂逐段拆一下。Q_angle和Q_bias是过程噪声值越大表示越不信任陀螺仪的预测R_measure是测量噪声值越大表示越不信任加速度计的观测。这三个参数直接决定滤波效果后面调参部分会细说。预测步里angle dt * rate是陀螺仪积分dt是两次采样的时间间隔。协方差矩阵P的更新公式看起来复杂本质是在传播不确定性。卡尔曼增益K是权重K[0]决定观测值对角度估计的修正力度K[1]用来估计陀螺仪零偏。更新步里y newAngle - angle是观测残差angle和bias分别按增益修正。注意bias这一项它能在线估计陀螺仪的零偏这是卡尔曼滤波比互补滤波强的地方。互补滤波需要手动标定零偏温度一变零偏就漂了卡尔曼能自动跟。3.3 加速度计角度计算与主循环加速度计算角度用反三角函数void Accel_To_Angle(MPU6050_Data_t *data) { >int main(void) { MPU6050_Data_t mpu_data; float roll_acc, pitch_acc; float roll_kalman 0, pitch_kalman 0; uint32_t last_time 0, now_time 0; float dt; USART1_Init(115200); MPU6050_Init(); while (1) { now_time Get_Ms_Tick(); dt (now_time - last_time) / 1000.0f; last_time now_time; MPU6050_ReadData(mpu_data); Accel_To_Angle(mpu_data); roll_acc mpu_data.Roll; pitch_acc mpu_data.Pitch; roll_kalman Kalman_Filter(roll_acc, mpu_data.Gyro_X, dt); pitch_kalman Kalman_Filter(pitch_acc, mpu_data.Gyro_Y, dt); printf(Roll:%.2f Pitch:%.2f\r\n, roll_kalman, pitch_kalman); Delay_ms(10); } }dt用毫秒计时器算出来每次循环更新。注意Kalman_Filter里用了static变量保存状态所以 Roll 和 Pitch 必须分开调用不能共用一个函数实例否则两个角度的状态会串。这里 Roll 用Gyro_XPitch 用Gyro_Y对应关系不能搞反搞反了滤波会发散。4. 编译、烧录与串口验证把代码跑起来4.1 工程编译环境这份源码用的是 Keil MDK 工程打开.uvprojx文件就能编译。如果你用 GCC 或者 IAR需要自己建工程把User、Hardware、Algorithm三个目录的源文件加进去头文件路径也要配好。标准外设库的路径在STM32F10x_StdPeriph_Driver下CMSIS 路径在CMSIS下。编译前检查两个地方一是stm32f10x_conf.h里有没有把 I2C、USART、GPIO 的宏打开二是system_stm32f10x.c里的晶振频率和你的板子对不对。这两处不对编译能过但跑起来没反应。4.2 硬件连接与烧录MPU6050 和 STM32 的连接用 I2C代码里是软件模拟 I2C引脚在i2c.h里定义MPU6050 引脚STM32 引脚说明VCC3.3V供电不要接 5VGNDGND共地SCLPB6I2C 时钟SDAPB7I2C 数据INT不接本代码用轮询不用中断烧录用 ST-Link 或者串口 ISP 都行。烧录后打开串口助手波特率 115200能看到 Roll 和 Pitch 的实时输出。把传感器平放Roll 和 Pitch 应该在 0° 附近绕 X 轴转Pitch 变化绕 Y 轴转Roll 变化。如果数值乱跳或者一直是 0先查 I2C 有没有应答。4.3 用串口波形验证滤波效果光看数字不够直观建议用串口波形软件把数据画出来。把printf的格式改成 CSV比如printf(%.2f,%.2f\r\n, roll_kalman, pitch_kalman)然后用波形工具实时显示。你会看到卡尔曼滤波后的曲线比原始加速度计角度平滑很多快速转动时又不会像纯陀螺仪积分那样漂。验证的时候做两个动作一是静止放置几分钟看角度会不会慢慢漂二是快速来回转动看角度能不能跟上。静止漂移大说明Q_bias太小或者R_measure太大快速转动跟不上说明Q_angle太小。这两个动作能覆盖大部分滤波参数问题。5. 避坑与常见问题排查5.1 串口输出全是 0 或者不变现象是串口有输出但数值一直是 0或者卡在某个值不动。原因通常是 I2C 通信失败MPU6050 没有应答。先检查接线SCL 和 SDA 有没有接反上拉电阻有没有。软件模拟 I2C 需要外部上拉很多模块自带上拉但有些裸芯片没有。再检查MPU6050_Init()里的MPU6050_WriteReg有没有返回值判断加上应答检测能快速定位。如果 I2C 地址不对MPU6050 的地址是 0x68AD0 接地或 0x69AD0 接 VCC代码里默认用 0x68。5.2 角度漂移越来越快现象是静止时角度慢慢往一个方向偏几分钟就偏几十度。原因是陀螺仪零偏没有估计准或者Q_bias设得太小。卡尔曼滤波的bias项需要时间收敛如果Q_bias太小收敛太慢看起来就像一直在漂。把Q_bias从 0.003 调到 0.01 试试收敛会快很多。另外检查dt是不是稳定如果主循环里有阻塞操作导致dt忽大忽小积分会出错。5.3 快速转动时角度滞后或震荡现象是快速转动传感器角度跟不上或者停下来后角度来回晃。滞后说明Q_angle太小滤波器太信任陀螺仪预测修正不够及时。震荡说明R_measure太小太信任加速度计观测而加速度计在动态下有噪声。这两个参数要配合调先调Q_angle让跟随变快再调R_measure把震荡压下去。我一般会先把Q_angle设成 0.01R_measure设成 0.1跑一遍看波形再微调。5.4 Roll 和 Pitch 对调或者方向反了现象是转动传感器Roll 和 Pitch 的变化对不上或者往正方向转角度反而变小。原因是加速度计和陀螺仪的轴向对应关系搞错了。MPU6050 的 X 轴和 Y 轴定义要对着数据手册看Accel_To_Angle里 Roll 用 Y 和 ZPitch 用 X这是有固定对应关系的。如果方向反了在角度计算前加负号就行。陀螺仪那边Roll 对应Gyro_XPitch 对应Gyro_Y如果发现滤波后角度往反方向跑把对应的陀螺仪读数取反。5.5 编译报错找不到头文件现象是编译时提示mpu6050.h或者kalman.h找不到。原因是头文件搜索路径没配全。在 Keil 里打开 Options for TargetC/C 选项卡Include Paths 里把User、Hardware、Algorithm、STM32F10x_StdPeriph_Driver/inc、CMSIS都加进去。如果用的是标准外设库还要确认stm32f10x.h里的宏定义和你的芯片型号匹配比如STM32F10X_MD对应中容量产品。6. 二次开发把这份代码改成你自己的姿态模块拿到这份代码之后最直接的改造方向是换传感器。MPU6050 是六轴如果你手上有 MPU9250 或者 ICM20602驱动层改一下寄存器地址和量程系数算法层几乎不用动。卡尔曼滤波的输入是角度和角速度只要新传感器能提供这两样融合逻辑就是通用的。第二个方向是加磁力计做九轴融合。MPU6050 的 Yaw 会漂加一个 HMC5883L 或者 QMC5883L用磁力计算绝对航向再和陀螺仪积分做融合Yaw 就能长期稳定。代码结构上在Algorithm目录下加一个mag.c主循环里多一路卡尔曼或者用互补滤波融合 Yaw改动量不大。第三个方向是优化实时性。现在主循环是Delay_ms(10)采样率 100Hz对大多数应用够了。如果要上 500Hz 或者 1kHz把延时去掉用定时器中断触发采样dt从定时器周期直接取比用Get_Ms_Tick()算更准。另外printf很耗时高频采样时先把数据存到数组里攒够一批再统一发。调参这块我自己的习惯是先把Q_angle、Q_bias、R_measure三个参数打印到串口配合波形软件边看边调。每次只改一个参数改完记录波形不然三个参数一起动根本分不清是谁的效果。还有一点dt一定要实测不要用理论值主循环里其他代码的耗时会影响实际采样间隔dt不准积分和协方差更新都会偏。最后说个移植到其他 MCU 的注意点。这份代码用的是 STM32 标准外设库换到 GD32 或者 STM32 HAL 库主要改 I2C 和延时函数。Kalman_Filter和Accel_To_Angle是纯 C 数学运算不依赖任何硬件直接拷过去就能用。我一般会先把算法层单独拿出来在 PC 上用固定数据跑一遍确认输出正确再往 MCU 上搬这样能省很多在线调试的时间。从那以后我每次移植算法都先在 PC 上验证一遍希望这个习惯也能帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/20 4:53:16 · 阅读 1.2k MPU6050姿态解算实战：STM32卡尔曼滤波源码与调参指南
简介这份资源面向嵌入式开发初学者与从事无人机、机器人、运动设备研发的工程师提供基于STM32平台读取MPU6050六轴惯性测量单元数据并完成姿态解算的完整源码重点解决加速度计动态噪声与陀螺仪积分漂移带来的姿态误差问题。压缩包共129个文件约3.64MB以c与h源码文件为核心配合uvproj、uvopt等Keil工程配置、axf与hex可执行产物、map与lst编译映射文件以及若干bak备份与说明文档覆盖驱动、滤波与工程构建各环节。资源已有5129人学习下载热度较高。源码中可看到inv_mpu、inv_mpu_dmp_motion_driver、MPU6050、IOI2C等模块完整呈现I2C初始化、寄存器读写、原始数据预处理与卡尔曼滤波状态预测、测量更新、协方差更新的实现路径便于读者对照理解传感器融合流程并直接移植到自己的嵌入式项目中调试与验证。1. 从一块 MPU6050 说起为什么你的姿态角总在跳如果你玩过平衡车、四轴飞控或者云台大概率绕不开 MPU6050 这颗六轴传感器。它便宜、好买、资料多但真正把它用起来的人都会遇到同一个问题读出来的加速度计和陀螺仪数据直接算角度要么抖得没法看要么漂得离谱。加速度计静态准但动态噪声大陀螺仪动态好但积分会累积漂移单靠任何一个都做不出能用的姿态解算。这份 MPU6050 姿态解算 STM32 源码核心就是用卡尔曼滤波把两路数据融合起来输出稳定的俯仰角、横滚角和偏航角。代码基于 STM32 标准外设库I2C 读取原始数据卡尔曼滤波在 MCU 上实时运行适合做平衡车、云台、姿态记录仪这类需要实时姿态反馈的项目。如果你正在找一份能直接编译、能看懂滤波过程、能改成自己项目的姿态解算代码这份资源值得花时间跑一遍。2. 项目架构与目录结构先看清代码怎么组织2.1 整体分层设计这份源码没有用 HAL 库走的是 STM32 标准外设库的路子代码量不大但分层清晰。整个工程可以分成四层硬件驱动层、数据采集层、算法层和应用层。硬件驱动层负责 I2C 初始化和 MPU6050 寄存器读写数据采集层把原始寄存器值转成物理量算法层就是卡尔曼滤波的核心应用层通过串口把姿态角输出到上位机。这种分层的意义在于你换传感器或者换 MCU 的时候只需要动驱动层算法层几乎不用改。我见过太多人把 I2C 读写和滤波计算揉在一个文件里后面想移植到别的平台改到怀疑人生。2.2 目录结构说明工程目录大致是这样的MPU6050_Kalman_STM32/ ├── CMSIS/ # 内核支持文件 ├── STM32F10x_StdPeriph_Driver/ # 标准外设库 ├── User/ │ ├── main.c # 主循环初始化 姿态输出 │ ├── stm32f10x_it.c # 中断服务函数 │ └── system_stm32f10x.c # 系统时钟配置 ├── Hardware/ │ ├── mpu6050.c # MPU6050 驱动I2C 读写 │ ├── mpu6050.h │ ├── i2c.c # 软件模拟 I2C │ └── i2c.h ├── Algorithm/ │ ├── kalman.c # 卡尔曼滤波实现 │ └── kalman.h └── Output/ # 编译输出Hardware目录放的是和芯片打交道的代码Algorithm目录只放滤波算法User目录是业务逻辑。这种划分在中小型嵌入式项目里很常见好处是算法可以单独拿出去做单元测试不用连着硬件一起跑。2.3 关键数据结构在mpu6050.h里定义了一个结构体来存姿态数据typedef struct { float Accel_X; // X轴加速度单位 g float Accel_Y; float Accel_Z; float Gyro_X; // X轴角速度单位 °/s float Gyro_Y; float Gyro_Z; float Roll; // 横滚角单位 ° float Pitch; // 俯仰角单位 ° float Yaw; // 偏航角单位 ° } MPU6050_Data_t;这个结构体贯穿整个数据流驱动层填充加速度和角速度算法层读取这两组值算出角度应用层直接拿Roll、Pitch、Yaw用。参数单位统一成 g 和 °/s后面做三角函数运算时不用再换算少一层出错的可能。3. 核心模块拆解I2C 读取与卡尔曼滤波实现3.1 MPU6050 初始化与数据读取MPU6050 上电后要先解除休眠、设置量程和采样率这些寄存器配置在MPU6050_Init()里完成void MPU6050_Init(void) { I2C_Init(); // 先初始化 I2C 总线 MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 解除休眠 MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07); // 采样率 1kHz/(17)125Hz MPU6050_WriteReg(MPU6050_CONFIG, 0x06); // 低通滤波 5Hz MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); // 陀螺仪 ±2000°/s MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x10); // 加速度 ±8g }这里有几个参数值得注意。SMPLRT_DIV设成 7采样率就是 125Hz对于姿态解算来说够用了太高反而增加 MCU 负担。CONFIG寄存器设成 0x06开启 5Hz 低通滤波能滤掉一部分电机振动带来的高频噪声。陀螺仪量程选 ±2000°/s是因为平衡车或云台在快速调整时角速度可能很大量程小了会饱和。读取原始数据的函数把六个寄存器值拼成有符号 16 位数void MPU6050_ReadData(MPU6050_Data_t *data) { uint8_t buf[14]; MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, buf, 14); int16_t raw_acc_x (buf[0] 8) | buf[1]; int16_t raw_acc_y (buf[2] 8) | buf[3]; int16_t raw_acc_z (buf[4] 8) | buf[5]; int16_t raw_gyro_x (buf[8] 8) | buf[9]; int16_t raw_gyro_y (buf[10] 8) | buf[11]; int16_t raw_gyro_z (buf[12] 8) | buf[13]; // ±8g 量程对应 4096 LSB/g >float Kalman_Filter(float newAngle, float newRate, float dt) { static float Q_angle 0.001f; // 角度过程噪声 static float Q_bias 0.003f; // 陀螺仪零偏过程噪声 static float R_measure 0.03f; // 测量噪声 static float angle 0.0f; // 最优角度估计 static float bias 0.0f; // 陀螺仪零偏估计 static float P[2][2] {{1, 0}, {0, 1}}; // 误差协方差矩阵 // 预测步用陀螺仪角速度积分 float rate newRate - bias; angle dt * rate; // 更新协方差矩阵 P[0][0] dt * (dt * P[1][1] - P[0][1] - P[1][0] Q_angle); P[0][1] - dt * P[1][1]; P[1][0] - dt * P[1][1]; P[1][1] Q_bias * dt; // 计算卡尔曼增益 float S P[0][0] R_measure; float K[2]; K[0] P[0][0] / S; K[1] P[1][0] / S; // 更新步用加速度计角度修正 float y newAngle - angle; angle K[0] * y; bias K[1] * y; // 更新协方差矩阵 float P00_temp P[0][0]; float P01_temp P[0][1]; P[0][0] - K[0] * P00_temp; P[0][1] - K[0] * P01_temp; P[1][0] - K[1] * P00_temp; P[1][1] - K[1] * P01_temp; return angle; }这段代码是整个项目的灵魂逐段拆一下。Q_angle和Q_bias是过程噪声值越大表示越不信任陀螺仪的预测R_measure是测量噪声值越大表示越不信任加速度计的观测。这三个参数直接决定滤波效果后面调参部分会细说。预测步里angle dt * rate是陀螺仪积分dt是两次采样的时间间隔。协方差矩阵P的更新公式看起来复杂本质是在传播不确定性。卡尔曼增益K是权重K[0]决定观测值对角度估计的修正力度K[1]用来估计陀螺仪零偏。更新步里y newAngle - angle是观测残差angle和bias分别按增益修正。注意bias这一项它能在线估计陀螺仪的零偏这是卡尔曼滤波比互补滤波强的地方。互补滤波需要手动标定零偏温度一变零偏就漂了卡尔曼能自动跟。3.3 加速度计角度计算与主循环加速度计算角度用反三角函数void Accel_To_Angle(MPU6050_Data_t *data) { >int main(void) { MPU6050_Data_t mpu_data; float roll_acc, pitch_acc; float roll_kalman 0, pitch_kalman 0; uint32_t last_time 0, now_time 0; float dt; USART1_Init(115200); MPU6050_Init(); while (1) { now_time Get_Ms_Tick(); dt (now_time - last_time) / 1000.0f; last_time now_time; MPU6050_ReadData(mpu_data); Accel_To_Angle(mpu_data); roll_acc mpu_data.Roll; pitch_acc mpu_data.Pitch; roll_kalman Kalman_Filter(roll_acc, mpu_data.Gyro_X, dt); pitch_kalman Kalman_Filter(pitch_acc, mpu_data.Gyro_Y, dt); printf(Roll:%.2f Pitch:%.2f\r\n, roll_kalman, pitch_kalman); Delay_ms(10); } }dt用毫秒计时器算出来每次循环更新。注意Kalman_Filter里用了static变量保存状态所以 Roll 和 Pitch 必须分开调用不能共用一个函数实例否则两个角度的状态会串。这里 Roll 用Gyro_XPitch 用Gyro_Y对应关系不能搞反搞反了滤波会发散。4. 编译、烧录与串口验证把代码跑起来4.1 工程编译环境这份源码用的是 Keil MDK 工程打开.uvprojx文件就能编译。如果你用 GCC 或者 IAR需要自己建工程把User、Hardware、Algorithm三个目录的源文件加进去头文件路径也要配好。标准外设库的路径在STM32F10x_StdPeriph_Driver下CMSIS 路径在CMSIS下。编译前检查两个地方一是stm32f10x_conf.h里有没有把 I2C、USART、GPIO 的宏打开二是system_stm32f10x.c里的晶振频率和你的板子对不对。这两处不对编译能过但跑起来没反应。4.2 硬件连接与烧录MPU6050 和 STM32 的连接用 I2C代码里是软件模拟 I2C引脚在i2c.h里定义MPU6050 引脚STM32 引脚说明VCC3.3V供电不要接 5VGNDGND共地SCLPB6I2C 时钟SDAPB7I2C 数据INT不接本代码用轮询不用中断烧录用 ST-Link 或者串口 ISP 都行。烧录后打开串口助手波特率 115200能看到 Roll 和 Pitch 的实时输出。把传感器平放Roll 和 Pitch 应该在 0° 附近绕 X 轴转Pitch 变化绕 Y 轴转Roll 变化。如果数值乱跳或者一直是 0先查 I2C 有没有应答。4.3 用串口波形验证滤波效果光看数字不够直观建议用串口波形软件把数据画出来。把printf的格式改成 CSV比如printf(%.2f,%.2f\r\n, roll_kalman, pitch_kalman)然后用波形工具实时显示。你会看到卡尔曼滤波后的曲线比原始加速度计角度平滑很多快速转动时又不会像纯陀螺仪积分那样漂。验证的时候做两个动作一是静止放置几分钟看角度会不会慢慢漂二是快速来回转动看角度能不能跟上。静止漂移大说明Q_bias太小或者R_measure太大快速转动跟不上说明Q_angle太小。这两个动作能覆盖大部分滤波参数问题。5. 避坑与常见问题排查5.1 串口输出全是 0 或者不变现象是串口有输出但数值一直是 0或者卡在某个值不动。原因通常是 I2C 通信失败MPU6050 没有应答。先检查接线SCL 和 SDA 有没有接反上拉电阻有没有。软件模拟 I2C 需要外部上拉很多模块自带上拉但有些裸芯片没有。再检查MPU6050_Init()里的MPU6050_WriteReg有没有返回值判断加上应答检测能快速定位。如果 I2C 地址不对MPU6050 的地址是 0x68AD0 接地或 0x69AD0 接 VCC代码里默认用 0x68。5.2 角度漂移越来越快现象是静止时角度慢慢往一个方向偏几分钟就偏几十度。原因是陀螺仪零偏没有估计准或者Q_bias设得太小。卡尔曼滤波的bias项需要时间收敛如果Q_bias太小收敛太慢看起来就像一直在漂。把Q_bias从 0.003 调到 0.01 试试收敛会快很多。另外检查dt是不是稳定如果主循环里有阻塞操作导致dt忽大忽小积分会出错。5.3 快速转动时角度滞后或震荡现象是快速转动传感器角度跟不上或者停下来后角度来回晃。滞后说明Q_angle太小滤波器太信任陀螺仪预测修正不够及时。震荡说明R_measure太小太信任加速度计观测而加速度计在动态下有噪声。这两个参数要配合调先调Q_angle让跟随变快再调R_measure把震荡压下去。我一般会先把Q_angle设成 0.01R_measure设成 0.1跑一遍看波形再微调。5.4 Roll 和 Pitch 对调或者方向反了现象是转动传感器Roll 和 Pitch 的变化对不上或者往正方向转角度反而变小。原因是加速度计和陀螺仪的轴向对应关系搞错了。MPU6050 的 X 轴和 Y 轴定义要对着数据手册看Accel_To_Angle里 Roll 用 Y 和 ZPitch 用 X这是有固定对应关系的。如果方向反了在角度计算前加负号就行。陀螺仪那边Roll 对应Gyro_XPitch 对应Gyro_Y如果发现滤波后角度往反方向跑把对应的陀螺仪读数取反。5.5 编译报错找不到头文件现象是编译时提示mpu6050.h或者kalman.h找不到。原因是头文件搜索路径没配全。在 Keil 里打开 Options for TargetC/C 选项卡Include Paths 里把User、Hardware、Algorithm、STM32F10x_StdPeriph_Driver/inc、CMSIS都加进去。如果用的是标准外设库还要确认stm32f10x.h里的宏定义和你的芯片型号匹配比如STM32F10X_MD对应中容量产品。6. 二次开发把这份代码改成你自己的姿态模块拿到这份代码之后最直接的改造方向是换传感器。MPU6050 是六轴如果你手上有 MPU9250 或者 ICM20602驱动层改一下寄存器地址和量程系数算法层几乎不用动。卡尔曼滤波的输入是角度和角速度只要新传感器能提供这两样融合逻辑就是通用的。第二个方向是加磁力计做九轴融合。MPU6050 的 Yaw 会漂加一个 HMC5883L 或者 QMC5883L用磁力计算绝对航向再和陀螺仪积分做融合Yaw 就能长期稳定。代码结构上在Algorithm目录下加一个mag.c主循环里多一路卡尔曼或者用互补滤波融合 Yaw改动量不大。第三个方向是优化实时性。现在主循环是Delay_ms(10)采样率 100Hz对大多数应用够了。如果要上 500Hz 或者 1kHz把延时去掉用定时器中断触发采样dt从定时器周期直接取比用Get_Ms_Tick()算更准。另外printf很耗时高频采样时先把数据存到数组里攒够一批再统一发。调参这块我自己的习惯是先把Q_angle、Q_bias、R_measure三个参数打印到串口配合波形软件边看边调。每次只改一个参数改完记录波形不然三个参数一起动根本分不清是谁的效果。还有一点dt一定要实测不要用理论值主循环里其他代码的耗时会影响实际采样间隔dt不准积分和协方差更新都会偏。最后说个移植到其他 MCU 的注意点。这份代码用的是 STM32 标准外设库换到 GD32 或者 STM32 HAL 库主要改 I2C 和延时函数。Kalman_Filter和Accel_To_Angle是纯 C 数学运算不依赖任何硬件直接拷过去就能用。我一般会先把算法层单独拿出来在 PC 上用固定数据跑一遍确认输出正确再往 MCU 上搬这样能省很多在线调试的时间。从那以后我每次移植算法都先在 PC 上验证一遍希望这个习惯也能帮到你。本文还有配套的精品资源点击获取