8.2 卡尔曼滤波与系统辨识

8.2 卡尔曼滤波与系统辨识

本节摘要：卡尔曼滤波是高斯噪声下最小方差的状态估计器，以"预测—更新"两步循环动态融合模型预测与传感器观测。本节从一维定位问题推导并实现滤波器，解读卡尔曼增益的"信谁多一点"逻辑，再介绍从输入输出数据反推模型参数的系统辨识（最小二乘法），补齐"没有模型就先辨识"的实战入口。

传感器会说谎，模型也会

无人叉车沿走廊行驶，里程计说"走了 4.92 米"，激光测距说"位置在 5.15 米"。信谁？两个都不完全可信：里程计会打滑累积误差，激光有测量噪声。朴素的办法是取平均，但更聪明的问法是：两个信息源各有一个不确定度（方差），谁更确定就多信谁——按方差的倒数加权融合，正是最小方差意义上的最优。卡尔曼滤波把这个思想推广到动态系统：状态随时间演化（模型给预测），传感器持续观测（数据给修正），滤波器按两路不确定度的此消彼长动态分配信任。1960 年卡尔曼发表这一算法，此后从阿波罗导航到 GPS 融合到手机计步，它成为估计理论的通用语。

一维卡尔曼：两步循环

模型：真实位置按 xₖ₊₁ = xₖ + v（匀速推步）演化，过程噪声 w 表示打滑等扰动；观测 zₖ = xₖ + vₖ，测量噪声方差 r 已知。滤波器维护两个量：状态估计 x̂ 与估计不确定度 p（方差）。每一步先预测（用模型推一步，不确定度变大），再更新（用观测修正，不确定度缩小）：

import numpy as np

def kalman_1d(zs, q=1e-3, r=0.05, x0=0.0, p0=1.0):
    x, p = x0, p0
    ests, gains = [], []
    for z in zs:
        # 预测：模型推进一步
        x_pred = x            # 静态模型；若匀速则 x_pred = x + v*dt
        p_pred = p + q
        # 更新：按不确定度分配信任
        K = p_pred / (p_pred + r)        # 卡尔曼增益
        x = x_pred + K * (z - x_pred)
        p = (1 - K) * p_pred
        ests.append(x); gains.append(K)
    return np.array(ests), np.array(gains)

rng = np.random.default_rng(3)
truth = 5.0                                   # 叉车真实位置
zs = truth + rng.normal(0, np.sqrt(0.05), 60) # 激光观测
ests, gains = kalman_1d(zs)
print(f"观测均值误差 {abs(zs.mean()-truth):.4f}，"
      f"滤波末段误差 {abs(ests[-10:].mean()-truth):.4f}")
print(f"增益从 {gains[0]:.2f} 收敛到 {gains[-1]:.2f}")
​

两个行为值得细看。增益的动态性：初始不确定度大（p0=1），增益接近 1，滤波器几乎全信观测；随着滤波收敛，不确定度缩小，增益降到 0.3 附近，观测只剩三成话语权——信任的分配随信心自动调节，这是"滤波"二字的精髓。平滑性：滤波估计的方差远小于单次观测，等于用时间换精度。

矩阵版与辨识：模型从哪来

多维状态与多传感器场景，同样的两步循环写成矩阵形式，增益 K = PCᵀ(CPCᵀ+R)⁻¹，P 由预测步的协方差传播与更新步的收缩交替驱动。融合 GPS（位置准、更新慢）与惯性测量（变化率准、会漂移）的多传感器组合，只需把它们装进 C 与 R 的相应位置——不确定度的账本，就是传感器融合的语法。

而 LQR 与卡尔曼都绕不开的前提是模型 A、B、C 从哪来。物理推导（7.1 的路子）在机理复杂时失效，系统辨识改从数据反推：给系统输入激励，记录输入输出序列，用最小二乘拟合参数。一阶系统 yₖ₊₁ = a·yₖ + b·uₖ 的辨识只需解一个线性方程：

def identify_arx(u, y):
    """最小二乘辨识 y[k+1] = a*y[k] + b*u[k]"""
    Y = y[1:]
    Phi = np.column_stack([y[:-1], u[:-1]])
    theta, *_ = np.linalg.lstsq(Phi, Y, rcond=None)
    resid = Y - Phi @ theta
    return theta, resid.std()

rng = np.random.default_rng(8)
a_true, b_true = 0.92, 0.08
u = rng.uniform(-1, 1, 400)
y = np.zeros(400)
for k in range(399):
    y[k+1] = a_true * y[k] + b_true * u[k] + rng.normal(0, 0.02)
theta, noise = identify_arx(u, y)
print(f"辨识结果 a={theta[0]:.4f}（真值0.92） b={theta[1]:.4f}（真值0.08） 残差σ={noise:.4f}")
​

辨识质量取决于两条：激励充分（输入要持续"晃"系统，阶跃到底的输入辨识不出高频模态——这叫持续激励条件）；数据干净（噪声大时用辅助变量法、预测误差法等带_bias 校正的变体）。辨识出模型后接 LQR，就构成"数据驱动控制"的最小闭环；而当连辨识都困难（非线性严重、环境漂移），就轮到下一节的路线——不建模型直接学策略。

⚠️ 常见坑：调小过程噪声 q 求平滑，结果滤波器"变瞎"。q 太小意味着滤波器过度信任模型，观测被长期忽略，模型漂移时估计会缓慢跑偏还反应迟钝。q 与 r 的比例本质是"模型可信度 vs 传感器可信度"，按工程现实标定，不要为了曲线好看单边压缩。

本节要点回顾

• 卡尔曼滤波 = 预测 + 更新两步循环，按不确定度动态分配模型与观测的信任权重；
• 增益即信任分配：不确定度大则信观测，收敛后增益下降、观测话语权回落；
• 多传感器融合的语法是协方差账本：各路信息按精度装进 R，自动加权；
• 模型可从数据辨识：最小二乘配持续激励输入，是最朴素的系统辨识；
• q 与 r 之比 = 模型与传感器的可信度之比，按现实标定而非按曲线美观。

模型与估计都齐了。最后一节抽掉最后一块地基——模型本身：马尔可夫决策过程与强化学习，让最优控制在纯试错中诞生。