研究选读：从预测误差看 11 维 EKF

11 维整车 EKF 最终要给出子弹到达时的装甲板位置。因此，评价的核心量是未来位置误差：从当前后验状态外推到未来，再与后续曝光中的真实装甲板位置比较。本教程将这个过程固定为 AUTOAIM_FUTURE_ARMOR_PREDICTION_V1，后续滤波器、观测器和运动模型都使用同一指标比较。

从当前后验外推未来位置

每次曝光更新后，EKF 保存车体中心 c、中心速度 v、车体角度 θ、角速度 ω、两组半径和高度。对预测时域 τ，同济基线按下列方式外推：

c(t + τ) = c(t) + v(t) · τ
θ(t + τ) = θ(t) + ω(t) · τ
装甲板位置 = 未来中心 + 旋转后的半径偏移

评价程序在每个 tracker_state=tracking 的时刻执行以下步骤：

1. 由当前 11 维后验解码出四块装甲板；
2. 建立当前观测装甲板与 EKF 候选之间的物理对应；
3. 保持这一物理装甲板的身份，计算 0–500 ms 内的预测位置；
4. 从后续实际曝光中取得同一块装甲板的真实位置。

记录的曝光时刻很少恰好等于 t + τ，因此真值在相邻曝光之间做线性插值。插值两端的时间间隔最多为 25 ms；更大的缺帧区间不生成评价样本。

这里的时间语义是物理的：起点是曝光时刻 t 完成当前帧更新后的状态，预测终点是 t + τ。评价程序在预测输出之后才读取后续真值，真值不进入 detector、PnP、tracker 或预测器。固定报告 100 / 200 / 300 / 500 ms，0 ms 作为当前后验误差的诊断点。

误差为什么分成两个方向

对四装甲刚体模型，装甲板法向与“车体中心指向该装甲板”的径向共线。这两个名称描述的是同一条轴，图中合并记为“法向/径向”。装甲板板面内的两条轴分别记为“切向”和“竖直”。

设预测误差向量为 e = 预测位置 - 真实位置，径向单位向量为 n，切向单位向量为 t：

法向/径向误差 = |e · n|
切向误差       = |e · t|
竖直误差       = |e · z|

每个预测时域统计绝对误差的 p50 和 p95。p50 表示典型帧，p95 表示尾部帧。

实验条件

实验使用 Daedalus 1.4.0-learning-r1 Linux Release，以无前端的高性能模式运行。被测程序是本教程固定的同济 sp_vision_25 11 维 EKF，检测使用 TensorRT FP16。检测、PnP 和 EKF 按生产链运行，评价程序在后验状态输出后读取曝光真值。

未来位置误差随预测时域增长

横轴是从当前曝光向未来外推的时间 τ，纵轴是绝对位置误差。实线为 p50，点线为 p95。τ = 0 对应当前后验解码出的装甲板位置；随着 τ 增长的部分，则来自中心速度、角速度和几何状态的联合外推。

往复平移的中位误差增长较慢，但 500 ms 的法向/径向 p95 已达 73.4 cm，切向 p95 达 81.6 cm。旋转工况的切向误差会随相位出现起伏，因此固定时域结果和完整曲线都要保留。竖直误差的 p95 为 1.4–2.4 cm，但装甲板半高只有 2.75 cm，不能在板面评价中省略这一轴。

角速度 RMSE 只描述状态向量中的一个分量。未来装甲板位置还同时受中心位置、中心速度、yaw、长短半径和装甲板对应关系影响。预测误差曲线将这些分量统一到最终的位置结果中。

把预测误差放进装甲板窗口

RoboMaster 装甲板在本实现中使用下列几何尺寸：

本次三组数据观测的都是小装甲板。每个样本同时满足切向与竖直边界时，记为预测点落入小装甲板的理想板面窗口。

法向/径向误差不是矩形板面的宽高边界，它进入距离和弹道评价。真正的射击窗口还要在这两个板面误差上继续叠加云台跟随、弹道解、发射时序和弹丸散布。

用状态曲线定位误差来源

