2.6.4 航迹外推与异步测量融合 (Track Extrapolation & Asynchronous Measurement Fusion)

这是数据处理模块（DataProcessor）的时空校准器。在 2.6.2 中，我们给每个 CPI 数据打上了高精度的“出生时间戳” (t_{meas})。在 2.6.3 中，我们完成了数据的物理拼装。现在，算法核心面临的问题是：当前的航迹停留在 t_{track} 时刻，而新来的量测数据产生于 t_{meas} 时刻。两者在时间上不对齐，如何在空间上进行关联？

对于相控阵雷达，波束调度是灵活的，数据到达是异步的，传统的“按扫描周期（Scan-based）”更新逻辑已经失效，必须转向 “按量测驱动（Measurement-Driven）” 的异步滤波机制。

一、约束输入与对齐 (Constraints & Alignment)

基于前序基线，我们面临以下硬性约束：

时间真值：所有计算必须基于 2.6.2 确立的 timestamp_us（物理生成时间）。严禁使用 Now()（处理时间）进行滤波，否则会将“处理延迟”和“排队延迟”错误地耦合进运动模型，导致速度估计偏差。
数据单调性：物理世界的时间是单调递增的。但网络传输可能导致 UDP 包乱序，使得 t_{meas} 偶尔出现回退。
计算模型：内部对象遵循 2.5.1 定义的 TrackData (POD, float state[8])。

二、权衡分析与选项呈现 (Trade-off Matrix)

议题 1：时间对齐策略 (Alignment Strategy)

选项	A. 统一同步到当前时刻 (Sync to Now)	B. 航迹外推到量测时刻 (Extrapolate to Meas) (推荐)
机制	将所有航迹和新量测都外推到 `Now()` 或固定的 `T_tick`，在同一时间切片上做关联。	保持量测不动（因为它是真值），将航迹状态外推 $\Delta t = t_{meas} - t_{track}$，在 `t_{meas}` 处做关联。
精度	中。引入了“处理延迟”的外推误差。	高。利用了最原始的测量时间，物理意义最严谨。
适用性	机械扫描雷达（整圈更新）。	相控阵雷达/异步多传感器融合。
副作用	航迹时间戳随系统时钟更新。	航迹时间戳随量测时间更新。

议题 2：乱序量测处理 (OOSM - Out of Sequence Measurement)

当 t_{meas} < t_{track} 时（即新收到的数据比航迹当前状态还老）：

选项	A. 状态回溯滤波 (Retrodiction)	B. 缓冲重排 (Buffering)	C. 直接丢弃 (Drop) (推荐)
机制	保存历史状态快照，回滚到 `t_{meas}` 更新后再推回来。	在接收端设置缓冲窗（如 50ms），排序后再送入算法。	拒收“来自过去”的数据。
复杂度	极高。内存和计算开销翻倍。	中。增加系统整体延迟。	极低。
适用性	数据极稀疏的场景（如空管雷达）。	对延迟不敏感的离线系统。	高更新率雷达。丢掉一帧数据对跟踪影响微乎其微。

三、基线确立与实施规范

为了适配相控阵体制并保证微秒级一致性，我们确立 B. 航迹外推到量测时刻 + C. 直接丢弃乱序 为基线。

1. 核心算法：异步外推更新 (Asynchronous Update)

在关联阶段（Association），针对每一个待匹配的航迹 $Tr_i$，执行以下逻辑：

时间差计算 (Time Difference)

计算量测时间 (t_{meas}) 与当前航迹时间 (t_{track}) 的偏差，转换为秒：


\Delta t = (t_{meas} - t_{track}) \times 10^{-6}

t_{meas}: 新到达的量测数据时间戳 (单位: 微秒)
t_{track}: 航迹当前状态的时间戳 (单位: 微秒)

状态外推方程 (State Extrapolation)

基于时间差 \Delta t 将航迹状态向前推演：


\hat{x}_{k|k-1} = F(\Delta t) \cdot \hat{x}_{k-1|k-1}

\hat{x}_{k|k-1}: 外推后的预测状态向量
F(\Delta t): 状态转移矩阵 (如 CV/CA 模型，随 \Delta t 变化)
\hat{x}_{k-1|k-1}: 上一时刻的更新状态

残差计算 (Innovation Calculation)

计算实际量测值与预测观测值之间的差异：


\tilde{y} = z_{meas} - H \cdot \hat{x}_{k|k-1}

\tilde{y}: 修正后的残差 (Innovation)
z_{meas}: 转换后的笛卡尔坐标量测值
H: 观测矩阵

3. 代码实现范式 (C++ Optimized)

利用 2.5.1 定义的定长数组和 SIMD 对齐特性：

// 2.5.1 TrackData (State[8]: x, y, z, vx, vy, vz, pad, pad)
// 2.5.1 DetectionResult (Block 0: r, azi, ele, v)

void KalmanFilter::predictAndAssociate(TrackData& track, const DetectionResult& meas) {
    // 1. 时间对齐 (使用 2.6.2 定义的硬件/内核时间戳)
    int64_t dt_us = (int64_t)meas.timestamp_us - (int64_t)track.timestamp_us;
    
    // 2. OOSM 保护 (基线：丢弃乱序)
    if (dt_us <= 0) return; 

    float dt = dt_us * 1e-6f;

    // 3. 状态外推 (CV 模型示例) - SIMD 友好
    // x' = x + vx * dt
    // vx' = vx
    float predicted_state[8];
    
    // 手动展开循环或利用编译器自动向量化
    predicted_state[0] = track.state[0] + track.state[3] * dt; // x
    predicted_state[1] = track.state[1] + track.state[4] * dt; // y
    predicted_state[2] = track.state[2] + track.state[5] * dt; // z
    predicted_state[3] = track.state[3]; // vx
    predicted_state[4] = track.state[4]; // vy
    predicted_state[5] = track.state[5]; // vz
    
    // 4. 转换量测到笛卡尔坐标 (z_meas)
    float z_x, z_y, z_z;
    SphericalToCartesian(meas.range, meas.azimuth, meas.elevation, z_x, z_y, z_z);

    // 5. 计算马氏距离 (关联判据)
    float dist = CalculateMahalanobisDist(predicted_state, track.covariance_diag, z_x, z_y, z_z);
    
    if (dist < GATING_THRESHOLD) {
        // 关联成功，准备 Update
        // …
    }
}

4. 时钟跳变防护 (Sanity Check)

为了防止 2.6.1 中的 PTP/NTP 发生阶跃导致 \Delta t 异常（例如突然跳变 100 年）：

基线策略：设置物理合理性阈值。
判据：若 abs(dt) > 10.0f (10 秒)，视为时钟故障。
动作：
- 不更新航迹状态。
- 重置航迹的 timestamp_us 为 meas.timestamp_us（强制同步，但不外推状态）。
- 触发 SystemClockJumpEvent 告警。

四、总结：2.6.4 基线图谱

维度	核心基线	设计意图
对齐方向	Track Extrapolates to Measurement	尊重数据的物理生成时间，消除处理延迟对估计的影响。
时间差计算	$\Delta t = t_{meas} - t_{track}$	基于绝对时间戳，而非处理间隔。
乱序处理	Drop OOSM (`\Delta t < 0`)	牺牲个别数据点的利用率，换取算法的低复杂度和高吞吐。
异常防护	Sanity Check (> 10s)	防止时钟源故障导致航迹“飞”出地球。

7.0 KiB Raw Permalink Blame History Unescape Escape