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

上一页已经用未来装甲板位置误差建立了问题。本页锁定已采集的 PnP 位置、yaw、曝光时间戳和缺失事件，只改变状态估计方法。

EKF 是什么

卡尔曼滤波用一个状态向量表示目标，并用协方差矩阵记录这个状态有多不确定。每到一个新时刻，它先用运动模型预测，再用观测修正：

预测： x̂⁻ₖ = f(x̂ₖ₋₁, Δt)
创新： yₖ  = zₖ - h(x̂⁻ₖ)
更新： x̂ₖ  = x̂⁻ₖ + Kₖ yₖ

f 是状态转移，h 是“这个状态应该被观测成什么”。标准卡尔曼滤波要求两者都是线性函数。扩展卡尔曼滤波（EKF）允许 f 和 h 非线性，它在当前估计周围用雅可比矩阵做一阶线性化。

EKF 可以处理带时间戳的不等间隔数据。某个曝光没有观测时，执行预测而跳过更新即可。它最擅长的情况是：单一状态假设能够解释运动，过程噪声和观测噪声可以用近似零均值的高斯分布描述，并且局部线性化足够准确。

11 维基线在假设什么

同济 sp_vision_25 基线维护的状态是：

[xᴄ, vₓ, yᴄ, vᵧ, zᴄ, vᶻ, θ, ω, r_even, Δr, Δz]

它把四块装甲板组成一辆刚体车：车体中心做匀速直线运动，车体 yaw 以恒定角速度变化，长短半径和高度差在预测中保持不变。已关联的装甲板槽位 j 通过非线性函数映射到位置和 yaw：

pⱼ = c - rⱼ [cos(θ + jπ/2), sin(θ + jπ/2), 0]
yawⱼ = θ + jπ/2

从这个模型可以直接列出四类假设：

Q 和 R 不只是“调平滑度”的参数。它们是对过程误差与观测误差形状的数学声明。

实测观测与这些假设的距离

这里使用 Daedalus 1.4.0-learning-r1 正式 Release、无前端高性能模式和 TensorRT FP16 采集的三组 20 s 记录。对每个成功匹配的 PnP 位置，将它与同曝光装甲板真值作差。

先认识图中的两个方向：

• 沿视线（深度）：从相机指向装甲板的方向。这个方向上的误差主要表现为“目标被算近了或算远了”。
• 水平切向：在水平面内垂直于视线的方向。这个方向上的误差会直接改变左右瞄准方向。

图中还反复使用两个分位数：P50 表示一半样本不超过这个数，可以把它看作典型帧；P95 表示 95% 的样本不超过这个数，用来观察较差的那一部分帧。

A：误差主要大在哪个方向

实心点是 P50，空心点是 P95，两点之间的线表示从典型帧到较差帧的跨度。纵轴采用对数刻度，是为了让几毫米和几百毫米能同时显示在一张图中；相邻的 10¹、10² 代表相差 10 倍。

三组工况中，沿视线误差的 P50 为 145–261 mm、P95 为 589–720 mm；水平切向误差的 P50 只有 2.7–3.8 mm、P95 为 22–78 mm。PnP 位置误差具有很强的方向差异，沿视线方向明显更大。

B：偶发大误差有多突出

这一图计算 P95 ÷ P50。数值越大，表示“较差帧”相对“典型帧”放大得越厉害。理想零均值高斯误差的绝对值大约是 2.9 倍，因此虚线只用作直观参考。

• 沿视线方向为 2.8–4.1 倍；
• 水平切向达到 6.8–25.4 倍。

水平切向多数帧只有几毫米，但偶尔会跳到几厘米。只看平均值或 P50，会把这些少数大误差隐藏起来。

C：误差会不会延续到下一帧

柱高是相邻两次成功 PnP 误差的相关系数：0 表示上一帧对下一帧没有提示，1 表示两帧几乎保持相同方向和幅度。沿视线方向为 0.49–0.59，水平切向为 0.10–0.17。

这说明沿视线误差具有明显的短时记忆。若某一帧把目标算近了，下一帧仍然偏近的概率会更高；它不是每帧重新抽取的一份独立白噪声。

