自动控制 / 系统课时

通过观测矩阵映射传感器值

通过观测矩阵映射传感器值

位置传感器只返回一个读数,滤波器却可能同时维护位置和速度。本文用观测矩阵连接状态与传感器,分别构造只测位置、同时测位置和速度的模型,再检查哪些状态能从连续读数中被区分出来。

学习目标

  • 按固定状态顺序,构造 1×21\times2 和 2×22\times2 的观测矩阵。
  • 计算预测测量与残差,检查测量的维度和单位。
  • 结合转移矩阵,验证只测位置与只测速度的可观测性差异。

按测量通道确定 H 的行列

固定状态为 xk=[pk,vk]Tx_k=[p_k,v_k]^T,位置用米、速度用米每秒。线性观测模型如下;ϵk\epsilon_k 表示测量噪声,避免与速度 vkv_k 混用记号。

zk=Hxk+ϵkz_k=H x_k+\epsilon_k

HH 的行对应测量通道,列对应状态分量。 两个状态配 mm 个读数时,HH 为 m×2m\times2,测量 zkz_k 和噪声均为 m×1m\times1 列向量。观测方程与维度说明。genui{"citation":{"ref":"turn1view0"}}

传感器配置 测量向量 zkz_k 观测矩阵 HH HH 的形状
只测位置 [pmeas][p_{\mathrm{meas}}] [10]\begin{bmatrix}1&0\end{bmatrix} 1×21\times2
同时直接测位置和速度 [pmeas,vmeas]T[p_{\mathrm{meas}},v_{\mathrm{meas}}]^T [1001]\begin{bmatrix}1&0\\0&1\end{bmatrix} 2×22\times2

只测位置时,第二列为零表示读数不直接包含速度。不能把缺失速度补成零,再使用单位矩阵,否则相当于凭空加入一条“速度为零”的测量。构造 HH 要依据传感器实际测量的物理量。

比较预测读数与实际读数

滤波器根据当前时刻的预测状态 x^k−\hat x_k^- 计算预测测量,再与传感器读数比较。这里的上标负号表示尚未用本拍测量修正,并不表示数值为负。

z^k=Hx^k−,rk=zk−z^k\hat z_k=H\hat x_k^-,\qquad r_k=z_k-\hat z_k

设预测位置为 2 m2\,\mathrm m、速度为 0.5 m/s0.5\,\mathrm{m/s}。只测位置时,预测读数是 2 m2\,\mathrm m;若实际读数为 2.1 m2.1\,\mathrm m,残差就是 0.1 m0.1\,\mathrm m。

同时测两者时,若实际速度读数为 0.45 m/s0.45\,\mathrm{m/s},预测测量为 [2,0.5]T[2,0.5]^T,残差为 [0.1,−0.05]T[0.1,-0.05]^T,两个分量分别以米和米每秒计。残差包含状态预测误差与测量噪声,不能直接当成真实测量噪声。

两种观测矩阵把预测状态映射到对应读数,残差在测量空间中计算

用连续位置读数检查可观测性

单个位置读数无法确定速度,但不同时间的位置可能包含速度信息。采用采样间隔 Δt=0.1 s\Delta t=0.1\,\mathrm s 的匀速模型,先忽略噪声,写成 xk+1=Fxkx_{k+1}=F x_k:

F=[10.101],O=[HHF]F=\begin{bmatrix}1&0.1\\0&1\end{bmatrix},\qquad \mathcal O=\begin{bmatrix}H\\HF\end{bmatrix}

对这个二状态线性模型,将当前和下一拍的观测系数按行堆叠,得到可观测矩阵 O\mathcal O。若其秩等于状态数 22,两个状态方向就能由测量序列区分。这一判据考察的是 FF 与 HH 的组合。可观测性秩判据。genui{"citation":{"ref":"turn2view0"}}

只测位置时:

Op=[1010.1],det⁡(Op)=0.1≠0\mathcal O_p=\begin{bmatrix}1&0\\1&0.1\end{bmatrix},\qquad \det(\mathcal O_p)=0.1\ne0

因此其秩为 22。例如,无噪声位置在 0.1 s0.1\,\mathrm s 内从 2.00 m2.00\,\mathrm m 变为 2.05 m2.05\,\mathrm m,可算出速度为 (2.05−2.00)/0.1=0.5 m/s(2.05-2.00)/0.1=0.5\,\mathrm{m/s}。速度通过下一拍的位置留下了信息。

若只测速度,Hv=[0,1]H_v=[0,1],则 HvF=HvH_vF=H_v,堆叠后仍只有一条独立方程,秩为 11。初始位置为 2 m2\,\mathrm m 或 20 m20\,\mathrm m,只要速度相同,就会产生相同的速度序列,无法确定初始位置。双通道的单位矩阵则直接提供两个独立测量方向。

可观测性描述理想模型中的可区分性,不保证两次带噪读数就能准确恢复速度;实际误差还取决于噪声大小、观测时长和运动模型是否合适。

用 NumPy 复算映射和矩阵秩

在已安装 NumPy 的 Python 3 环境运行以下完整示例。状态和测量保留二维列向量,只在打印时展平成列表;这样可以防止一维测量与二维预测相减时,广播出错误的残差矩阵。

python
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())

预期输出

text
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]

输出中的三种配置依次是只测位置、同时测位置速度、只测速度,秩应为 22、22、11。双通道的 O\mathcal O 为 4×24\times2:两拍共有四条方程,但待区分的状态仍只有两个。

数值秩带有浮点容差,不能仅凭一个满秩结果评价带噪估计的精度;函数行为见 NumPy matrix_rank 文档。genui{"citation":{"ref":"turn1view1"}}

接入传感器前检查顺序与噪声

  1. 测量顺序一致。 若数据改成先速度、后位置,HH 应改为 [0110]\begin{bmatrix}0&1\\1&0\end{bmatrix},测量噪声协方差 RR 的对应行列也要一起重排。
  2. 单位和时刻一致。 位置读数 2000 mm2000\,\mathrm{mm} 要先换成 2 m2\,\mathrm m;双通道读数应对应同一时刻,不能直接把上一拍速度与本拍位置拼接。
  3. 用方差描述噪声。 若两通道噪声不相关,位置标准差为 0.2 m0.2\,\mathrm m、速度标准差为 0.1 m/s0.1\,\mathrm{m/s},则 R=diag⁡(0.04,0.01)R=\operatorname{diag}(0.04,0.01);两个对角项的单位分别为 m2\mathrm m^2 和 (m/s)2(\mathrm{m/s})^2。只测位置时,R=[0.04]R=[0.04]。不要把标准差直接填入 RR,也不要靠缩小 HH 的系数表示“不太相信传感器”。
按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

通过观测矩阵映射传感器值

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

登录哪都通