卡尔曼滤波SOC算法模型
扩展卡尔曼滤波(EKF)与自适应卡尔曼滤波(AEKF) SOC估算实现文档
目录
1. [理论基础](#理论基础)
2. [电池等效电路模型](#电池等效电路模型)
3. [EKF算法实现](#ekf算法实现)
4. [AEKF算法实现](#aekf算法实现)
5. [系统集成方案](#系统集成方案)
6. [代码实现](#代码实现)
7. [参数调优](#参数调优)
8. [测试验证](#测试验证)
一、理论基础
1.1 卡尔曼滤波原理
卡尔曼滤波是一种最优递归状态估计算法，通过融合系统模型预测和测量值，得到最优状态估计。
基本方程:
状态预测: x̂(k|k-1) = F·x̂(k-1|k-1) + B·u(k-1)
协方差预测: P(k|k-1) = F·P(k-1|k-1)·F^T + Q
卡尔曼增益: K(k) = P(k|k-1)·H^T·(H·P(k|k-1)·H^T + R)^(-1)
状态更新: x̂(k|k) = x̂(k|k-1) + K(k)·(z(k) - H·x̂(k|k-1))
协方差更新: P(k|k) = (I - K(k)·H)·P(k|k-1)
```
1.2 扩展卡尔曼滤波(EKF)
EKF是KF在非线性系统中的应用，通过线性化处理非线性函数。
关键点:
- 使用雅可比矩阵线性化非线性函数
- 适用于弱非线性系统
- 计算复杂度适中
1.3 自适应卡尔曼滤波(AEKF)
AEKF在EKF基础上，自适应调整过程噪声Q和测量噪声R。
优势:
- 适应系统参数变化
- 提高鲁棒性
- 减少模型误差影响
二、电池等效电路模型
2.1 一阶RC模型（推荐用于BMS）
R0
+---/\/\/---+
| |
| +--C1--+
| | |
| R1 |
| |
+Vbatt GND
状态方程:
SOC(k+1) = SOC(k) - (η·I(k)·Δt) / Qn
V1(k+1) = V1(k)·exp(-Δt/(R1·C1)) + I(k)·R1·(1-exp(-Δt/(R1·C1)))
观测方程:
V(k) = OCV(SOC(k)) - V1(k) - I(k)·R0
其中:
- `SOC`: 电池荷电状态
- `V1`: RC网络电压
- `I`: 电池电流（充电为正，放电为负）
- `R0`: 欧姆内阻
- `R1, C1`: RC网络参数
- `OCV`: 开路电压（SOC的函数）
- `η`: 库伦效率
- `Qn`: 电池额定容量
2.2 二阶RC模型（更高精度）
R0
+---/\/\/---+
| |
| +--C1--+ +--C2--+
| | | | |
| R1 | R2 |
| | |
+Vbatt GND GND
状态方程:
SOC(k+1) = SOC(k) - (η·I(k)·Δt) / Qn
V1(k+1) = V1(k)·exp(-Δt/(R1·C1)) + I(k)·R1·(1-exp(-Δt/(R1·C1)))
V2(k+1) = V2(k)·exp(-Δt/(R2·C2)) + I(k)·R2·(1-exp(-Δt/(R2·C2)))
观测方程:
V(k) = OCV(SOC(k)) - V1(k) - V2(k) - I(k)·R0
三、EKF算法实现
3.1 状态向量定义
c
// 状态向量: x = [SOC, V1, V2]^T (二阶模型)
// 或 x = [SOC, V1]^T (一阶模型)
typedef struct {
float soc; // SOC (0.0 ~ 1.0)
float v1; // RC网络1电压 (V)
float v2; // RC网络2电压 (V) - 仅二阶模型
} EkfState_t;
3.2 系统参数结构
c
typedef struct
{
// 电池参数
float qn; // 额定容量 (Ah)
float r0; // 欧姆内阻 (Ω)
float r1; // RC网络1电阻 (Ω)
float c1; // RC网络1电容 (F)
float r2; // RC网络2电阻 (Ω) - 仅二阶模型
float c2; // RC网络2电容 (F) - 仅二阶模型
float eta; // 库伦效率 (0.95 ~ 1.0)
// OCV-SOC查表 (需要根据电池特性标定)
float ocv_table[101]; // OCV表，对应SOC 0%~100%
// 噪声协方差
float q_soc; // SOC过程噪声
float q_v1; // V1过程噪声
float q_v2; // V2过程噪声 (仅二阶模型)
float r_volt; // 电压测量噪声
// 模型类型
u8 model_order; // 1: 一阶模型, 2: 二阶模型
} EkfParam_t;
```
3.3 EKF核心算法
3.3.1 状态预测（预测步）
c
/**
* @brief EKF状态预测
* @param ekf: EKF结构体指针
* @param current: 电流 (A), 充电为正，放电为负
* @param dt: 时间间隔 (s)
*/
void EkfPredict(EkfStruct_t *ekf, float current, float dt)
{
float soc_prev = ekf->state.soc;
float v1_prev = ekf->state.v1;
float v2_prev = ekf->state.v2;
// 1. 状态预测
// SOC更新 (安时积分)
ekf->state.soc = soc_prev - (ekf->param.eta * current * dt) / (ekf->param.qn * 3600.0f);
// 限制SOC范围 [0, 1]
if(ekf->state.soc > 1.0f) ekf->state.soc = 1.0f;
if(ekf->state.soc < 0.0f) ekf->state.soc = 0.0f;
// RC网络电压更新
float tau1 = ekf->param.r1 * ekf->param.c1; // 时间常数
float exp1 = expf(-dt / tau1);
ekf->state.v1 = v1_prev * exp1 + current * ekf->param.r1 * (1.0f - exp1);
if(ekf->param.model_order == 2) {
float tau2 = ekf->param.r2 * ekf->param.c2;
float exp2 = expf(-dt / tau2);
ekf->state.v2 = v2_prev * exp2 + current * ekf->param.r2 * (1.0f - exp2);
}
// 2. 计算状态转移矩阵 F
// F = [1, 0, 0; 0, exp(-dt/tau1), 0; 0, 0, exp(-dt/tau2)]
ekf->F[0][0] = 1.0f;
ekf->F[0][1] = 0.0f;
ekf->F[1][0] = 0.0f;
ekf->F[1][1] = exp1;
if(ekf->param.model_order == 2) {
ekf->F[0][2] = 0.0f;
ekf->F[1][2] = 0.0f;
ekf->F[2][0] = 0.0f;
ekf->F[2][1] = 0.0f;
ekf->F[2][2] = expf(-dt / (ekf->param.r2 * ekf->param.c2));
}
// 3. 计算过程噪声协方差矩阵 Q
ekf->Q[0][0] = ekf->param.q_soc;
ekf->Q[1][1] = ekf->param.q_v1;
if(ekf->param.model_order == 2) {
ekf->Q[2][2] = ekf->param.q_v2;
}
// 4. 协方差预测: P(k|k-1) = F·P(k-1|k-1)·F^T + Q
MatrixMultiply(ekf->F, ekf->P, ekf->FP, ekf->state_dim, ekf->state_dim, ekf->state_dim);
MatrixTranspose(ekf->F, ekf->FT, ekf->state_dim, ekf->state_dim);
MatrixMultiply(ekf->FP, ekf->FT, ekf->P_pred, ekf->state_dim, ekf->state_dim, ekf->state_dim);
MatrixAdd(ekf->P_pred, ekf->Q, ekf->P_pred, ekf->state_dim, ekf->state_dim);
}
```
3.3.2 状态更新（更新步）
```c
/**
* @brief EKF状态更新
* @param ekf: EKF结构体指针
* @param voltage: 测量电压 (V)
* @param current: 测量电流 (A)
*/
void EkfUpdate(EkfStruct_t *ekf, float voltage, float current)
{
// 1. 计算观测值（预测电压）
float ocv = GetOcvBySoc(ekf->state.soc, ekf->param.ocv_table);
float v_pred = ocv - ekf->state.v1 - current * ekf->param.r0;
if(ekf->param.model_order == 2) {
v_pred -= ekf->state.v2;
}
// 2. 计算观测矩阵 H (雅可比矩阵)
// H = [dOCV/dSOC, -1, -1] (二阶模型)
// H = [dOCV/dSOC, -1] (一阶模型)
float dOCV_dSOC = GetOcvDerivativeBySoc(ekf->state.soc, ekf->param.ocv_table);
ekf->H[0] = dOCV_dSOC;
ekf->H[1] = -1.0f;
if(ekf->param.model_order == 2) {
ekf->H[2] = -1.0f;
}
// 3. 计算新息（测量残差）
float innovation = voltage - v_pred;
// 4. 计算新息协方差: S = H·P·H^T + R
float S = 0.0f;
for(int i = 0; i < ekf->state_dim; i++) {
float temp = 0.0f;
for(int j = 0; j < ekf->state_dim; j++) {
temp += ekf->H[j] * ekf->P_pred[j][i];
}
S += temp * ekf->H[i];
}
S += ekf->param.r_volt;
// 5. 计算卡尔曼增益: K = P·H^T / S
for(int i = 0; i < ekf->state_dim; i++) {
float temp = 0.0f;
for(int j = 0; j < ekf->state_dim; j++) {
temp += ekf->P_pred[i][j] * ekf->H[j];
}
ekf->K[i] = temp / S;
}
// 6. 状态更新: x(k|k) = x(k|k-1) + K·innovation
ekf->state.soc += ekf->K[0] * innovation;
ekf->state.v1 += ekf->K[1] * innovation;
if(ekf->param.model_order == 2) {
ekf->state.v2 += ekf->K[2] * innovation;
}
// 限制SOC范围
if(ekf->state.soc > 1.0f) ekf->state.soc = 1.0f;
if(ekf->state.soc < 0.0f) ekf->state.soc = 0.0f;
// 7. 协方差更新: P(k|k) = (I - K·H)·P(k|k-1)
for(int i = 0; i < ekf->state_dim; i++) {
for(int j = 0; j < ekf->state_dim; j++) {
ekf->P[i][j] = ekf->P_pred[i][j] - ekf->K[i] * ekf->H[j] * ekf->P_pred[i][j];
}
}
// 确保P矩阵对称正定
MatrixMakeSymmetric(ekf->P, ekf->state_dim);
}
```
3.4 OCV-SOC查表函数
```c
/**
* @brief 根据SOC查表获取OCV
* @param soc: SOC值 (0.0 ~ 1.0)
* @param ocv_table: OCV表指针
* @return OCV值 (V)
*/
float GetOcvBySoc(float soc, const float *ocv_table)
{
if(soc <= 0.0f) return ocv_table[0];
if(soc >= 1.0f) return ocv_table[100];
int index = (int)(soc * 100.0f);
float ratio = soc * 100.0f - index;
// 线性插值
return ocv_table[index] + (ocv_table[index + 1] - ocf_table[index]) * ratio;
}
/**
* @brief 计算OCV对SOC的导数
* @param soc: SOC值 (0.0 ~ 1.0)
* @param ocv_table: OCV表指针
* @return dOCV/dSOC (V)
*/
float GetOcvDerivativeBySoc(float soc, const float *ocv_table)
{
if(soc <= 0.0f) {
return (ocv_table[1] - ocv_table[0]) * 100.0f;
}
if(soc >= 1.0f) {
return (ocv_table[100] - ocv_table[99]) * 100.0f;
}
int index = (int)(soc * 100.0f);
// 使用中心差分法
float dOCV = (ocv_table[index + 1] - ocv_table[index - 1]) * 50.0f;
return dOCV;
}
```
四、AEKF算法实现
4.1 自适应噪声协方差调整
AEKF在EKF基础上，根据新息序列自适应调整Q和R。
```c
/**
* @brief AEKF自适应噪声协方差调整
* @param ekf: EKF结构体指针
* @param innovation: 新息（测量残差）
*/
void AekfAdaptiveNoise(EkfStruct_t *ekf, float innovation)
{
static float innovation_buffer[AEKF_BUFFER_SIZE];
static int buffer_index = 0;
static int buffer_count = 0;
// 1. 保存新息到缓冲区
innovation_buffer[buffer_ind
未完待续......................