自动控制 / 系统课时
预测状态均值如何随模型变化
预测状态均值如何随模型变化
已知上一采样时刻的位置、速度估计,以及随后施加的控制输入,怎样在新测量到来前预测下一时刻状态?本文从条件期望推导均值预测式,再用一次加速运动和 NumPy 计算,得到可核对的位置、速度先验。
学习目标
- 区分上一时刻后验与下一时刻先验,说明预测可以使用哪些信息。
- 根据采样间隔构造状态转移矩阵和控制矩阵,独立算出位置、速度预测值,并检查单位与数组形状。
预测从哪份信息出发
约定状态为 ,第一项是位置,第二项是速度。估计符号 的左下标表示被估计的时刻,右下标表示已经使用的测量截至哪一拍。
假设刚处理完第 拍测量,后验估计 为位置 2 m、速度 1 m/s。间隔 0.1 s 后,要得到第 拍的先验 ;此时尚未使用新的位置测量 。
这里的“均值”是给定已有信息后的状态期望,不是把最近几次位置读数直接求平均。预测从完整的位置、速度估计出发;单独把上一条位置测量代入,既丢失速度,也丢失此前滤波积累的信息。
对状态方程取条件期望
采用离散线性模型,约定 、 描述从 到 的传播, 是该区间的已知控制输入:
传播已有状态, 把控制量映射为状态增量, 表示未知扰动。若状态有 2 个分量、控制有 1 个分量,则 为 , 为 ,两项相加后仍是 列向量。
把此前测量、初始信息和已知控制记为 。假设矩阵与控制量对这些信息而言已知,且过程噪声独立于已有信息、均值为零,因此 。对模型两侧取条件期望:
推导只用了期望的线性性质与噪声的零条件均值,这一步本身不要求高斯分布。所得预测式可与 Welch 与 Bishop 的技术报告 表 1-1 核对,其中状态转移矩阵记作 。genui{"citation":{"ref":"turn2view0"}}
用恒加速度算出两项增量
本例把控制量定义为加速度,假定它在一个采样区间内恒为 。取 ,位置、速度模型的矩阵为:
转移矩阵先按原速度推进位置,并保留原速度;控制矩阵再增加 的位置变化和 的速度变化。本例 两个系数分别为 、,乘加速度后单位正好对应米和米每秒。
| 计算部分 | 位置分量(m) | 速度分量(m/s) |
|---|---|---|
| 模型传播 $F_k\hat{x}_{k-1 | k-1}$ | 2.100 |
| 控制增量 | 0.010 | 0.200 |
| 两项相加得到先验 | 2.110 | 1.200 |

用 NumPy 验证均值预测
在已安装 NumPy 的 Python 3.10 及以上环境运行下面的完整脚本。状态、输入均使用二维列向量;@ 执行矩阵乘法,不能替换为逐元素相乘的 *。
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}')预期输出:
模型传播: 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 |
第二行的位置是 。时间间隔加倍,速度控制增量加倍,位置控制增量则变为四倍;只修改转移矩阵而沿用旧控制矩阵,会漏算这部分变化。
控制量的物理含义也必须与矩阵一致:若输入改成力 ,物体质量为 ,则应先换算加速度 ,或把控制矩阵改为原矩阵除以 。两种做法只能选一种,否则会重复除以质量。