扩展卡尔曼滤波(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模型推荐用于BMSR0---/\/\/---| || --C1--| | || R1 || |Vbatt GND状态方程:SOC(k1) SOC(k) - (η·I(k)·Δt) / QnV1(k1) 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(k1) SOC(k) - (η·I(k)·Δt) / QnV1(k1) V1(k)·exp(-Δt/(R1·C1)) I(k)·R1·(1-exp(-Δt/(R1·C1)))V2(k1) 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 系统参数结构ctypedef 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. 计算过程噪声协方差矩阵 Qekf-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 QMatrixMultiply(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 Rfloat 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 / Sfor(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·innovationekf-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未完待续......................