实现整车 EKF

为什么常见方案会选择 EKF

目标运动随时间连续，而 PnP 观测会抖动、漏帧。卡尔曼滤波把两类信息放在一起：运动模型给出“按照上一帧的状态，现在大概在哪里”，观测给出“相机这一帧看到了哪里”。模型越可信，更新越保守；观测越可信，更新越靠近观测。

本节用 EKF 建立一条完整基线：定义整车状态，根据时间预测，再用当前装甲板观测更新。掌握这条链路后，才能分清问题来自运动模型、观测噪声、数据关联还是状态机。

整车模型中的装甲板位置由“车体中心 + 半径 + yaw”计算，里面包含正弦和余弦，是非线性关系。扩展卡尔曼滤波（EKF）会在当前估计附近对这个关系求一阶近似，因此可以继续使用卡尔曼更新。这里的“扩展”主要来自观测模型，而不是因为目标运动一定非常复杂。

同济大学 SuperPower 的 sp_vision_25 提供了一个方便阅读的整车 EKF 实例：状态同时描述中心、速度、yaw、角速度、半径以及交替装甲板的结构差，并配合跟踪状态机与火控选板。下面借它认识常见工程结构，再根据本教程的数据逐项检查模型前提和参数。

先用一个状态描述整台车

以四装甲板车辆为例，可以使用 11 维状态：

x = [cₓ, vₓ, cᵧ, vᵧ, c_z, v_z, θ, ω, r₀, Δr, Δz]ᵀ

c 是车体中心，v 是中心速度，θ 是参考装甲板的连续 yaw，ω 是旋转角速度。r₀ 是一组装甲板到中心的半径，Δr 和 Δz 描述另一组装甲板的半径与高度差。普通步兵结构近似对称时，可以先把差值设为零，再根据数据决定是否需要释放这些自由度。

第 i 块装甲板的朝向为 θᵢ = θ + i·π/2。若 yaw 表示装甲板朝向车体中心的法向，装甲板中心可写成：

aₓ = cₓ - rᵢ cos(θᵢ)
aᵧ = cᵧ - rᵢ sin(θᵢ)
a_z = c_z + Δzᵢ

这三行非常重要：PnP 观察到的是 a，滤波器真正维护的是 c、θ、r。当可见装甲板从 i 切换到 i+1 时，整车中心不需要瞬间跳变，只需改变使用哪个几何分支。

预测：状态在两帧之间如何运动

最常见的起点是匀速、匀角速度模型。设两次曝光相隔 Δt：

cₓ ← cₓ + vₓΔt       cᵧ ← cᵧ + vᵧΔt       c_z ← c_z + v_zΔt
θ  ← θ  + ωΔt
速度、角速度和几何参数在这一小段时间内保持不变

写成 EKF 形式是：

x̂⁻ₖ = f(x̂⁺ₖ₋₁, Δt)
P⁻ₖ = Fₖ P⁺ₖ₋₁ Fₖᵀ + Qₖ

x̂⁻ 是看这一帧之前的预测，P⁻ 是预测不确定度；F 是运动模型在当前状态附近的雅可比。Q 表示模型没有描述的加速度、急停、碰撞和转向等变化。Q 太小，滤波器相信目标永远匀速，会产生明显滞后；Q 太大，状态会紧跟每一次 PnP 抖动。

Q 还应随 Δt 变化。帧间隔越长，未建模加速度能造成的位移越大。使用连续白噪声加速度模型推导各项随 Δt 的幂次，比给所有状态随手填同一个常数更容易维护。

更新：这一帧 PnP 应当把状态拉向哪里

本章使用的观测是：

zₖ = [pnp_x, pnp_y, pnp_z, pnp_yaw]ᵀ

根据当前候选装甲板编号 i，用上一节的车体几何从状态生成预测观测 h(x̂⁻ₖ, i)。创新，也就是“这一帧带来的新信息”，为：

yₖ = zₖ - h(x̂⁻ₖ, i)

yaw 的差必须先折回到同一角度区间，否则 179° 与 -179° 会被错误地算成相差 358°。随后计算：