D：更新是不是固定帧率

实心点、空心点和叉号分别表示成功 PnP 间隔的 P50、P95 和最大值。典型间隔约为 5.4–5.7 ms，P95 为 8.5–9.7 ms，最长间隔达到 19.4–53.1 ms。因此滤波器应当使用曝光时间戳计算每一次 Δt，而不是把更新周期写死为某个 FPS 的倒数。

把四幅图合在一起，可以得到更具体的建模要求：观测协方差需要区分方向；更新要限制少数大误差的影响；沿视线误差需要短时记忆；状态转移必须读取真实时间戳。

第一个想法：把 EKF 换成 UKF 或 PF

EKF 使用一阶雅可比传播均值与协方差。无迹卡尔曼滤波（UKF）改用一组 sigma points 穿过非线性函数，可以更好地保留局部非线性对前两阶统计量的影响。粒子滤波（PF）用带权样本表示后验，可以表达非高斯甚至多峰分布。

三者的数值积分方式不同，但如果共享同一个 f、h、Q/R 和状态结构，它们对世界的基本描述仍然相同。下面的离线回放将这一点做成对照实验。

同输入回放合同

• 状态、转移、观测函数、Q/R、初始化和时间戳完全相同；
• 三种方法逐帧读取同一份 PnP 位置和 yaw，并保留全部缺失事件；
• 离线保存的物理装甲板槽位只用于选择观测分支，位置、速度、yaw、角速度和半径真值均不进入滤波器；
• PF 使用 2048 个粒子，先按时间顺序执行前 20 次 EKF 更新以收窄 11 维先验，评分从 2 s 后开始；
• 每个后验状态都按同一规则外推到 100 / 200 / 300 / 500 ms，再读取未来真值评分。

图中横向错开 EKF 与 UKF 的标记点，仅用于让两条重合曲线可见。纵轴是未来装甲板三维位置误差 p95。

EKF 与 UKF 在所有工况和时域上几乎重合，p95 的最大差值是 2.6 mm。这表明一阶线性化不是当前的主要损失。在这个高维状态、宽先验和强时间相关的观测下，有限粒子的 bootstrap PF 会把计算量消耗在大量低权状态上，没有追上 EKF。

这批数据上的下一个高杠杆改动，是改变残差怎样进入模型，以及状态怎样表达真实运动结构。

三组实验的评价口径

三组实验分别评价滤波器形式、射线短时预测和组合运动建模：

射线预测输出相机观察方向，因此用预测射线与未来真值射线的夹角评分。组合运动测试在 tracker 坐标系中计算横向位置误差 sqrt(e_y² + e_z²)。当前回放则直接比较未来装甲板的三维位置。三项结果分别回答不同问题，后续候选方法统一进入 1.4.0 回放。

按数据分布改写状态估计

实测分布给出了比“换一种滤波器”更具体的方向：

恒速外推＋历史窗口线性校正

它预测什么

这是一种相机射线预测器。对一条已经关联好的目标轨迹，它保存最近 16 次曝光的

(tᵢ, uᵢ, vᵢ)

其中 tᵢ 是曝光时间戳，uᵢ = atan2(x_C, z_C)、vᵢ = atan2(y_C, z_C) 是装甲板在相机中的水平角和竖直角。给定 50 / 100 / 200 ms 的预测时长 h，它输出未来射线方向：

d̂(t + h) ∝ [tan(û), tan(v̂), 1]

这个输出可以直接回答“未来应该朝哪个方向看”。目标距离和三维击打点由距离估计与弹道模块继续计算。

第一步：用整个窗口拟合恒速趋势

令最后一个观测时刻为 tₖ，把窗口内时间写成 τᵢ = tᵢ - tₖ。方法分别对 u 和 v 做一次普通最小二乘直线拟合：

uᵢ ≈ aᵤ + bᵤτᵢ       u_恒速(h) = aᵤ + bᵤh
vᵢ ≈ aᵥ + bᵥτᵢ       v_恒速(h) = aᵥ + bᵥh

