扩展卡尔曼滤波多状态在线估计：融合加速度计与位置、速度测量，同时估计位置、速度、温度、电流与 SOC

最近在整理状态估计的资料时碰到一个很常见但不太容易一步到位的问题手头只有加速度计和位置、速度传感器的测量数据但真正关心的量并不只是位置和速度还有温度、电流和电池荷电状态SOC。这些量要么没有直接传感器要么直接测量成本高、噪声大更合理的做法是用滤波算法从可测信号里把它们反推出来。我于是围绕扩展卡尔曼滤波EKF多状态估计位置 速度 温度 电流 SOC 估计这些关键词搜索想找一个结构完整、可以直接跑起来复现的例子而不是只给公式的零散博客。后来在 MoHub 上看到一个《卡尔曼滤波多状态估计案例》它正好把位置、速度、温度、电流、SOC 五个状态放进同一个 EKF 框架用加速度计输入配合位置、速度测量做在线递推还附带了原始数据和一份 Julia 脚本。这个案例在解决什么问题从工程角度看这个案例演示的是多物理量耦合状态估计。车辆运动过程中位置和速度遵循运动学关系电机电流由加速度扭矩决定电流流过电机绕组产生 I²R 发热同时热量向环境散失决定温度变化电流对电池积分则决定 SOC 的下降。这几个量在物理上相互关联但只有位置和速度是可直接测量的传感器数据还带噪声温度、电流、SOC 都需要靠模型递推和滤波校正得到。案例的输入是一段 20 秒的传感器数据采样间隔 0.1 秒共 201 个采样点包含四列关键信号加速度输入控制量、真实位置、真实速度以及带噪声的位置、速度测量值。其中真实位置和真实速度只在评估精度时使用不参与滤波。输出则是五个状态的估计序列以及位置、速度相对真实值的均方根误差RMSE。运行方法案例用 Syslab 的 Julia 环境运行依赖 CSV、DataFrames、LinearAlgebra、Statistics 和 TyPlot 这几个包。目录里有三个文件kalman_filter.jl主脚本包含 EKF 实现、RMSE 评估和 TyPlot 可视化kalman_filter_state_estimation_raw_data.csv原始传感器数据提示词.txt任务说明。把脚本和数据放在同一目录下在 Syslab 里执行include(kalman_filter.jl)脚本会先在控制台打印 RMSE 与各状态摘要再生成一张包含 10 个子图的图窗分别展示位置、速度的估计与误差温度、电流、SOC 的 ±2σ 置信区间加速度输入以及新息序列。运行不需要额外配置默认参数已经写在脚本里。原理一个五状态 EKF状态向量定义为x [p, v, T, I, SOC]分别对应位置m、速度m/s、温度°C、电流A和 SOC%。量测向量z [z_pos, z_vel, z_cur]有三个分量位置和速度直接来自传感器电流则是伪量测由加速度按z_cur k_cur·|a|推算。预测步的过程模型把这五个量耦合在一起位置p_k p_{k-1} v_{k-1}·Δt ½·a_k·Δt²匀加速运动学速度v_k v_{k-1} a_k·Δt温度T_k T_{k-1} [α·I² − (T_{k-1} − T_amb)/(R_th·C_th)]·ΔtI²R 发热加散热电流I_k k_cur·|a_k|由加速度直接决定取绝对值处理制动工况SOCSOC_k SOC_{k-1} − I_k·Δt/(C_bat·3600)·100安时积分其中电流转换系数k_cur m·r_wheel/gear_ratio本例取整车质量 1000 kg、车轮半径 0.3 m、减速比 10算出来是 30发热系数α R_motor/C_thermal。其余车辆与电机参数电机绕组电阻 0.5 Ω、系统热容 50 J/K、热阻 0.2 K/W、环境温度 25 °C、电池容量 50 Ah。因为过程模型是非线性的温度项、电流的绝对值、SOC 与电流的乘积标准卡尔曼滤波不适用脚本改用了 EKF先计算雅可比矩阵F ∂f/∂x对模型线性化再按协方差传播P F·P·Fᵀ Q→ 新息y z − H·x→ 新息协方差S H·P·Hᵀ R→ 卡尔曼增益K P·Hᵀ·S⁻¹→ 状态与协方差校正的顺序递推。量测矩阵 H 把 5 维状态映射到 3 维量测空间只有位置、速度、电流三个量被直接观测温度和 SOC 完全依赖模型传播与协方差的间接校正。噪声协方差的设置也体现了对模型的判断过程噪声Q diag([0.005, 0.05, 0.1, 5.0, 1e-6])·Δt电流的不确定性最大因为它每一步都被输入重置SOC 最小积分过程缓慢量测噪声R diag([0.5, 0.3, 2.0])。初始状态里位置和速度取首次测量值温度设为环境温度加 5 °C电流由初始加速度推算SOC 假设为 80%。运行结果分析先看最有说服力的精度指标 RMSE。脚本把 EKF 估计值和原始测量值分别与真实值对比我实际跑出来的结果是位置 RMSE测量 0.6311 m → EKF 0.1375 m精度提升约 78.2%速度 RMSE测量 0.2026 m/s → EKF 0.0728 m/s精度提升约 64.1%。图1位置估计对比图2速度估计对比从图 1、图 2 能直观看到原始测量值红色散点在真实值附近抖动明显而 EKF 估计蓝色实线几乎贴着黑色真实曲线走。位置测量噪声在 20 秒内把误差拉到了 0.6 m 量级滤波后收敛到 0.1 m 量级这正是卡尔曼滤波用模型约束去平滑传感器噪声的典型效果。图3位置与速度估计误差对比图 3 的误差曲线进一步说明问题测量误差红色整体波动大且不随时间收敛EKF 估计误差蓝色围绕零线小幅波动幅度明显更小速度误差同样被压制在一个更窄的带宽内。需要说明的是EKF 估计误差并不是严格趋近于零而是收敛到一个由过程噪声和量测噪声共同决定的稳定水平这是符合卡尔曼滤波理论的正常现象。图4温度估计及 ±2σ 置信区间温度是完全没有直接量测的状态只能靠模型传播和协方差间接校正。跑出来的温度从初始 29.95 °C 逐步降到约 28.09 °C峰值约 30.79 °C。由于电流产生的 I²R 发热量不大峰值电流约 13.4 A对应发热有限温度没有明显升高反而因为散热项缓慢向环境温度靠拢。图 4 里 ±2σ 置信带由协方差对角元开方得到反映估计不确定性——温度因缺乏直接观测置信带明显比位置、速度宽。图5电流估计及 ±2σ 置信区间电流这一步比较特殊它在预测步完全由加速度 |a| 决定k_cur·|a|又通过伪量测被观测了一次所以估计值基本贴着伪量测走只在数值上做了微小修正。电流从初始 0 A 随加速度变化峰值约 13.41 A对应加速度峰值 0.447 m/s²13.41 ≈ 30 × 0.447量级是自洽的。图6SOC 估计及 ±2σ 置信区间SOC 从假设的 80% 开始20 秒内只下降了约 0.055%终值约 79.9452%。这个幅度很小原因是这段工况的电流平均值低、时间又短安时积分累积有限。对 SOC 估计而言20 秒的短时间窗更多是演示库仑计数 滤波的机制而不是给出有工程意义的 SOC 变化量。总结这个案例的价值在于把一个多物理量耦合的状态估计问题用尽量少的代码完整跑通了。它把运动学、电热、安时积分三个子模型放进同一个 5 维状态空间让读者能看清 EKF 的预测—更新循环到底每一步在算什么。这是一个很适合用来入门多状态 EKF 的案例数据、脚本、参数都给全了跑一遍就能对照着看模型怎么预测、量测怎么校正、协方差怎么收敛。对正在做传感器融合、状态估计或者想理解电池 SOC 与温度在线估计的人来说把它的过程模型和雅可比矩阵逐行看一遍比只看公式有用得多。案例地址https://mohub.net/education/26096/summary