自动控制 / 系统课时

融合两路位置传感器完成估计

融合两路位置传感器完成估计

两路位置传感器精度不同,直接取平均可能让较准的结果变差。本篇用二维位置与速度模型,把同一时刻的两路观测依次送入一个卡尔曼滤波器,并用相同轨迹上的单传感器结果验证融合收益。

学习目标

  • 将每轴 1 米、3 米的测量标准差正确转换成测量噪声协方差。
  • 实现每拍一次预测、两次观测更新,得到二维位置与速度估计。
  • 用位置 RMSE 和更新顺序检查,对比单传感器与融合结果。

统一两路观测的时间与坐标

固定状态为 x=[px,vx,py,vy]T\mathbf{x}=[p_x,v_x,p_y,v_y]^T,位置单位为米,速度单位为米/秒。两路传感器都测量同一坐标系中的 [px,py]T[p_x,p_y]^T,因此观测矩阵 HH 只取状态的第 1、3 项。采样间隔为 0.1 秒,共模拟 600 拍。

下表中的标准差均指单个坐标轴;I2I_2 是二阶单位矩阵,RR 的对角元素是方差,单位为平方米。

传感器 每轴标准差 测量噪声协方差
A 1 m RA=I2R_A=I_2
B 3 m RB=9I2R_B=9I_2

假设测量误差均值为零,两路之间、两轴之间以及不同时刻之间相互独立,并且独立于过程噪声和初始误差。两路读数共享真实位置,读数本身会相关;这里要求独立的是测量误差。

预测均值采用匀速模型。模拟中的随机加速度每隔 0.1 秒重新抽取,区间内恒定,每轴标准差为 0.6 m/s20.6\,\mathrm{m/s^2}。矩阵 GG 将加速度映射成位置增量 aΔt2/2a\Delta t^2/2 和速度增量 aΔta\Delta t,所以 Q=0.62GGTQ=0.6^2GG^T;该参数不是连续白噪声谱密度。

同刻只预测一次,再依次更新

每个采样时刻执行以下三步;两次更新之间时间不前进,过程噪声也只加入一次。

  1. 从上一拍后验预测到当前时刻,计算状态先验和协方差先验。
  2. 用 A 的位置读数和 RAR_A 更新,得到 A 更新后的状态与协方差。
  3. 把这两个结果一起传入 B 的更新,用 B 的读数和 RBR_B 重新计算新息、增益及后验。

每次更新都执行下面的公式,其中 x^,P\hat{\mathbf{x}},P 表示这一次更新的输入。第一遍输入本拍先验,第二遍输入 A 的后验;II 为四阶单位矩阵,最后一行是 Joseph 协方差更新。

ν=z−Hx^,S=HPHT+RK=PHTS−1,x^new=x^+KνPnew=(I−KH)P(I−KH)T+KRKT\begin{aligned} \nu &= \mathbf{z}-H\hat{\mathbf{x}}, \qquad S=HPH^T+R\\ K &= PH^TS^{-1}, \qquad \hat{\mathbf{x}}_{\mathrm{new}}=\hat{\mathbf{x}}+K\nu\\ P_{\mathrm{new}}&=(I-KH)P(I-KH)^T+KRK^T \end{aligned}

同一采样时刻先预测一次,A 的后验作为 B 的更新输入

只看横向位置:先验为 10 米、方差为 4 平方米,A 读数为 12 米,则增益为 4/(4+1)=0.84/(4+1)=0.8,更新后位置为 11.6 米、方差为 0.8 平方米。

若 B 随后读到 9 米,其增益要重算为 0.8/(0.8+9)≈0.08160.8/(0.8+9)\approx0.0816,最终位置约为 11.388 米、方差约为 0.735 平方米。B 不能继续使用最初的方差 4,也不能沿用 A 的新息。

在线性、同刻、测量噪声相互独立的条件下,顺序更新与堆叠观测的一次联合更新等价,交换 A、B 顺序只会产生浮点舍入差异;等价性可参见顺序离散卡尔曼滤波论文。

运行二维双传感器实验

前置环境为 Python 3.12 和 NumPy;下列完整脚本在 Python 3.12.14、NumPy 2.3.5 下验证。真值、A 读数和 B 读数只生成一次,所有方案共用这些数据以及相同的初始状态、初始协方差和过程噪声。

代码中的 np.linalg.solve 用于求解线性方程组,通过转置求出增益,避免显式计算逆矩阵。其输入维度规则见 NumPy 官方文档。

python
import numpy as np

DT, N, SIGMA_ACC = 0.1, 600, 0.6
# 状态顺序固定为 [px, vx, py, vy],两轴各用匀速模型。
F = np.array([[1, DT, 0, 0], [0, 1, 0, 0],
              [0, 0, 1, DT], [0, 0, 0, 1]], dtype=float)
G = np.array([[0.5 * DT**2, 0], [DT, 0],
              [0, 0.5 * DT**2], [0, DT]])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]], dtype=float)
Q = SIGMA_ACC**2 * (G @ G.T)
R = {"A": np.eye(2), "B": 9.0 * np.eye(2)}

def update(x, P, z, Ri):
    innovation = z - H @ x
    S = H @ P @ H.T + Ri
    # 解线性方程得到增益,避免显式求逆。
    K = np.linalg.solve(S.T, (P @ H.T).T).T
    x = x + K @ innovation
    M = np.eye(4) - K @ H
    # Joseph 形式使用本次更新的输入协方差。
    P = M @ P @ M.T + K @ Ri @ K.T
    return x, 0.5 * (P + P.T)

