4、卡尔曼滤波入门：状态空间模型、预测与更新步骤、一维卡尔曼滤波实现

各位同学，欢迎来到第四讲。

前面我们聊了电池建模和开路电压法。说实话，那些方法在静态场景下还行。但电动牙刷一开机，电机嗡嗡转起来，电压瞬间波动，传统方法就抓瞎了。

这时候，卡尔曼滤波就该登场了。

我个人觉得，卡尔曼滤波是电池电量估算里最优雅的工具之一。它不直接告诉你电量是多少，而是通过「预测」和「修正」两个步骤，一步步逼近真实值。有点像你闭着眼睛走路，先迈一步（预测），然后睁开眼看看偏了多少（更新），再调整方向。

4.1 状态空间模型：把问题装进数学框子里

要理解卡尔曼滤波，先得搞懂状态空间模型。说白了，就是把我们的系统用数学语言描述出来。

一个典型的离散时间系统，包含两个核心方程：

  • 状态方程：描述系统内部状态如何随时间演变。
  • 观测方程：描述我们如何通过传感器测量到系统状态。

拿电动牙刷的电池来说，我们关心的状态是电池的荷电状态（SOC）。但SOC不能直接测量，我们只能测到电压和电流。

状态空间模型长这样：

x(k) = A * x(k-1) + B * u(k-1) + w(k-1)
z(k) = H * x(k) + v(k)

这里：

  • x(k)：k时刻的系统状态（比如SOC）
  • u(k)：控制输入（比如电流）
  • z(k)：观测值（比如电压）
  • A：状态转移矩阵
  • B：控制输入矩阵
  • H：观测矩阵
  • w(k)：过程噪声（系统模型的不确定性）
  • v(k)：观测噪声（传感器的测量误差）

嗯，这里要注意。w和v都是高斯白噪声，均值是0。这个假设很重要，卡尔曼滤波就是建立在噪声是高斯分布的基础上的。

我在项目中遇到过一个问题：有同事把过程噪声设得太小，结果滤波器对测量值过度信任，反而放大了噪声。后来我建议他把w的协方差矩阵调大一些，效果立马好了。

小提示： 状态空间模型的关键在于选择合适的A、B、H矩阵。对于电池SOC估算，A通常取1（因为SOC变化缓慢），B取-Δt/Capacity（电流积分），H取开路电压与SOC的关系曲线。

4.2 预测与更新：卡尔曼滤波的两步舞

卡尔曼滤波的核心就两个步骤：预测和更新。你想想看，这就像你每天早上出门前，先预估一下今天会不会下雨（预测），然后到了外面感受一下实际天气（更新），再调整你的判断。

4.2.1 预测步骤（时间更新）

预测步骤，就是根据上一时刻的状态，推算当前时刻的状态。公式如下：

// 状态预测
x_pred = A * x_est + B * u

// 误差协方差预测
P_pred = A * P_est * A^T + Q

这里：

  • x_pred：预测的状态
  • P_pred：预测的误差协方差（表示我们对预测的信任程度）
  • Q：过程噪声协方差矩阵

Q值越大，说明我们对模型越不信任，预测的误差范围就越大。

4.2.2 更新步骤（测量更新）

更新步骤，就是用实际测量值来修正预测值。公式如下：

// 卡尔曼增益
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)

// 状态更新
x_est = x_pred + K * (z - H * x_pred)

// 误差协方差更新
P_est = (I - K * H) * P_pred

这里：

  • K：卡尔曼增益（核心中的核心）
  • R：观测噪声协方差矩阵
  • z：实际测量值

卡尔曼增益K决定了我们更相信预测值还是测量值。如果K大，说明我们更相信测量值；如果K小，说明我们更相信预测值。

核心理解： 卡尔曼滤波的本质就是「加权平均」。它根据预测和测量的不确定性，动态地计算出一个最优的权重（卡尔曼增益），然后把两者融合起来。

我曾经调试过一个项目，电池电压测量噪声特别大。一开始我把R设得很小，结果滤波器跟着测量值乱跳。后来我把R调大了，滤波器就平滑多了。说白了，R和Q的调参，就是卡尔曼滤波的「艺术」所在。

