更新：1、航迹关联时增加高度门限判断，同时加大统计距离门限，提高机动目标关联概率；
2、修复TAS波束控制时，数据率判断有误的问题； 3、更新文档； Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
@@ -12,7 +12,7 @@
|
||||
| ② 调度 | 模式码 | 分流到 TWS / TAS 通道 | — |
|
||||
| ③ 凝聚 | 单 CPI 或相邻两 CPI 点迹 | 同类点合并（保幅度大者） | 凝聚点迹表 |
|
||||
| ④ 关联 | 凝聚点迹 + 已有航迹 | IMM 交互 → 波门筛选 → 最近邻一对一 → EKF 更新 | 更新航迹 + 剩余点迹 |
|
||||
| ⑤ 起始 | 剩余（未关联）点迹 | 逻辑法：航迹头 → 2 点 → ≥3 点 → 起批 | 新可靠航迹 |
|
||||
| ⑤ 起始 | 剩余（未关联）点迹 | 逻辑法（每圈顺序固定）：点 × ≥2 点临时航迹关联 → 点 × 1 点航迹头关联 → 剩余点转新航迹头 → 达标临时航迹起批 | 新可靠航迹 + 更新后的临时航迹表 |
|
||||
| ⑥ 外推/消亡 | 未关联航迹 | 按数据率预测一帧；超限则消亡 | 补点航迹 / 消亡批号 |
|
||||
| ⑦ 波束控制 | TAS 队列 + 航迹 | 数据率门控 + 队列调度 → 位置外推 | 跟踪波束指令 |
|
||||
|
||||
@@ -38,7 +38,7 @@ flowchart TD
|
||||
|
||||
M1 --> A1["IMM 交互 + 波门筛选 + 最近邻关联 + EKF 更新"]
|
||||
A1 --> A2["未关联航迹外推 / 超限消亡"]
|
||||
A2 --> A3["剩余点迹 → 逻辑法航迹起始"]
|
||||
A2 --> A3["剩余点迹 → 逻辑法起始：点×临时航迹 → 点×航迹头 → 剩余点转航迹头 → 起批"]
|
||||
A3 --> OUT1["输出更新航迹 + 起批航迹历史点 + 消亡批号"]
|
||||
|
||||
M3 --> B1["TAS 凝聚（单帧）"]
|
||||
@@ -254,106 +254,51 @@ VT > 300 且 R > 1200 → 拒绝（高速目标给固定 1200 m 上限）
|
||||
|
||||
---
|
||||
|
||||
## 5. IMM + EKF 关键逻辑
|
||||
## 5. 航迹起始（逻辑法）
|
||||
|
||||
### 5.1 IMM 框架
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
A["上一拍三模型状态/协方差 + 模型概率 u"] --> B["交互 mix：算归一化常数 c̄j 与混合概率"]
|
||||
B --> C["得到三个混合初值 X0j / P0j"]
|
||||
C --> D["各子模型独立预测（Singer F 和 Qj）"]
|
||||
D --> E["各子模型 EKF 量测更新 → 新息协方差 Sj、马氏距离 dj"]
|
||||
E --> F["似然 Λj = N(dj; 0, Sj)，模型概率 μj' ∝ Λj·c̄j"]
|
||||
F --> G["融合输出：X̂ = Σμj'X̂j<br/>P = Σμj'[Pj + (X̂j−X̂)(X̂j−X̂)ᵀ]"]
|
||||
```
|
||||
|
||||
- 状态布局 `[x, vx, ax, y, vy, ay]`，x/y 两轴解耦；
|
||||
- 模型转移概率矩阵固定：
|
||||
```
|
||||
Pt = [ 0.8 0.15 0.05
|
||||
0.3 0.4 0.3
|
||||
0.05 0.15 0.8 ]
|
||||
```
|
||||
- 机动相关时间常数 α = 0.2；
|
||||
- 数值保护：归一化常数 c̄j ≤ 1e-12 时钳位；似然分母（det S）≤ 1e-12 时该模型似然置 0；概率分母异常时**保持上一拍模型概率**；EKF 似然用 3×3 新息协方差（保证与量测维数一致）。
|
||||
|
||||
### 5.2 三个 Singer 模型的过程噪声强度分档
|
||||
|
||||
按航迹速度 v_track 切换 q 值（模型 1 恒用 slow）：
|
||||
|
||||
| 模型 | TWS 通道 | TAS 通道 |
|
||||
|---|---|---|
|
||||
| 模型 1（低机动） | `Model1_Q_slow`（恒定） | 同左 |
|
||||
| 模型 2（中机动） | v≤5：slow/20；5<v≤30：slow；v>30：fast | v≤5：slow/20；50<v≤100：10·fast；其余：fast |
|
||||
| 模型 3（高机动） | v≤5：slow/20；5<v≤20：slow；20<v≤50：fast；50<v≤100：2·fast；v>100：10·fast | v≤5：slow/20；50<v≤100：10·fast；其余：fast |
|
||||
|
||||
> 语义：低速目标按"低机动"处理（把中/高机动模型噪声压到极小，避免噪声被当成机动），速度越高逐步放大高机动模型的过程噪声。`Model1_Q_fast` 字段不参与计算。
|
||||
|
||||
### 5.3 量测与残差修正（EKF）
|
||||
|
||||
量测向量 Z = [r, θ, v_r]，量测函数由预测状态给出：
|
||||
|
||||
```
|
||||
h(X) = [ √(x²+y²), atan2(y,x) 归一化到 [0,2π), −(x·vx + y·vy)/√(x²+y²) ]
|
||||
```
|
||||
|
||||
在预测点处线性化得到雅可比 H（3×6），量测噪声 R = diag(σr², σa², σv²)（10 m / 0.02 rad / 2 m/s），S = H·P·Hᵀ + R。
|
||||
|
||||
**两处残差修正（关键）**
|
||||
|
||||
1. **方位环绕**：νθ 归一到 [−π, π)，避免 0°/360° 附近的假大残差；
|
||||
2. **多普勒盲速折叠**：`νv ← νv − round(νv / v_bind) · v_bind`，其中
|
||||
|
||||
```
|
||||
v_bind = 150000 / (f_GHz · PRI_µs), f_GHz = 9.2 + 0.02 · 频点号
|
||||
```
|
||||
|
||||
PRI ≤ 0 或频率非法时返回安全值 1.0（接口约定 PRI 单位 µs 且不允许为 0）。
|
||||
|
||||
判据（关联）与模型似然共用同一个 `d = νᵀS⁻¹ν`，且滤波函数把 3×3 新息协方差 S 一并回传给似然使用。
|
||||
|
||||
### 5.4 航迹初始化
|
||||
|
||||
| 场景 | 维数 | 做法 |
|
||||
|---|---|---|
|
||||
| 两点（航迹头 + 1 点，临时航迹） | 4 维 [x, vx, y, vy] | 位置取末点，速度取两点差分；协方差按 Z1 处的**转换量测协方差** R 填充，速度项按 R/T、2R/T² 传播 |
|
||||
| 三点（起批，可靠航迹） | 6 维 | 位置取末点、速度取末两点差分、加速度取两段速度差分；协方差按 R0/R1/R2 误差传播构造 |
|
||||
| 起批时 IMM 初始化 | — | 三个子模型用同一状态/协方差，模型概率 u = [1/3, 1/3, 1/3] |
|
||||
|
||||
**转换量测协方差**（极坐标量测转直角坐标时的等效噪声，含方位非线性引起的附加项，由 σr=10 m、σa=0.02 rad 决定）用于上述初始化与临时航迹统计距离计算。
|
||||
|
||||
> 约束：两点/三点初始化对时间差 T 无除零保护，因此**起批路径要求时间戳严格递增、不允许重复**。
|
||||
|
||||
---
|
||||
|
||||
## 6. 航迹起始（逻辑法）
|
||||
|
||||
### 6.1 状态机
|
||||
### 5.1 状态机（每圈按 ①→⑤ 的固定顺序执行）
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["本圈未被可靠航迹关联的点迹"] --> B["全部转成'航迹头'（1 点，buff_round=1）"]
|
||||
B --> C{"下一圈的点迹"}
|
||||
C --> D["点-航迹头关联（两点起始）<br/>速度区间 + 多普勒一致性"]
|
||||
D -->|命中| E["两点卡尔曼初始化 → 2 点临时航迹"]
|
||||
D -->|未命中| B
|
||||
E --> F{"后续圈的点迹"}
|
||||
F --> G["点-临时航迹关联（三点及以上）<br/>EKF 统计距离 + 折角 + 多普勒同号"]
|
||||
G -->|命中| H["航迹延长（复制原航迹 + 追加新点）"]
|
||||
G -->|未命中| F
|
||||
H --> I{"长度 ≥ 起批点数<br/>且不在起批屏蔽区?"}
|
||||
I -- 是 --> J["同源去重（保留末点 d 更小者）"]
|
||||
J --> K["三点卡尔曼初始化 → 分配航迹号 1~500<br/>IMM 三模型同状态，u=1/3"]
|
||||
K --> L["输出该航迹的历史点（前 N 个点）"]
|
||||
I -- 否 --> F
|
||||
F --> M{"末点 buff_round > 2<br/>或长度 ≥ 10 ?"}
|
||||
M -- 是 --> N["临时航迹消亡（不对外输出）"]
|
||||
A["本圈未被可靠航迹关联的点迹"] --> A0["临时航迹所有点的 buff_round +1"]
|
||||
A0 --> B["① 点 × ≥2 点临时航迹关联（三点及以上）<br/>EKF 统计距离 + 折角 + 多普勒同号<br/>命中 → 复制原航迹 + 追加新点"]
|
||||
B --> C["② 点 × 1 点航迹头关联（两点起始）<br/>速度区间 + 多普勒一致性<br/>命中 → 两点卡尔曼初始化 → 2 点临时航迹"]
|
||||
C --> D["③ 删除 Use_Flag==1 的点迹<br/>剩余点全部转成新航迹头（1 点，buff_round=1）<br/>当圈不再参与关联"]
|
||||
D --> E{"④ 长度 ≥ 起批点数<br/>且 ≤10 且不在起批屏蔽区?"}
|
||||
E -- 是 --> F["同源去重（保留末点 d 更小者）"]
|
||||
F --> G["三点卡尔曼初始化 → 分配航迹号 1~500<br/>IMM 三模型同状态，u=1/3"]
|
||||
G --> H["输出该航迹的历史点（前 N 个点）"]
|
||||
E -- 否 --> I["留在临时航迹表，下一圈继续关联"]
|
||||
H --> J["⑤ 临时航迹消亡（不对外输出）<br/>末点 buff_round > 2 或 长度 ≥ 10"]
|
||||
I --> J
|
||||
```
|
||||
|
||||
### 6.2 关联判据汇总
|
||||
**每圈顺序不可颠倒（`track_init.cpp` L49-126）**：点迹**先**做点-临时航迹关联（延长已有 ≥2 点临时航迹），**再**做点-航迹头关联（产生新的 2 点临时航迹），两阶段都结束后才把未被消费的点迹转成新航迹头，最后才把长度达标的临时航迹转可靠航迹，消亡放在最末。由此可得：
|
||||
|
||||
**阶段一：点 ↔ 航迹头（两点起始）**
|
||||
- 当圈新建的 2 点临时航迹**不可能当圈起批**——长度不足，且新点 `buff_round=1` 要到下一圈才具备关联资格；
|
||||
- 当圈刚被延长到达标长度的临时航迹**可以当圈起批**（第 ④ 步在第 ① 步之后执行）；
|
||||
- 两个关联阶段**互不排斥**：都只把命中的点迹标记 `Use_Flag=1`，航迹头阶段不检查临时航迹阶段留下的标记，因此同一点迹可同时延长一条临时航迹、又开启一条 2 点临时航迹，只有走完两阶段后才统一删除被标记的点迹。
|
||||
|
||||
> 第 ⑤ 步的消亡判据对**未起批**的临时航迹生效；已起批的航迹在第 ④ 步中已从临时航迹表移除。
|
||||
|
||||
### 5.2 关联判据汇总
|
||||
|
||||
每圈按 **① 点-临时航迹关联 → ② 点-航迹头关联** 的顺序依次执行，两阶段的关联判据如下。
|
||||
|
||||
**阶段①：点 ↔ ≥2 点临时航迹（三点及以上）**
|
||||
|
||||
```
|
||||
|r_point − r_末点| ≥ 5 m 距离差硬门限（近距点不参与）
|
||||
d ≤ 8（代码写作 d² ≤ TRACK_START_THRESHOLD²=64） 3 维 EKF 统计距离波门
|
||||
α < ALPHA_START = 60° 折角：最近三点连线夹角
|
||||
v_r,point · v_r,末点 > 0 多普勒同号
|
||||
```
|
||||
|
||||
- d 由 4 维 CV 状态一步预测 + 「距离-方位-径向速度」EKF 得到（过程噪声 0.03²、同样做方位环绕与盲速折叠）；
|
||||
- 折角 α 为向量 D12（前一点→末点）与 D23（末点→新点）的夹角；α 越小越接近直线；
|
||||
- 门限同样写作 `d² ≤ TRACK_START_THRESHOLD²`，而 d 本身已是马氏距离平方，实际生效为 d ≤ 8（与可靠航迹关联门限的语义一致）。
|
||||
|
||||
**阶段②：点 ↔ 航迹头（两点起始）**
|
||||
|
||||
用航迹头位置与点迹的平面距离 `dis` 与 ΔT（秒）：
|
||||
|
||||
@@ -369,27 +314,14 @@ dis ≥ |v_r,head| · ΔT 同上，用航迹头的多普勒约
|
||||
- 判据隐含要求 ΔT > 0（ΔT ≤ 0 时 `dis ≤ V_MAX·ΔT` 必然不成立）；
|
||||
- 近程/中远程分支当前取值相同（都是 V_MAX），**实际不做距离分段**。
|
||||
|
||||
**阶段二：点 ↔ ≥2 点临时航迹（三点及以上）**
|
||||
### 5.3 临时航迹的"圈数"生命周期
|
||||
|
||||
```
|
||||
|r_point − r_末点| ≥ 5 m 距离差硬门限（近距点不参与）
|
||||
d ≤ 8（代码写作 d² ≤ TRACK_START_THRESHOLD²=64） 3 维 EKF 统计距离波门
|
||||
α < ALPHA_START = 60° 折角：最近三点连线夹角
|
||||
v_r,point · v_r,末点 > 0 多普勒同号
|
||||
```
|
||||
|
||||
- d 由 4 维 CV 状态一步预测 + 「距离-方位-径向速度」EKF 得到（过程噪声 0.03²、同样做方位环绕与盲速折叠）；
|
||||
- 折角 α 为向量 D12（前一点→末点）与 D23（末点→新点）的夹角；α 越小越接近直线；
|
||||
- 同样存在"门限对马氏距离平方再平方"的语义，实际生效为 d ≤ 8。
|
||||
|
||||
### 6.3 临时航迹的"圈数"生命周期
|
||||
|
||||
`buff_round` 只在起始流程里自增，并且**每次调用（= 每圈）先把所有临时航迹所有点的 buff_round +1，再做关联**：
|
||||
`buff_round` 只在起始流程里自增，并且**每次调用（= 每圈）先把所有临时航迹所有点的 buff_round +1，再做关联**；因此关联判据看到的是自增后的圈数，当圈刚建立/刚追加的点（buff_round=1）当圈不可能再被关联：
|
||||
|
||||
| 动作 | 效果 |
|
||||
|---|---|
|
||||
| 新点加入 | buff_round = 1 |
|
||||
| 关联资格 | 只看**末点**，要求 buff_round ≥ 2 → **上一圈加入的点本圈才能被延续**，同一航迹每圈最多吸收一个点 |
|
||||
| 关联资格 | 只看**末点**，要求 buff_round ≥ 2 → **上一圈加入的点本圈才能被延续**；同一圈内一条临时航迹可被多个点同时延续（每个点各复制出一条新航迹，见 6.4），并非一对一 |
|
||||
| 消亡条件 | 末点 buff_round > 2（连续 2 圈无新点）或长度 ≥ 10 |
|
||||
|
||||
**时间轴示例（以第 k 圈加入新点为起点）**
|
||||
@@ -402,13 +334,14 @@ v_r,point · v_r,末点 > 0 多普勒同号
|
||||
|
||||
即：**允许中间空 1 圈**，最大关联间隔 2 圈；生命周期完全由"圈数"驱动，与绝对时间无关。
|
||||
|
||||
### 6.4 航迹分裂与去重
|
||||
### 5.4 航迹分裂与去重
|
||||
|
||||
- 关联阶段**不做一对一分配**：一个点满足多条临时航迹条件时，每条都各自复制出一条新航迹；多条点命中同一条航迹时也各自复制 → 同一圈会产生大量同源临时航迹；
|
||||
- 关联阶段**不做一对一分配**：一个点满足多条临时航迹条件时，每条都各自复制出一条新航迹；多个点命中同一条临时航迹时也各自复制；点-航迹头阶段同理（一个航迹头可与多个点各生成一条 2 点临时航迹）→ 同一圈会产生大量同源临时航迹；
|
||||
- 两个关联阶段也**互不排斥**：点-航迹头阶段不检查点-临时航迹阶段留下的 `Use_Flag`，同一点迹可同时参与两边的分裂试探；
|
||||
- 起批前做**同源去重**：比较两条候选航迹的**前三个点**，只要有任一点的"时间 T + 距离 r"完全相同即判为同源，保留末点统计距离 d 更小者；
|
||||
- 这样既保证不漏关联（允许分裂试探），又不会让同一批点重复起批。
|
||||
|
||||
### 6.5 起批与输出
|
||||
### 5.5 起批与输出
|
||||
|
||||
| 条件 | 说明 |
|
||||
|---|---|
|
||||
@@ -422,9 +355,9 @@ v_r,point · v_r,末点 > 0 多普勒同号
|
||||
|
||||
---
|
||||
|
||||
## 7. 外推、消亡与航迹号管理
|
||||
## 6. 外推、消亡与航迹号管理
|
||||
|
||||
### 7.1 消亡条件
|
||||
### 6.1 消亡条件
|
||||
|
||||
| 通道 | 条件（满足其一即消亡） |
|
||||
|---|---|
|
||||
@@ -434,7 +367,7 @@ v_r,point · v_r,末点 > 0 多普勒同号
|
||||
|
||||
消亡时把批号写入消亡输出数组并从航迹表删除；**临时航迹消亡不对外输出**（起批失败的中间态对宿主不可见）。
|
||||
|
||||
### 7.2 航迹号分配
|
||||
### 6.2 航迹号分配
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
@@ -454,9 +387,9 @@ flowchart TD
|
||||
|
||||
---
|
||||
|
||||
## 8. TAS 通道（model=3）
|
||||
## 7. TAS 通道（model=3）
|
||||
|
||||
### 8.1 与 TWS 通道的差异
|
||||
### 7.1 与 TWS 通道的差异
|
||||
|
||||
| 项 | TWS | TAS |
|
||||
|---|---|---|
|
||||
@@ -467,7 +400,7 @@ flowchart TD
|
||||
| 外推时间 | 按 work_mode 数据率固定步长 | 按 `最新时间戳 − 航迹时间` |
|
||||
| 输出 | 全部航迹 | 仅该批号航迹 |
|
||||
|
||||
### 8.2 TAS 关联与更新
|
||||
### 7.2 TAS 关联与更新
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
@@ -501,9 +434,9 @@ flowchart TD
|
||||
|
||||
---
|
||||
|
||||
## 9. TAS 波束控制（外部接口 Beam_Ctrl）
|
||||
## 8. TAS 波束控制（外部接口 Beam_Ctrl）
|
||||
|
||||
### 9.1 队列管理
|
||||
### 8.1 队列管理
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
@@ -527,7 +460,7 @@ flowchart TD
|
||||
|
||||
> 自动开启/退出 TAS 的逻辑（按距离/速度/高度阈值）已全部注释停用，**只支持手动**。
|
||||
|
||||
### 9.2 波束输出调度（关键）
|
||||
### 8.2 波束输出调度（关键）
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
@@ -550,9 +483,9 @@ flowchart TD
|
||||
|
||||
---
|
||||
|
||||
## 10. 参数脉络（决定行为的旋钮）
|
||||
## 9. 参数脉络（决定行为的旋钮）
|
||||
|
||||
### 10.1 编译期宏（随扫描体制切换）
|
||||
### 9.1 编译期宏（随扫描体制切换）
|
||||
|
||||
| 宏 | 相扫 | 机扫 | 作用点 |
|
||||
|---|---|---|---|
|
||||
@@ -570,7 +503,7 @@ flowchart TD
|
||||
| `FREQ0` | 9.2 GHz | 同左 | 盲速计算的基准频率 |
|
||||
| `RDP_LOG_ENABLE / RDP_LOG_DIR` | — | — | 日志落盘开关与目录 |
|
||||
|
||||
### 10.2 运行时参数（宿主传入，随任务变化）
|
||||
### 9.2 运行时参数（宿主传入，随任务变化）
|
||||
|
||||
| 参数 | 影响环节 |
|
||||
|---|---|
|
||||
@@ -589,7 +522,7 @@ flowchart TD
|
||||
|
||||
---
|
||||
|
||||
## 11. 关键约定与注意事项
|
||||
## 10. 关键约定与注意事项
|
||||
|
||||
**时序与时间戳**
|
||||
|
||||
@@ -635,5 +568,8 @@ flowchart TD
|
||||
| 起始输出历史点 | "临时航迹的全部 L 个点" | 实际输出**前 `track_start_point_num` 个点** |
|
||||
| 起始输入过滤 | "track_init 跳过 Use_Flag==1 的点" | 上游 `track_asso_process()` 结束前已把 `Use_Flag==1` 的点迹从 `point_recv` 删除；`track_init` 入口（L59-70）也会再剔除一次 `Use_Flag==1` 的点，两道处理叠加，效果一致 |
|
||||
| 日志开关 | "RDP_LOG_ENABLE 默认开启" | `parameters.h` 中该宏当前为**注释状态**（不落盘，日志进控制台） |
|
||||
| 起始步骤顺序 | "航迹头（1 点）→ 点-航迹头关联（2 点）→ 点-临时航迹关联（≥3 点）→ 起批" | 每圈实际顺序为**点 × ≥2 点临时航迹关联 → 点 × 1 点航迹头关联 → 剩余点转新航迹头 → 长度达标的临时航迹起批**，消亡最后执行（`track_init.cpp` L49-126，见第 6 节） |
|
||||
|
||||
> 后续：README 与 `track_init_readme.md`（航迹起始专项说明）已按上表结论同步（见 CHANGELOG 的 Unreleased 记录）；流程落地实现仍以本文为准。
|
||||
>
|
||||
> 补充修订（本次）：起始流程的**步骤顺序**原先在本文档（第 0、6 节）与 README（第 2、5.2、8.3、9.3 节）中都写作「航迹头 → 2 点 → ≥3 点 → 起批」，与源码每圈「点 × 临时航迹关联 → 点 × 航迹头关联 → 剩余点转航迹头 → 达标者起批」的顺序相反，现已按源码修正，并补充说明两个关联阶段互不排斥、均不做一对一分配。
|
||||
|
||||
Reference in New Issue
Block a user