def run(order, measurements):
    # 相同独立先验,不用任何观测初始化,避免首帧重复计数。
    x = np.zeros(4)
    P = np.diag([25.0, 4.0, 25.0, 4.0])
    states = np.empty((N, 4))
    covariances = np.empty((N, 4, 4))
    for k in range(N):
        x = F @ x
        P = F @ P @ F.T + Q
        # 同刻只预测一次;每次 update 都接收上一次的后验。
        for sensor in order:
            x, P = update(x, P, measurements[sensor][k], R[sensor])
        states[k], covariances[k] = x, P
    return states, covariances

def main():
    rng = np.random.default_rng(2026)
    true_state = np.array([0.0, 1.0, 0.0, 0.5])
    truth = np.empty((N, 4))
    for k in range(N):
        # 每个区间随机抽取恒定加速度,协方差与 Q 一致。
        true_state = F @ true_state + G @ rng.normal(0, SIGMA_ACC, 2)
        truth[k] = true_state
    true_xy = truth[:, [0, 2]]
    # 两路读数各自从真值加噪声,不从另一传感器读数生成。
    measurements = {
        "A": true_xy + rng.normal(0, 1.0, (N, 2)),
        "B": true_xy + rng.normal(0, 3.0, (N, 2)),
    }
    a, _ = run(("A",), measurements)
    b, _ = run(("B",), measurements)
    ab, Pab = run(("A", "B"), measurements)
    ba, Pba = run(("B", "A"), measurements)

    # 剔除前 20 拍,计算二维位置误差向量的 RMSE。
    for name, states in (("A only", a), ("B only", b), ("A+B", ab)):
        error = states[20:, [0, 2]] - true_xy[20:]
        rmse = np.sqrt(np.mean(np.sum(error**2, axis=1)))
        print(f"{name}: position RMSE = {rmse:.3f} m")
    same_order = (
        np.allclose(ab, ba, rtol=1e-10, atol=1e-10)
        and np.allclose(Pab, Pba, rtol=1e-10, atol=1e-10)
    )
    print(f"Order check: {same_order}")

if __name__ == "__main__":
    main()

预期输出:

text
A only: position RMSE = 0.437 m
B only: position RMSE = 0.943 m
A+B: position RMSE = 0.430 m
Order check: True

初始估计全为零,位置标准差为 5 米、速度标准差为 2 米/秒,没有用任何测量初始化。第一条观测对应 0.1 秒,首次预测恰好把零时刻的先验推进到该时刻,避免首条观测被重复使用。

按同一指标判断融合收益

位置 RMSE 使用二维误差向量的长度计算。代码排除前 20 拍,在余下 580 拍上比较;下式把这 580 个评估样本重新编号为 1 到 MM,结果单位为米。

RMSEp=1M∑k=1M[(p^x,k−px,k)2+(p^y,k−py,k)2]\mathrm{RMSE}_p= \sqrt{\frac{1}{M}\sum_{k=1}^{M} \left[(\hat p_{x,k}-p_{x,k})^2+(\hat p_{y,k}-p_{y,k})^2\right]}

不要直接对形状为 (580,2)(580,2) 的误差数组使用平方后整体求均值,那会得到跨坐标平均的指标,比这里的二维位置 RMSE 小 2\sqrt{2} 倍。状态中的位置列是索引 0、2,索引 1、3 是速度。

本次融合的 0.430 米略优于 A 的 0.437 米,明显优于 B 的 0.943 米。它是这份有限样本的结果,不保证每个时刻或每个随机种子下都严格更好;模型匹配时的协方差改善,也不能代替实测误差检查。

收益较小有数值依据:仅合并两路原始测量、暂不考虑运动先验时,最小方差权重为 0.9zA+0.1zB0.9\mathbf{z}_A+0.1\mathbf{z}_B,每轴方差从 1 降到 0.9。简单平均的方差却为 (1+9)/4=2.5(1+9)/4=2.5,会比只用 A 更大。滤波状态还结合了运动先验,不等于这个测量加权和。

Order check: True 表示全部时刻的状态与协方差,在相对和绝对容差均为 1e-10 的检查下相等。若加入依赖顺序的门控、更新间重新线性化或自适应修改测量噪声,就不能继续套用这项顺序等价保证。

融合变差时先检查什么

现象 优先检查 修正做法
加入 B 后抖动放大 是否把标准差 3 当成方差 3,或沿用 A 的噪声设置 B 的每轴方差应为 9,使用 RB=9I2R_B=9I_2
交换更新顺序后差异明显 两次更新之间是否又预测,B 是否仍用旧先验或旧增益 每拍只预测一次;B 接收 A 的状态和协方差,重新计算新息与增益
协方差缩小却持续偏离真值 两路是否共享定位源、地图偏差,或存在固定偏置 先校准偏置;相关误差要在联合更新中加入交叉协方差,不能直接当作独立
运动越快,两路越不一致 时间戳、坐标轴方向和米/厘米单位是否一致 先统一坐标与单位;异步观测按真实采样时间推进状态

本示例假设每拍两路观测都有效。接入实际数据时,缺测或包含非有限值的一路应跳过更新,两路都缺测则保留预测;同一条样本不能重复更新。先确认时间、单位和误差独立性,再用同一段记录比较单路与融合误差。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

融合两路位置传感器完成估计

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

登录哪都通