4.3 一维卡尔曼滤波实现：从理论到代码

好了，理论讲完了，咱们直接上代码。一维卡尔曼滤波是最简单的形式，状态只有一个变量（比如SOC）。

我习惯用C语言写嵌入式代码，这里给一个完整的实现：

#include <stdio.h>
#include <math.h>

// 一维卡尔曼滤波器结构体
typedef struct {
    float x;  // 状态估计值
    float P;  // 误差协方差
    float Q;  // 过程噪声协方差
    float R;  // 观测噪声协方差
    float K;  // 卡尔曼增益
} KalmanFilter1D;

// 初始化滤波器
void Kalman_Init(KalmanFilter1D *kf, float init_x, float init_P, float Q, float R) {
    kf->x = init_x;
    kf->P = init_P;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0;
}

// 预测步骤
void Kalman_Predict(KalmanFilter1D *kf, float u) {
    // 状态预测：x = x + u（假设A=1，B=1）
    kf->x = kf->x + u;
    
    // 误差协方差预测：P = P + Q
    kf->P = kf->P + kf->Q;
}

// 更新步骤
void Kalman_Update(KalmanFilter1D *kf, float z) {
    // 计算卡尔曼增益
    kf->K = kf->P / (kf->P + kf->R);
    
    // 状态更新
    kf->x = kf->x + kf->K * (z - kf->x);
    
    // 误差协方差更新
    kf->P = (1 - kf->K) * kf->P;
}

// 使用示例
int main() {
    KalmanFilter1D kf;
    
    // 初始化：初始SOC=50%，初始误差=10，Q=0.01，R=1
    Kalman_Init(&kf, 50.0f, 10.0f, 0.01f, 1.0f);
    
    // 模拟测量数据（带噪声的SOC值）
    float measurements[] = {49.5f, 48.8f, 48.2f, 47.6f, 47.0f};
    float control_input = -0.5f;  // 每次放电0.5%
    
    printf("原始测量值\t滤波后值\n");
    for(int i = 0; i < 5; i++) {
        // 预测
        Kalman_Predict(&kf, control_input);
        
        // 更新
        Kalman_Update(&kf, measurements[i]);
        
        printf("%.2f\t\t%.2f\n", measurements[i], kf.x);
    }
    
    return 0;
}

这段代码虽然简单，但已经包含了卡尔曼滤波的全部精髓。你想想看，每次调用Predict和Update，滤波器都在做一次「预测-修正」的循环。

避坑指南： 我曾经在初始化时把P设得太小（比如0.01），结果滤波器收敛特别慢，前几十个数据点基本没用。后来我把P设成10，滤波器很快就跟上真实值了。记住：初始P值代表你对初始状态的信任程度，如果不确定，就设大一点。

4.4 参数调优：Q和R的博弈

卡尔曼滤波的性能，很大程度上取决于Q和R的取值。我总结了一个经验表格：

参数 物理含义 调大效果 调小效果
Q（过程噪声） 模型的不确定性 更相信测量值，响应快但噪声大 更相信模型，响应慢但平滑
R（观测噪声） 传感器的噪声水平 更相信预测值，平滑但滞后 更相信测量值，灵敏但易受干扰

对于电动牙刷的电池SOC估算，我建议：

  • Q：取0.001~0.01（因为SOC变化很慢，模型相对准确）
  • R：取0.5~2（取决于电压测量电路的噪声水平）

嗯，这里要注意。实际项目中，Q和R往往需要根据实验数据来标定。我习惯的做法是：先采集一段真实数据，然后用不同的Q和R跑一遍滤波器，看哪个组合的误差最小。

小技巧： 如果你不确定Q和R怎么设，可以试试自适应卡尔曼滤波。它能根据残差（测量值与预测值的差）自动调整Q和R。不过那是进阶内容了，咱们后面会讲到。

好了，这一讲就到这里。卡尔曼滤波入门其实不难，关键是要理解「预测-更新」这个循环，以及Q和R的物理含义。下一讲，我们会把一维卡尔曼滤波扩展到二维，真正用于电池SOC和电压的联合估算。

记住：理论是死的，应用是活的。多动手调参，你就能找到感觉。

← 上一章 📖 返回目录 下一章 →