自动控制 / 系统课时
通过观测矩阵映射传感器值
通过观测矩阵映射传感器值
位置传感器只返回一个读数,滤波器却可能同时维护位置和速度。本文用观测矩阵连接状态与传感器,分别构造只测位置、同时测位置和速度的模型,再检查哪些状态能从连续读数中被区分出来。
学习目标
- 按固定状态顺序,构造 和 的观测矩阵。
- 计算预测测量与残差,检查测量的维度和单位。
- 结合转移矩阵,验证只测位置与只测速度的可观测性差异。
按测量通道确定 H 的行列
固定状态为 ,位置用米、速度用米每秒。线性观测模型如下; 表示测量噪声,避免与速度 混用记号。
的行对应测量通道,列对应状态分量。 两个状态配 个读数时, 为 ,测量 和噪声均为 列向量。观测方程与维度说明。genui{"citation":{"ref":"turn1view0"}}
| 传感器配置 | 测量向量 | 观测矩阵 | 的形状 |
|---|---|---|---|
| 只测位置 | |||
| 同时直接测位置和速度 |
只测位置时,第二列为零表示读数不直接包含速度。不能把缺失速度补成零,再使用单位矩阵,否则相当于凭空加入一条“速度为零”的测量。构造 要依据传感器实际测量的物理量。
比较预测读数与实际读数
滤波器根据当前时刻的预测状态 计算预测测量,再与传感器读数比较。这里的上标负号表示尚未用本拍测量修正,并不表示数值为负。
设预测位置为 、速度为 。只测位置时,预测读数是 ;若实际读数为 ,残差就是 。
同时测两者时,若实际速度读数为 ,预测测量为 ,残差为 ,两个分量分别以米和米每秒计。残差包含状态预测误差与测量噪声,不能直接当成真实测量噪声。

用连续位置读数检查可观测性
单个位置读数无法确定速度,但不同时间的位置可能包含速度信息。采用采样间隔 的匀速模型,先忽略噪声,写成 :
对这个二状态线性模型,将当前和下一拍的观测系数按行堆叠,得到可观测矩阵 。若其秩等于状态数 ,两个状态方向就能由测量序列区分。这一判据考察的是 与 的组合。可观测性秩判据。genui{"citation":{"ref":"turn2view0"}}
只测位置时:
因此其秩为 。例如,无噪声位置在 内从 变为 ,可算出速度为 。速度通过下一拍的位置留下了信息。
若只测速度,,则 ,堆叠后仍只有一条独立方程,秩为 。初始位置为 或 ,只要速度相同,就会产生相同的速度序列,无法确定初始位置。双通道的单位矩阵则直接提供两个独立测量方向。
可观测性描述理想模型中的可区分性,不保证两次带噪读数就能准确恢复速度;实际误差还取决于噪声大小、观测时长和运动模型是否合适。
用 NumPy 复算映射和矩阵秩
在已安装 NumPy 的 Python 3 环境运行以下完整示例。状态和测量保留二维列向量,只在打印时展平成列表;这样可以防止一维测量与二维预测相减时,广播出错误的残差矩阵。
import numpy as np
dt = 0.1
F = np.array([[1.0, dt], [0.0, 1.0]])
# 状态和测量均保留二维列向量,避免减法发生广播。
x_pred = np.array([[2.0], [0.5]])
models = [
("position", np.array([[1.0, 0.0]]), np.array([[2.1]])),
("position_velocity", np.eye(2), np.array([[2.1], [0.45]])),
("velocity_only", np.array([[0.0, 1.0]]), np.array([[0.45]])),
]
for name, H, z in models:
# 将同一时刻的预测状态映射到对应传感器空间。
z_pred = H @ x_pred
residual = z - z_pred
assert residual.shape == z.shape == (H.shape[0], 1)
# 对二状态模型,按行堆叠 H、HF 即可检查可观测性。
O = np.vstack((H, H @ F))
rank = int(np.linalg.matrix_rank(O))
print(f"{name}: H={H.shape}, O={O.shape}, rank={rank}")
print("predicted =", z_pred.ravel().tolist())
print("residual =", np.round(residual.ravel(), 2).tolist())预期输出
position: H=(1, 2), O=(2, 2), rank=2
predicted = [2.0]
residual = [0.1]
position_velocity: H=(2, 2), O=(4, 2), rank=2
predicted = [2.0, 0.5]
residual = [0.1, -0.05]
velocity_only: H=(1, 2), O=(2, 2), rank=1
predicted = [0.5]
residual = [-0.05]输出中的三种配置依次是只测位置、同时测位置速度、只测速度,秩应为 、、。双通道的 为 :两拍共有四条方程,但待区分的状态仍只有两个。
数值秩带有浮点容差,不能仅凭一个满秩结果评价带噪估计的精度;函数行为见 NumPy matrix_rank 文档。genui{"citation":{"ref":"turn1view1"}}
接入传感器前检查顺序与噪声
- 测量顺序一致。 若数据改成先速度、后位置, 应改为 ,测量噪声协方差 的对应行列也要一起重排。
- 单位和时刻一致。 位置读数 要先换成 ;双通道读数应对应同一时刻,不能直接把上一拍速度与本拍位置拼接。
- 用方差描述噪声。 若两通道噪声不相关,位置标准差为 、速度标准差为 ,则 ;两个对角项的单位分别为 和 。只测位置时,。不要把标准差直接填入 ,也不要靠缩小 的系数表示“不太相信传感器”。