自动控制 / 系统课时
将一维模型扩展为二维跟踪
将一维模型扩展为二维跟踪
二维跟踪要同时估计平面位置和两轴速度,最容易出错的是状态排列与矩阵索引不一致。本篇把两套一维匀速模型组合成四维状态,构造转移、观测和噪声矩阵,并用 NumPy 完成一次预测与更新,得到可核对的数值结果。
学习目标
- 按固定状态顺序构造 的转移矩阵和 的观测矩阵。
- 从两轴加速度扰动和位置测量标准差计算 、。
- 运行二维更新,验证位置观测如何修正两个速度分量。
固定状态顺序与坐标单位
假设传感器每隔 1 s 提供同一目标的两个位置读数,二者属于同一时刻、同一平面直角坐标系。位置用 m,速度用 m/s;本例没有已知控制输入。固定状态和观测的排列为:
这与 MathWorks 二维匀速模型 的按轴分组约定一致。代码中位置在索引 0、2,速度在索引 1、3;协方差矩阵的行列也必须采用同一顺序。
四维状态包含两个位置和两个速度,但传感器只直接测量位置。不能把观测扩成四项,再把未测得的速度填成 0;那等于向滤波器额外提交两条“速度为零”的观测。初始速度估计可以设为 0,但要用对应的速度方差表达不确定性。
拼出转移矩阵与观测矩阵
匀速预测时,新位置等于旧位置加上速度与时间间隔的乘积,速度均值保持不变。把两个一维模型放在对角位置,得到:
观测模型为 ,其中 为二维测量噪声。 的两行分别对应 、;它的列顺序仍然对应四维状态:
例如初始估计为 ,经过 1 s,预测状态应为 ,预测位置读数应为 。先手算这两个向量,再检查代码的矩阵乘法。

为过程与测量分别设定噪声
本例假设每个采样区间内存在零均值、保持常值的随机加速度,区间之间独立,两轴扰动也彼此独立,并与测量噪声独立。两轴加速度标准差均取 。每个轴()的过程噪声块为:
当 s 时,每个 的两项对角元素是 0.01、0.04,两个非对角元素都是 0.02。代码通过加速度映射矩阵 构造相同的 ;随机扰动的均值为零,因此预测均值中不额外抽取随机数。
初始位置标准差取 1 m,速度标准差取 0.5 m/s,因此 P = np.diag([1.0, 0.25, 1.0, 0.25])。测量噪声设为零均值、跨拍独立;横向、纵向标准差分别取 0.5 m、1 m,且两轴测量误差不相关,因此 R = np.diag([0.25, 1.0])。这些对角项都是方差。
运行二维预测与更新
下面是完整 Python 3 示例,需要预先安装 NumPy,输出已在 NumPy 2.3.5 核对。初始状态属于 0 s,观测 [1.2, 0.4] 属于 1 s。状态与观测分别使用形状为 (4,)、(2,) 的一维数组。
先预测状态与协方差,再计算位置新息和增益。代码使用 NumPy 的线性方程求解接口 计算增益,避免显式求逆;S 为 2×2,K 为 4×2,后验协方差使用 Joseph 形式。
import numpy as np
def main():
dt = 1.0
# 状态按每轴的位置、速度分组;P 的行列必须采用同一顺序。
state = np.array([0.0, 1.0, 0.0, 0.5])
P = np.diag([1.0, 0.25, 1.0, 0.25])
F = np.array([[1.0, dt, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, dt],
[0.0, 0.0, 0.0, 1.0]])
H = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0]])
# 两轴随机加速度映射到四维状态;噪声参数在这里平方。
G = np.array([[0.5 * dt**2, 0.0],
[dt, 0.0],
[0.0, 0.5 * dt**2],
[0.0, dt]])
Q = G @ np.diag([0.2**2, 0.2**2]) @ G.T
R = np.diag([0.5**2, 1.0**2])
z = np.array([1.2, 0.4]) # t=1 s 的两个位置读数,单位为米。
predicted = F @ state
P_pred = F @ P @ F.T + Q
innovation = z - H @ predicted
S = H @ P_pred @ H.T + R
# 解 K S = P_pred H.T,避免显式求逆。
K = np.linalg.solve(S.T, (P_pred @ H.T).T).T
updated = predicted + K @ innovation
# Joseph 形式使用同一份先验协方差计算后验。
A = np.eye(4) - K @ H
P_post = A @ P_pred @ A.T + K @ R @ K.T
assert np.allclose(P_post, P_post.T, atol=1e-12)
assert np.linalg.eigvalsh(P_post).min() >= -1e-12
for label, value in (("predicted", predicted),
("innovation", innovation),
("updated", updated),
("P_diag", np.diag(P_post))):
print(f"{label}: " + ", ".join(f"{v:.4f}" for v in value))
print("checks: passed")
if __name__ == "__main__":
main()预期输出:
predicted: 1.0000, 1.0000, 0.5000, 0.5000
innovation: 0.2000, -0.1000
updated: 1.1669, 1.0358, 0.4442, 0.4881
P_diag: 0.2086, 0.2417, 0.5575, 0.2577
checks: passed横向位置增益约为 0.8344,纵向约为 0.5575,因此噪声较小的横向观测获得更大权重。两轴预测协方差中的位置—速度互协方差均为 0.27,使位置新息也能修正速度:横向速度增大到 1.0358 m/s,纵向减小到 0.4881 m/s。
P_diag 依次给出横向位置、横向速度、纵向位置、纵向速度的后验方差。两个断言只检查协方差的对称性与半正定性;它们通过并不代表真实目标一定服从当前运动模型。
用三个检查排除扩维错误
- 检查索引含义。 用测试状态 代入 ,必须得到 。若改用 ,必须同步调整 、、、,只检查矩阵尺寸发现不了这类错误。
- 检查数组形状。 保持观测数组的形状为
(2,)。若写成(2,1),它与(2,)的预测读数相减会广播成(2,2),产生错误的新息矩阵。 - 检查跨轴相关性。 本例的初始误差、过程扰动和测量误差均不含跨轴相关项,所以可等价拆成两个一维滤波器。坐标轴相互垂直,并不能保证这些误差不相关。
例如测量误差相关系数为 0.3,标准差仍为 0.5 m、1 m,则 ,本例应保留该项进行联合更新,不能直接套用两个独立的一维滤波器。
连续跟踪时,把 updated、P_post 作为下一拍的 state、P,并按实际采样间隔重新计算 F、G、Q。状态与观测维数固定后,流式处理 N 次观测的计算量随 N 线性增长;只保留当前估计、不保存轨迹时,额外存储不随 N 增长。