魔法滤镜：在单片机上实现卡尔曼滤波 (1D)

提到卡尔曼滤波，大家的印象往往是满屏幕的矩阵运算、协方差矩阵求逆，直接把单片机算爆。
其实，对于单变量（一维）系统（例如只测量温度，或只测量高度），卡尔曼滤波可以简化为几个简单的加减乘除公式，任何 8 位机都能轻松运行。

1. 核心思想

卡尔曼滤波的核心是数据融合。它融合了两个来源的信息：

**预测值 (Prediction)**：基于物理模型猜出来的。
**测量值 (Measurement)**：传感器实际读出来的。

它并不完全相信模型（可能有误差），也不完全相信传感器（有噪声）。它通过计算两者各自的不确定性（方差），来决定更听谁的。

2. 只有 5 个公式

对于一维系统：

$x$: 估计值 (Estimate)
$p$: 估计协方差 (Error Covariance) —— 代表我们对自己估计值的“不自信程度”
$q$: 过程噪声 (Process Noise) —— 比如环境风的扰动
$r$: 测量噪声 (Measurement Noise) —— 传感器本身的精度
$k$: 卡尔曼增益 (Kalman Gain) —— 0~1 之间的权重系数

预测阶段 (Time Update)

$x_{pred} = x_{last}$ (假设状态不变，或者根据速度推算)
$p_{pred} = p_{last} + q$ (每次预测都会增加不确定性)

更新阶段 (Measurement Update)

$k = p_{pred} / (p_{pred} + r)$ (计算增益：如果不确定性 p 很大，k 就接近 1，更相信测量值)
$x_{new} = x_{pred} + k * (measured - x_{pred})$ (修正估计值)
$p_{new} = (1 - k) * p_{pred}$ (更新不确定性：测量后我们更自信了，p 变小)

3. C 语言实现

仅需一个结构体和几行代码：

typedef struct {
    float x;  // 状态估计值
    float p;  // 估计误差协方差
    float q;  // 过程噪声协方差 (如 0.01)
    float r;  // 测量噪声协方差 (如 1.0)
    float k;  // 卡尔曼增益
} KalmanFilter;

void Kalman_Init(KalmanFilter *kf, float q, float r, float initial_val) {
    kf->x = initial_val;
    kf->p = 1.0f;
    kf->q = q;
    kf->r = r;
}

float Kalman_Update(KalmanFilter *kf, float measurement) {
    // 1. 预测
    kf->p = kf->p + kf->q;

    // 2. 更新
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (measurement - kf->x);
    kf->p = (1.0f - kf->k) * kf->p;

    return kf->x;
}

4. 调参指南

Q 值：代表你认为系统状态变化有多快。
- Q 大 -> Filter 认为状态变化快 -> 响应更灵敏，但噪声平滑效果变差。
- Q 小 -> Filter 认为状态稳定 -> 滞后更明显，但波形更平滑。
R 值：代表你认为传感器有多烂。
- R 大 -> 不信传感器 -> 也就是更信预测值 -> 滤波效果极强，波形非常平滑。

总结

一维卡尔曼滤波本质上就是一个自适应权重的低通滤波器。相比于固定权重的互补滤波，它能动态调整信任度，达到收敛速度和稳定性的最佳平衡。

hello world