这里使用全部 16 个点和各自的真实时间戳。相比只用最后两点相减，窗口拟合对单帧抖动更稳定，也能处理不等间隔更新。

第二步：学习恒速外推反复出现的剩余误差

恒速直线只能描述窗口内的一阶趋势。目标旋转、视角投影和不均匀采样会让未来射线产生可重复的弯曲。方法把当前窗口整理成一个特征向量：

φₕ = [τ₁…τ₁₆,
      u₁-uₖ…u₁₆-uₖ,
      v₁-vₖ…v₁₆-vₖ,
      uₖ, vₖ, h]

前三组特征描述窗口的时间形状和相对运动，最后三个量描述当前观察方向与预测时长。离线训练时，未来真值射线只用于构造恒速外推的残差：

rₕ = [u_真值(t+h), v_真值(t+h)] - [u_恒速(h), v_恒速(h)]

特征标准化后，为每个预测时长分别拟合一个带 L2 约束的线性回归：

min Σ ‖rₕ - (Bₕφₕ + bₕ)‖² + λ‖Bₕ‖²

L2 约束会压制过大的系数，降低模型对某几轮采集的过拟合。当前实现取 λ = 10。在线运行只读取历史 u/v 和时间戳，计算过程为：

[û, v̂] = [u_恒速(h), v_恒速(h)] + Bₕφₕ + bₕ

第三步：处理历史不足和观测中断

处理器按目标轨迹分别维护历史。水平角跨过 ±180° 时先展开成连续角度；相邻观测间隔超过 120 ms 时，从间断之后重新积累窗口；历史最长保留 0.75 s。连续样本达到 16 个时使用线性校正，样本不足时保持最后一次射线方向。输出置信度还会同时考虑历史跨度、最大间隔、观测覆盖率和本次校正量。

它与 11 维 EKF 的区别

因此，这种方法用更窄的输出换取更简单的短时预测问题：它直接优化瞄准方向，同时把车辆中心、半径和角速度留给其他估计模块。

实验共采集 120 轮运动记录，其中 118 轮形成有效历史。评价采用“留一距离”：每次用一个距离组测试、其余距离训练，再让不同工况等权汇总。指标是预测射线与未来真值射线夹角的 P95。

表中的周期状态 EKF/UKF 使用傅里叶周期状态和 u/v + yaw 输入；前半段回放使用同济 11 维车辆状态。历史窗口线性校正因此进入下一轮统一回放，与 11 维 EKF 使用同一批三维位置数据重新评价。

对应的120 轮研究记录、历史窗口线性校正实现和机器可读筛选汇总共同记录了来源与指标。

按视线方向分权，再联合拟合整车轨迹

这条方法链直接预测装甲板的三维位置，适用于“车辆中心平移，同时四块装甲板绕中心旋转”的组合运动。它包含一个短窗口估计器和一个长窗口估计器：前者快速跟随当前运动，后者在历史充分时恢复完整的往返平移与旋转轨迹。

每条历史观测写成 (tᵢ, jᵢ, pᵢ)：tᵢ 是曝光时刻，jᵢ 表示第几块装甲板，pᵢ 是 PnP 位置。车辆几何 qⱼ 给出第 j 块装甲板相对车体中心的固定位置。实验中的 jᵢ 由离线物理槽位提供；在线实现需要由装甲板关联器产生同样的身份关系。

短窗口：方向分权的稳健刚体拟合

在最近 400 ms 内，目标被写成“中心匀速平移＋四块装甲板共享旋转”：

p̂ⱼ(t) = c₀ + vt + Rz(ωt)Rz(θ₀)qⱼ

对一个给定的角速度 ω，中心 c₀、中心速度 v 和旋转相位可以写成一个小型线性最小二乘问题。实现先在 [-16, 16] rad/s 中搜索 ω，找到最低残差区域后再连续细化；同一窗口内所有可见装甲板共同参与拟合。

PnP 的深度误差明显大于横向误差，因此残差先转换到锚点时刻的 tracker 坐标，再使用：

‖r‖²_W = 0.1 r²_深度 + r²_横向y + r²_横向z

