---
tags: []
date created: 星期三, 十一月 26日 2025, 10:30:58 晚上
date modified: 星期三, 十一月 26日 2025, 10:48:13 晚上
---

# 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)

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

1. **时间真值**：所有计算必须基于 **2.6.2** 确立的 `timestamp_us`（物理生成时间）。严禁使用 `Now()`（处理时间）进行滤波，否则会将“处理延迟”和“排队延迟”错误地耦合进运动模型，导致速度估计偏差。
2. **数据单调性**：物理世界的时间是单调递增的。但网络传输可能导致 UDP 包乱序，使得 $t_{meas}$ 偶尔出现回退。
3. **计算模型**：内部对象遵循 **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$，执行以下逻辑：

1. **时间差计算 (Time Difference)**

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

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

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

2. **状态外推方程 (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}$: 上一时刻的更新状态

3. **残差计算 (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 对齐特性：

```cpp
// 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)** | 防止时钟源故障导致航迹“飞”出地球。 |

-----