【附下载链接】STM32 卡尔曼滤波 3 步跑通:传感器数据去噪实战指南
【附下载链接】STM32 卡尔曼滤波 3 步跑通:传感器数据去噪实战指南
在 STM32 上做卡尔曼滤波,是做过数据采集的人都绕不开的一道坎。理论读过了,公式却停留在纸面上,真要落地到单片机还得自己啃矩阵运算和类型适配。这个仓库直接给了一套 STM32 卡尔曼滤波器实现:KF_Init 负责初始化,Kalman_Filter_Update 负责每周期滤波,拿到就能接进自己的工程。
为什么单片机上需要它
先看场景:IMU 采回来的加速度在真值附近上下跳,GPS 位置会漂移,电机转速反馈带毛刺,这些随机噪声轻则图表难看,重则直接把控制回路带崩。简单的滑动平均能抹平一点,但滞后明显。卡尔曼滤波的核心思路其实就一句话:递归 + 预测校正——先用上一步状态做出预测,新采样到来时再按最优增益把预测拉回观测附近,每个时刻都输出一组兼顾噪声和动态的估计。目标跟踪、状态估计这类 STM32 目标跟踪滤波场景之所以都选它,就是因为单片机资源有限,只能递归算,没法攒一批数据再处理。
这套 STM32 实现做了哪些事
接口被拆成了初始化和更新两个动作,这是它用起来顺手的地方。KF_Init(kf, xhatsize, usize, zsize) 里把状态维数、控制维数、观测维数传进去,内部就把 F、H、Q、R、P 等矩阵的内存分配和结构搭建全做完了;之后主循环只需要不断喂新样本调用 Kalman_Filter_Update,拿回滤波后的状态。配置和运行彻底解耦,你不需要理解内部矩阵怎么转置、怎么求逆。实现细节可以看 源码 里的 KF.c 和 KF.h。
类型定义这块它做了一层统一封装:浮点统一用 fp32,整型走 int8_t、uint16_t 这套标准 typedef,Keil 和 GCC 下行为一致,不会出现隐式转换警告。嵌入式代码能不能从"能编译"走到"能移植",往往就差这一层。
性能上它没有重造轮子,矩阵运算全部走 CMSIS-DSP 接口(arm_mat_mult_f32、arm_mat_inverse_f32 这一套),压缩包里还按 Cortex-M4、M7 的不同配置(大小端、是否带 FPU)附了预编译数学库。链接时按自己芯片型号选对应的那个即可,比如带 FPU 的 M4 直接挂 arm_cortexM4lf_math.lib,矩阵运算就能走硬件加速路径,不用额外写优化代码。
3 步把它跑起来
- 把
KF.c、KF.h连同arm_math.h加进工程,再按芯片型号链上对应的armmath-lib数学库,把库路径和包含目录配好。 - 定义一个
kF_t实例,调KF_Init传入状态维、控制维、观测维三个数字。 - 每个采样周期把新样本写进
MeasuredVector,调用Kalman_Filter_Update,返回值就是滤波后的估计。
下面这段是最小接入骨架,演示初始化加一个更新周期长什么样:
kF_t kf;
KF_Init(&kf, 2, 0, 1); // 状态 2 维,无控制量,观测 1 维
// 填好 F/H/Q/R 后,采样周期里:
kf.MeasuredVector[0] = rawData;
float *x = Kalman_Filter_Update(&kf);
调参这块有几句掏心窝的叮嘱,都是踩过的坑:
Q 和 R 别直接抄网上的数。这两个噪声协方差一个决定滤波收敛快慢、一个决定平滑程度,必须对照你自己传感器的噪声特性和被跟踪对象的运动特性重新调,"滤波输出不收敛"九成出在这。
数学库的匹配很娇气:库路径、大小端、FPU 变体、编译器的浮点精度选项,任何一个对不上,要么链接不过,要么算出来差好几个数量级,排查起来很费时间。
状态向量也别拍脑袋。选哪几个量当状态分量,直接决定 F、H 矩阵长什么样,维度和传感器实际配置对不上时滤波根本跑不起来,前期多花十分钟想清楚,能省后面几小时的调试。
这套代码的价值在于,嵌入式卡尔曼滤波器实现里最脏的活——内存分配、矩阵封装、库适配——都替你干完了,你只管把传感器接上去。下一步建议:先在自己的传感器上跑一次 KF_Init,把一路原始数据喂进去,看看输出再谈调参。