自动控制 / 系统课时

预测状态均值如何随模型变化

预测状态均值如何随模型变化

已知上一采样时刻的位置、速度估计,以及随后施加的控制输入,怎样在新测量到来前预测下一时刻状态?本文从条件期望推导均值预测式,再用一次加速运动和 NumPy 计算,得到可核对的位置、速度先验。

学习目标

  • 区分上一时刻后验与下一时刻先验,说明预测可以使用哪些信息。
  • 根据采样间隔构造状态转移矩阵和控制矩阵,独立算出位置、速度预测值,并检查单位与数组形状。

预测从哪份信息出发

约定状态为 xk=[pk,vk]Tx_k=[p_k,v_k]^{\mathsf T},第一项是位置,第二项是速度。估计符号 x^a∣b\hat{x}_{a|b} 的左下标表示被估计的时刻,右下标表示已经使用的测量截至哪一拍。

假设刚处理完第 k−1k-1 拍测量,后验估计 x^k−1∣k−1\hat{x}_{k-1|k-1} 为位置 2 m、速度 1 m/s。间隔 0.1 s 后,要得到第 kk 拍的先验 x^k∣k−1\hat{x}_{k|k-1};此时尚未使用新的位置测量 zkz_k。

这里的“均值”是给定已有信息后的状态期望,不是把最近几次位置读数直接求平均。预测从完整的位置、速度估计出发;单独把上一条位置测量代入,既丢失速度,也丢失此前滤波积累的信息。

对状态方程取条件期望

采用离散线性模型,约定 FkF_k、BkB_k 描述从 k−1k-1 到 kk 的传播,uk−1u_{k-1} 是该区间的已知控制输入:

xk=Fkxk−1+Bkuk−1+wk−1.x_k=F_kx_{k-1}+B_ku_{k-1}+w_{k-1}.

FkF_k 传播已有状态,BkB_k 把控制量映射为状态增量,wk−1w_{k-1} 表示未知扰动。若状态有 2 个分量、控制有 1 个分量,则 FkF_k 为 2×22\times2,BkB_k 为 2×12\times1,两项相加后仍是 2×12\times1 列向量。

把此前测量、初始信息和已知控制记为 Ik−1\mathcal I_{k-1}。假设矩阵与控制量对这些信息而言已知,且过程噪声独立于已有信息、均值为零,因此 E[wk−1∣Ik−1]=0\mathbb E[w_{k-1}\mid\mathcal I_{k-1}]=0。对模型两侧取条件期望:

x^k∣k−1=E[xk∣Ik−1]=FkE[xk−1∣Ik−1]+Bkuk−1+0=Fkx^k−1∣k−1+Bkuk−1.\begin{aligned} \hat{x}_{k|k-1} &=\mathbb E[x_k\mid\mathcal I_{k-1}]\\ &=F_k\mathbb E[x_{k-1}\mid\mathcal I_{k-1}]+B_ku_{k-1}+0\\ &=F_k\hat{x}_{k-1|k-1}+B_ku_{k-1}. \end{aligned}

推导只用了期望的线性性质与噪声的零条件均值,这一步本身不要求高斯分布。所得预测式可与 Welch 与 Bishop 的技术报告 表 1-1 核对,其中状态转移矩阵记作 AA。genui{"citation":{"ref":"turn2view0"}}

用恒加速度算出两项增量

本例把控制量定义为加速度,假定它在一个采样区间内恒为 uk−1=2 m/s2u_{k-1}=2\,\mathrm{m/s^2}。取 Δt=0.1 s\Delta t=0.1\,\mathrm{s},位置、速度模型的矩阵为:

Fk=[1Δt01],Bk=[12Δt2Δt].F_k=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}, \qquad B_k=\begin{bmatrix}\frac12\Delta t^2\\\Delta t\end{bmatrix}.

转移矩阵先按原速度推进位置,并保留原速度;控制矩阵再增加 12uΔt2\tfrac12u\Delta t^2 的位置变化和 uΔtu\Delta t 的速度变化。本例 BkB_k 两个系数分别为 0.005 s20.005\,\mathrm{s^2}、0.1 s0.1\,\mathrm{s},乘加速度后单位正好对应米和米每秒。

计算部分 位置分量(m) 速度分量(m/s)
模型传播 $F_k\hat{x}_{k-1 k-1}$ 2.100
控制增量 Bkuk−1B_ku_{k-1} 0.010 0.200
两项相加得到先验 2.110 1.200

上一时刻后验经模型传播,再叠加已知控制增量得到下一时刻先验

用 NumPy 验证均值预测

在已安装 NumPy 的 Python 3.10 及以上环境运行下面的完整脚本。状态、输入均使用二维列向量;@ 执行矩阵乘法,不能替换为逐元素相乘的 *。

python
import numpy as np

dt = 0.1
x_post = np.array([[2.0], [1.0]])
u = np.array([[2.0]])
# 状态顺序为位置、速度;控制量为这一采样区间的恒定加速度。
F = np.array([[1.0, dt], [0.0, 1.0]])
B = np.array([[0.5 * dt**2], [dt]])
# 三个结果均为(2, 1)列向量,避免混用一维数组触发广播。
model_part = F @ x_post
control_part = B @ u
x_prior = model_part + control_part
assert x_prior.shape == (2, 1)
assert np.allclose(x_prior, [[2.11], [1.20]])
print(f'模型传播: p={model_part[0, 0]:.3f} m, v={model_part[1, 0]:.3f} m/s')
print(f'控制增量: dp={control_part[0, 0]:.3f} m, dv={control_part[1, 0]:.3f} m/s')
print(f'先验估计: p={x_prior[0, 0]:.3f} m, v={x_prior[1, 0]:.3f} m/s')
print(f'状态形状: {x_prior.shape}')

预期输出:

text
模型传播: p=2.100 m, v=1.000 m/s
控制增量: dp=0.010 m, dv=0.200 m/s
先验估计: p=2.110 m, v=1.200 m/s
状态形状: (2, 1)

若把 x_post 写成形状为 (2,) 的一维数组,模型传播结果与形状为 (2, 1) 的控制增量相加时,会广播成 (2, 2)。代码未必报错,所以示例同时断言数值和形状,避免把错误矩阵当作状态。

采样或输入变化时怎样检查

下面两组检查都从位置 2 m、速度 1 m/s 的同一后验重新起算,不连续推进。修改 dt 或 u 后重建 F、B,并把 np.allclose 中的期望列向量换成表中数值。

采样间隔(s) 加速度(m/s²) 预测位置(m) 预测速度(m/s)
0.1 0 2.100 1.000
0.2 2 2.240 1.400

第二行的位置是 2+1×0.2+12×2×0.22=2.242+1\times0.2+\tfrac12\times2\times0.2^2=2.24。时间间隔加倍,速度控制增量加倍,位置控制增量则变为四倍;只修改转移矩阵而沿用旧控制矩阵,会漏算这部分变化。

控制量的物理含义也必须与矩阵一致:若输入改成力 ff,物体质量为 mm,则应先换算加速度 u=f/mu=f/m,或把控制矩阵改为原矩阵除以 mm。两种做法只能选一种,否则会重复除以质量。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

预测状态均值如何随模型变化

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通