4. SOC估算扩展卡尔曼滤波

任务定义

本案例实现一个用于电池SOC（荷电状态）估算的扩展卡尔曼滤波器（EKF）。系统输入为电池端电压与负载电流的离散采样序列，输出为动态SOC估算值及其不确定性（协方差）。采用一阶RC等效电路模型描述电池动态特性，通过EKF在线融合电压电流数据，解决安时积分法因电流传感器偏置和初始SOC误差导致的累积漂移问题。

电池模型与状态方程

选用一阶RC等效电路模型，其连续域状态方程与观测方程如下：

状态变量: x = [SOC, V_RC]^T
输入: u = I (负载电流, 放电为正)
状态方程:
  SOC_dot   = -η·I / Q_max
  V_RC_dot  = -V_RC/(R_CT·C_DL) + I/C_DL
观测方程:
  V_t = OCV(SOC) - V_RC - I·R_0

其中Q_max为最大可用容量(Ah)，η为库仑效率，R_0为欧姆内阻，R_CT为电荷转移电阻，C_DL为双电层电容。OCV(SOC)为开路电压与SOC的非线性映射关系，采用查表插值实现。EKF通过一阶泰勒展开对非线性OCV曲线线性化，实现状态递推与增益更新。

扩展卡尔曼滤波器实现

以下代码完整实现EKF核心算法，包含状态预测、协方差预测、卡尔曼增益计算、状态更新与协方差更新五个步骤。OCV-SOC曲线采用分段线性插值，其导数通过数值差分获得，保证雅可比矩阵计算的准确性。


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

#define N_STATE 2
#define N_MEAS  1

/* 电池参数 */
#define Q_MAX    2.0f    /* 最大容量 Ah */
#define ETA      1.0f    /* 库仑效率 */
#define R0       0.05f   /* 欧姆内阻 Ω */
#define RCT      0.08f   /* 电荷转移电阻 Ω */
#define CDL      1000.0f /* 双电层电容 F */
#define DT       1.0f    /* 采样周期 s */

/* 过程噪声与测量噪声协方差 */
#define Q_NOISE  1e-5f
#define R_NOISE  1e-3f

/* OCV-SOC 查找表 (SOC: 0.0~1.0) */
static const float soc_pts[11] = {0.0f,0.1f,0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f};
static const float ocv_pts[11] = {3.20f,3.35f,3.45f,3.55f,3.63f,3.70f,3.78f,3.88f,4.00f,4.12f,4.20f};

/* 状态: x[0]=SOC, x[1]=V_RC */
static float x[N_STATE] = {0.5f, 0.0f};  /* 初始SOC=50% */
static float P[N_STATE][N_STATE] = {{0.1f,0.0f},{0.0f,0.1f}};

/* 线性插值查表 */
static float interp1(const float* xs, const float* ys, int n, float xq) {
    if (xq <= xs[0]) return ys[0];
    if (xq >= xs[n-1]) return ys[n-1];
    for (int i = 0; i < n-1; i++) {
        if (xq >= xs[i] && xq <= xs[i+1]) {
            float t = (xq - xs[i]) / (xs[i+1] - xs[i]);
            return ys[i] + t * (ys[i+1] - ys[i]);
        }
    }
    return ys[n-1];
}

/* OCV及其导数 */
static float ocv(float soc) { return interp1(soc_pts, ocv_pts, 11, soc); }
static float docv_dsoc(float soc) {
    float eps = 0.001f;
    float soc_lo = fmaxf(0.0f, soc - eps);
    float soc_hi = fminf(1.0f, soc + eps);
    return (ocv(soc_hi) - ocv(soc_lo)) / (soc_hi - soc_lo);
}

