4. UKF算法推导（下）：观测更新与协方差修正
好，咱们接着往下聊。上一节我们把UKF的时间更新（预测步）讲完了，说白了就是怎么用上一时刻的状态，猜出当前时刻的状态长什么样。这一节，我们要解决另一个核心问题：有了观测值（比如电压、电流），怎么修正这个猜测？
我个人习惯把UKF的观测更新看作一个「纠偏」过程。你想想看，模型算出来的状态肯定有误差，传感器测回来的数据也有噪声，怎么把这两者巧妙地融合在一起？这就是卡尔曼滤波的精髓所在。
4.1 观测更新的核心思想
观测更新，本质上是在做一件事：用实际测量值，去修正模型预测值。
我在做BMS项目时，遇到过这样一个情况：模型预测SOC还有60%，但实际电压已经掉到3.3V了。这时候如果不做修正，后面估计会越来越偏。UKF的观测更新，就是用来处理这种「模型与实测不一致」的问题。
具体来说，观测更新包含三个关键步骤：
- 预测观测值：用Sigma点通过观测函数，算出理论上应该测到什么值
- 计算卡尔曼增益：决定「相信模型多一点」还是「相信传感器多一点」
- 更新状态与协方差：用实测值修正状态估计
核心公式记忆口诀：
预测观测算残差，增益权重来平衡。
状态更新靠修正，协方差缩显自信。
4.2 预测观测值：从状态空间到测量空间
首先，我们要把上一节得到的Sigma点（经过时间更新后的那些点），通过观测函数映射到测量空间。
观测函数一般长这样：
z = h(x, u) + v其中v是观测噪声，服从N(0, R)分布。在电池模型中，观测函数通常就是电压方程：
V_t = OCV(SOC) - I * R_0 - V_1 - V_2嗯，这里要注意：观测函数h(·)可能是非线性的，比如OCV-SOC曲线就是典型的非线性关系。这正是UKF派上用场的地方——它不需要线性化，直接用Sigma点去传播。
具体计算步骤：
- 对每个Sigma点χ_i，计算对应的观测值：
Z_i = h(χ_i, u_k) - 计算预测观测值的均值：
z_pred = Σ W_i * Z_i - 计算观测协方差矩阵：
S = Σ W_i * (Z_i - z_pred) * (Z_i - z_pred)^T + R
避坑指南：我曾经在计算观测协方差时，忘记加观测噪声协方差矩阵R。结果卡尔曼增益算出来特别大，导致状态估计对测量噪声特别敏感，波动非常剧烈。后来排查了半天才发现是这里漏了。R矩阵的取值很关键，太小了滤波器会「过拟合」测量值，太大了又跟不上变化。
4.3 卡尔曼增益：信任的权衡
卡尔曼增益K，是UKF里最核心的「智慧」所在。它决定了：
- 当测量噪声小（R小）时，K变大，更相信传感器
- 当模型噪声大（Q大）时，K变大，更相信测量值
- 当模型很准（P小）时，K变小，更相信模型
计算卡尔曼增益需要先算状态-观测互协方差：
C = Σ W_i * (χ_i - x_pred) * (Z_i - z_pred)^T然后卡尔曼增益就是：
K = C * S^(-1)这个公式看着简单，但物理意义很深刻。说白了，C衡量的是「状态变化会引起观测怎么变化」，S衡量的是「观测的不确定性有多大」。两者的比值，就是最优的融合权重。
我个人理解：卡尔曼增益就像是一个「自适应调节器」。当模型预测很准、传感器噪声很大时，它自动降低对测量的依赖；反过来，当模型不准、传感器可靠时，它又大胆地使用测量值去修正。这种自适应能力，是卡尔曼滤波最迷人的地方。
4.4 状态更新与协方差修正
有了卡尔曼增益，更新就变得很直接了：
x_updated = x_pred + K * (z_actual - z_pred)这里(z_actual - z_pred)叫做残差或新息。它代表了「实际测量值」与「模型预测值」之间的差异。卡尔曼增益乘以这个残差，就是对状态估计的修正量。
协方差更新也很关键：
P_updated = P_pred - K * S * K^T这个公式告诉我们：每次观测更新后，状态的不确定性都会降低。因为新的测量信息帮助我们缩小了可能的范围。
重要提醒：协方差矩阵P必须保持对称正定。在实际计算中，由于浮点误差，P可能会变得不对称。我建议每次更新后做一次对称化处理：
P = (P + P^T) / 2这个小技巧能避免很多数值稳定性问题。
4.5 完整的观测更新流程
把上面所有步骤串起来，完整的观测更新流程如下：
| 步骤 | 操作 | 公式 |
|---|---|---|
| 1 | Sigma点通过观测函数 | Z_i = h(χ_i, u_k) |
| 2 | 计算预测观测均值 | z_pred = Σ W_i * Z_i |
| 3 | 计算观测协方差 | S = Σ W_i * (Z_i - z_pred)(Z_i - z_pred)^T + R |
| 4 | 计算互协方差 | C = Σ W_i * (χ_i - x_pred)(Z_i - z_pred)^T |
| 5 | 计算卡尔曼增益 | K = C * S^(-1) |
| 6 | 更新状态 | x_updated = x_pred + K * (z_actual - z_pred) |
| 7 | 更新协方差 | P_updated = P_pred - K * S * K^T |
4.6 观测更新流程图
下面这张图，把观测更新的整个流程画出来了。我建议你对照着代码看，会清晰很多。
4.7 代码实现要点
最后，我给出一个观测更新的Python代码片段。这是我在实际项目中用过的模板，你可以直接参考：
def ukf_observation_update(sigma_points, weights, x_pred, P_pred, z_actual, R):
"""
UKF观测更新
sigma_points: 时间更新后的Sigma点 (n_sigma, n_states)
weights: 权重向量
x_pred: 预测状态均值
P_pred: 预测协方差
z_actual: 实际测量值
R: 观测噪声协方差
"""
n_sigma = sigma_points.shape[0]
n_obs = len(z_actual)
# 步骤1: Sigma点通过观测函数
Z = np.array([h_func(sp) for sp in sigma_points])
# 步骤2: 预测观测均值
z_pred = np.dot(weights, Z)
# 步骤3: 观测协方差
S = np.zeros((n_obs, n_obs))
for i in range(n_sigma):
diff = Z[i] - z_pred
S += weights[i] * np.outer(diff, diff)
S += R # 别忘了加观测噪声
# 步骤4: 互协方差
C = np.zeros((x_pred.shape[0], n_obs))
for i in range(n_sigma):
diff_x = sigma_points[i] - x_pred
diff_z = Z[i] - z_pred
C += weights[i] * np.outer(diff_x, diff_z)
# 步骤5: 卡尔曼增益
K = C @ np.linalg.inv(S)
# 步骤6: 状态更新
x_updated = x_pred + K @ (z_actual - z_pred)
# 步骤7: 协方差更新
P_updated = P_pred - K @ S @ K.T
return x_updated, P_updated避坑指南：我曾经在写这段代码时，把np.outer和np.dot搞混了。np.outer是计算外积（向量变矩阵），np.dot是内积（矩阵乘法）。在计算协方差时一定要用np.outer，否则维度会完全不对。
好了，观测更新这部分就讲到这里。核心就是：用卡尔曼增益做加权融合，用残差做修正，用协方差更新来量化不确定性。把这个流程理解透了，UKF的整个框架你就掌握了80%。剩下的就是调参和工程化的问题了。
公众号:蓝海资料掘金营，微信deep3321