165 lines
7.0 KiB
Markdown
165 lines
7.0 KiB
Markdown
|
---
|
|||
|
|
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)** | 防止时钟源故障导致航迹“飞”出地球。 |
|
|||
|
|
|
|||
|
|
-----
|