【内含安装包】STM32 卡尔曼滤波三步接入:把带噪传感器数据送进控制回路

原创2026-10-28 12:59:031,881 阅读

【内含安装包】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。

五分钟跑起来:滤波库最小接入步骤

  1. 拉取代码 git clone https://gitcode.com/open-source-toolkit/abda0,解开 卡尔曼滤波.zip,里面是 KF.c、KF.h、arm_math.h 和一排预编译库 📦
  2. 工程里加入 KF.c,引用 KF.h 和 arm_math.h;再按芯片挑库:带 FPU 的 F4 选 arm_cortexM4lf_math.lib,M7 核选 lfsp/lfdp 系列;
  3. 建实例、配维度和量测映射,填模型矩阵:
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
  1. 主循环里把新读数、控制量写进 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 的中文注释逐条读一遍,比教科书直观。

登录后查看全文
STM32上的卡尔曼滤波器实现