深度仍参与约束，其平方权重为横向的 0.1。随后对加权残差使用阈值为 20 mm 的 Huber 损失：小残差按平方惩罚，大残差改为线性增长。这样既保留多数正常观测的拟合精度，也限制单个异常 PnP 对整段轨迹的拉动。

单个 400 ms 窗口得到一个角速度估计。连续运行时，最近 31 个窗口的角速度取中位数，再用这个慢变化角速度重新拟合当前窗口并外推目标位置。中位数会削弱少数窗口跳到错误角速度候选的影响。本次离线实验还提供了真实运动分段边界；在线运行时由变化点检测决定何时清空旧窗口。

长窗口：直接联合拟合往返平移与四板旋转

靶车沿有限直线路径往返时，中心速度会在端点翻转。长窗口使用最近 4 s 的全部可见装甲板，把这种运动直接写进轨迹：

p̂ⱼ(t) = cₐ
        + a [T(φ + st; L) - T(φ; L)]
        + Rz(ωt)Rz(θ)qⱼ

T(x; L) = L - |((x mod 2L) - L)|

a 是平移轴，s 是线速度大小，L 是单程行程，三角波 T 自动表达在两个端点换向；φ 表示车辆当前处于往返路径的哪个位置；ω 和 θ 分别表示共享角速度和旋转相位。

当前离线实现从场景配置读取 a、s、L，再完成以下计算：

1. 枚举角速度 ω 和往返相位 φ 的候选组合；
2. 对每个候选，用全部历史装甲板共同求解共享中心 cₐ 和旋转相位；实现以 C = ρcosθ、S = ρsinθ 表示旋转，使这一步成为线性最小二乘，其中 ρ 是四块板共享的平面尺度；
3. 使用同样的方向权重和 Huber 损失计算整段历史的拟合代价；
4. 在最低代价候选附近连续细化 ω、φ，再代入未来时刻 h 得到 p̂ⱼ(h)。

这里的“联合”有两层含义：同一时刻出现的多块板共享车辆状态，不同时刻出现的板也通过固定几何 qⱼ 连接到同一条轨迹。单帧 PnP 的深度跳动不会直接生成一个新的车辆中心；中心、旋转相位和往返相位由整段历史共同确定。

它与 11 维 EKF 的区别

11 维 EKF 以较低计算量连续递推车辆状态；窗口联合拟合使用更强的轨迹和场地先验，集中处理 PnP 的方向差异、异常值和端点换向。

这张图来自 2026-08-11 的一组 20 秒组合运动测试：距离约 4.56 m，线速度约 1.865 m/s，角速度约 11.868 rad/s。每种方法使用相同装甲板和相同评分时刻。指标是 tracker 坐标中横向误差 sqrt(e_y² + e_z²) 的 P95；50 / 100 / 200 ms 的样本数分别是 53 / 54 / 50。

在这组组合运动中，方向分权和跨窗口角速度把 200 ms 横向 P95 从 351.6 mm 降到 52.0 mm，再加入四块装甲板联合轨迹后降到 30.9 mm。这组结果把“方向性、时间相关和刚体结构”各自对应到可见的改进步骤。完整采集条件、样本数和逐方法结果见组合运动最终研究结果。

方向一的下一组实验

这一方向已经形成三级可检验路线：

1. 保留现有 11 维 EKF 作为对照，让沿视线与垂直视线使用不同的观测权重，并限制单帧大误差的影响；
2. 在相同回放接口中加入“恒速外推＋历史窗口线性校正”，将射线预测候选迁入装甲板位置评价；
3. 对稳定旋转与组合运动加入“四块装甲板联合轨迹”，使用同一套 100 / 200 / 300 / 500 ms 未来位置误差与装甲板窗口覆盖率评分。

下一轮将三个候选放入同一批 1.4.0 输入，统一使用相同的未来时刻和位置指标进行横向比较。

完整脚本、汇总 JSON、图的 PNG/SVG/PDF、原始数据哈希和保留清单位于 aim-stack 固定输入实验。下一个研究方向是保持滤波器不变，提高观测质量。