4. 数字孪生体状态估计的卡尔曼滤波实现

问题设定：一维匀速运动孪生体的状态估计

本节以一个沿直线运动的小车为物理实体，建立其数字孪生体的状态估计链路。系统状态取位置 x 与速度 v，状态向量 X = [x, v]ᵀ。物理实体按恒定速度运动，但传感器只能直接测量位置，且测量值叠加了高斯白噪声；孪生体需要利用含噪的位置观测，实时估计出比原始测量更准确的位置与速度。

离散时间状态方程与观测方程如下，采样周期取 Δt = 0.1 s：

X(k) = F · X(k-1) + w(k-1),   w ~ N(0, Q)
Z(k) = H · X(k)   + v(k),     v ~ N(0, R)

F = [[1, Δt],      H = [1, 0]
     [0,  1]]

Q = q · [[Δt³/3, Δt²/2],     R = [r]
          [Δt²/2, Δt   ]]

其中 q 为过程噪声功率谱密度，反映孪生体对“小车可能受未建模扰动（如微小加速度）”的信任程度；r 为位置观测噪声方差。卡尔曼滤波的核心是在预测与更新之间用卡尔曼增益 K 动态加权：K 越大越信任观测，越小越信任模型预测。

卡尔曼滤波递推实现

下面给出可直接运行的 Python 实现。代码同时生成含噪观测序列、执行滤波、并统计滤波前后的位置与速度估计误差，用于定量比较精度。

import numpy as np

np.random.seed(7)
dt = 0.1
N  = 200
t  = np.arange(N) * dt

# 真实状态：x0=0 m, v0=1.2 m/s
x_true = np.zeros(N); v_true = np.full(N, 1.2)
x_true = 1.2 * t

# 传感器：仅测位置，噪声标准差 0.8 m
r = 0.8**2
z = x_true + np.random.normal(0, 0.8, N)

# 过程噪声功率谱密度
q = 0.05
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
Q = q * np.array([[dt**3/3, dt**2/2],
                  [dt**2/2, dt    ]])
R = np.array([[r]])

# 初始化
X = np.array([[0.0], [0.0]])   # 状态估计
P = np.eye(2) * 1.0            # 估计协方差

est_x = np.zeros(N); est_v = np.zeros(N)
for k in range(N):
    # 预测
    X = F @ X
    P = F @ P @ F.T + Q
    # 更新
    y = z[k] - (H @ X)[0, 0]          # 新息
    S = H @ P @ H.T + R               # 新息协方差
    K = P @ H.T @ np.linalg.inv(S)    # 卡尔曼增益
    X = X + K * y
    P = (np.eye(2) - K @ H) @ P
    est_x[k] = X[0, 0]; est_v[k] = X[1, 0]

# 精度对比（RMSE）
rmse_meas = np.sqrt(np.mean((z - x_true)**2))
rmse_est  = np.sqrt(np.mean((est_x - x_true)**2))
print(f"位置 RMSE  观测: {rmse_meas:.3f} m   滤波: {rmse_est:.3f} m")
print(f"速度 RMSE  滤波: {np.sqrt(np.mean((est_v - v_true)**2)):.3f} m/s")

典型输出（示例数据，非实测）：观测位置 RMSE 约 0.79 m，滤波后位置 RMSE 约 0.18 m，速度 RMSE 约 0.35 m/s。可见滤波显著抑制了观测噪声，同时从位置序列中“反推”出速度。

过程噪声与观测噪声参数的影响

q 与 r 的比值决定滤波器的“信任倾向”，是调参的核心。下表给出同一组观测数据下不同参数组合的定性表现，便于建立调参直觉。

滤波链路与状态更新关系

下图展示孪生体状态估计的完整信号链：物理实体产生真实状态，传感器输出含噪观测，卡尔曼滤波器在预测与更新之间循环，最终输出位置与速度估计，并与真实状态比较得到精度指标。

结果判读与验证要点

• 位置精度提升：滤波后位置 RMSE 应明显低于原始观测 RMSE，说明滤波器有效抑制了观测噪声。
• 速度可观测性：虽然传感器不直接测速度，但通过位置序列的时序关联，滤波器能估计出速度，验证了状态可观测性。
• 新息白化检验：若滤波器参数正确，新息序列 y(k) 应近似零均值白噪声；若出现自相关，说明 Q 或 R 设置不合理。
• 协方差收敛：估计协方差 P 应随迭代收敛到稳态值，反映估计不确定度的下降。