自动控制 / 系统课时
用状态向量组织位置和速度
用状态向量组织位置和速度
位置读数只有一个数,运动估计却通常需要同时记录位置和速度。本篇把一维小车的这两个量组织成列向量,明确分量顺序、单位和初始估计,再用代码检查数组形状与一次匀速预测。
学习目标
- 按位置在前、速度在后的顺序写出二维列向量,区分空间维度与状态维度。
- 根据可用测量与先验设置初始估计,不把未知速度当作已知真值。
- 在 NumPy 中建立形状为
(2, 1)的数组,检查索引、单位与预测结果。
把同一时刻的两个量放进一列
对一维直线运动,先选定固定原点,规定向右为正。时刻 的位置记为 ,速度记为 。把分量顺序固定为位置在前、速度在后,后续代码与模型都沿用这一约定:
下标 表示采样时刻,不是数组行号; 表示两行一列。一维运动只有一个空间方向,但这里保存两个状态量,因此状态维数为 ,它也不等于传感器数量。
位置相同不代表运动状态相同。两辆车都在 2.0 m 处,一辆静止,另一辆以 −0.5 m/s 向左运动;若保持匀速,0.2 s 后的位置分别为 2.00 m 和 1.90 m。加入速度,才能区分这两种后续运动。
给初始估计赋值
真实状态记为 ,估计器保存的初始猜测记为 ,帽子表示估计。本例把位置初值设为 2.0 m,速度先验设为 −0.5 m/s;负速度表示估计小车向左运动,不代表位置也为负。
若只有首个位置读数 ,且没有速度信息,可以取 、 开始。这里的零是暂用猜测,不证明小车静止。初始估计向量只记录各分量的估计值,不记录可信程度,不能因此把速度误差也视为零。

用 NumPy 验证列向量与单位
在已安装 NumPy 的 Python 3 环境中运行以下完整示例。数组显式写成两行,每行只有一个元素;dtype=float 用于保存小数。示例采用给定先验做 0.2 s 的匀速预测,没有新测量参与,也没有执行卡尔曼校正。
import numpy as np
# 两行分别保存同一时刻的位置和速度;使用浮点数保存小数。
x_hat0 = np.array([[2.0], [-0.5]], dtype=float)
assert x_hat0.shape == (2, 1)
dt = 0.2
# 复制后预测,保留初始估计;速度乘以秒后才成为位移。
x_pred = x_hat0.copy()
x_pred[0, 0] = x_hat0[0, 0] + x_hat0[1, 0] * dt
print(f"shape={x_hat0.shape}")
print(f"initial: p={x_hat0[0, 0]:.2f} m, v={x_hat0[1, 0]:.2f} m/s")
print(f"predicted: p={x_pred[0, 0]:.2f} m, v={x_pred[1, 0]:.2f} m/s")
print(f"initial unchanged={np.array_equal(x_hat0, [[2.0], [-0.5]])}")预期输出:
shape=(2, 1)
initial: p=2.00 m, v=-0.50 m/s
predicted: p=1.90 m, v=-0.50 m/s
initial unchanged=Truex_hat0[0, 0] 取位置标量,x_hat0[1, 0] 取速度标量;Python 从零开始编号。copy() 让预测数组与初始数组分开,因此预测后初始位置仍是 2.00 m,速度仍是 −0.50 m/s。
速度乘以时间得到 −0.10 m 的位移,因此预测位置为 1.90 m。不能直接把位置与速度相加:两者单位分别是 m 与 m/s,只有速度先乘以时间,才得到可以与位置相加的位移。
排除形状和索引错误
NumPy 中下面三种写法都能保存两个数,但形状不同。本课统一采用第一种;这种显式列向量约定方便后续检查矩阵运算的输入和输出。
| 构造方式 | 形状 | 本课中的含义 |
|---|---|---|
np.array([[2.0], [-0.5]]) |
(2, 1) |
两行一列的状态向量 |
np.array([2.0, -0.5]) |
(2,) |
一维数组,没有二维列形状 |
np.array([[2.0, -0.5]]) |
(1, 2) |
一行两列的行向量 |
对形状为 (2,) 的数组使用 .T,形状仍然是 (2,),不会自动变成列向量。更隐蔽的错误是把 (2, 1) 与 (2,) 数组相加:NumPy 会通过广播生成 (2, 2),程序可能不报错,却已经改变了数据含义。
接入新的计算函数时,先检查 shape == (2, 1),再核对第一行是位置、第二行是速度。形状正确仍不能保证物理意义正确;若位置传感器输出 200 cm,必须先换算为 2.0 m,才能与本例按 m/s 表示的速度配合使用。