自动控制 / 系统课时
融合两路位置传感器完成估计
融合两路位置传感器完成估计
两路位置传感器精度不同,直接取平均可能让较准的结果变差。本篇用二维位置与速度模型,把同一时刻的两路观测依次送入一个卡尔曼滤波器,并用相同轨迹上的单传感器结果验证融合收益。
学习目标
- 将每轴 1 米、3 米的测量标准差正确转换成测量噪声协方差。
- 实现每拍一次预测、两次观测更新,得到二维位置与速度估计。
- 用位置 RMSE 和更新顺序检查,对比单传感器与融合结果。
统一两路观测的时间与坐标
固定状态为 ,位置单位为米,速度单位为米/秒。两路传感器都测量同一坐标系中的 ,因此观测矩阵 只取状态的第 1、3 项。采样间隔为 0.1 秒,共模拟 600 拍。
下表中的标准差均指单个坐标轴; 是二阶单位矩阵, 的对角元素是方差,单位为平方米。
| 传感器 | 每轴标准差 | 测量噪声协方差 |
|---|---|---|
| A | 1 m | |
| B | 3 m |
假设测量误差均值为零,两路之间、两轴之间以及不同时刻之间相互独立,并且独立于过程噪声和初始误差。两路读数共享真实位置,读数本身会相关;这里要求独立的是测量误差。
预测均值采用匀速模型。模拟中的随机加速度每隔 0.1 秒重新抽取,区间内恒定,每轴标准差为 。矩阵 将加速度映射成位置增量 和速度增量 ,所以 ;该参数不是连续白噪声谱密度。
同刻只预测一次,再依次更新
每个采样时刻执行以下三步;两次更新之间时间不前进,过程噪声也只加入一次。
- 从上一拍后验预测到当前时刻,计算状态先验和协方差先验。
- 用 A 的位置读数和 更新,得到 A 更新后的状态与协方差。
- 把这两个结果一起传入 B 的更新,用 B 的读数和 重新计算新息、增益及后验。
每次更新都执行下面的公式,其中 表示这一次更新的输入。第一遍输入本拍先验,第二遍输入 A 的后验; 为四阶单位矩阵,最后一行是 Joseph 协方差更新。

只看横向位置:先验为 10 米、方差为 4 平方米,A 读数为 12 米,则增益为 ,更新后位置为 11.6 米、方差为 0.8 平方米。
若 B 随后读到 9 米,其增益要重算为 ,最终位置约为 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 官方文档。
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()预期输出:
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 到 ,结果单位为米。
不要直接对形状为 的误差数组使用平方后整体求均值,那会得到跨坐标平均的指标,比这里的二维位置 RMSE 小 倍。状态中的位置列是索引 0、2,索引 1、3 是速度。
本次融合的 0.430 米略优于 A 的 0.437 米,明显优于 B 的 0.943 米。它是这份有限样本的结果,不保证每个时刻或每个随机种子下都严格更好;模型匹配时的协方差改善,也不能代替实测误差检查。
收益较小有数值依据:仅合并两路原始测量、暂不考虑运动先验时,最小方差权重为 ,每轴方差从 1 降到 0.9。简单平均的方差却为 ,会比只用 A 更大。滤波状态还结合了运动先验,不等于这个测量加权和。
Order check: True 表示全部时刻的状态与协方差,在相对和绝对容差均为 1e-10 的检查下相等。若加入依赖顺序的门控、更新间重新线性化或自适应修改测量噪声,就不能继续套用这项顺序等价保证。
融合变差时先检查什么
| 现象 | 优先检查 | 修正做法 |
|---|---|---|
| 加入 B 后抖动放大 | 是否把标准差 3 当成方差 3,或沿用 A 的噪声设置 | B 的每轴方差应为 9,使用 |
| 交换更新顺序后差异明显 | 两次更新之间是否又预测,B 是否仍用旧先验或旧增益 | 每拍只预测一次;B 接收 A 的状态和协方差,重新计算新息与增益 |
| 协方差缩小却持续偏离真值 | 两路是否共享定位源、地图偏差,或存在固定偏置 | 先校准偏置;相关误差要在联合更新中加入交叉协方差,不能直接当作独立 |
| 运动越快,两路越不一致 | 时间戳、坐标轴方向和米/厘米单位是否一致 | 先统一坐标与单位;异步观测按真实采样时间推进状态 |
本示例假设每拍两路观测都有效。接入实际数据时,缺测或包含非有限值的一路应跳过更新,两路都缺测则保留预测;同一条样本不能重复更新。先确认时间、单位和误差独立性,再用同一段记录比较单路与融合误差。