未来位置误差表示最终结果，状态曲线用来查看哪些分量参与了误差的形成。下面的四联图保留角速度、中心速度、长短半径和俯视轨迹。

原地 8 rad/s 旋转

角速度 RMSE 为 0.494 rad/s，中心真值速度为零。两组真值半径约为 0.211 m，估计值分别稳定在约 0.198 m 和 0.191 m。俯视图中的 PnP 点云沿视线方向拉伸，沿视线误差中位数约为 0.207 m，横向误差约为 0.007 m。

1.5 m/s 往复平移

真实角速度为零，估计角速度 RMSE 为 0.774 rad/s。中心速度在往复运动换向时出现过渡，长、短半径也向两个方向分开。这些状态变化对应到预测图中逐渐增长的尾部误差。

1 m/s 平移＋6 rad/s 旋转

角速度 RMSE 为 0.527 rad/s，中心速度会跟随真值换向。两个半径在稳定后约为 0.17–0.18 m，真值约为 0.211 m。PnP 沿视线误差中位数约为 0.261 m，横向误差约为 0.008 m。

这组实验建立的问题：误差从哪里进入预测链

实验的最终结果是未来装甲板位置。在 100 ms 时，三种工况的小装甲板窗口覆盖率只有 29.4%–50.8%；到 500 ms，覆盖率为 18.6%–29.4%。这些数字表明，从当前观测到未来装甲板位置的整条数据流仍有很大的提升空间。

再回看中间数据，问题并不只出现在长时外推。τ = 0 时已经存在装甲板位置偏差；中心速度、角速度和半径状态也与真值存在差异。与此同时，俯视图中的 PnP 观测沿视线方向被明显拉长，并伴随跳变和缺失。即使模拟器中设置的是匀速或匀角速度运动，交给滤波器的观测序列也不是一条只有少量均匀抖动的理想轨迹。

沿着实际数据流回溯，后续研究分成两个主要方向：

图像 → 角点 → PnP 位置 → 坐标/时间对齐 → 观测序列 → 11 维滤波器 → 未来装甲板
                                  ↑                      ↑
                         方向二：提升输入质量      方向一：改进状态估计

方向一：输入不变，改进滤波器

第一条路线完整锁定已采集的观测序列：每一帧 PnP 位置、曝光时间戳、装甲板对应和缺失事件都保持不变，只改变下游状态估计方法。这条路线要回答：

• 当前 11 维状态和运动模型，能否表达观测序列中的运动特征；
• 滤波器对误差尺度、误差方向和状态之间关系的描述是否合适；
• 在不改变 detector 和 PnP 输入的情况下，未来位置误差与装甲板窗口覆盖率能够提升多少。

这类实验使用同一批时间锚点回放，并继续用 100 / 200 / 300 / 500 ms 未来位置误差评价。输入完全相同时，结果变化就可以归因于状态估计环节。

进入方向一：固定输入，改进状态估计

方向二：滤波器不变，改进输入质量

第二条路线保持当前 11 维滤波器、参数和评价方法不变，逐步提高交给它的观测质量。首先要将真值运动轨迹与 PnP 观测轨迹分开绘制，判断“不符合匀速假设”的特征究竟来自真实运动，还是在角点、PnP、坐标变换、时间对齐或装甲板对应中被引入。

这条路线将关注：

• 观测误差在法向/径向、切向和竖直方向上的尺度与形状；
• 观测轨迹相对真值轨迹的偏移、跳变、缺失和装甲板切换；
• 当角点与 PnP 精度提高后，同一个 11 维滤波器的状态估计和未来预测能够提升多少。

这两个方向构成两组单变量对照：第一个实验固定数据、改变滤波器；第二个实验固定滤波器、改变数据质量。后续章节将分别沿这两条路线建立实验，最后再评价它们组合后的未来装甲板预测质量。

进入方向二：固定滤波器，提高观测质量

实验数据锁、完整分位数、绘图脚本与 V1 评价标准保存在 aim-stack 的 EKF11 基线目录。跟踪预测主线从验证与调试跟踪器进入火控与弹道。