【免费开源】STM32G431九轴姿态传感器IMU融合卡尔曼滤波四元数解算开源项目分享一、项目背景九轴 IMU加速度计陀螺仪磁力计是无人机、平衡车、AR/VR、运动捕捉等设备的核心传感器。本项目基于 STM32G431 的九轴姿态传感器使用主流的 ICM20948 / MPU9250 等芯片配合 STM32G431 的高性能 Cortex-M4 FPU完整实现传感器原始数据采集、三轴校准、Mahony/EKF 姿态融合算法、四元数解算与欧拉角输出以及 USB-CDC 上位机姿态显示。二、项目流程图STM32G431 上电 170MHzI2C/SPI 初始化读取 WHO_AM_I 检测芯片配置陀螺±500dps加速度±4g磁力 0.6μT/LSB6 面校准: 加速度零偏椭球拟合: 磁力计软硬铁启动 1ms 采样读取陀螺 gx,gy,gz读取加速度 ax,ay,az读取磁力 mx,my,mzMahony 融合更新四元数四元数 → Roll/Pitch/YawUSB-CDC 发送三、硬件接线ICM20948STM32G431SCL/SCKPB6SDA/SDIPB7INTPA0nCSPA4四、核心代码Mahony 互补滤波#includemath.htypedefstruct{floatw,x,y,z;}Quat;staticQuat q{1,0,0,0};staticfloatKp2.0f,Ki0.005f;staticfloatex0,ey0,ez0;voidMahony_Update(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatdt){floatnsqrtf(ax*axay*ayaz*az);if(n1e-6f)return;ax/n;ay/n;az/n;floatvx2*(q.x*q.z-q.w*q.y);floatvy2*(q.w*q.xq.y*q.z);floatvzq.w*q.w-q.x*q.x-q.y*q.yq.z*q.z;floatex0ay*vz-az*vy;floatey0az*vx-ax*vz;floatez0ax*vy-ay*vx;exex0*dt;eyey0*dt;ezez0*dt;gxKp*ex0Ki*ex;gyKp*ey0Ki*ey;gzKp*ez0Ki*ez;floatdx0.5f*(-q.x*gx-q.y*gy-q.z*gz)*dt;floatdy0.5f*(q.w*gxq.y*gz-q.z*gy)*dt;floatdz0.5f*(q.w*gy-q.x*gzq.z*gx)*dt;floatdw0.5f*(q.w*gzq.x*gy-q.y*gx)*dt;q.wdx;q.xdy;q.ydz;q.zdw;nsqrtf(q.w*q.wq.x*q.xq.y*q.yq.z*q.z);q.w/n;q.x/n;q.y/n;q.z/n;}voidQuat2Euler(float*r,float*p,float*y){*ratan2f(2*(q.w*q.xq.y*q.z),1-2*(q.x*q.xq.y*q.y))*57.2958f;*pasinf(2*(q.w*q.y-q.z*q.x))*57.2958f;*yatan2f(2*(q.w*q.zq.x*q.y),1-2*(q.y*q.yq.z*q.z))*57.2958f;}五、关键技术点5.1 G4 高速时钟G431 主频 170MHzFPU 单精度浮点运算速度极快1kHz 姿态融合 CPU 占用 10%。5.2 椭球校准磁力计因周围铁磁干扰需要做椭球拟合把数据从椭球归一化到单位球否则航向角误差会非常大。5.3 时间戳精度建议使用 TIM2 32 位定时器输出 1us 时间戳姿态融合 dt 取真实时间而非简单设为 0.001s。六、应用场景四轴飞行器/穿越机姿态控制平衡车直立环AR/VR 头盔姿态追踪工业机械臂末端姿态校准运动手环。七、调试经验加速度零偏在常温下会有 ±50mg 偏差开机要做静态校准Yaw 角必须依赖磁力计否则会无界漂移板子安装方向必须明确否则解算结果会反向使用 USB-CDC 发送数据比 UART 快方便上位机三维可视化。八、总结本项目代码精炼但功能完整基于 STM32G431 的高性能为复杂的 IMU 融合提供了优秀的算力基础。完整源码、上位机姿态可视化软件、磁力计校准工具均已打包是学习 IMU 融合算法的最佳实战项目。
【免费开源】STM32G431九轴姿态传感器IMU融合卡尔曼滤波四元数解算开源项目分享
发布时间：2026/5/30 20:23:03
【免费开源】STM32G431九轴姿态传感器IMU融合卡尔曼滤波四元数解算开源项目分享一、项目背景九轴 IMU加速度计陀螺仪磁力计是无人机、平衡车、AR/VR、运动捕捉等设备的核心传感器。本项目基于 STM32G431 的九轴姿态传感器使用主流的 ICM20948 / MPU9250 等芯片配合 STM32G431 的高性能 Cortex-M4 FPU完整实现传感器原始数据采集、三轴校准、Mahony/EKF 姿态融合算法、四元数解算与欧拉角输出以及 USB-CDC 上位机姿态显示。二、项目流程图STM32G431 上电 170MHzI2C/SPI 初始化读取 WHO_AM_I 检测芯片配置陀螺±500dps加速度±4g磁力 0.6μT/LSB6 面校准: 加速度零偏椭球拟合: 磁力计软硬铁启动 1ms 采样读取陀螺 gx,gy,gz读取加速度 ax,ay,az读取磁力 mx,my,mzMahony 融合更新四元数四元数 → Roll/Pitch/YawUSB-CDC 发送三、硬件接线ICM20948STM32G431SCL/SCKPB6SDA/SDIPB7INTPA0nCSPA4四、核心代码Mahony 互补滤波#includemath.htypedefstruct{floatw,x,y,z;}Quat;staticQuat q{1,0,0,0};staticfloatKp2.0f,Ki0.005f;staticfloatex0,ey0,ez0;voidMahony_Update(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatdt){floatnsqrtf(ax*axay*ayaz*az);if(n1e-6f)return;ax/n;ay/n;az/n;floatvx2*(q.x*q.z-q.w*q.y);floatvy2*(q.w*q.xq.y*q.z);floatvzq.w*q.w-q.x*q.x-q.y*q.yq.z*q.z;floatex0ay*vz-az*vy;floatey0az*vx-ax*vz;floatez0ax*vy-ay*vx;exex0*dt;eyey0*dt;ezez0*dt;gxKp*ex0Ki*ex;gyKp*ey0Ki*ey;gzKp*ez0Ki*ez;floatdx0.5f*(-q.x*gx-q.y*gy-q.z*gz)*dt;floatdy0.5f*(q.w*gxq.y*gz-q.z*gy)*dt;floatdz0.5f*(q.w*gy-q.x*gzq.z*gx)*dt;floatdw0.5f*(q.w*gzq.x*gy-q.y*gx)*dt;q.wdx;q.xdy;q.ydz;q.zdw;nsqrtf(q.w*q.wq.x*q.xq.y*q.yq.z*q.z);q.w/n;q.x/n;q.y/n;q.z/n;}voidQuat2Euler(float*r,float*p,float*y){*ratan2f(2*(q.w*q.xq.y*q.z),1-2*(q.x*q.xq.y*q.y))*57.2958f;*pasinf(2*(q.w*q.y-q.z*q.x))*57.2958f;*yatan2f(2*(q.w*q.zq.x*q.y),1-2*(q.y*q.yq.z*q.z))*57.2958f;}五、关键技术点5.1 G4 高速时钟G431 主频 170MHzFPU 单精度浮点运算速度极快1kHz 姿态融合 CPU 占用 10%。5.2 椭球校准磁力计因周围铁磁干扰需要做椭球拟合把数据从椭球归一化到单位球否则航向角误差会非常大。5.3 时间戳精度建议使用 TIM2 32 位定时器输出 1us 时间戳姿态融合 dt 取真实时间而非简单设为 0.001s。六、应用场景四轴飞行器/穿越机姿态控制平衡车直立环AR/VR 头盔姿态追踪工业机械臂末端姿态校准运动手环。七、调试经验加速度零偏在常温下会有 ±50mg 偏差开机要做静态校准Yaw 角必须依赖磁力计否则会无界漂移板子安装方向必须明确否则解算结果会反向使用 USB-CDC 发送数据比 UART 快方便上位机三维可视化。八、总结本项目代码精炼但功能完整基于 STM32G431 的高性能为复杂的 IMU 融合提供了优秀的算力基础。完整源码、上位机姿态可视化软件、磁力计校准工具均已打包是学习 IMU 融合算法的最佳实战项目。
