【内含安装包】STM32 卡尔曼滤波三步接入:把带噪传感器数据送进控制回路
【内含安装包】STM32 卡尔曼滤波三步接入:把带噪传感器数据送进控制回路
IMU 读数抖得你敢直接进环路吗?低通滤波一上,响应又迟钝。你要的是"先猜一步、再对照实测校准"这种玩法——这个仓库就是一套 STM32 卡尔曼滤波实现,核心的 KF.c/KF.h 加上预编译的 CMSIS-DSP 库都给你备齐了。
它解决什么问题:测量噪声与响应滞后,一次搞定
传感器原始值直接进控制回路,是嵌入式项目里的高频痛点:噪声大,控制量跟着抖;换个低通滤波吧,它是"被动平均",滞后又躲不掉。卡尔曼滤波把这件事拆成"预测 → 修正"的循环:先用状态模型往前推一步,再用实测值校准,修正多少由 Q、R 两个方差自动裁决——测量越吵越信模型,模型越不确定越信测量。
这个项目把这个循环做成了 KF.c、KF.h 两个文件,顺手处理了几个手写实现容易忽略的工程细节:
- 多路测量可以"缺哪路补哪路":某路测值为 0 时,自动重构 H、R 矩阵,只融合有效通道;
User_Func0_f到User_Func6_f是夹在每个方程之间的 7 个钩子,后续想换 EKF/UKF 变体有地方下手;- zip 里自带
arm_cortexM4lf_math.lib、arm_cortexM7lfsp_math.lib等预编译库,不用自己编译 CMSIS-DSP。
五分钟跑起来:滤波库最小接入步骤
- 拉取代码
git clone https://gitcode.com/open-source-toolkit/abda0,解开卡尔曼滤波.zip,里面是 KF.c、KF.h、arm_math.h 和一排预编译库 📦 - 工程里加入 KF.c,引用 KF.h 和 arm_math.h;再按芯片挑库:带 FPU 的 F4 选
arm_cortexM4lf_math.lib,M7 核选lfsp/lfdp系列; - 建实例、配维度和量测映射,填模型矩阵:
kF_t kf;
KF_Init(&kf, 2, 1, 2); // 状态维2、控制维1、量测维2
kf.MeasurementMap[0] = 1; // 量测0 对应状态1(1 起始)
kf.MeasurementDegree[0] = 1.0f; // 对应的 H 矩阵元素
kf.MeasurementMap[1] = 2;
kf.MeasurementDegree[1] = 1.0f;
kf.UseAutoAdjustment = 1; // 量测缺失时自动剔除该通道
// 再按模型填 F、Q、R
- 主循环里把新读数、控制量写进
kf.MeasuredVector、kf.ControlVector,调用Kalman_Filter_Update(&kf),返回的fp32 *(即kf.FilteredValue)就是融合后的状态。
库内部五个方程——预测状态、预测方差、算增益、修正状态、修正方差——全部封装好了,不用你手写矩阵推导。
参数速查:Q、R、F 怎么调
| 参数 | 含义 | 调参建议 |
|---|---|---|
kf.Q_data |
过程噪声协方差:模型偏离现实的程度 | 从小起步,输出追不上突变再放大对角线 |
kf.R_data / MatR_DiagonalElements |
测量噪声协方差:传感器可信度 | 调大输出更平滑,调小则更快跟随测量 |
kf.F_data |
状态转移矩阵:状态每拍如何演化 | 恒速模型按采样周期 dt 填,单位先对齐 |
kf.B_data |
控制输入增益 | 有控制量才需要,否则 usize 直接给 0 |
kf.StateMinVariance |
P 矩阵对角线下限 | 给小正值,防 P 塌缩后滤波器"听不进"新测量 |
kf.UseAutoAdjustment |
通道为 0 时自动重构 H/R | 多源融合开,单通道固定场景不用开 |
MeasurementMap / MeasurementDegree |
每路量测对应哪个状态维、比例多少 | 按 z = H·x 填,Map 是 1 起始索引 |
实操起点:Q、R 对角线按实测抖动幅度的平方给初值,跑起来看输出——拖沓就降 R 升 Q,抖动就反过来。
常见坑:三种典型现象、原因与解法
- 现象:链接报 undefined
arm_mat_mult_f32之类符号。 原因:漏加对应arm_cortexMx_math.lib,或者 M4 芯片链上了 M7 的库。 怎么办:按芯片型号、字节序、有无 FPU 三要素挑库,别想当然。 - 现象:输出恒为 0,或跑一阵子发散。
原因:
KF_Init把所有矩阵都清零了,你却没填 F、Q(不用自动调整时还得自己填 H、R)。 怎么办:初始化后按上表补齐模型矩阵,并给StateMinVariance一个小正值。 - 现象:芯片不是 STM32F407 时,一编译就报错。
原因:KF.h 顶部写死了
#include "stm32f407xx.h"和#define ARM_MATH_CM4。 怎么办:把 include 换成自家芯片头文件,ARM_MATH_CMx宏挪到 IDE 全局预定义,避免重复定义。
什么时候适合、什么时候别用
适合:多通道传感器融合、目标跟踪、既要平滑又要预测的状态估计;你能写出(哪怕近似的)状态空间模型;芯片是带 FPU 的 Cortex-M4/M7。
不适合:单通道只想抹平毛刺,两行 IIR/低通更快;模型明显非线性,基类方程不够用、又不想借 User_Func 钩子自己扩展;无 FPU 的 M0/M3 核,这套预编译库没覆盖到。
完整说明见 README.md;五个方程各自的矩阵写法,对着 卡尔曼滤波.zip 里 KF.c 的中文注释逐条读一遍,比教科书直观。
登录后查看全文