4、一维EKF实战：温度估计的EKF实现

好了，前面我们把EKF的理论框架搭完了。说实话，光看公式推导，很多人会卡住。我当年第一次接触EKF时，也是盯着那五个公式看了三天，脑子里全是矩阵和雅可比。

但真正让我开窍的，是动手写一个最简单的例子——一维温度估计。你想想看，状态量只有一个：温度。观测也只有一个：温度计读数。没有复杂的坐标变换，没有多传感器融合，纯粹就是看看EKF到底怎么工作的。

这一章，我们就来干这件事。

4.1 问题定义：一个恒温箱的温度估计

假设我们有一个恒温箱，目标温度是25°C。但实际情况是：

• 箱体不是完全隔热的，会有热量散失
• 加热器有随机扰动
• 温度计有测量噪声

我们要做的，就是根据温度计的读数，实时估计箱内的真实温度。

嗯，这里要注意：真实温度我们永远不知道，只能通过滤波去逼近它。

4.2 系统建模

一维系统的建模其实很直观。我们先定义状态向量：

x = [温度]  (1×1 的标量)

状态方程（预测模型）：

假设温度变化遵循一阶动态：

x_k = x_{k-1} + Δt * (-α * (x_{k-1} - T_env)) + w_k

其中：

• α 是散热系数（我习惯取0.1左右）
• T_env 是环境温度（比如20°C）
• w_k 是过程噪声，服从 N(0, Q)

观测方程：

z_k = x_k + v_k

其中 v_k 是测量噪声，服从 N(0, R)

你看，这个模型很简单。但我在项目中遇到过一个问题：很多人直接把状态方程写成 x_k = x_{k-1} + w_k，忽略了物理过程。结果滤波出来的温度曲线平滑是平滑了，但跟真实值偏差很大。说白了，模型越准，滤波效果越好。

4.3 EKF的五个步骤（一维版本）

因为是一维，所有矩阵都退化成了标量。我们一步步来：

步骤1：预测状态

x_pred = x_est + Δt * (-α * (x_est - T_env))

步骤2：预测协方差

P_pred = F * P_est * F + Q

这里的 F 是状态转移矩阵的雅可比。在一维情况下，F = ∂f/∂x = 1 - α*Δt

步骤3：计算卡尔曼增益

K = P_pred * H / (H * P_pred * H + R)

观测矩阵 H = 1（因为观测直接就是状态）

步骤4：更新状态估计

x_est = x_pred + K * (z - H * x_pred)

步骤5：更新协方差

P_est = (1 - K * H) * P_pred

是不是很眼熟？其实就是标准卡尔曼滤波的公式。因为系统是线性的，雅可比退化为常数，EKF就退化成KF了。

4.4 代码实现

我个人习惯用Python写原型，因为调试方便。下面是一个完整的实现：

import numpy as np
import matplotlib.pyplot as plt

# 参数设置
dt = 0.1          # 采样间隔 0.1秒
alpha = 0.1       # 散热系数
T_env = 20.0      # 环境温度
T_true = 25.0     # 真实温度（模拟用）

# 噪声参数
Q = 0.01          # 过程噪声方差
R = 0.1           # 测量噪声方差

# 初始估计
x_est = 20.0      # 初始猜测
P_est = 1.0       # 初始协方差

# 模拟数据
np.random.seed(42)
n_steps = 100
true_temps = []
meas_temps = []
est_temps = []

for k in range(n_steps):
    # 模拟真实温度变化（带过程噪声）
    w = np.random.normal(0, np.sqrt(Q))
    T_true = T_true + dt * (-alpha * (T_true - T_env)) + w
    true_temps.append(T_true)
    
    # 模拟测量（带测量噪声）
    v = np.random.normal(0, np.sqrt(R))
    z = T_true + v
    meas_temps.append(z)
    
    # --- EKF 核心 ---
    # 预测
    x_pred = x_est + dt * (-alpha * (x_est - T_env))
    F = 1 - alpha * dt
    P_pred = F * P_est * F + Q
    
    # 更新
    H = 1.0
    K = P_pred * H / (H * P_pred * H + R)
    x_est = x_pred + K * (z - H * x_pred)
    P_est = (1 - K * H) * P_pred
    
    est_temps.append(x_est)

# 绘图
plt.figure(figsize=(10, 6))
plt.plot(true_temps, 'g-', label='真实温度', linewidth=2)
plt.plot(meas_temps, 'r.', label='测量值', alpha=0.5)
plt.plot(est_temps, 'b-', label='EKF估计', linewidth=2)
plt.legend()
plt.xlabel('时间步')
plt.ylabel('温度 (°C)')
plt.title('一维EKF温度估计')
plt.grid(True)
plt.show()

4.5 参数调优经验

调参是EKF应用中最头疼的部分。我总结了一个表格，方便你对照：

参数	增大效果	减小效果	我的建议
Q（过程噪声）	滤波器更相信测量，响应快但抖动大	滤波器更相信模型，平滑但滞后	先设小，看效果再慢慢调大
R（测量噪声）	滤波器更相信模型，平滑但可能偏离	滤波器更相信测量，跟踪快但噪声大	根据传感器手册的精度来设
P₀（初始协方差）	初始收敛快，但前期波动大	初始收敛慢，但更稳定	设大一点（比如1~10），让滤波器自己调整

4.6 核心逻辑流程图

下面我用SVG画了一张图，把一维EKF的整个流程串起来。你看完应该能对「预测-更新」这个循环有更直观的理解。

4.7 小结

一维EKF虽然简单，但它是理解多维EKF的基石。你想想看，如果连一个温度都估不准，怎么去估无人机的位置、机械臂的姿态？

我个人建议你亲手跑一遍上面的代码。改改Q和R，看看曲线怎么变。改改初始P₀，看看收敛速度。这种手感，是看多少书都换不来的。

嗯，这一章就到这里。代码我已经放在GitHub上了，需要的可以自己去拉。