/* 扩展卡尔曼滤波单步 */
void ekf_step(float v_meas, float i_meas) {
    float dt = DT;
    float tau = RCT * CDL;

    /* --- 1. 状态预测 (先验) --- */
    float x_pred[N_STATE];
    x_pred[0] = x[0] - (ETA * i_meas / Q_MAX) * dt;
    x_pred[1] = x[1] * expf(-dt/tau) + i_meas * RCT * (1.0f - expf(-dt/tau));

    /* --- 2. 协方差预测 --- */
    /* 雅可比矩阵 F = df/dx */
    float F[N_STATE][N_STATE] = {
        {1.0f, 0.0f},
        {0.0f, expf(-dt/tau)}
    };
    float Qm[N_STATE][N_STATE] = {{Q_NOISE,0.0f},{0.0f,Q_NOISE}};

    float P_pred[N_STATE][N_STATE] = {{0.0f}};
    float tmp[N_STATE][N_STATE] = {{0.0f}};
    /* F * P */
    for (int i = 0; i < N_STATE; i++)
        for (int j = 0; j < N_STATE; j++)
            for (int k = 0; k < N_STATE; k++)
                tmp[i][j] += F[i][k] * P[k][j];
    /* F*P*F^T */
    for (int i = 0; i < N_STATE; i++)
        for (int j = 0; j < N_STATE; j++) {
            P_pred[i][j] = 0.0f;
            for (int k = 0; k < N_STATE; k++)
                P_pred[i][j] += tmp[i][k] * F[j][k];
            P_pred[i][j] += Qm[i][j];
        }

    /* --- 3. 卡尔曼增益 --- */
    /* 观测雅可比 H = dh/dx = [dOCV/dSOC, -1] */
    float H[N_MEAS][N_STATE] = {{docv_dsoc(x_pred[0]), -1.0f}};
    float S = 0.0f;
    float S_inv;
    float K[N_STATE][N_MEAS] = {{0.0f}};

    /* S = H*P_pred*H^T + R */
    float HP[N_MEAS][N_STATE] = {{0.0f}};
    for (int j = 0; j < N_STATE; j++)
        for (int k = 0; k < N_STATE; k++)
            HP[0][j] += H[0][k] * P_pred[k][j];
    for (int j = 0; j < N_STATE; j++)
        S += HP[0][j] * H[0][j];
    S += R_NOISE;
    S_inv = 1.0f / S;

    /* K = P_pred * H^T * S^-1 */
    for (int i = 0; i < N_STATE; i++)
        for (int j = 0; j < N_MEAS; j++) {
            float sum = 0.0f;
            for (int k = 0; k < N_STATE; k++)
                sum += P_pred[i][k] * H[j][k];
            K[i][j] = sum * S_inv;
        }

    /* --- 4. 状态更新 (后验) --- */
    float v_pred = ocv(x_pred[0]) - x_pred[1] - i_meas * R0;
    float innov = v_meas - v_pred;  /* 新息 */
    x[0] = x_pred[0] + K[0][0] * innov;
    x[1] = x_pred[1] + K[1][0] * innov;
    /* SOC限幅 */
    if (x[0] < 0.0f) x[0] = 0.0f;
    if (x[0] > 1.0f) x[0] = 1.0f;

    /* --- 5. 协方差更新 --- */
    float I_KH[N_STATE][N_STATE] = {{1.0f,0.0f},{0.0f,1.0f}};
    for (int i = 0; i < N_STATE; i++)
        for (int j = 0; j < N_STATE; j++)
            for (int k = 0; k < N_MEAS; k++)
                I_KH[i][j] -= K[i][k] * H[k][j];
    for (int i = 0; i < N_STATE; i++)
        for (int j = 0; j < N_STATE; j++) {
            P[i][j] = 0.0f;
            for (int k = 0; k < N_STATE; k++)
                P[i][j] += I_KH[i][k] * P_pred[k][j];
        }
}

/* 测试主函数 */
int main() {
    /* 模拟真实SOC从50%开始，恒定0.5C放电 (1A) */
    float i_load = 1.0f;
    float v_meas;
    float true_soc = 0.5f;

    printf("step  true_soc  ekf_soc   voltage\n");
    for (int t = 0; t < 20; t++) {
        /* 真实电压: 用真实SOC计算OCV并加测量噪声(此处用确定性值) */
        true_soc -= (ETA * i_load / Q_MAX) * DT;
        if (true_soc < 0.0f) true_soc = 0.0f;
        v_meas = ocv(true_soc) - i_load * R0;  /* 忽略RC动态以简化验证 */

        ekf_step(v_meas, i_load);

        printf("%4d  %8.4f  %8.4f  %.4f\n", t, true_soc, x[0], v_meas);
    }
    return 0;
}

EKF数据流与状态递推结构

电流 I(t) 采样输入 电压 V(t) 测量输入 状态预测 SOC⁻ = SOC + f(I)·dt V_RC⁻ = g(V_RC, I) 协方差预测 P⁻ = F·P·Fᵀ + Q 卡尔曼增益 K = P⁻·Hᵀ·(H·P⁻·Hᵀ+R)⁻¹ 状态更新 SOC = SOC⁻ + K·(V−V̂) V_RC = V_RC⁻ + K·(V−V̂) 协方差更新 P = (I−K·H)·P⁻ 输出: SOC(t), P(t) 反馈回路: 后验状态作为下一时刻先验

编译与运行

将上述代码保存为ekf_soc.c，使用任意C99兼容编译器编译运行：

gcc -o ekf_soc ekf_soc.c -lm
./ekf_soc

验证方法与判定标准

程序输出20个仿真步的SOC估算结果。判定标准如下：

  • 收敛性验证：初始SOC设为50%，真实SOC从50%开始以0.5C速率下降。EKF估算SOC应跟随真实SOC变化，稳态误差小于2%（即|SOC_ekf − SOC_true| < 0.02）。
  • 新息合理性：卡尔曼增益K应保持有界且非负，协方差P对角元素单调递减并趋于稳定值，表明滤波器收敛。
  • 预期输出特征：由于代码中真实电压未加入RC动态和测量噪声，EKF应快速收敛至真实SOC轨迹。第20步时，估算SOC与真实SOC之差应小于0.005（理想情况下接近0）。

若需验证滤波器对初始误差的修正能力，可将x[0]初始值改为0.7（70%），观察EKF是否能在数步内将估算值拉回真实SOC附近，这体现了EKF融合电压测量修正SOC漂移的核心能力。

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