Sₖ = Hₖ P⁻ₖ Hₖᵀ + Rₖ
Kₖ = P⁻ₖ Hₖᵀ Sₖ⁻¹
x̂⁺ₖ = x̂⁻ₖ + Kₖ yₖ
P⁺ₖ = (I - KₖHₖ) P⁻ₖ

H 是装甲板几何观测模型的雅可比，R 是 PnP 观测噪声协方差，K 决定本帧观测能把状态拉动多少。若 R 很大，说明观测不可靠，更新较小；若 P⁻ 很大，说明预测已经不确定，滤波器会更愿意接受新观测。

实现时推荐 Joseph 形式更新协方差：

P⁺ = (I-KH)P⁻(I-KH)ᵀ + KRKᵀ

它与理想数学推导等价，却更能抵抗浮点误差导致的协方差非对称或非正定。

R 不是“平滑程度旋钮”

R 应描述 PnP 误差的尺度和各维之间的相关性。例如远距离深度误差通常明显增大，位置与 yaw 也可能相关。如果所有距离都使用同一个对角 R，滤波器相当于假设：2 m 与 6 m 的 PnP 同样可靠，并且 x、y、z、yaw 的误差互不相关。

更合理的工程起点是让 R 依赖距离和观测质量：

Rₖ = R(distance, corner_quality, reprojection_error, view_angle, ...)

这里仍然要区分“偏差”和“随机波动”。R 只能告诉滤波器观测围绕某个中心有多分散，不能把一个随距离变化的系统性偏差自动消掉。确定性偏差应优先从标定、时间同步、坐标语义或显式偏差模型中修正。

用 NIS 判断这一帧是否合理

创新是否过大不能只比较 |y|，因为近距离和远距离本来就有不同不确定度。归一化创新平方（NIS）把创新放进它自己的协方差中衡量：

NIS = yᵀS⁻¹y

本章观测为 4 维。如果模型与协方差描述合理，NIS 的长期分布应接近 4 自由度卡方分布。95% 阈值约为 9.49。一次超过阈值可以拒绝本次更新；连续超过则更可能意味着目标切换、几何分支错误或滤波器已经发散，需要进入重捕获流程。

EKF 之外还需要跟踪状态机

完整跟踪器至少要区分：

• 未建立目标：用第一批可靠观测初始化，速度先设为零并给较大不确定度；
• 确认中：连续若干帧匹配后才允许开火，避免一帧误检直接建立目标；
• 稳定跟踪：正常预测、关联和更新；
• 短时丢失：只预测并放大不确定度，允许短暂漏检；
• 彻底丢失：超时后清空目标，等待重新初始化。

同一帧有多个候选装甲板时，应分别计算几何预测与创新距离，在通过门限的候选中选择最合理者。候选都不合理时，不要强行更新。跟踪状态机解决“什么时候相信、什么时候等待、什么时候重置”，EKF 解决的是目标已建立时怎样融合模型与观测，两者不能互相替代。

从实现到可验证结果

建议先实现三个可单独测试的函数：

State predict(const State& x, double dt);
Measurement observe(const State& x, int armor_slot);
FutureArmor decode(const State& x, int armor_slot, double horizon);

先用无噪声合成状态验证 observe(predict(x)) 的几何与角度语义，再接入 EKF。这样出现错误时，可以区分是坐标/几何实现不一致，还是滤波参数不合适。下一步进入验证与调试跟踪器，建立统一的评价方法。

这条基线解决不了什么

整车 EKF 依赖几个前提：当前观测关联到了正确目标；局部运动模型大致成立；确定性坐标与标定偏差已经处理；剩余误差可以由 Q/R 近似描述。

错误关联不是“更大的测量噪声”，系统偏差也不会因为增大 R 自动消失。因此应先建立可工作的基线，再用创新、预测误差和失锁场景逐项检查这些前提，而不是把所有问题都交给 Q/R。

参考实现

• TongjiSuperPower/sp_vision_25：项目总览与系统设计；
• target.cpp：整车状态、运动模型与装甲板观测几何；
• tracker.cpp：目标建立、匹配、丢失与重置流程；
• extended_kalman_filter.hpp：EKF 接口组织方式。