IMU数据处理（卡尔曼滤波+四元数计算欧拉角一条龙服务）

先给你最终标准答案直接照做就行结论必须寄存器读出来的原始16位 raw 数据 → 先卡尔曼/均值滤波 → 再换算单位转成 g、rad/s为什么不能先转单位再滤波寄存器原始值是整数整型噪声是均匀高斯噪声滤波效果最好先转 g、rad/s 变成浮点数相当于做了一次缩放噪声特性被破坏滤波容易滞后、飘嵌入式单片机整型运算比浮点快太多先整型滤波省算力、不卡很多漂移、尖峰干扰在原始raw值上最好滤转成物理量后反而难处理。标准正确流程IMU 六轴通用加速度陀螺仪寄存器读取 → 原始16位 RAW数据(ax_raw,ay_raw,az_raw,gx_raw,gy_raw,gz_raw) ↓ 【卡尔曼滤波 / 一阶低通 / 滑动平均滤波】 ✅在这里滤波 ↓ 滤波后的平稳 raw 值 ↓ 单位换算 加速度 ÷ 灵敏度 → 转换成 g 陀螺仪 ÷ 灵敏度 → 转换成 rad/s 或 °/s ↓ 得到可用的平稳IMU数据两种滤波选哪种给你直接推荐1. 简单够用新手首选滑动平均 / 一阶低通代码极简、不占资源、STM32/ESP32随便跑最适合IMU三轴原始数据。2. 效果更好平滑无滞后一维卡尔曼滤波X、Y、Z 每一个轴单独做一维卡尔曼加速度三轴各自独立滤波陀螺仪三轴各自独立滤波不用多维姿态卡尔曼一维就够用代码超简单关键细节你一定会踩坑的点IMU每个轴单独滤波不要把XYZ混在一起ax单独滤、ay单独滤、az单独滤陀螺仪同理。滤波只用int16_t 原始采样值不要转float再滤波浪费性能还效果差。单位换算公式放在滤波之后举个例子常用MPU6050加速度 ±2g 灵敏度16384 LSB/g[a_g \frac{raw_{滤波后}}{16384}]陀螺仪 ±250°/s 灵敏度131 LSB/(°/s)[w_{dps} \frac{raw_{滤波后}}{131}]全部滤波完再除。陀螺仪一定要先去零偏校准再滤波上电静止采集200次求平均减去零点偏移再进滤波器效果直接翻倍。我现在可以直接给你现成可用版本你告诉我两点就行你用的什么芯片STM32 还是 ESP32想要哪种滤波一阶低通还是一维卡尔曼我给你写好三轴IMU原始数据专用滤波代码直接复制就能用不用改参数。IMU全套完整教程原始数据读取→卡尔曼滤波→单位转换→四元数解算欧拉角适配STM32/ESP32 通用C语言流程完全按工程标准寄存器原始RAW → 单轴卡尔曼滤波 → 单位转g/°/s → 四元数姿态解算 → 输出欧拉角(横滚/俯仰/偏航)一、先定核心原则牢记滤波时机先对16位原始整型RAW数据滤波再做单位换算滤波方式加速度X/Y/Z、陀螺仪X/Y/Z6个轴单独一维卡尔曼滤波姿态解算滤波后的物理量( g °/s ) 送入四元数互补滤波/Madgwick解欧拉角二、整体完整流程图IMU寄存器 → int16_t ax_raw,ay_raw,az_raw,gx_raw,gy_raw,gz_raw ↓ 6轴分别一维卡尔曼滤波 ↓ 滤波后RAW值 → 单位换算 ↓ ax(g) ay(g) az(g) gx(°/s) gy(°/s) gz(°/s) ↓ Madgwick四元数姿态解算 ↓ 四元数q0 q1 q2 q3 → 换算欧拉角 Roll Pitch Yaw三、第一步极简一维卡尔曼滤波C库无依赖直接嵌单片机不用Eigen、不用复杂矩阵单轴专用卡尔曼占资源极小1. kalman_filter.h#ifndefKALMAN_FILTER_H#defineKALMAN_FILTER_H#includestdint.h// 单轴卡尔曼结构体typedefstruct{floatx;// 最优估计值floatP;// 协方差floatQ;// 过程噪声floatR;// 测量噪声}Kalman_t;// 初始化卡尔曼参数voidKalman_Init(Kalman_t*kf,floatQ,floatR);// 单轴卡尔曼滤波输入原始数据floatKalman_Filter(Kalman_t*kf,floatmeasure);#endif2. kalman_filter.c#includekalman_filter.hvoidKalman_Init(Kalman_t*kf,floatQ,floatR){kf-x0.0f;kf-P1.0f;kf-QQ;kf-RR;}floatKalman_Filter(Kalman_t*kf,floatmeasure){// 1. 预测kf-Pkf-Pkf-Q;// 2. 卡尔曼增益floatKkf-P/(kf-Pkf-R);// 3. 更新估计值kf-xkf-xK*(measure-kf-x);// 4. 更新协方差kf-P(1-K)*kf-P;returnkf-x;}3. 参数说明直接抄参数就能用加速度推荐Q0.01, R0.5陀螺仪推荐Q0.001, R0.1Q越小越平滑R越小越跟随原始数据。四、第二步IMU原始数据滤波 单位转换教程以通用MPU6050/MPU9250为例其他IMU照搬公式即可。1. 定义6轴卡尔曼实例// 加速度三轴Kalman_t kf_ax,kf_ay,kf_az;// 陀螺仪三轴Kalman_t kf_gx,kf_gy,kf_gz;// 初始化放在上电初始化里voidIMU_Filter_Init(void){// 加速度参数Kalman_Init(kf_ax,0.01f,0.5f);Kalman_Init(kf_ay,0.01f,0.5f);Kalman_Init(kf_az,0.01f,0.5f);// 陀螺仪参数Kalman_Init(kf_gx,0.001f,0.1f);Kalman_Init(kf_gy,0.001f,0.1f);Kalman_Init(kf_gz,0.001f,0.001f);}2. 原始数据读取 滤波 单位转换// 原始寄存器数据int16_tax_raw,ay_raw,az_raw;int16_tgx_raw,gy_raw,gz_raw;// 滤波后物理量floatax_g,ay_g,az_g;floatgx_dps,gy_dps,gz_dps;// MPU6050 默认配置#defineACC_SENS16384.0f// ±2g 灵敏度 LSB/g#defineGYRO_SENS131.0f// ±250dps 灵敏度 LSB/(°/s)voidIMU_Read_And_Filter(void){// 1. 从I2C读取寄存器原始16位数据IMU_Read_Raw_Data(ax_raw,ay_raw,az_raw,gx_raw,gy_raw,gz_raw);// 2. 先对原始RAW数据做卡尔曼滤波 ✅关键先滤波floatax_filtKalman_Filter(kf_ax,(float)ax_raw);floatay_filtKalman_Filter(kf_ay,(float)ay_raw);floataz_filtKalman_Filter(kf_az,(float)az_raw);floatgx_filtKalman_Filter(kf_gx,(float)gx_raw);floatgy_filtKalman_Filter(kf_gy,(float)gy_raw);floatgz_filtKalman_Filter(kf_gz,(float)gz_raw);// 3. 滤波后再做单位转换 ✅后换算ax_gax_filt/ACC_SENS;ay_gay_filt/ACC_SENS;az_gaz_filt/ACC_SENS;gx_dpsgx_filt/GYRO_SENS;gy_dpsgy_filt/GYRO_SENS;gz_dpsgz_filt/GYRO_SENS;}重点重申raw原始值 → 滤波 → 除以灵敏度转g、°/s绝对不要先除再滤波五、第三步四元数解算欧拉角Madgwick算法 极简移植输入滤波后的ax_g,ay_g,az_ggx_dps,gy_dps,gz_dps输出Roll横滚、Pitch俯仰、Yaw偏航 欧拉角角度制1. 四元数结构体与全局变量// 四元数floatq01.0f,q10.0f,q20.0f,q30.0f;// 欧拉角floatRoll,Pitch,Yaw;#defineIMU_SAMPLE_DT0.01f// 采样周期10ms 根据你的任务周期改#defineBETA0.1f// 收敛参数2. Madgwick姿态解算函数voidIMU_Madgwick_Update(floatax,floatay,floataz,floatgx,floatgy,floatgz){floatrecipNorm;floats0,s1,s2,s3;floatqDot1,qDot2,qDot3,qDot4;float_2q0,_2q1,_2q2,_2q3,_4q0,_4q1,_4q2,_8q1,_8q2;floatq0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;// 陀螺仪角速度转弧度gx*3.1415926f/180.0f;gy*3.1415926f/180.0f;gz*3.1415926f/180.0f;// 归一化加速度recipNormsqrtf(ax*axay*ayaz*az);if(recipNorm0.0f)return;recipNorm1.0f/recipNorm;ax*recipNorm;ay*recipNorm;az*recipNorm;// 中间变量_2q02.0f*q0;_2q12.0f*q1;_2q22.0f*q2;_2q32.0f*q3;_4q04.0f*q0;_4q14.0f*q1;_4q24.0f*q2;_8q18.0f*q1;_8q28.0f*q2;q0q0q0*q0;q0q1q0*q1;q0q2q0*q2;q0q3q0*q3;q1q1q1*q1;q1q2q1*q2;q1q3q1*q3;q2q2q2*q2;q2q3q2*q3;q3q3q3*q3;// 梯度下降s0_4q0*q2q3-_4q0*q1q2-_2q0*ax_2q1*ay_2q2*az;s1_4q1*q0q3_4q1*q2q3-_2q1*ax-_2q0*ay-_8q1*az;s2_4q2*q0q1_4q2*q1q3-_2q2*ax-_8q2*ay-_2q0*az;s3_4q3*q0q2-_4q3*q1q1-_8q3*ax_2q3*ay;// 四元数微分qDot10.5f*(-q1*gx-q2*gy-q3*gz)-BETA*s0;qDot20.5f*(q0*gxq2*gz-q3*gy)-BETA*s1;qDot30.5f*(q0*gy-q1*gzq3*gx)-BETA*s2;qDot40.5f*(q0*gzq1*gy-q2*gx)-BETA*s3;// 积分更新四元数q0qDot1*IMU_SAMPLE_DT;q1qDot2*IMU_SAMPLE_DT;q2qDot3*IMU_SAMPLE_DT;q3qDot4*IMU_SAMPLE_DT;// 四元数归一化recipNormsqrtf(q0*q0q1*q1q2*q2q3*q3);q0/recipNorm;q1/recipNorm;q2/recipNorm;q3/recipNorm;}3. 四元数转欧拉角角度制voidQuat_To_Euler(void){// Roll 横滚Rollatan2f(2.0f*(q0*q1q2*q3),1.0f-2.0f*(q1*q1q2*q2));// Pitch 俯仰Pitchasinf(2.0f*(q0*q2-q1*q3));// Yaw 偏航Yawatan2f(2.0f*(q0*q3q1*q2),1.0f-2.0f*(q2*q2q3*q3));// 弧度转角度Roll*180.0f/3.1415926f;Pitch*180.0f/3.1415926f;Yaw*180.0f/3.1415926f;}六、主循环完整调用流程// 10ms定时任务里循环执行voidIMU_Task(void){// 1. 读原始数据 卡尔曼滤波 单位转g/°sIMU_Read_And_Filter();// 2. 姿态解算IMU_Madgwick_Update(ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps);// 3. 转欧拉角Quat_To_Euler();// 此时直接用 Roll Pitch Yaw 即可}七、你直接可以落地的关键点采样周期IMU_SAMPLE_DT按你的定时器改10ms就填0.01换其他IMU只改ACC_SENS、GYRO_SENS两个灵敏度系数卡尔曼Q/R参数不用乱改我给的是IMU通用最优值整套代码纯标准CSTM32/ESP32/Arduino 直接复制编译无任何依赖。需要我把整套代码整合成一个完整工程模板.h/.c分开、带注释可直接烧录发给你吗想看IMU的原始数据、滤波波形、欧拉角曲线甚至3D姿态用串口/网络 上位机可视化工具最方便、最常用。下面给你一套直接能用、适合STM32/ESP32的工具代码方案。一、最推荐VOFA新手/工程首选免费、轻量、多通道波形、支持FireWater协议、可存数据1. 下载官网https://www.vofa.plus/→ 选 Windows 安装包2. 单片机端代码串口发IMU数据在你之前的IMU_Task()里加一句// 每10ms发一次ax, ay, az, gx, gy, gz, Roll, Pitch, Yaw// 用 FireWater 格式通道名:值,通道名:值,...;\nvoidIMU_Send_VOFA(void){printf(ax:%f,ay:%f,az:%f,gx:%f,gy:%f,gz:%f,roll:%f,pitch:%f,yaw:%f;\r\n,ax_g,ay_g,az_g,gx_dps,gy_dps,gz_dps,Roll,Pitch,Yaw);}3. VOFA 配置打开 VOFA → 左上角「连接」→ 选串口、波特率 115200波形窗口 → 右键「通道」→ 勾选 ax/ay/az/gx/gy/gz/roll/pitch/yaw直接看到9条实时波形可缩放、暂停、保存CSV优点不用写解析直接逗号分隔就能显示最多几十通道看滤波前后对比超方便支持导出数据到 Excel 做分析二、SerialPlot极简开源纯波形、无多余功能、启动快、适合快速调试串口发ax,ay,az,gx,gy,gz,roll,pitch,yaw\n打开 SerialPlot → 选串口 → 自动解析逗号分隔自动画多通道曲线Y轴可自动缩放适合只想快速看波形、不想折腾复杂界面三、Serial Studio功能最强可做仪表盘、3D姿态、波形、自定义面板支持 JSON / 自定义协议能同时显示波形图滤波前后对比3D 立方体实时姿态Roll/Pitch/Yaw数字表盘、柱状图适合做演示、完整调试界面四、Web 在线工具不用安装Web Serial Plotter浏览器直接用https://captainkaz.github.io/web-serial-plotter/支持 Chrome 直接连串口、画多通道波形、3D 姿态显示优点跨平台、零安装、即开即用五、Keil / STM32CubeIDE 逻辑分析仪调试器看不用串口、用 ST-Link/J-Link 直接看内存变量波形把 ax_g, ay_g, Roll 等设为全局变量进入 Debug 模式View → Analysis Windows → Logic Analyzer把变量拖进去 → 全速运行 →直接看波形适合不想改代码、不想发串口、只想临时看变量六、你现在最该用的方案一步到位1. 工具用 VOFA最稳、最通用、工程常用2. 单片机代码直接加// 在你之前的滤波欧拉角代码后加voidIMU_Task(void){IMU_Read_And_Filter();// 原始→滤波→单位转换IMU_Madgwick_Update(...);// 四元数Quat_To_Euler();// Roll/Pitch/Yaw// 发给上位机可视化IMU_Send_VOFA();}// VOFA 发送格式FireWatervoidIMU_Send_VOFA(void){// 通道名自定义方便在VOFA里识别printf(ax_raw:%d,ax_filt:%f,ay_raw:%d,ay_filt:%f,az_raw:%d,az_filt:%f,roll:%f,pitch:%f,yaw:%f;\r\n,ax_raw,ax_g,ay_raw,ay_g,az_raw,az_g,Roll,Pitch,Yaw);}3. 你能看到什么ax_raw / ax_filt原始 vs 滤波后对比看滤波效果roll/pitch/yaw姿态角波形看抖动、漂移9条波形同屏一眼判断卡尔曼效果好不好七、想看 3D 姿态立方体实时动用Serial Studio或Web Serial Plotter3D 模式或IMU Visualizer专门IMU 3D工具把四元数 q0/q1/q2/q3 或欧拉角发上去直接显示3D模型旋转。八、总结你直接选只想看波形、快速调试→VOFA首选极简、启动快→ SerialPlot要3D姿态仪表盘→ Serial Studio不改代码、调试器看→ Keil 逻辑分析仪我可以直接给你VOFA 完整配置文件 单片机发送代码你导入就能用要不要只看滤波效果首选VOFA最简单、傻瓜式、不用改复杂协议专门用来对比原始RAW波形 VS 卡尔曼滤波后波形一眼看出有没有滤干净、有没有滞后。一、第一步下载 VOFA官网直接下https://www.vofa.plus/免费、无广告、不用装驱动。二、第二步单片机只发2路数据就行最简版比如只看X轴滤波对比Y、Z轴同理照着抄。发送格式固定死直接复制// 每10ms发一次printf(raw:%.2f,filt:%.2f;\r\n,(float)ax_raw,ax_filt);rawIMU寄存器原始数据filt卡尔曼滤波后的数据末尾必须分号 ;VOFA自动识别通道三、第三步VOFA 打开设置选串口、波特率 115200协议选FireWater默认就是直接自动弹出两条曲线红线原始抖动数据绿线卡尔曼平滑后数据立马就能看到原始跳得乱七八糟滤波后变平稳、顺滑四、想看三轴全部对比就这样发printf(ax_raw:%.2f,ax_filt:%.2f,ay_raw:%.2f,ay_filt:%.2f,az_raw:%.2f,az_filt:%.2f;\r\n,(float)ax_raw,ax_filt,(float)ay_raw,ay_filt,(float)az_raw,az_filt);VOFA 自动分出6条曲线每轴原始vs滤波并排对比。五、关键要点不用折腾别的不用转单位直接发原始int16转float就行只看波形形状采样周期固定 10ms 发一次波形最稳觉得滤波太飘/太滞后直接改卡尔曼Q、R看波形实时变化可以暂停波形、放大局部、保存截图分析。六、我给你现成可用代码你只要告诉我你的串口波特率是多少我直接给你一段可以复制就跑的打印代码不用你改任何格式打开VOFA就能看滤波对比波形。