C 语言工业级通用组件手写 23：卡尔曼滤波（简易版）

#ifndef FILTER_KALMAN_H
#define FILTER_KALMAN_H

#include <stdint.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    int32_t x;      // 估计值
    int32_t p;      // 估计误差协方差
    int32_t q;      // 过程噪声
    int32_t r;      // 测量噪声
} filter_kalman_t;

void filter_kalman_init(filter_kalman_t *k, int32_t init_val, int32_t q, int32_t r);
int32_t filter_kalman_calc(filter_kalman_t *k, int32_t meas);

#ifdef __cplusplus
}
#endif

#endif

filter_kalman.c

复制代码

#include "filter_kalman.h"

#define KALMAN_SCALE 1024

void filter_kalman_init(filter_kalman_t *k, int32_t init_val, int32_t q, int32_t r) {
    if (!k) return;
    k->x = init_val * KALMAN_SCALE;
    k->p = 32 * KALMAN_SCALE;
    k->q = q;
    k->r = r * KALMAN_SCALE;
}

int32_t filter_kalman_calc(filter_kalman_t *k, int32_t meas) {
    if (!k) return meas;

    int32_t m = meas * KALMAN_SCALE;

    // 预测
    k->p += k->q;

    // 卡尔曼增益
    int32_t gain = (k->p * KALMAN_SCALE) / (k->p + k->r);

    // 更新
    k->x += gain * (m - k->x) / KALMAN_SCALE;
    k->p = (KALMAN_SCALE - gain) * k->p / KALMAN_SCALE;

    return k->x / KALMAN_SCALE;
}

五、实战演示

复制代码

#include <stdio.h>
#include "filter_kalman.h"

int main(void) {
    filter_kalman_t k;
    filter_kalman_init(&k, 1000, 5, 100);

    int32_t data[] = {1000, 1002, 998, 1050, 999, 1001, 997, 1003, 1100, 1000};
    for (int i = 0; i < 10; i++) {
        int32_t res = filter_kalman_calc(&k, data[i]);
        printf("raw:%5d  kalman:%5d\n", data[i], res);
    }
    return 0;
}

六、进阶优化方向

带加速度 / 变速度模型的自适应卡尔曼
互补滤波 + 卡尔曼级联
动态调整 Q/R 噪声参数
多传感器融合扩展（加速度 + 陀螺仪）

七、面试考点与易错坑点

面试问答

Q1：卡尔曼滤波为什么适合动态信号？

答：它是预测 + 修正闭环，不需要窗口缓存，响应极快，同时能最优降噪。

Q2：Q 和 R 分别代表什么？

答：Q 是过程噪声，Q 越大越信任测量；R 是测量噪声，R 越大越信任预测。

Q3：卡尔曼和一阶滞后滤波区别？

答：一阶滞后系数固定；卡尔曼增益动态自适应，效果更智能。

Q4：为什么要用定点运算？

答：低端 MCU 无 FPU，浮点慢且精度差，定点运算更快更稳定。

Q5：卡尔曼滤波缺点是什么？

答：对非高斯噪声效果一般；强尖峰干扰前需加限幅。

Q6：工程上如何调 Q、R？

答：噪声大则 R 调大；变化快则 Q 调大。

常见坑点

Q/R 设置极端导致滤波失效
未做定点缩放，数值溢出
多通道共用实例导致状态错乱
初始值设置不合理导致启动漂移
直接用浮点版本导致 MCU 卡顿

总结

卡尔曼滤波是嵌入式高端动态降噪天花板算法，完美兼顾响应速度与平滑度，是传感器融合、电机控制、姿态检测的必备组件。本篇轻量化定点实现无门槛移植，工业级鲁棒稳定，可直接用于量产项目。

创作不易，如果对你有帮助，欢迎点赞、收藏、转发。