Compare commits
2
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
a76da34756 | ||
|
|
f249fdc50d |
+7
-1
@@ -14,9 +14,12 @@
|
||||
- `Tracking_Target` 新增 `last_track_time` 字段,记录各 TAS 目标最近一次波束输出时间(f438ac5)。
|
||||
- 新增 requirements.md,说明 TAS 波束控制逻辑的修改背景与约定(b3e2f52)。
|
||||
- 更新 README.md:目录结构补充 CHANGELOG.md / requirements.md;版本历史表补充 93d3924、b3e2f52、e729c5b、f438ac5 提交记录。
|
||||
- 新增日志文件输出功能:`parameters.h` 增加 `RDP_LOG_ENABLE` 开关(默认开启)与 `RDP_LOG_DIR` 目录宏(`.\\rdp_logs\\`);新增 `rdp_log.h/.cpp` 日志模块,首次 `Data_Process_Factory::GetB()` 时按 `RDP_LOG_ENABLE` 决定是否用 `rdbuf()` 将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件(`rdp_YYYYMMDD_HHMMSS.log`,目录不存在自动创建),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭文件;开关注释关闭时不重定向、日志仍输出到标准输出;日志文件在 `CMakeLists.txt` 的 SOURCES 中注册。
|
||||
- 新增日志文件输出功能:`parameters.h` 增加 `RDP_LOG_ENABLE` 开关(定义时启用;**当前源码中该宏为注释状态,即默认不重定向、日志输出到控制台**)与 `RDP_LOG_DIR` 目录宏(`.\\rdp_logs\\`);新增 `rdp_log.h/.cpp` 日志模块,首次 `Data_Process_Factory::GetB()` 时按 `RDP_LOG_ENABLE` 决定是否用 `rdbuf()` 将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件(`rdp_YYYYMMDD_HHMMSS.log`,目录不存在自动创建),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭文件;开关注释关闭时不重定向、日志仍输出到标准输出;日志文件在 `CMakeLists.txt` 的 SOURCES 中注册。
|
||||
- 新增 `RDP_LOG` 日志输出宏(`rdp_log.h`):自动在每条日志前附加 `[时间戳][代码文件:行号]` 信息后再输出到标准输出(重定向开启时随之写入日志文件),供后续新增日志统一使用。
|
||||
- 更新 README.md(604f158、d6cdbc0):目录结构补充 `archived_docs/` 归档目录;9.6 节改写 TWS 关联 ΔT≤0 的处理说明;功能特性、模块职责、已知问题与参考文档的文档引用路径同步指向 `archived_docs/`;版本历史表补充 604f158、d6cdbc0 提交记录。
|
||||
- 输出航迹补充「靠近/远离」方向标志 `Flag_Direction`(f249fdc):TWS 关联输出(`track_asso.cpp`)与 TAS 关联输出(`track_asso_tas.cpp`)在写入 `vr_point` 后追加 `Flag_Direction = vr_point > 0 ? 1 : 0`,即径向速度为正判为靠近(1)、为零或负判为远离(0)。该字段此前已在 `data_process_class_dll.h` 的 `Track` 结构体中声明(注释「方向标志 1 靠近/0 远离」)但从未被赋值,宿主读到的是未初始化/旧值。
|
||||
- 新增三份流程与专项文档(f249fdc):`docs/data_process_flow_report.md`(数据处理流程与关键逻辑说明:总体数据流、前处理与判圈、凝聚、TWS/TAS 关联与滤波、航迹起始、外推消亡与航迹号管理、TAS 波束控制、参数脉络与关键约定)、`docs/data_process_flow_report.html`(同内容的 HTML 渲染版)、`docs/track_init_readme.md`(航迹起始 Track Init 详细逻辑说明:分阶段流程、起始条件汇总、参数调参、输出字段映射、已知风险与代码索引)。
|
||||
- 更新 README.md(7d5a56f、f249fdc):目录结构新增 `docs/` 目录;2、7.2、8.1 节满圈判据改为「方位波位号 == 0」并说明 `last_beam_num` 不再参与判断;8.3、9.3 节改写点-航迹头关联判据与起批输出历史点数;9.6 节补充方向标志输出与点迹消费说明;2、10.1 节 `RDP_LOG_ENABLE` 说明改为当前实际状态(注释停用、默认不重定向);11.4 节新增航迹起始未修复观察;版本历史表补充 7d5a56f、f249fdc,参考文档表补充 `docs/` 三份文档。
|
||||
|
||||
### Changed
|
||||
|
||||
@@ -31,6 +34,9 @@
|
||||
- requirements.md 内容更新为「雷达数据处理项目修改说明」(1521592):现描述日志文件输出功能需求(rdbuf() 重定向、日志开关宏、日期+时间命名日志、前缀日志宏,附参考示例)。
|
||||
- TWS 点航关联中 ΔT ≤ 0(点迹 CPI 时间不晚于航迹时间,如 TAS 航迹遇上时间戳更早的 TWS 点迹、或重复时间戳)的候选不再被剔除(604f158):此类候选仍参与最近邻最小 d 的比较与选取,但不执行 IMM/EKF 更新、不刷新航迹时间与新鲜度(`T_track`、`Extrapolate_round`、`point_flag`、`associate_point_number` 均保持原值,该航迹仍按未关联航迹外推),同时把该点迹标记为已使用(`Use_Flag=1`)消费掉,使其无法再参与航迹起始,避免由此生成虚假 TWS 航迹;`track_asso.cpp` 中 ΔT≤0 的 `RDP_LOG` 告警日志随之注释停用(`track_asso_tas.cpp` 中同名告警日志仍启用)。
|
||||
- 过期文档归档(604f158):`BUG_REPORT.md`、`BUG_FIX_REPORT.md`、`requirements.md` 由仓库根目录移入 `archived_docs/` 留档,仓库内对它们的引用路径同步改为 `archived_docs/...`。
|
||||
- 扫描一圈完成判据改为「方位波位号 == 0」(7d5a56f):原判据「当前方位波位号 < 上一帧波位号」在机扫模式下遇到转台反转时波位号会变小但并未扫完一圈,造成误判;现 `data_preprocess()` 仅在 `Data_Input[0].Beam_index_aiz == 0` 时返回 1(TWS 满一圈,执行航迹处理),否则返回 2(继续蓄数据,只做滑窗凝聚)。原判据代码注释保留,`last_beam_num` 仍被赋值但不再参与判断。调用方需保证点迹的方位波位号按 0~Beam_num-1 编号且每圈必然出现 0。
|
||||
- 点迹-航迹头关联判据改为统一速度区间并增加多普勒-几何一致性约束(7d5a56f,`track_init.cpp`):原先按扫描体制与距离分段收紧速度门限的分支(近程 `r≥1000` 全速 / `r<1000` 半速,中远程 `r≥2000` 全速 / `r<2000` 取 1/2~1/3 速)整体注释停用,改为 `dis ≤ V_MAX·ΔT` 且 `dis ≥ V_MIN·ΔT`,同时要求 `vr_point·v_track_head > 0`,并新增 `dis ≥ |vr_point|·ΔT` 与 `dis ≥ |v_track_head|·ΔT` 两条约束——切向高速目标在 ΔT 内的实际位移远小于 `|vr|·ΔT`,据此被排除,用于限制由切向高速点迹起批出的短航迹。其中 `vmax` 的近程/中远程分支取值相同,均取 `Work_Parameter.V_MAX`。
|
||||
- 同步 `docs/` 内两份专项文档(本次文档更新):`docs/data_process_flow_report.md` 附录与 `docs/track_init_readme.md` 第 12.1 节的「与 README 的差异」说明改为「差异已按结论同步到 README」并保留更新前的旧表述作为改动缘由;其中「起始输入过滤」一行更正为「上游 `track_asso_process()` 已删除 `Use_Flag==1` 的点迹 + `track_init` 入口再次剔除」,「初始化用点」一行更正为「取下标 `track_start_point_num-3 ~ -1`,航迹更长时不是真正的末三点」;`docs/track_init_readme.md` 第 15 节第 2 条相应更正(`track_init.cpp` L505 的 `L` 覆盖为 `track_start_point_num`,第 L 个点之后的尾部点既不参与初始化也不输出),第 14 节补充说明 `Flag_Direction` 仅由关联输出路径赋值、起批输出不写该字段。
|
||||
|
||||
### Fixed
|
||||
|
||||
|
||||
@@ -51,16 +51,16 @@
|
||||
|
||||
## 2. 功能特性
|
||||
|
||||
- **双模式处理管线**:data_preprocess 按输入类型与扫描圈完成标志返回 0/1/2/3,track_process 据此调度 TWS 凝聚/关联/起始或 TAS 凝聚/关联。
|
||||
- **双模式处理管线**:data_preprocess 按输入类型与扫描圈完成标志返回 0/1/2/3(TWS 以方位波位号 == 0 判为一圈完成,机扫转台反转不再误判),track_process 据此调度 TWS 凝聚/关联/起始或 TAS 凝聚/关联。
|
||||
- **两帧滑窗点迹凝聚**:TWS 点迹先与上一帧缓存合并凝聚,再进入关联,兼顾跨波位(CPI)相邻点迹的分裂问题;近程模式增加速度维约束。
|
||||
- **逻辑法航迹起始**:航迹头(单点)→ 点-头速度法关联(两点)→ 点-临时航迹 EKF 波门 + 折角约束关联(三点及以上)→ 三点卡尔曼初始化转可靠航迹;带起批屏蔽区与重复航迹去重。
|
||||
- **3 模型 IMM 滤波**:三个 Singer 模型(低/中/高过程噪声),量测为「距离-方位-径向速度」三维的 EKF,多普勒残差按盲速折叠,似然用 3×3 新息协方差。
|
||||
- **最近邻数据关联**:TWS 按航迹距离分段收紧模型 3 波门并附加距离门限;TAS 附加按距离分段的高度门限。ΔT ≤ 0(点迹时间戳不晚于航迹时间)的候选不更新航迹状态、仅消费该点迹(标记已使用),避免其继续起批而生成虚假 TWS 航迹。
|
||||
- **最近邻数据关联**:TWS 按航迹距离分段收紧模型 3 波门并附加距离门限;TAS 附加按距离分段的高度门限。ΔT ≤ 0(点迹时间戳不晚于航迹时间)的候选不更新航迹状态、仅消费该点迹(标记已使用),避免其继续起批而生成虚假 TWS 航迹。关联输出的航迹同时给出**靠近/远离方向标志**:`Flag_Direction = vr_point > 0 ? 1 : 0`(1 靠近 / 0 远离),TWS、TAS 两通道一致。
|
||||
- **高度维独立平滑**:按距离分段选择滑动平均窗长(5/6/7/9 点),抑制俯仰测量跳变。
|
||||
- **TAS 波束控制**:手动转 TAS 目标入队(队列长度与容量随扫描体制切换),按最新时间戳外推输出跟踪波束(距离/方位/俯仰/批号)。
|
||||
- **航迹生命周期管理**:外推轮数计数、超时消亡、手动删除、全清空(含各模块 reset,防重连后幽灵航迹)。
|
||||
- **防御性加固**:见 archived_docs/BUG_FIX_REPORT.md——越界防护、时间戳正序校验、方位角 ±π 环绕、asin 定义域钳位、IMM 概率奇异保护等已修复项。
|
||||
- **日志文件输出**:`RDP_LOG_ENABLE` 开启时(默认),首次 `Data_Process_Factory::GetB()` 自动用 `rdbuf()` 将 `std::cout` 重定向到 `RDP_LOG_DIR`(`.\rdp_logs\`)下按日期+时间命名的日志文件,`Destroy()` 时恢复控制台并关闭文件;`RDP_LOG` 日志宏在每条日志前附加 `[时间戳][文件:行号]`,开关注释关闭时不重定向、日志仍输出到控制台。
|
||||
- **日志文件输出**:`RDP_LOG_ENABLE` 定义时,首次 `Data_Process_Factory::GetB()` 自动用 `rdbuf()` 将 `std::cout` 重定向到 `RDP_LOG_DIR`(`.\rdp_logs\`)下按日期+时间命名的日志文件,`Destroy()` 时恢复控制台并关闭文件;`RDP_LOG` 日志宏在每条日志前附加 `[时间戳][文件:行号]`。该宏在 `parameters.h` 中**当前为注释状态(默认不启用)**,即默认不做重定向、日志输出到控制台(见 10.1)。
|
||||
|
||||
---
|
||||
|
||||
@@ -71,10 +71,14 @@
|
||||
├── CMakeLists.txt # CMake 构建脚本(扫描模式切换、导出宏、输出目录)
|
||||
├── CHANGELOG.md # 变更日志(Keep a Changelog 格式,按 git 提交记录维护)
|
||||
├── README.md # 本文件:项目说明(架构、接口、流程、算法、参数)
|
||||
├── archived_docs/ # 过期文档留档(仅作历史参考;接口与流程以本 README 为准)
|
||||
├── archived_docs/ # 过期文档留档(仅作历史参考;接口与流程以本 README 与 docs/ 专项说明为准)
|
||||
│ ├── BUG_REPORT.md # 全量逻辑 BUG 分析报告(P0~P3,37 项)
|
||||
│ ├── BUG_FIX_REPORT.md # BUG 修复报告(修复明细 + 编译验证结果)
|
||||
│ └── requirements.md # 需求说明:日志文件输出功能修改要求(含 rdbuf() 重定向参考示例)
|
||||
├── docs/ # 专项说明文档(按当前源码整理,与 README 互补)
|
||||
│ ├── data_process_flow_report.md # 数据处理流程与关键逻辑说明(前处理/判圈、凝聚、关联与滤波、起始、外推消亡、TAS 波束、参数脉络)
|
||||
│ ├── data_process_flow_report.html # 上文的 HTML 渲染版(浏览器直接打开)
|
||||
│ └── track_init_readme.md # 航迹起始(Track Init)详细逻辑说明(阶段流程、条件汇总、输出字段映射、风险点、代码索引)
|
||||
├── .vscode/ # VS Code 工程配置
|
||||
│ ├── settings.json # cmake.generator=NMake、MSVC2013 x86 环境变量、compile_commands
|
||||
│ ├── cmake-variants.yaml # X256_PS / X256_MS × Release / Debug 变体
|
||||
@@ -267,7 +271,7 @@ classDiagram
|
||||
| 结构体 | 用途 | 要点 |
|
||||
|---|---|---|
|
||||
| DataRev | 输入点迹 | 距离(m)、方位/俯仰(°)、速度(m/s)、幅度/门限/SNR/RCS、码盘值、CPI/GNSS 时间(ms, long long)、方位波位(0-24)/俯仰波位(0-13)、PRI(µs,**不允许为 0**)、频点号、点数、点迹号、使用标记、TAS 批号、点迹类型、十字星数据 speed_dim[129]/range_dim[8] |
|
||||
| Track | 输出航迹 | 位置/速度、极坐标量、航迹号、更新点数、方向/实虚点标志、一步预测、跟踪模式、航向角、目标类型、关联点信息、航迹时间(s)/GNSS、SNR/RCS、十字星数据透传 |
|
||||
| Track | 输出航迹 | 位置/速度、极坐标量、航迹号、更新点数、方向标志 Flag_Direction(关联输出赋值:1 靠近 / 0 远离,见 9.6)、实虚点标志 Flag_Point、一步预测、跟踪模式、航向角、目标类型、关联点信息、航迹时间(s)/GNSS、SNR/RCS、十字星数据透传 |
|
||||
| TrackingBeam | TAS 跟踪波束指令 | open_flag、type、Range、Azi、Elev、TAS_track_index |
|
||||
| Target_direct_tracking | 引导跟踪信息 | 批号、距离/方位/俯仰、开关(接口未启用) |
|
||||
| RadarPara | 工作参数 | 见 [参数说明](#10-参数说明) |
|
||||
@@ -306,18 +310,18 @@ classDiagram
|
||||
| tracking_point(azimuth) | 0 | 手动打跟踪波束——**未实现(保留空实现)** |
|
||||
| direct_tracking_process(...) | 0 | 引导跟踪——**未实现(保留空实现)** |
|
||||
|
||||
> 日志输出为 DLL 内部功能(不对外导出,接口见 10.1 的宏开关):首次 `GetB()` 创建实例时自动初始化(`RDP_LOG_ENABLE` 开启则将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭日志文件。
|
||||
> 日志输出为 DLL 内部功能(不对外导出,接口见 10.1 的宏开关):首次 `GetB()` 创建实例时自动初始化(`RDP_LOG_ENABLE` 定义则将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件,该宏当前为注释状态,见 10.1),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭日志文件。
|
||||
|
||||
### 7.2 data_preprocess 返回值的含义
|
||||
|
||||
| 返回值 | 含义 | 触发条件 |
|
||||
|---|---|---|
|
||||
| 0 | 无效数据 | point_type 既非 0 也非 1 |
|
||||
| 1 | **TWS 满一圈,执行航迹处理** | point_type==0 且当前方位波位号 < 上一帧波位号(波位号由大变小判为扫描一圈完成) |
|
||||
| 2 | **TWS 未满一圈,继续蓄数据** | point_type==0 且波位号未变小;调用 track_process(model=2) 做滑窗凝聚 |
|
||||
| 1 | **TWS 满一圈,执行航迹处理** | point_type==0 且当前帧方位波位号 == 0(7d5a56f 起改用波位号为 0 判满圈:机扫转台反转时波位号会变小但并未扫完一圈,原「波位号变小」判据会误判) |
|
||||
| 2 | **TWS 未满一圈,继续蓄数据** | point_type==0 且方位波位号 != 0;调用 track_process(model=2) 做滑窗凝聚 |
|
||||
| 3 | **TAS 处理** | point_type==1;同时记录 TAS_track_idx 与最新时间戳 |
|
||||
|
||||
> 首次调用时 last_beam_num 为 INT_MAX,因此必然返回 1;track_clear_all() 后同样复位为 INT_MAX。
|
||||
> 7d5a56f 起满圈判据不再依赖上一帧波位号:`last_beam_num` 仍被赋值(`track_clear_all()` 复位为 INT_MAX),但不参与判断;因此首帧是否返回 1 取决于该帧方位波位号是否为 0。调用方需保证方位波位号按 0~Beam_num-1 编号且每圈均出现 0,否则 TWS 通道将持续返回 2(不满圈)。
|
||||
|
||||
### 7.3 典型调用序列
|
||||
|
||||
@@ -349,7 +353,7 @@ flowchart TD
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["data_preprocess: 点迹入 Data_buffer(度→弧度、高度=Range·sin(Elev)、时间戳缓存)"] --> B["波位号变小(一圈完成)→ model=1"]
|
||||
A["data_preprocess: 点迹入 Data_buffer(度→弧度、高度=Range·sin(Elev)、时间戳缓存)"] --> B["方位波位号 == 0(一圈完成)→ model=1"]
|
||||
B --> C["track_process(1)"]
|
||||
C --> D["dot_coh 两帧滑窗凝聚 → point_recv"]
|
||||
D --> E["track_asso: IMM 交互 → 点航波门筛选(NN)→ EKF 滤波 → 模型输出"]
|
||||
@@ -380,13 +384,13 @@ flowchart TD
|
||||
flowchart TD
|
||||
A["未关联点迹"] --> B["转航迹头 Temp_track(1点) buff_round=1"]
|
||||
B --> C{"下一帧点迹"}
|
||||
C --> D["点-航迹头关联:速度区间门限 + 径向速度同号 → 两点卡尔曼初始化(航迹头→2 点临时航迹)"]
|
||||
C --> D["点-航迹头关联:V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT、径向速度同号、dis ≥ |vr_point|·ΔT 且 dis ≥ |v_track_head|·ΔT → 两点卡尔曼初始化(航迹头→2 点临时航迹)"]
|
||||
D --> E{"后续点迹"}
|
||||
E --> F["点-临时航迹关联:EKF 统计距离波门 d²≤64、折角<60°、径向速度同号、|Δr|≥5m"]
|
||||
F --> G{"长度 ≥ track_start_point_num(3~9) 且 ≤10"}
|
||||
G -- 是 --> H["屏蔽区判断 → 重复航迹去重(保留末点 d 更小者)→ 三点卡尔曼初始化"]
|
||||
H --> I["转可靠航迹:分配航迹号 1~500,IMM 三模型同状态初始化 u=1/3"]
|
||||
I --> J["按历史点数输出 Trust_Track_Output[i][0..L-1]"]
|
||||
I --> J["输出前 track_start_point_num 个历史点 Trust_Track_Output[i][0..L-1]"]
|
||||
G -- 否 --> K{"buff_round>2 或 长度≥10 ?"}
|
||||
K -- 是 --> L["临时航迹消亡"]
|
||||
K -- 否 --> E
|
||||
@@ -445,23 +449,20 @@ $$
|
||||
|
||||
#### (1) 点-航迹头关联(两点起始)
|
||||
|
||||
对单点航迹头(buff_round ≥ 2),量测点需同时满足**速度区间门限**与**径向速度同号**:
|
||||
|
||||
- 近程(work_mode==0):
|
||||
对单点航迹头(buff_round ≥ 2),量测点需同时满足以下四个条件(7d5a56f 起的判据):
|
||||
|
||||
$$
|
||||
r_p \ge 1000:\ V_{min}\Delta T \le d_{12} \le V_{max}\Delta T;\qquad
|
||||
r_p < 1000:\ \tfrac12 V_{min}\Delta T \le d_{12} \le \tfrac12 V_{max}\Delta T
|
||||
V_{min}\Delta T \le d_{12} \le V_{max}\Delta T,\qquad
|
||||
v_{r,point}\cdot v_{r,head}>0,\qquad
|
||||
d_{12} \ge \left|v_{r,point}\right|\Delta T,\qquad
|
||||
d_{12} \ge \left|v_{r,head}\right|\Delta T
|
||||
$$
|
||||
|
||||
- 中/远程(work_mode!=0):
|
||||
其中 d12 为两点间平面距离,ΔT 为时间差(秒),V_min/V_max 取 `RadarPara.V_MIN / V_MAX`(代码中 vmax 的近程/中远程分支取值相同,均为 V_MAX)。后两条为**多普勒-几何一致性约束**:切向飞行目标在 ΔT 内的实际位移远小于 |vr|·ΔT,据此被排除,用于限制切向高速点迹起批出的短航迹。
|
||||
|
||||
$$
|
||||
r_p \ge 2000:\ V_{min}\Delta T \le d_{12} \le V_{max}\Delta T;\qquad
|
||||
r_p < 2000:\ \tfrac12 V_{min}\Delta T \le d_{12} \le \tfrac13 V_{max}\Delta T
|
||||
$$
|
||||
> 历史行为(已注释停用):原先按扫描体制与距离分段收紧速度门限——近程 r≥1000 用全速、r<1000 用半速;中远程 r≥2000 用全速、r<2000 取下限 1/2·V_min·ΔT / 上限 1/3·V_max·ΔT。
|
||||
|
||||
其中 d12 为两点间平面距离,ΔT 为时间差(秒),并需 vr_point · vr_head > 0。关联成功用 **两点卡尔曼初始化**(见 9.5.2)。
|
||||
关联成功用 **两点卡尔曼初始化**(见 9.5.2)。
|
||||
|
||||
#### (2) 点-临时航迹关联(三点及以上)
|
||||
|
||||
@@ -481,11 +482,12 @@ $$
|
||||
|
||||
#### (3) 转可靠航迹(起批)
|
||||
|
||||
临时航迹长度 L 满足 track_start_point_num ≤ L ≤ 10(起批点数由参数给定,典型 3~4,接口层校验 3~9)且不在起批屏蔽区内(屏蔽区为距离-方位矩形区域,最多 30 个)时进入起批队列;同源(共享前三点)的重复航迹只保留末点 d 最小者。起批时用 **最近三点卡尔曼初始化** 得到 6 维状态,构造 Trust_Track:
|
||||
临时航迹长度 N 满足 track_start_point_num ≤ N ≤ 10(起批点数由参数给定,典型 3~4,接口层校验 3~9)且不在起批屏蔽区内(屏蔽区为距离-方位矩形区域,最多 30 个)时进入起批队列;同源(前三点时间/距离相同)的重复航迹只保留 d 较小者(比较下标 `track_start_point_num-1` 处的 d)。起批时令 L = track_start_point_num,用航迹中下标 L-3、L-2、L-1 的三个点做 **三点卡尔曼初始化** 得到 6 维状态(航迹长度恰为 L 时即最近三点),构造 Trust_Track:
|
||||
|
||||
- 三个 IMM 子模型用同一状态/协方差初始化,模型概率 u = [1/3, 1/3, 1/3];
|
||||
- 分配航迹号(1~500,见 9.8);
|
||||
- 将临时航迹的全部 L 个点写入 Trust_Track_Output(每条航迹最多 10 个历史点)。
|
||||
- 输出仅写入前 L 个历史点(`Trust_Track_Output[i][0..L-1]`,`Point_Sum = L`),航迹长度大于 L 时其余点不输出;
|
||||
- 起批输出不写 `Flag_Direction`、`x_Predict/y_Predict/z_Predict`、`Range_Predict/Azimuth_Predict/Elevation_Predict`、`Target_Type`、`prf_point`、`v_z` 等字段,且 DLL 不清零输出数组,宿主读到的是自身缓存的旧值(详见 docs/track_init_readme.md 第 14 节)。
|
||||
|
||||
### 9.4 IMM 框架(3 个 Singer 模型)
|
||||
|
||||
@@ -692,7 +694,7 @@ VT<100\ \text{且}\ R>300\ \Rightarrow 拒绝;\qquad
|
||||
VT>300\ \text{且}\ R>1200\ \Rightarrow 拒绝
|
||||
$$
|
||||
|
||||
候选对按 d_min = min(d1,d2,d3) 全局排序,**最近邻**原则逐个取点:每条航迹每圈最多关联一个点、每个点只属于一条航迹。关联成功后:三模型分别 EKF 更新 → 模型概率更新 → 更新航迹时间/幅度/SNR/RCS/十字星数据 → 高度平滑。
|
||||
候选对按 d_min = min(d1,d2,d3) 全局排序,**最近邻**原则逐个取点:每条航迹每圈最多关联一个点、每个点只属于一条航迹。关联成功后:三模型分别 EKF 更新 → 模型概率更新 → 更新航迹时间/幅度/SNR/RCS/十字星数据 → 高度平滑。关联输出的航迹同时写入**方向标志**(f249fdc):`Flag_Direction = (vr_point > 0) ? 1 : 0`,即径向速度为正判为靠近(1)、为零或负判为远离(0),取值来自航迹记录的径向速度(最近一次关联点迹的速度)。
|
||||
|
||||
**ΔT ≤ 0 的候选(604f158 起的行为)**:ΔT = (点迹 CPI 时间 − 航迹 T_track)/1000,ΔT ≤ 0 表示该点迹时间戳不晚于航迹时间(典型场景:航迹已按 TAS 点迹更新过,本圈 TWS 点迹的时间戳更早,或存在重复时间戳)。这种候选**不再被剔除**,仍参与最近邻最小 d 的比较与选取,但选中后:
|
||||
|
||||
@@ -700,7 +702,7 @@ $$
|
||||
- 不刷新航迹时间 T_track / GNSS_time 与点迹信息;
|
||||
- 不置实点标志:`Extrapolate_round`、`point_flag`、`associate_point_number` 均保持原值,因此该航迹仍按「未关联航迹」在后续外推环节推进一帧(见 9.9);
|
||||
- 该候选对连同该航迹本圈的其余候选对被一并移出候选表,因此该航迹本圈不会再关联其他点迹;由于时间戳更早的候选仍参与最小 d 比较,它可能先于有效候选(ΔT>0)被取中;
|
||||
- 该点迹的 `Use_Flag` 被置 1 消费掉——点迹使用标记是航迹起始的输入过滤条件(track_init 跳过 Use_Flag==1 的点),因此该点迹不会再参与起批,避免生成虚假 TWS 航迹。
|
||||
- 该点迹的 `Use_Flag` 被置 1 消费掉:`track_asso_process()` 在关联结束后会把 `Use_Flag==1` 的点迹从 `point_recv` 中删除(track_init 入口也会再剔除一次 `Use_Flag==1` 的点),因此该点迹不会进入航迹起始、不会再参与起批,避免生成虚假 TWS 航迹。
|
||||
|
||||
> 历史行为:604f158 之前,ΔT ≤ 0 的候选在选取时被直接剔除并 `continue`,点迹保持未被使用,可能被后续用于航迹起始。该分支原有的 `RDP_LOG` 告警日志现已注释停用;TAS 关联(track_asso_tas.cpp,见下)中同类告警日志仍启用。
|
||||
|
||||
@@ -714,6 +716,8 @@ $$
|
||||
|
||||
高度门限按航迹距离分段:≤1000 m:|h_t−h_p| ≤ 150 m;1000~2000 m:≤ 200 m;2000~3000 m:≤ 200 m;3000~5000 m:≤ 300 m;>5000 m:不约束。
|
||||
|
||||
输出航迹同样写入**方向标志** `Flag_Direction`(由 `vr_point` 符号决定,f249fdc),规则与 TWS 通道一致。
|
||||
|
||||
> ⚠ **波门语义提示**:代码中 d 本身已是马氏距离平方,但门限写作 d*d < ASSO_THORD*ASSO_THORD,等效于 d < 3(而非 √d < 3)。该行为在 BUG-20 中被识别,**有意保留未改**;ASSO_THORD/1000、/100 已改为浮点除法(BUG-21 修复)。
|
||||
|
||||
### 9.7 高度平滑
|
||||
@@ -767,7 +771,7 @@ $$
|
||||
|
||||
| 宏 | 相扫 PHASE_SCANNING | 机扫 MECHANICAL_SCANNING | 含义 |
|
||||
|---|---|---|---|
|
||||
| RDP_LOG_ENABLE | 定义(默认开启) | 同左 | 日志文件输出开关:定义时将 `std::cout` 重定向到日志文件;注释掉则不重定向(日志仍输出到标准输出/控制台) |
|
||||
| RDP_LOG_ENABLE | 注释停用(默认不启用) | 同左 | 日志文件输出开关:定义时将 `std::cout` 重定向到日志文件;**当前源码中该宏为注释状态**(parameters.h 的注释文字仍写「定义(默认)」,以代码实际状态为准),不重定向、日志输出到标准输出/控制台 |
|
||||
| RDP_LOG_DIR | `.\rdp_logs\` | 同左 | 日志文件输出目录(相对宿主进程当前工作目录,目录不存在时自动创建) |
|
||||
| DATA_RATE_TAS | 已停用(原 0.3) | 已停用(原 0.0625) | 编译期宏已注释停用;TAS 数据率统一由运行时参数 RadarPara.DATA_RATE_TAS 提供(见 10.2) |
|
||||
| SIGMA_R / SIGMA_A / SIGMA_E / SIGMA_V | 10.0 / 0.02 / 0.2 / 2.0(两体制相同) | 同左 | 量测误差:距离 m / 方位 rad / 俯仰 rad / 速度 m/s |
|
||||
@@ -839,6 +843,16 @@ $$
|
||||
- RadarPara 中的起批/关联波门字段未被实现读取(见 10.2)。
|
||||
- build/output/ 下为早期打包导出物,其 data_process_class_dll.h 时间戳仍为 32 位 int,与当前 64 位接口不一致,勿作为对接依据。
|
||||
|
||||
### 11.4 航迹起始相关的未修复观察(详见 docs/track_init_readme.md 第 15 节)
|
||||
|
||||
以下为阅读源码得到的观察,**未在本轮文档更新中改动代码**;修改前请结合实测数据评估:
|
||||
|
||||
- 点-临时航迹关联硬编码 `|Δr| ≥ 5 m` 门限,切向/远距离慢速目标可能漏关联导致起批失败。
|
||||
- 起批判据对马氏距离平方再次平方(`d*d ≤ TRACK_START_THRESHOLD²`),实际门限为 `d ≤ 8`,按字面 `d² ≤ 64` 理解会严重放宽;调 `TRACK_START_THRESHOLD` 时需注意这一层。
|
||||
- 折角计算 `acos` 无定义域钳位,浮点越界产生 NaN 时条件恒假(静默丢弃该关联)。
|
||||
- 起批输出数组行号可能越过容量:容量判据用 `trust_track->size() < MAX_TRACK_INDEX`,而输出行计数在 track_asso 阶段(track_die 删除航迹之前)已推进,极端场景(当圈消亡 ≥ 2 条且起批占满配额)可写到 `MAX_TRACK_NUM` 之外。
|
||||
- 起批输出有多个字段未赋值(见 9.3(3) 与 docs/track_init_readme.md 第 14 节),宿主不应依赖这些字段。
|
||||
|
||||
---
|
||||
|
||||
## 12. 版本历史
|
||||
@@ -847,6 +861,8 @@ $$
|
||||
|
||||
| 提交 | 内容 |
|
||||
|---|---|
|
||||
| f249fdc | 输出航迹新增「靠近/远离」方向标志:`track_asso.cpp`(TWS)与 `track_asso_tas.cpp`(TAS)在写入 `vr_point` 后补 `Flag_Direction = vr_point > 0 ? 1 : 0`;新增 `docs/data_process_flow_report.md`、`docs/data_process_flow_report.html`、`docs/track_init_readme.md` 三份流程与专项文档 |
|
||||
| 7d5a56f | 满圈判据由「波位号变小」改为「方位波位号 == 0」(避免机扫转台反转误判,`data_process.cpp`);点-航迹头关联改为统一速度区间 `V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT` 并新增 `dis ≥ |vr_point|·ΔT`、`dis ≥ |v_track_head|·ΔT` 两条多普勒-几何一致性约束,限制切向高速短航迹(`track_init.cpp`);同步更新 README/CHANGELOG |
|
||||
| 604f158 | TWS 关联中 ΔT≤0(点迹时间戳不晚于航迹时间)的候选不再剔除:仍参与最近邻选取但不更新 IMM/EKF、不刷新航迹时间与新鲜度,并标记该点迹已使用(`Use_Flag=1`)消费掉,避免其继续参与航迹起始生成虚假 TWS 航迹;`track_asso.cpp` 的 ΔT≤0 告警日志注释停用;过期文档(BUG_REPORT.md、BUG_FIX_REPORT.md、requirements.md)移入 archived_docs/ 留档 |
|
||||
| d6cdbc0 | 删除 `data_process.cpp`、`dot_coh.cpp` 中已注释停用的调试输出与早期 1 km SNR 过滤代码残留(不改变运行逻辑);同步更新 README/CHANGELOG |
|
||||
| 1521592 | 新增日志模块(`rdp_log.h/.cpp`,CMake SOURCES 注册、.gitignore 忽略 `rdp_logs/`):`RDP_LOG_ENABLE` / `RDP_LOG_DIR` 宏(parameters.h)、std::cout 经 `rdbuf()` 重定向到 `.\rdp_logs\` 下日期+时间命名日志、`RDP_LOG` 前缀宏、ΔT≤0 告警日志;`RadarPara.DATA_RATE_TAS` 单位由 Hz 改为 s;修复 `tas_beam_output()` 将向量下标误赋给 `Tracking_beam->TAS_track_index` 的问题;更新 README/CHANGELOG |
|
||||
@@ -878,6 +894,9 @@ $$
|
||||
| 文档 | 内容 |
|
||||
|---|---|
|
||||
| CHANGELOG.md | 变更日志(Keep a Changelog 格式,按 git 提交记录维护) |
|
||||
| docs/data_process_flow_report.md | 数据处理流程与关键逻辑说明(按当前源码整理:前处理与判圈、凝聚、TWS/TAS 关联与滤波、航迹起始、外推消亡、TAS 波束控制、参数脉络;含与本文档的历史差异提示) |
|
||||
| docs/data_process_flow_report.html | 上文同内容的 HTML 渲染版 |
|
||||
| docs/track_init_readme.md | 航迹起始(Track Init)专项说明:分阶段流程、起始条件汇总、调参直觉、输出字段映射(含未赋值字段)、已知风险与代码索引 |
|
||||
| archived_docs/BUG_REPORT.md | 全量逻辑 BUG 分析(P0~P3 共 37 项,含位置、说明、建议、处置结论)——过期留档,仅作历史参考 |
|
||||
| archived_docs/BUG_FIX_REPORT.md | 修复明细、修复状态汇总表与 MSVC 2013 x86 编译验证记录——过期留档(对应 01d28e0 时期) |
|
||||
| archived_docs/requirements.md | 修改需求说明:日志文件输出功能(rdbuf() 重定向、parameters.h 日志开关宏、日期+时间命名日志、前缀日志宏,附参考示例)——已实现,留档 |
|
||||
|
||||
@@ -110,6 +110,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = static_cast<float>((*trust_track)[i].prf_point);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point>0?1:0;
|
||||
|
||||
//直接输出点迹高度-20260605
|
||||
//Trust_Track_Output[*Trust_track_num_Output-1][0].z = (*trust_track)[i].range_point * sin((*trust_track)[i].elev_point/180.0*PI);
|
||||
|
||||
@@ -87,6 +87,7 @@ long long latest_timestamp //最新时间戳
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point>0?1:0;
|
||||
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
|
||||
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,639 @@
|
||||
# 雷达数据处理流程与关键逻辑说明
|
||||
|
||||
> 适用对象:本仓库 `data_process_class_dll`(X256 雷达数据处理动态链接库)
|
||||
|
||||
---
|
||||
|
||||
## 0. 一页速览
|
||||
|
||||
| 环节 | 输入 | 处理 | 输出 |
|
||||
|---|---|---|---|
|
||||
| ① 前处理 | 一帧原始点迹(TWS 或 TAS) | 单位换算、缓存、判圈 | 处理模式码 0/1/2/3 |
|
||||
| ② 调度 | 模式码 | 分流到 TWS / TAS 通道 | — |
|
||||
| ③ 凝聚 | 单 CPI 或相邻两 CPI 点迹 | 同类点合并(保幅度大者) | 凝聚点迹表 |
|
||||
| ④ 关联 | 凝聚点迹 + 已有航迹 | IMM 交互 → 波门筛选 → 最近邻一对一 → EKF 更新 | 更新航迹 + 剩余点迹 |
|
||||
| ⑤ 起始 | 剩余(未关联)点迹 | 逻辑法:航迹头 → 2 点 → ≥3 点 → 起批 | 新可靠航迹 |
|
||||
| ⑥ 外推/消亡 | 未关联航迹 | 按数据率预测一帧;超限则消亡 | 补点航迹 / 消亡批号 |
|
||||
| ⑦ 波束控制 | TAS 队列 + 航迹 | 数据率门控 + 队列调度 → 位置外推 | 跟踪波束指令 |
|
||||
|
||||
**贯穿全流程的三条主线**
|
||||
|
||||
1. **时间主线**:所有时间戳为 64 位毫秒(CPI/GNSS),参与运算时除 1000 转秒;ΔT = 点迹时间 − 航迹时间,ΔT 的正负决定"有效更新 / 只消费不更新 / 跳过"。
|
||||
2. **状态主线**:每条可靠航迹维护一套 IMM(3 个 Singer 模型)状态 [x, vx, ax, y, vy, ay],x/y 两轴解耦,量测为「距离-方位-径向速度」三维 EKF。
|
||||
3. **生命周期主线**:可靠航迹靠"外推轮数"消亡;临时航迹靠"缓存圈数 buff_round"消亡。
|
||||
|
||||
---
|
||||
|
||||
## 1. 总体数据流
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
IN["宿主逐波位/逐CPI送入 DataRev[150]"] --> DP["data_preprocess 前处理与判圈"]
|
||||
DP -->|"model=2 TWS未满圈"| M2["滑窗凝聚(只凝聚,不关联)"]
|
||||
DP -->|"model=1 TWS满一圈"| M1["TWS 通道"]
|
||||
DP -->|"model=3 TAS数据"| M3["TAS 通道"]
|
||||
|
||||
M2 --> BUF["本帧凝聚结果留作下一圈缓存"]
|
||||
M2 --> PR["上一帧凝聚点 → point_recv(参与下一圈关联)"]
|
||||
|
||||
M1 --> A1["IMM 交互 + 波门筛选 + 最近邻关联 + EKF 更新"]
|
||||
A1 --> A2["未关联航迹外推 / 超限消亡"]
|
||||
A2 --> A3["剩余点迹 → 逻辑法航迹起始"]
|
||||
A3 --> OUT1["输出更新航迹 + 起批航迹历史点 + 消亡批号"]
|
||||
|
||||
M3 --> B1["TAS 凝聚(单帧)"]
|
||||
B1 --> B2["仅对指定批号航迹做关联/EKF 或外推"]
|
||||
B2 --> B3["TAS 超时消亡"]
|
||||
B3 --> OUT2["输出该批号航迹"]
|
||||
|
||||
OUT1 --> BC["Beam_Ctrl 波束控制"]
|
||||
OUT2 --> BC
|
||||
BC --> OUT3["TrackingBeam:距离/方位/俯仰/批号"]
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. 入口前处理与"判圈"
|
||||
|
||||
### 2.1 前处理动作
|
||||
|
||||
对每一个输入点迹统一做下面几件事(TWS 与 TAS 相同):
|
||||
|
||||
| 动作 | 说明 |
|
||||
|---|---|
|
||||
| 单位换算 | 方位 度 → 弧度(内部统一弧度,输出再转度) |
|
||||
| 高度折算 | `Height = Range · sin(Elevation)`(由量测俯仰直接折算,不经坐标滤波) |
|
||||
| 字段搬运 | 幅度、SNR、RCS、时间戳、频点号、PRI、俯仰波位、十字星数据(`speed_dim[129]`/`range_dim[8]`)全部透传 |
|
||||
| 使用标记 | 全部置 0(未使用) |
|
||||
| 数量保护 | TWS:`min(Point_Sum, 150)`;TAS:`clamp(Point_Sum, 0, 150)` |
|
||||
| 全局时间 | 每次调用都用本帧首点的 CPI 时间刷新"最新时间戳",供 TAS 外推/波束使用 |
|
||||
|
||||
### 2.2 判圈逻辑(决定走哪条通道)
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["收到一帧点迹"] --> B{"point_type ?"}
|
||||
B -->|"0 = TWS"| C["写入 TWS 缓存"]
|
||||
C --> D{"方位波位号 Beam_index_aiz == 0 ?"}
|
||||
D -->|"是"| E["return 1:满一圈,执行 TWS 航迹处理"]
|
||||
D -->|"否"| F["return 2:未满圈,仅做滑窗凝聚"]
|
||||
B -->|"1 = TAS"| G["写入 TAS 缓存<br/>记录 TAS 批号与本帧时间戳"]
|
||||
G --> H["return 3:TAS 处理"]
|
||||
B -->|"其它"| I["return 0:无效数据,丢弃"]
|
||||
```
|
||||
|
||||
**要点**
|
||||
|
||||
- 判圈条件由"波位号由大变小"改为 **波位号为 0**(7d5a56f),避免机扫转台反转时误判一圈完成(代码中保留了旧判断的注释)。
|
||||
- 波位号 0 是每圈的起始波位,因此**每圈恰好触发一次 model=1**。
|
||||
- 清空(`track_clear_all`)后圈状态复位,下一次调用必然判为"满一圈"。
|
||||
- TAS 数据不参与判圈,**每来一帧就处理一次**(TAS 是调度的跟踪波位,本身就是单目标 CPI)。
|
||||
|
||||
### 2.3 调度:model 与动作的对应
|
||||
|
||||
| model | 触发 | 实际执行的动作(顺序固定) |
|
||||
|---|---|---|
|
||||
| 2 | TWS 未满圈 | 两帧滑窗凝聚;缓存本帧点迹 |
|
||||
| 1 | TWS 满一圈 | ① 关联/滤波/外推 ② 航迹消亡 ③ 航迹起始 |
|
||||
| 3 | TAS 帧 | ① TAS 凝聚 ② TAS 关联/外推 ③ TAS 消亡 |
|
||||
| 0 | 非法类型 | 无 |
|
||||
|
||||
> 顺序很关键:**消散在起始之前**,消亡腾出的资源不会与本圈更新冲突;**起始在最后**,只有"没被任何可靠航迹用掉"的点迹才有资格起批。
|
||||
|
||||
---
|
||||
|
||||
## 3. 点迹凝聚
|
||||
|
||||
### 3.1 目的与判据
|
||||
|
||||
同一目标在相邻波位/相邻 CPI 上会分裂成多个点,需先合并(保留幅度最大者,其余标记删除)。
|
||||
|
||||
| 模式 | 距离门限 | 方位门限 | 速度门限 |
|
||||
|---|---|---|---|
|
||||
| 近程 `work_mode==0` | ≤ 80 m | ≤ 6°(按环绕最小角差) | ≤ 2 m/s |
|
||||
| 中/远程 `work_mode!=0` | ≤ 80 m | ≤ 6° | 不约束 |
|
||||
|
||||
方位差按环绕处理:
|
||||
|
||||
```
|
||||
Δθ = min( |θ0 − θ1| , 2π − |θ0 − θ1| )
|
||||
```
|
||||
|
||||
### 3.2 合并规则(链式比较)
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["两两比较点迹 (0, i)"] --> B{"距离/方位/速度<br/>均在门限内?"}
|
||||
B -- 否 --> Z["跳过"]
|
||||
B -- 是 --> C{"幅度:点0 < 点i ?"}
|
||||
C -- 是 --> D["点0 标记删除<br/>并把点i 的 R/方位/V/幅度<br/>接管为当前基准值"]
|
||||
C -- 否 --> E["点i 标记删除"]
|
||||
D --> F["后续比较以幅度更大者为基准(链式凝聚)"]
|
||||
E --> F
|
||||
F --> G["统一删除:被标记点 + 量程外点<br/>r < 100 m 或 r > 100000 m"]
|
||||
```
|
||||
|
||||
### 3.3 TWS 两帧滑窗(关键机制)
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["上一帧缓存点(区号=1)"] --> C["合并成同一张表"]
|
||||
B["本帧新点(区号=2)"] --> C
|
||||
C --> D["整表凝聚 + 量程过滤"]
|
||||
D --> E{"点的区号"}
|
||||
E -->|"1(较早的点)"| F["输出到 point_recv<br/>→ 本圈参与关联/起始"]
|
||||
E -->|"2(本帧的点)"| G["留在缓存中<br/>→ 与下一帧合并后再输出"]
|
||||
F --> H["缓存清空,本帧剩余点成为新缓存"]
|
||||
```
|
||||
|
||||
**为什么这样设计**
|
||||
|
||||
- 相邻两个 CPI 的同一目标点会落在不同帧里,单帧凝聚无法合并 → 延迟一帧输出,换取跨 CPI 的分裂点合并;
|
||||
- 代价是关联/起始用的是"上一帧"的点,时间上滞后一圈,但由于后续所有 ΔT 都按真实 CPI 时间戳计算,滤波本身不受影响;
|
||||
- 因此 TWS 通道的正常节奏是:**未满圈 model=2 只做凝聚 + 缓存;满圈 model=1 用上一帧凝聚结果做关联与起始**。
|
||||
|
||||
> TAS 凝聚为**单帧**不缓存,门限恒为「距离 ≤ 80 m、方位 ≤ 6°、速度 ≤ 2 m/s」,之后同样做量程过滤。
|
||||
|
||||
---
|
||||
|
||||
## 4. TWS 点航关联与滤波(model=1 的核心)
|
||||
|
||||
### 4.1 处理骨架
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["全部可靠航迹"] --> A1["IMM 交互(mix)"]
|
||||
A1 --> B["滤波前先统一置 point_flag=0(本圈默认未关联实点)"]
|
||||
B --> C["逐航迹 × 逐点:算 ΔT、三个模型马氏距离 d1/d2/d3"]
|
||||
C --> D{"波门判定<br/>距离分段 + 距离门限 d_r + 俯仰门限 d_p"}
|
||||
D -- 通过 --> E["记入候选表(含 d_min = min(d1,d2,d3))"]
|
||||
D -- 不通过 --> F["丢弃该候选对"]
|
||||
E --> G["最近邻:反复取全表 d_min 最小者"]
|
||||
G --> H["该航迹本圈候选全部移除<br/>该点迹候选全部移除<br/>(一对一)"]
|
||||
H --> I{"ΔT > 0 ?"}
|
||||
I -- 是 --> J["三模型 EKF 更新 → 模型概率更新 → 融合输出<br/>更新航迹时间/点信息/高度平滑<br/>Extrapolate_round=0, point_flag=1"]
|
||||
I -- 否 --> K["不更新状态、不刷新航迹时间<br/>但把该点迹标记为已使用(消费掉)"]
|
||||
J --> L{"还有候选?"}
|
||||
K --> L
|
||||
L -- 有 --> G
|
||||
L -- 无 --> M["未关联航迹(point_flag==0 且非手动TAS)按数据率外推"]
|
||||
M --> N["输出全部航迹(更新行 + 补点行)"]
|
||||
```
|
||||
|
||||
### 4.2 波门(三步都通过才算候选)
|
||||
|
||||
**(1)统计距离波门**——按航迹距离分段收紧高机动模型:
|
||||
|
||||
| 航迹距离 r_track | 模型 1 / 2 | 模型 3 |
|
||||
|---|---|---|
|
||||
| ≤ 500 m | d² < 9 | d² < 9/1000(≈ 关闭,抑制近程高机动模型的乱关联) |
|
||||
| 500 ~ 1000 m | d² < 9 | d² < 9/100(收紧) |
|
||||
| ≥ 1000 m | d² < 9 | d² < 9 |
|
||||
|
||||
> ⚠ **门限语义**:代码里 d 已经是马氏距离平方,判据又写作 `d*d < 9`,等效于 **d < 3**(约对应 3 自由度卡方分布 95% 分位的量级)。调整 ASSO_THORD 时务必记住这一层"二次方"。
|
||||
|
||||
**(2)距离门限 d_r**——抑制远距离突跳。用航迹-点迹弦距 R 与航迹位移 VT = v_track·ΔT 判断:
|
||||
|
||||
```
|
||||
VT < 100 且 R > 300 → 拒绝(慢速目标不允许 300 m 以上的跳变)
|
||||
100 ≤ VT ≤ 300 且 R/VT > 3 → 拒绝(允许的跳变与速度位移成比例)
|
||||
VT > 300 且 R > 1200 → 拒绝(高速目标给固定 1200 m 上限)
|
||||
```
|
||||
|
||||
**(3)俯仰门限 d_p**:分距离段 7°/5°/10° 的逻辑当前被注释停用,**恒为通过**。
|
||||
|
||||
### 4.3 最近邻(NN)选取规则
|
||||
|
||||
- 候选对按 `d_min = min(d1, d2, d3)` 排序,**每轮取全表最小值**;
|
||||
- 选中后,候选表中"该航迹的全部候选"和"该点迹的全部候选"一并删除 → **每条航迹每圈最多关联一个点,每个点最多被一条航迹使用**;
|
||||
- 直到候选表为空或遍历完航迹数。
|
||||
|
||||
### 4.4 ΔT ≤ 0 的特殊处理(当前行为)
|
||||
|
||||
ΔT = (点迹 CPI 时间 − 航迹 T_track)/1000 ≤ 0,典型场景是"航迹刚被 TAS 点迹更新过,本圈 TWS 点迹时间更早"或"重复时间戳"。
|
||||
|
||||
| 环节 | 行为 |
|
||||
|---|---|
|
||||
| 是否参与最近邻比较 | **参与**(仍可能与最小 d 一起被选中) |
|
||||
| IMM/EKF 更新 | **不执行**,X1/X2/X3、P1/P2/P3、模型概率均保持原值 |
|
||||
| 航迹时间/点信息 | **不刷新**(T_track、GNSS_time、关联点信息、幅度/SNR/RCS 都不变) |
|
||||
| 新鲜度标志 | `point_flag` 保持 0、`Extrapolate_round` 不归零、`associate_point_number` 不递增 |
|
||||
| 本圈后续 | 该航迹本圈不再关联其它点迹;由于 point_flag 仍为 0,**在随后的外推环节被推进一帧** |
|
||||
| 点迹归属 | 该点迹被标记"已使用"消费掉 → **不会再进入航迹起始**,避免生成虚假 TWS 航迹 |
|
||||
|
||||
### 4.5 关联成功后的更新内容
|
||||
|
||||
1. 三个模型的 EKF 状态/协方差更新;
|
||||
2. 模型概率按新息协方差似然更新(见第 5 节);
|
||||
3. 航迹时间 T_track / GNSS 时间跟进到该点迹;
|
||||
4. 关联点信息(距离/方位/俯仰/径向速度/PRI)、幅度、SNR、RCS、俯仰波位、十字星数据刷新;
|
||||
5. **高度维平滑**(见 4.6);
|
||||
6. `Extrapolate_round = 0`、`point_flag = 1`、`associate_point_number += 1`。
|
||||
|
||||
### 4.6 高度平滑
|
||||
|
||||
关联实点后把点迹高度压入滑窗,输出高度取最近 w 个点的均值(不足 w 个则全平均):
|
||||
|
||||
| 点迹距离 | 窗口长度 w |
|
||||
|---|---|
|
||||
| ≤ 1000 m | 5 |
|
||||
| 1000 ~ 2000 m | 6 |
|
||||
| 2000 ~ 4000 m | 7 |
|
||||
| > 4000 m | 9 |
|
||||
|
||||
> 距离越远俯仰测量跳变越大,窗口随之放长;仅 TWS/TAS 关联到实点时才刷新,外推补点不改变高度。
|
||||
|
||||
### 4.7 未关联航迹的外推
|
||||
|
||||
对 `point_flag == 0` 且 `manual_tracking_flag == 0` 的航迹(含 ΔT≤0 被消费的情形):
|
||||
|
||||
- 推算步长取运行时数据率:`work_mode` 0/1/2 → `DATA_RATE_SHORT / MIDDLE / FAR`(秒);
|
||||
- 用各子模型的 Singer F、Q 推进一帧(**不做量测更新**);
|
||||
- `T_track`、`GNSS_time` 各推进 ΔT×1000 毫秒;
|
||||
- `Extrapolate_round += 1`,`point_flag = 0`(输出为补点/虚点)。
|
||||
|
||||
---
|
||||
|
||||
## 5. IMM + EKF 关键逻辑
|
||||
|
||||
### 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 状态机
|
||||
|
||||
```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["临时航迹消亡(不对外输出)"]
|
||||
```
|
||||
|
||||
### 6.2 关联判据汇总
|
||||
|
||||
**阶段一:点 ↔ 航迹头(两点起始)**
|
||||
|
||||
用航迹头位置与点迹的平面距离 `dis` 与 ΔT(秒):
|
||||
|
||||
```
|
||||
dis ≤ V_MAX · ΔT 平均速度不超上限
|
||||
dis ≥ V_MIN · ΔT 平均速度不低于下限(滤除静止/慢速杂波)
|
||||
v_r,point · v_r,head > 0 两点多普勒同号(同接近/同远离)
|
||||
dis ≥ |v_r,point| · ΔT 几何位移不小于"点迹自身多普勒"位移
|
||||
dis ≥ |v_r,head| · ΔT 同上,用航迹头的多普勒约束
|
||||
```
|
||||
|
||||
- 后两条是**多普勒-几何一致性**约束,用于剔除"切向高速短航迹"(7d5a56f 新增):切向飞行时真实几何位移大而多普勒径向速度小,纯速度区间门限会放进虚假航迹;
|
||||
- 判据隐含要求 ΔT > 0(ΔT ≤ 0 时 `dis ≤ V_MAX·ΔT` 必然不成立);
|
||||
- 近程/中远程分支当前取值相同(都是 V_MAX),**实际不做距离分段**。
|
||||
|
||||
**阶段二:点 ↔ ≥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 ≤ 8。
|
||||
|
||||
### 6.3 临时航迹的"圈数"生命周期
|
||||
|
||||
`buff_round` 只在起始流程里自增,并且**每次调用(= 每圈)先把所有临时航迹所有点的 buff_round +1,再做关联**:
|
||||
|
||||
| 动作 | 效果 |
|
||||
|---|---|
|
||||
| 新点加入 | buff_round = 1 |
|
||||
| 关联资格 | 只看**末点**,要求 buff_round ≥ 2 → **上一圈加入的点本圈才能被延续**,同一航迹每圈最多吸收一个点 |
|
||||
| 消亡条件 | 末点 buff_round > 2(连续 2 圈无新点)或长度 ≥ 10 |
|
||||
|
||||
**时间轴示例(以第 k 圈加入新点为起点)**
|
||||
|
||||
```
|
||||
第 k 圈:新点加入 → 末点 buff_round=1(本圈不再关联)
|
||||
第 k+1 圈:+1 → 2 → 允许关联;成功则新点 1,失败则 2(不 >2,存活)
|
||||
第 k+2 圈:+1 → 3 → 仍允许关联;成功则新点 1,失败则 3>2 → 消亡
|
||||
```
|
||||
|
||||
即:**允许中间空 1 圈**,最大关联间隔 2 圈;生命周期完全由"圈数"驱动,与绝对时间无关。
|
||||
|
||||
### 6.4 航迹分裂与去重
|
||||
|
||||
- 关联阶段**不做一对一分配**:一个点满足多条临时航迹条件时,每条都各自复制出一条新航迹;多条点命中同一条航迹时也各自复制 → 同一圈会产生大量同源临时航迹;
|
||||
- 起批前做**同源去重**:比较两条候选航迹的**前三个点**,只要有任一点的"时间 T + 距离 r"完全相同即判为同源,保留末点统计距离 d 更小者;
|
||||
- 这样既保证不漏关联(允许分裂试探),又不会让同一批点重复起批。
|
||||
|
||||
### 6.5 起批与输出
|
||||
|
||||
| 条件 | 说明 |
|
||||
|---|---|
|
||||
| 长度 | `track_start_point_num ≤ L ≤ 10`(接口层校验 3~9,典型 3 或 4) |
|
||||
| 位置 | 末点不落在**起批屏蔽区**(距离-方位矩形,最多 30 个,用严格不等号判断) |
|
||||
| 容量 | 可靠航迹数 < 500 且能分配到一个未占用航迹号 |
|
||||
| 初始化 | 用**最后三个点**做三点卡尔曼初始化 → 6 维状态;三模型同状态、u=1/3 |
|
||||
| 输出 | 该航迹的**前 N 个点**(N = 起批点数)作为历史点一次性输出,每条航迹最多 10 个历史点 |
|
||||
|
||||
> 达到长度即当圈起批,没有"连续 M 圈确认"的环节;若被屏蔽区拦住,航迹会继续在临时航迹表里增长,直到离开屏蔽区(此时长度已大于起批点数)或长度达到 10 被判消亡。
|
||||
|
||||
---
|
||||
|
||||
## 7. 外推、消亡与航迹号管理
|
||||
|
||||
### 7.1 消亡条件
|
||||
|
||||
| 通道 | 条件(满足其一即消亡) |
|
||||
|---|---|
|
||||
| TWS 航迹 | `Track_Mode==0` 且 `Extrapolate_round ≥ 4` |
|
||||
| TAS 航迹 | `Track_Mode==1` 且 `Extrapolate_round ≥ TRACK_DIE_ROUND_TAS`(相扫 5 / 机扫 96) |
|
||||
| 手动删除 | `manual_delete_flag == 1`(两种通道通用,由外部删除接口置位) |
|
||||
|
||||
消亡时把批号写入消亡输出数组并从航迹表删除;**临时航迹消亡不对外输出**(起批失败的中间态对宿主不可见)。
|
||||
|
||||
### 7.2 航迹号分配
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["需要起批"] --> B{"航迹表为空?"}
|
||||
B -- 是 --> C["分配 1"]
|
||||
B -- 否 --> D["用航迹表建立 1~500 占用位图"]
|
||||
D --> E["从 lastest_index+1 向上找第一个空闲号(含 500)"]
|
||||
E --> F{"找到?"}
|
||||
F -- 是 --> G["分配并更新 lastest_index"]
|
||||
F -- 否 --> H["从 1 回绕扫一遍找空闲号"]
|
||||
H --> I{"找到?"}
|
||||
I -- 是 --> G
|
||||
I -- 否 --> J["返回 0:全满,本候选不起批"]
|
||||
```
|
||||
|
||||
特点:**优先顺序递增分配**(便于人工按批号识别新老),只有走到表尾才回绕复用早期空号。
|
||||
|
||||
---
|
||||
|
||||
## 8. TAS 通道(model=3)
|
||||
|
||||
### 8.1 与 TWS 通道的差异
|
||||
|
||||
| 项 | TWS | TAS |
|
||||
|---|---|---|
|
||||
| 凝聚 | 相邻两帧滑窗,近程多速度维约束 | 单帧,恒有速度维约束 |
|
||||
| 处理对象 | 全部可靠航迹 × 全部点 | **仅指定批号**的这一条航迹 × 全部点 |
|
||||
| 俯仰/高度门限 | 俯仰门限当前恒通过 | **高度门限生效**(按距离分段) |
|
||||
| ΔT ≤ 0 | 参与选取但只消费点迹、不更新 | **直接跳过该点**(并记告警日志) |
|
||||
| 外推时间 | 按 work_mode 数据率固定步长 | 按 `最新时间戳 − 航迹时间` |
|
||||
| 输出 | 全部航迹 | 仅该批号航迹 |
|
||||
|
||||
### 8.2 TAS 关联与更新
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["TAS 本帧点迹 + 指定批号航迹"] --> B{"该批号航迹存在?"}
|
||||
B -- 否 --> Z["直接返回(不做任何处理)"]
|
||||
B -- 是 --> C["IMM 交互;航迹 point_flag 置 0"]
|
||||
C --> D["逐点:ΔT ≤ 0 → 记告警并跳过"]
|
||||
D --> E["逐点:三模型统计距离 d1/d2/d3 + 高度门限"]
|
||||
E --> F{"d² < 9(任一模型)且高度门限通过?"}
|
||||
F -- 是 --> G["记入候选表"]
|
||||
F -- 否 --> H["丢弃"]
|
||||
G --> I["取 d_min 最小的一点"]
|
||||
I --> J["三模型 EKF 更新 → 概率更新 → 融合<br/>刷新时间/点信息/高度平滑"]
|
||||
J --> K["输出该批号航迹"]
|
||||
E --> L{"候选表为空?"}
|
||||
L -- 是 --> M["按 ΔT = 最新时间戳 − 航迹时间 外推一帧<br/>(ΔT ≤ 0 则跳过)"]
|
||||
M --> K
|
||||
```
|
||||
|
||||
**高度门限**(|h_航迹 − h_点迹| 上限,按航迹距离分段):
|
||||
|
||||
| 航迹距离 | 门限 |
|
||||
|---|---|
|
||||
| ≤ 1000 m | 150 m |
|
||||
| 1000 ~ 2000 m | 200 m |
|
||||
| 2000 ~ 3000 m | 200 m |
|
||||
| 3000 ~ 5000 m | 300 m |
|
||||
| > 5000 m | 不约束 |
|
||||
|
||||
**TAS 后续处理**:外推不受 work_mode 数据率约束,而是直接推进到"本帧最新时间戳",保证 TAS 航迹时间与波束调度时刻同步。
|
||||
|
||||
---
|
||||
|
||||
## 9. TAS 波束控制(外部接口 Beam_Ctrl)
|
||||
|
||||
### 9.1 队列管理
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["Beam_Ctrl 调用"] --> B["输出航迹计数清零"]
|
||||
B --> C["入队 tas_target_add"]
|
||||
C --> D["出队 tas_target_del"]
|
||||
D --> E["波束输出 tas_beam_output"]
|
||||
E --> F["输出 TrackingBeam"]
|
||||
```
|
||||
|
||||
**入队条件**(只支持手动转 TAS):
|
||||
|
||||
- 该航迹 `manual_tracking_flag == 1`(由外部 `tracking_start` 置位);
|
||||
- 航迹当前 `Track_Mode == 0`(还不是 TAS);
|
||||
- 队列未满(相扫最多 4 个 / 机扫最多 1 个);
|
||||
- 不在 TAS 禁止区(距离-方位矩形)。
|
||||
|
||||
入队动作:写批号、标记非空、`last_track_time = T_track`、**航迹 Track_Mode 置 1**,并**立即输出一次该航迹更新**(通知宿主跟踪状态变化)。
|
||||
|
||||
**出队条件**:队列中的批号在航迹表中已不存在,或不再处于"手动 + TAS"状态 → 清空该队列项。
|
||||
|
||||
> 自动开启/退出 TAS 的逻辑(按距离/速度/高度阈值)已全部注释停用,**只支持手动**。
|
||||
|
||||
### 9.2 波束输出调度(关键)
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["遍历 TAS 队列"] --> B["按批号在航迹表中找到该航迹"]
|
||||
B --> C{"距上次波束输出 > DATA_RATE_TAS 秒<br/>且 最新时间戳 − 航迹时间 > DATA_RATE_TAS 秒?"}
|
||||
C -- 否 --> A
|
||||
C -- 是 --> D["在合格目标中选 CPI 时间最早者"]
|
||||
D --> E["位置外推:x + vx·ΔT, y + vy·ΔT"]
|
||||
E --> F["Range = √(xp²+yp²),Azi = atan2(yp,xp)<br/>Elev = asin(Height / Range)"]
|
||||
F --> G["open_flag=1, type=1, 批号=真实航迹号<br/>更新该目标 last_track_time = 最新时间戳"]
|
||||
C -.->|"无任何合格目标"| H["open_flag = 0(其余字段保持旧值)"]
|
||||
```
|
||||
|
||||
**要点**
|
||||
|
||||
- 门控用**两个条件**:距上次输出的时间、航迹时间与最新时间戳之差都要大于 `DATA_RATE_TAS` 秒(单位是秒,由宿主经参数初始化传入,须为 > 0 的有效值)→ 保证 TAS 目标的数据率**不随 Beam_Ctrl 的调用频率变化**;
|
||||
- 多目标时选**队列中 CPI 时间最早**的目标,等价于"按最久未更新者优先"的轮转;
|
||||
- 原来的"每次调用整队循环移位"已停用;
|
||||
- 俯仰用航迹高度直接算(叠加雷达平台高度的处理只影响对外输出的 z,不参与波束俯仰)。
|
||||
|
||||
---
|
||||
|
||||
## 10. 参数脉络(决定行为的旋钮)
|
||||
|
||||
### 10.1 编译期宏(随扫描体制切换)
|
||||
|
||||
| 宏 | 相扫 | 机扫 | 作用点 |
|
||||
|---|---|---|---|
|
||||
| `SIGMA_R / A / E / V` | 10 / 0.02 / 0.2 / 2.0 | 同左 | 量测噪声、EKF 的 R、转换量测协方差 |
|
||||
| `DOT_COH_RANGE / V / AZI` | 80 / 2 / 6 | 同左 | 凝聚门限(距离 m / 速度 m/s / 方位 °) |
|
||||
| `R_MIN / R_MAX` | 100 / 100000 | 同左 | 有效量程(凝聚后过滤) |
|
||||
| `TRACK_START_THRESHOLD` | 8 | 同左 | 起始统计距离波门(d ≤ 8) |
|
||||
| `ALPHA_START` | 60 | 同左 | 起始折角上限(°) |
|
||||
| `ASSO_THORD` | 3 | 同左 | 关联波门(d² < 9,等效 d < 3) |
|
||||
| `TRACK_DIE_ROUND` | 4 | 4 | TWS 消亡外推轮数 |
|
||||
| `TRACK_DIE_ROUND_TAS` | 5 | 96 | TAS 消亡外推轮数 |
|
||||
| `MAX_TRACK_NUM / MAX_TRACK_INDEX` | 500 / 500 | 同左 | 航迹数上限 / 航迹号上限 |
|
||||
| `TAS_QUEUE_LENGTH / MAX_TAS_NUM` | 4 / 4 | 1 / 1 | TAS 队列长度 / 最大 TAS 目标数 |
|
||||
| `H_F_WIN_LEN` | 3 | 同左 | 高度平滑窗长基数(实际 +2/+3/+4/+6) |
|
||||
| `FREQ0` | 9.2 GHz | 同左 | 盲速计算的基准频率 |
|
||||
| `RDP_LOG_ENABLE / RDP_LOG_DIR` | — | — | 日志落盘开关与目录 |
|
||||
|
||||
### 10.2 运行时参数(宿主传入,随任务变化)
|
||||
|
||||
| 参数 | 影响环节 |
|
||||
|---|---|
|
||||
| `work_mode`(0 近程/1 中程/2 远程) | 凝聚是否带速度维约束;TWS 外推步长;关联距离门限分支 |
|
||||
| `track_start_point_num`(校验 3~9) | 起批所需点数 |
|
||||
| `V_MAX / V_MIN` | 航迹头-点关联的速度区间 |
|
||||
| `DATA_RATE_SHORT / MIDDLE / FAR` | TWS 未关联航迹外推步长(秒) |
|
||||
| `DATA_RATE_TAS`(秒,须 > 0) | TAS 波束输出间隔门控 |
|
||||
| `Height` | 输出 z = 航迹高度 + 平台高度 |
|
||||
| `Sys_delay` | 输出位置做 x + vx·Sys_delay 的系统延迟补偿 |
|
||||
| `Model1/2/3_Q_slow/fast` | 三个 Singer 模型的过程噪声强度(分档见 5.2) |
|
||||
| `track_prohibite_area_*`(≤30 组) | 起批屏蔽区(距离 m + 方位 °) |
|
||||
| `TAS_prohibite_area_*`(≤30 组) | TAS 禁止区 |
|
||||
|
||||
> `track_start_threshold / track_asso_threshold / track_asso_threshold_tas` 三个字段**当前未被实现读取**,实际门限来自编译期宏。
|
||||
|
||||
---
|
||||
|
||||
## 11. 关键约定与注意事项
|
||||
|
||||
**时序与时间戳**
|
||||
|
||||
- 时间戳为 64 位毫秒;`T_track` 输出为秒(`/1000`);所有 ΔT 用秒参与运算;
|
||||
- TWS 关联中 ΔT ≤ 0 的候选"只消费点迹、不更新航迹"(点迹不会再去起批);
|
||||
- TAS 关联中 ΔT ≤ 0 直接跳过该点;TAS 外推 ΔT ≤ 0 跳过本轮;
|
||||
- 两点/三点卡尔曼初始化对 T 无除零保护 → **起批路径的时间戳必须严格递增**。
|
||||
|
||||
**量纲与坐标**
|
||||
|
||||
- 外部接口角度为度,内部统一弧度,输出再度;
|
||||
- 平面坐标为水平投影,z = 目标高度 + 雷达平台高度;
|
||||
- 点高度由 `R·sin(俯仰)` 直接折算,不参与滤波状态(由高度平滑单独维护)。
|
||||
|
||||
**门限语义**
|
||||
|
||||
- 关联与起始的波门写作 `d² < 门限²`,而 d 已是马氏距离平方 → 实际等效 **d < 门限**;
|
||||
- 起始的"距离差 ≥ 5 m"为硬编码,对切向远距离慢速目标可能造成漏关联;
|
||||
- 起批去重只比较**前三个点**,若起批点数 > 3,共享前三点后分叉的两条航迹会被判为同源。
|
||||
|
||||
**容量与边界**
|
||||
|
||||
- 点迹数上限 150(前处理已限幅);航迹数/航迹号上限 500,全满时新目标不再起批;
|
||||
- 屏蔽区/禁止区的组数按 30 组数组定义,配置数量未做上限校验;
|
||||
- 高度平滑缓存只增不减,长时运行下每条航迹的内存占用会持续增长。
|
||||
|
||||
**未启用**
|
||||
|
||||
- 引导跟踪(direct tracking)、手动打跟踪波束接口为空实现;
|
||||
- TAS 自动开启/退出逻辑停用,只支持手动转/停 TAS;
|
||||
- `tracking_stop` 后,队列项要等到下一次 `Beam_Ctrl` 才真正移除。
|
||||
|
||||
---
|
||||
|
||||
## 附录:与现有 `README.md` 的差异提示
|
||||
|
||||
> 说明:下表是本文档成文时(README 尚未同步 7d5a56f 改动)发现的差异,保留以说明改动缘由;随后已按下表结论更新 README 第 2、7.2、8.1、8.3、9.3、9.6、10.1 等节,「README 描述」列对应更新前的旧表述,现以本文档与 README 的描述为准。
|
||||
|
||||
| 项 | README 描述(更新前) | 当前源码 |
|
||||
|---|---|---|
|
||||
| 满圈判据 | 当前方位波位号 < 上一帧波位号 | **方位波位号 == 0**(7d5a56f,适应机扫转台反转) |
|
||||
| 点-航迹头判据 | 按距离分段收紧速度门限(半速、1/2~1/3 速) | 该分支注释停用;改为统一速度区间 **+ 两条多普勒-几何一致性约束**(7d5a56f) |
|
||||
| 起始输出历史点 | "临时航迹的全部 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` 中该宏当前为**注释状态**(不落盘,日志进控制台) |
|
||||
|
||||
> 后续:README 与 `track_init_readme.md`(航迹起始专项说明)已按上表结论同步(见 CHANGELOG 的 Unreleased 记录);流程落地实现仍以本文为准。
|
||||
@@ -0,0 +1,752 @@
|
||||
# 航迹起始(Track Init)详细逻辑说明
|
||||
|
||||
> 适用范围:`data_process_class_dll/track_init.cpp` / `track_init.h`(类 `Track_Init`),TWS 通道逻辑法航迹起始。
|
||||
> 关联文件:`kalman.cpp`(滤波与统计距离)、`coor_trans.cpp`(坐标转换)、`track_index_mangement.cpp`(航迹号分配)、`data_process.cpp`(调用与参数)。
|
||||
> 本文按**当前源码实际行为**梳理,行号均指当前版本文件行号;与 `README.md` 第 8.3 / 9.3 节存在差异之处,见第 12 节。
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [模块定位与调用时机](#1-模块定位与调用时机)
|
||||
2. [数据结构与状态容器](#2-数据结构与状态容器)
|
||||
3. [处理流程总览](#3-处理流程总览)
|
||||
4. [阶段一:输入装填与圈数计数](#4-阶段一输入装填与圈数计数)
|
||||
5. [阶段二:点迹与临时航迹关联](#5-阶段二点迹与临时航迹关联)
|
||||
6. [阶段三:点迹与航迹头关联](#6-阶段三点迹与航迹头关联)
|
||||
7. [阶段四:点迹消费与航迹头生成](#7-阶段四点迹消费与航迹头生成)
|
||||
8. [阶段五:临时航迹转可靠航迹(起批)](#8-阶段五临时航迹转可靠航迹起批)
|
||||
9. [阶段六:临时航迹消亡](#9-阶段六临时航迹消亡)
|
||||
10. [起始条件汇总表](#10-起始条件汇总表)
|
||||
11. [关键算法细节](#11-关键算法细节)
|
||||
12. [与其他文档/实现的差异](#12-与其他文档实现的差异)
|
||||
13. [参数与调参](#13-参数与调参)
|
||||
14. [输出字段映射](#14-输出字段映射)
|
||||
15. [已知问题与风险点](#15-已知问题与风险点)
|
||||
16. [代码索引](#16-代码索引)
|
||||
|
||||
---
|
||||
|
||||
## 1. 模块定位与调用时机
|
||||
|
||||
### 1.1 在整体流程中的位置
|
||||
|
||||
航迹起始处于 TWS 处理链的**末端**:只有「当圈未被任何可靠航迹关联上」的剩余点迹才会进入起始流程。
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
A["data_preprocess<br/>点迹入 Data_buffer"] --> B["dot_coh(model=2)<br/>两帧滑窗凝聚"]
|
||||
B --> C["track_asso(model=1)<br/>IMM + EKF 点航关联<br/>已用点从 point_recv 删除"]
|
||||
C --> D["track_die<br/>可靠航迹消亡"]
|
||||
D --> E["<b>track_init</b><br/>逻辑法航迹起始"]
|
||||
E --> F["Trust_Track_Output<br/>(起批航迹 + 历史点)"]
|
||||
```
|
||||
|
||||
调用点(`data_process.cpp`):
|
||||
|
||||
```cpp
|
||||
// L130-161,仅 model==1(TWS 满一圈)时执行
|
||||
track_asso.track_asso_process(&point_recv, &trust_track, ...); // 入口删除 Use_Flag==1 的点
|
||||
track_die.track_die_process(&trust_track, ...);
|
||||
track_init.track_init_process_logic(&point_recv, &trust_track, &temp_track,
|
||||
Trust_Track_Output, Trust_track_num_Output, Work_Parameter);
|
||||
```
|
||||
|
||||
### 1.2 调用频率 = 扫描圈
|
||||
|
||||
`data_preprocess` 在 `Data_Input[0].Beam_index_aiz == 0` 时返回 1(`data_process.cpp` L51),宿主据此调用 `track_process(model=1)`。
|
||||
因此 **`track_init_process_logic()` 每次调用对应 TWS 一圈**,`Temp_track::buff_round`(已缓存圈数)正是以「调用次数」为单位累加的。
|
||||
|
||||
### 1.3 类接口
|
||||
|
||||
| 成员 | 可见性 | 作用 |
|
||||
|---|---|---|
|
||||
| `track_init_process_logic(...)` | public | 起始主流程(唯一入口) |
|
||||
| `reset()` | public | 清空内部点迹缓存 + 复位航迹号分配器 |
|
||||
| `point_temp_track_asso(...)` | private | 点迹 ↔ ≥2 点临时航迹关联 |
|
||||
| `point_track_head_asso(...)` | private | 点迹 ↔ 1 点航迹头关联 |
|
||||
| `tmp_track_to_trust_track(...)` | private | 收集 → 去重 → 起批 → 输出 |
|
||||
| `tmp_track_die(...)` | private | 临时航迹消亡 |
|
||||
| `alpha_cal_track_init(...)` | private | 三点折角(度) |
|
||||
| `track_init_prohibit(...)` | private | 起批屏蔽区判断 |
|
||||
| `track_index_mangement` | private | 航迹号(1~500)分配器 |
|
||||
|
||||
> `temp_track`(`std::vector<std::vector<Temp_track>>`)**不是** `Track_Init` 的成员,而是 `Data_Process::temp_track`,由调用方传入并跨圈保持;`track_clear_all()` 中清空(`data_process.cpp` L233),`Track_Init::reset()` 只清自己的 `point_process`。
|
||||
|
||||
---
|
||||
|
||||
## 2. 数据结构与状态容器
|
||||
|
||||
### 2.1 Temp_track(临时航迹点,`struct.h` L94-121)
|
||||
|
||||
| 字段 | 含义 | 起始流程中的作用 |
|
||||
|---|---|---|
|
||||
| `X[4]` | 4 维状态 **[x, vx, y, vy]** | EKF 一步预测的状态;由两点卡尔曼初始化填充 |
|
||||
| `P[4][4]` | 状态协方差 | 同上的协方差 |
|
||||
| `r / azi / vr / height / snr / RCS / Amp` | 该点的极坐标测量 | 初始化、输出、条件判据(`azi` 为**弧度**) |
|
||||
| `T` / `GNSS_time` | CPI 时间 / GNSS 时间(ms) | ΔT 计算(除 1000 转秒) |
|
||||
| `d` | 该点关联时的马氏统计距离 | 去重阶段用于「保留更优者」 |
|
||||
| `buff_round` | **已缓存圈数** | 生命周期与关联时机控制(见 4.2) |
|
||||
| `asso_flag` | 0 未关联 / 1 已关联(待删除)/ 2 去重淘汰 | 临时状态标记 |
|
||||
| `pitch_num`、`speed_dim[129]`、`range_dim[8]` | 俯仰波位、十字星数据 | 纯透传 |
|
||||
| `Temp_track_section_idx` | 临时航迹区号 | 相关赋值已被注释停用,当前不参与逻辑 |
|
||||
|
||||
> ⚠ **下标陷阱**:`Temp_track::X` 是 4 维 `[x, vx, y, vy]`(y 在 index 2),而 `Trust_Track::X` 是 6 维 `[x, vx, ax, y, vy, ay]`(y 在 index 3)。两套结构混用下标必错。
|
||||
|
||||
### 2.2 容器语义
|
||||
|
||||
| 容器 | 语义 |
|
||||
|---|---|
|
||||
| `temp_track`(外层 vector) | 「临时航迹表」:外层每一项 = 一条临时航迹,**下标是不稳定的位置号**(擦除后会重排);内层 vector = 该航迹按时间正序的点序列 |
|
||||
| `point_process`(`Track_Init` 成员) | 本次调用待处理的点迹工作副本 |
|
||||
| `point_recv`(`Data_Process` 成员) | TWS 点迹文件:凝聚输出 → 被 `track_asso` 消费 → 剩余点进入 `track_init` → 被清空 |
|
||||
|
||||
### 2.3 输入与输出
|
||||
|
||||
| 参数 | 方向 | 说明 |
|
||||
|---|---|---|
|
||||
| `point_recv` | 输入/输出 | 输入:当圈未被可靠航迹关联的点迹;**输出:被清空**(`track_init.cpp` L118) |
|
||||
| `trust_track` | 输入/输出 | 可靠航迹表;起批成功的临时航迹 `push_back` 到此 |
|
||||
| `temp_track` | 输入/输出 | 临时航迹表(跨圈状态) |
|
||||
| `Trust_Track_Output[MAX_TRACK_NUM][10]` | 输出 | 与 `track_asso` **共用同一数组与同一计数**;起始流程在 `track_asso` 已写入的行之后追加新航迹的历史点 |
|
||||
| `Trust_track_num_Output` | 输出 | 累计行数(track_asso 输出 N 行后,track_init 在其后继续追加) |
|
||||
| `Work_Parameter` | 输入 | 运行时雷达参数(`RadarPara`) |
|
||||
|
||||
> **输出行数核算**:`track_asso` 先为当圈全部可靠航迹写 N 行;`track_die` 随后删除 D 条(已写出的行与计数不变);起始流程每次起批前判 `trust_track->size() < MAX_TRACK_INDEX(500)`,故最多再追加 `499 − N + D` 行,**合计最多 499 + D 行**。正常情况下 D 很小,远小于 500;但理论上当 D ≥ 2 且当圈起批数量恰好占满配额(需数百条同时起批)时,行号会超过第一维容量 500,存在越界写入风险(极端场景,见 15.14)。第二维 `[j]`(j < L ≤ 10)始终安全。代价是可靠航迹数达 500 时不再允许新起批。
|
||||
|
||||
---
|
||||
|
||||
## 3. 处理流程总览
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
S["track_init_process_logic 入口"] --> A1["① 取出 point_recv → point_process"]
|
||||
A1 --> A2["② temp_track 全部点的 buff_round += 1"]
|
||||
A2 --> Q{"point_process 空?"}
|
||||
Q -- 是 --> DIE["⑥ tmp_track_die"]
|
||||
Q -- 否 --> B["③ point_temp_track_asso<br/>点 ×(≥2 点临时航迹)"]
|
||||
B --> C["④ point_track_head_asso<br/>点 ×(1 点航迹头)"]
|
||||
C --> E["⑤ 删除 Use_Flag==1 的点<br/>剩余点 → 新建航迹头(buff_round=1)"]
|
||||
E --> F["⑦ tmp_track_to_trust_track<br/>收集 → 去重 → 起批 → 输出"]
|
||||
F --> G["清空 point_process / point_recv"]
|
||||
G --> DIE
|
||||
DIE --> R["return 0"]
|
||||
```
|
||||
|
||||
要点顺序(不可颠倒):
|
||||
|
||||
1. **buff_round 先自增,再做关联** —— 这正是「同一圈内不重复关联同一条临时航迹」的实现方式(见 4.2)。
|
||||
2. **先关联 ≥2 点临时航迹,再关联 1 点航迹头** —— 前者优先,但两者**不互斥**:同一圈可同时被两边关联(点迹可以既延续长航迹,又延续航迹头)。
|
||||
3. **关联结果统一用 `Use_Flag` 标记,最后一次性物理删除** —— 只有 `Use_Flag==0` 的剩余点才会变成新航迹头。
|
||||
4. **转可靠航迹在航迹头生成之后** —— 因此当圈新建的航迹头(1 点)不参与当圈起批。
|
||||
5. **消亡在最后执行** —— 且即使当圈无剩余点迹(跳过 ③~⑦)也照常执行。
|
||||
|
||||
---
|
||||
|
||||
## 4. 阶段一:输入装填与圈数计数
|
||||
|
||||
### 4.1 装填(`track_init.cpp` L36-38)
|
||||
|
||||
```cpp
|
||||
for (size_t i=0;i<(*point_recv).size();i++)
|
||||
point_process.push_back((*point_recv)[i]); // 全量拷贝,不检查 Use_Flag
|
||||
```
|
||||
|
||||
- 上游 `track_asso` 已把关联上的点从 `point_recv` 中删除,所以这里进来的都是「未关联点」;函数内部**不再**用 `Use_Flag` 过滤输入(`Use_Flag` 只用于本次调用内部两阶段关联的标记)。
|
||||
- 点迹的 `Range` 已在凝聚阶段做过 `[R_MIN=100, R_MAX=100000] m` 量程过滤。
|
||||
|
||||
### 4.2 buff_round 自增(L40-47)
|
||||
|
||||
```cpp
|
||||
for (每一条临时航迹 j) (*temp_track)[i][j].buff_round += 1;
|
||||
```
|
||||
|
||||
含义与后果(关键机制):
|
||||
|
||||
| 动作 | 效果 |
|
||||
|---|---|
|
||||
| 新建点(航迹头 / 新关联点) | `buff_round = 1` |
|
||||
| 每经过一次 `track_init` 调用(一圈) | 该航迹内**所有**点的 `buff_round` +1;关联判据只看**末点** |
|
||||
| 关联判据要求 `末点.buff_round >= 2` | 等价于:**上一圈加入的点才能被本圈延续** ⇒ 同一条临时航迹每圈最多吸收一个点(除分裂,见 5.5) |
|
||||
| 消亡判据 `末点.buff_round > 2` | 等价于:**连续 2 圈没有新点**就消亡(允许中间空 1 圈,最大关联间隔 2 圈) |
|
||||
|
||||
> 时间戳只在关联时用 `ΔT = (T_point − T_末点)/1000` 秒表示,**不参与生命周期判断**;生命周期完全由「圈数」驱动。这也意味着临时航迹的存活时间与扫描周期成正比,与航迹的绝对时间无关。
|
||||
|
||||
**生命周期时间轴(以 scan k 为起点)**
|
||||
|
||||
```text
|
||||
scan k : 新点加入 → 末点 buff_round = 1 (本圈不能再关联)
|
||||
scan k+1 : 自增 → 2 → 允许关联;若关联成功 → 新点 1;若失败 → 2 > 2 不成立,存活
|
||||
scan k+2 : 自增 → 3 → 仍然允许关联;若成功 → 新点 1;若失败 → 3 > 2 成立,消亡
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 阶段二:点迹与临时航迹关联
|
||||
|
||||
函数:`point_temp_track_asso()`(L145-304)。双重循环:外层点迹、内层临时航迹,**无一对一竞争约束**。
|
||||
|
||||
### 5.1 参与资格(L164)
|
||||
|
||||
```cpp
|
||||
if( (*temp_track)[j].size() > 1 && (*temp_track)[j][L-1].buff_round >= 2 )
|
||||
```
|
||||
|
||||
- 只处理 **≥2 点** 的临时航迹(1 点的是航迹头,留给阶段三);
|
||||
- 末点必须来自历史圈次。
|
||||
|
||||
### 5.2 关联判据(L167-221)
|
||||
|
||||
设点迹量测(`Z[3]`):
|
||||
|
||||
$$
|
||||
Z=\begin{bmatrix} r_p \\ \theta_p \\ v_{r,p}\end{bmatrix},\qquad
|
||||
\mathbf{X}_{k-1}=\text{末点状态}[x,v_x,y,v_y],\qquad
|
||||
\Delta T=\frac{T_p-T_{\text{末点}}}{1000}\ \text{(s)}
|
||||
$$
|
||||
|
||||
**前置硬门限(L191)**:`|r_p − r_末点| >= 5` —— 距离差小于 5 m 的点直接不参与(连 d 都不算)。
|
||||
|
||||
**统计距离(L194-195)**:调用 `kalman::d_cal_track_init_EKF(Z, X, P, ΔT, prt, freq_ind)` 得到 3 维量测的马氏距离平方 `d`(详见 11.2)。
|
||||
|
||||
**折角(L199-205)**:取末点前一点 → 末点 → 新点,计算折角 `α`(度,详见 11.3)。
|
||||
|
||||
**三条件同时成立(L221)**:
|
||||
|
||||
$$
|
||||
\boxed{\;d^2 \le \text{TRACK\_START\_THRESHOLD}^2=64
|
||||
\quad\wedge\quad
|
||||
\alpha < \text{ALPHA\_START}=60^\circ
|
||||
\quad\wedge\quad
|
||||
v_{r,p}\cdot v_{r,\text{末点}} > 0\;}
|
||||
$$
|
||||
|
||||
> ⚠ 由于 `d_cal_track_init_EKF` 返回的 `d` **本身就是马氏距离平方**,而判据又写作 `d*d <= 64`(即 `d <= 8`),实际生效的门限是「马氏距离平方 ≤ 8」。3 自由度卡方分布 95% 分位为 7.815,因此该写法恰好给出约 95.4% 的置信门限(详见 11.2 与 15.6)。
|
||||
|
||||
### 5.3 记录关联(L223-231)
|
||||
|
||||
命中后向 `asso_info` 记录 `(point_idx=i+1, track_idx=j+1, d)`,同时:
|
||||
|
||||
- `末点.asso_flag = 1` → 标记**原航迹**待删除;
|
||||
- `point_process[i].Use_Flag = 1` → 标记该点已被消费。
|
||||
|
||||
索引在整个扫描循环中保持稳定(循环期间不修改两个容器),因此后面的 1-based 索引回查是安全的。
|
||||
|
||||
### 5.4 生成新临时航迹(L237-281)
|
||||
|
||||
对每一条关联记录:**复制原航迹的全部 L 个点**(并把各点 `asso_flag` 清 0)、再**追加关联上的新点**,作为一条**新的临时航迹** push 到 `temp_track` 末尾。
|
||||
|
||||
新点字段:极坐标测量、幅度、SNR、RCS、时间、俯仰波位、十字星数据全套透传;`asso_flag=0`、`buff_round=1`、`d = asso_info[i].d`;
|
||||
状态与协方差由 **两点卡尔曼初始化** 得到(L268-279):
|
||||
|
||||
```cpp
|
||||
Z0 = (末点.r·cos(末点.azi), 末点.r·sin(末点.azi)); // 原航迹末点
|
||||
Z1 = (点.Range·cos(点.Azimuth), 点.Range·sin(点.Azimuth));
|
||||
T = (点.CPI_Time − 末点.T)/1000.0;
|
||||
Kalman.kalman_filter_init_2dots(Z0, Z1, T, X, P); // 4 维 CV 初始化
|
||||
```
|
||||
|
||||
### 5.5 航迹分裂(重要特性)
|
||||
|
||||
关联循环**不做一对一分配**:
|
||||
|
||||
- 一个点若满足多条临时航迹的判据,会为**每一条**各自复制出一条新航迹 —— 一条母航迹可以派生多条子航迹;
|
||||
- 一条临时航迹若被多个点同时命中,也会**各自复制**成多条子航迹(每个子航迹只带一个点);
|
||||
- 因此同一圈内可能产生大量同源临时航迹,靠第 8.3 节的**去重**收敛。
|
||||
|
||||
### 5.6 删除被关联的原航迹(L283-303)
|
||||
|
||||
遍历 `temp_track`,凡 `size()>=2` 且 **末点 `asso_flag==1`** 者整体 `erase`(每次擦除后 `Iter = begin()` 重扫,O(n²) 但结果正确;1 点航迹不在此处理)。新生成的子航迹末点 `asso_flag==0`,不会被误删。
|
||||
|
||||
---
|
||||
|
||||
## 6. 阶段三:点迹与航迹头关联
|
||||
|
||||
函数:`point_track_head_asso()`(L306-445)。这是**两点起始**:航迹头 + 1 个点 → 2 点临时航迹。
|
||||
|
||||
### 6.1 参与资格(L325)
|
||||
|
||||
```cpp
|
||||
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
|
||||
```
|
||||
|
||||
只处理 **1 点**且该点来自历史圈次的航迹头。
|
||||
|
||||
### 6.2 关联判据(L328-372)
|
||||
|
||||
点迹与航迹头都转直角坐标后取**平面距离**:
|
||||
|
||||
$$
|
||||
x_p = r_p\cos\theta_p,\quad y_p=r_p\sin\theta_p;\qquad
|
||||
x_h=\text{头.X}[0],\quad y_h=\text{头.X}[2]
|
||||
$$
|
||||
|
||||
$$
|
||||
dis=\sqrt{(x_p-x_h)^2+(y_p-y_h)^2},\qquad \Delta T=\frac{T_p-T_h}{1000}\ \text{(s)}
|
||||
$$
|
||||
|
||||
**当前生效的判据(L368-372)**:
|
||||
|
||||
$$
|
||||
\boxed{\;
|
||||
dis \le V_{max}\Delta T
|
||||
\;\wedge\;
|
||||
dis \ge V_{min}\Delta T
|
||||
\;\wedge\;
|
||||
v_{r,p}\cdot v_{r,h} > 0
|
||||
\;\wedge\;
|
||||
dis \ge |v_{r,p}|\,\Delta T
|
||||
\;\wedge\;
|
||||
dis \ge |v_{r,h}|\,\Delta T\;}
|
||||
$$
|
||||
|
||||
各条件的物理含义:
|
||||
|
||||
| 条件 | 含义 |
|
||||
|---|---|
|
||||
| `dis ≤ V_max·ΔT` | 平均速度不超过最大速度(`V_max = Work_Parameter.V_MAX`) |
|
||||
| `dis ≥ V_min·ΔT` | 平均速度不低于最小速度(滤除静止/慢速点) |
|
||||
| `v_r,p · v_r,h > 0` | 两点多普勒径向速度**同号**(同接近/同远离) |
|
||||
| `dis ≥ abs(v_r,p)·ΔT` | 位置变化不小于**点迹自身多普勒**在这段时间内的径向位移(多普勒-几何一致性) |
|
||||
| `dis ≥ abs(v_r,h)·ΔT` | 同上,用航迹头的多普勒约束 |
|
||||
|
||||
**近/远程分支(L348-356)当前是死分支**:`work_mode==0` 与 `else` 都取 `vmax = Work_Parameter.V_MAX`(注释"近程模式最大速度减小一点"未生效;原 `V_MAX/5` 写法仅保留在未启用的 `track_init_direct_tracking.cpp` 注释里)。
|
||||
|
||||
**ΔT 语义**:判据隐含要求 `ΔT > 0`(若 `ΔT < 0`,则 `dis ≤ 负数` 必假)。`ΔT == 0` 时仅在 `dis == 0`(同时间同位置的点)才可能成立,属退化情形。
|
||||
|
||||
**与文档差异**:源码中被注释停用的旧判据按距离分段收紧门限(`r<1000` 时半速、中远程 `r<2000` 时 1/2~1/3 速),见第 12 节。
|
||||
|
||||
### 6.3 命中后的处理(L374-427)
|
||||
|
||||
- 记录 `(point_idx, track_idx)`,标记 `头.asso_flag=1`、`point_process[i].Use_Flag=1`;
|
||||
- 逐条生成 2 点临时航迹:**第 1 点 = 原航迹头**(`asso_flag` 清 0),**第 2 点 = 新点**(全套测量透传,`buff_round=1`,`asso_flag=0`,**`d` 未赋值,保持 0**);
|
||||
- 同样调用 `kalman_filter_init_2dots` 得到 4 维状态/协方差(`Z0` = 头点、`Z1` = 新点,`T = (点.CPI_Time − 头.T)/1000`)。
|
||||
|
||||
### 6.4 删除被关联的航迹头(L429-443)
|
||||
|
||||
凡 `[0].asso_flag==1` 的 1 点航迹头整条 `erase`(同样每次重扫)。
|
||||
注意:本阶段遍历的是**全部** `temp_track`,其中由阶段二新建的 ≥2 点航迹因 `size()!=1` 不会被误处理。
|
||||
|
||||
---
|
||||
|
||||
## 7. 阶段四:点迹消费与航迹头生成
|
||||
|
||||
### 7.1 删除已消费点迹(L57-70)
|
||||
|
||||
```cpp
|
||||
if((*Iter).Use_Flag==1) { point_process.erase(Iter); Iter = point_process.begin(); }
|
||||
```
|
||||
|
||||
物理删除所有被阶段二或阶段三消费的点(两阶段共用同一 `Use_Flag`)。
|
||||
|
||||
> 结果:一个点迹**要么**参与延续已有临时航迹/航迹头(可同时延续两者),**要么**成为新航迹头,二者互斥。这保证不会「用同一个点既延续又起新头」。
|
||||
|
||||
### 7.2 剩余点回写(L72-75,等效空操作)
|
||||
|
||||
```cpp
|
||||
std::vector<PointRecv>().swap((*point_recv));
|
||||
for (...) (*point_recv).push_back(point_process[i]); // 回写未被使用的点
|
||||
```
|
||||
|
||||
紧接着 L117-118 又把 `point_process` 与 `point_recv` 双双清空,因此这段回写**不产生任何对外效果**(`point_recv` 最终必然为空),属历史残留代码。
|
||||
|
||||
### 7.3 剩余点转航迹头(L77-114)
|
||||
|
||||
对每个剩余点构造 1 点 `Temp_track`:
|
||||
|
||||
| 字段 | 取值 |
|
||||
|---|---|
|
||||
| `r / azi / height / Amp / vr / snr / RCS` | 点迹对应量(`azi` 为弧度) |
|
||||
| `pitch_num / speed_dim / range_dim` | 透传 |
|
||||
| `X[0] = Range·cos(Azimuth)`、`X[1]=0`、`X[2] = Range·sin(Azimuth)`、`X[3]=0` | 位置赋值,速度置 0(**注意 4 维布局**) |
|
||||
| `T`、`GNSS_time` | 点迹时间 |
|
||||
| `asso_flag = 0`、`buff_round = 1` | 生命周期起点 |
|
||||
| `d` | 未赋值 → 0 |
|
||||
|
||||
---
|
||||
|
||||
## 8. 阶段五:临时航迹转可靠航迹(起批)
|
||||
|
||||
函数:`tmp_track_to_trust_track()`(L458-671),四步。
|
||||
|
||||
### 8.1 步骤 1:收集候选(L467-499)
|
||||
|
||||
```cpp
|
||||
int L = (*Iter).size();
|
||||
double range = (*Iter)[L-1].r;
|
||||
double azi = (*Iter)[L-1].azi/PI*180; // 转度,供屏蔽区比较
|
||||
|
||||
if( (L <= 10 && L >= Work_Parameter.track_start_point_num) && track_init_prohibit(range, azi, Work_Parameter)==0 )
|
||||
{ 复制到 Track_to_start; 从 temp_track 中 erase; }
|
||||
else
|
||||
Iter++;
|
||||
```
|
||||
|
||||
**入选条件**:
|
||||
|
||||
1. **长度**:`track_start_point_num ≤ L ≤ 10`(`track_start_point_num` 由宿主经 `track_process_parameters_initial` 传入,接口层校验范围 **3~9**,`data_process.cpp` L208);
|
||||
2. **不在起批屏蔽区**内(用**末点**的距离/方位判断,见 8.5)。
|
||||
|
||||
命中即从 `temp_track` 摘出(先复制后删除,`Track_to_start` 成为候选表)。
|
||||
|
||||
> 结果:**一旦长度达到阈值且不落在屏蔽区,当圈即起批**(没有"连续 M 圈满足才起批"的确认期)。若因屏蔽区被拦住,航迹会继续在 `temp_track` 中增长,直到离开屏蔽区(此时 `L > track_start_point_num`,见 8.3 的副作用)或达到 `L=10` 被判消亡。
|
||||
|
||||
### 8.2 步骤 2:同源去重(L504-550)
|
||||
|
||||
```cpp
|
||||
int L = Work_Parameter.track_start_point_num; // 注意:此 L 是"起批点数",不是航迹长度
|
||||
for (i: 0 .. size-2) for (j: i+1 .. size-1) {
|
||||
if (两者都未被标记 asso_flag==2) {
|
||||
if (某条长度 < 3) continue; // 防御性判断(实际不会触发)
|
||||
if (前三点中存在一点 "T 与 r 完全相同") { // 硬编码比较点 0/1/2
|
||||
if (Track_to_start[i][L-1].d < Track_to_start[j][L-1].d) j 标记 asso_flag=2;
|
||||
else i 标记 asso_flag=2;
|
||||
}
|
||||
}
|
||||
}
|
||||
// 然后统一删除 [0].asso_flag==2 的航迹
|
||||
```
|
||||
|
||||
- **同源判定**:两条候选航迹的前三个点中,只要有一个点的 `T` 与 `r` 完全相等(同一点迹被复制而来),就视为同源重复;
|
||||
- **择优准则**:比较 `[L-1].d`(第 `track_start_point_num` 个点的关联统计距离),**保留 d 更小者**;
|
||||
- **标记位置**:只写 `[0].asso_flag = 2`,最后统一擦除;
|
||||
- 去重仅看**前三点**,且无论航迹实际多长都用固定下标 0/1/2。
|
||||
|
||||
### 8.3 步骤 3:起批与构造可靠航迹(L552-667)
|
||||
|
||||
```cpp
|
||||
if (Track_to_start[i].size() < 3) continue; // 防御
|
||||
if (trust_track->size() < MAX_TRACK_INDEX) { // 500 条上限
|
||||
int index = track_index_mangement.track_ind_get(trust_track);
|
||||
if (index > 0) { ... }
|
||||
}
|
||||
```
|
||||
|
||||
**三点卡尔曼初始化**(L563-571,注意使用的是 `L-3, L-2, L-1`,即**最后三个点**):
|
||||
|
||||
```cpp
|
||||
Z0 = 极坐标→直角(点[L-3]); Z1 = (点[L-2]); Z2 = (点[L-1]);
|
||||
T1 = (点[L-2].T − 点[L-3].T)/1000.0; T2 = (点[L-1].T − 点[L-2].T)/1000.0;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1, T2, X, P); // 6 维 [x,vx,ax,y,vy,ay]
|
||||
```
|
||||
|
||||
> ⚠ **不一致点**:初始化用「最后三点」,而输出历史点(8.4)只写「前 `L` 个点」。当航迹长度 = `track_start_point_num` 时两者一致;当因屏蔽区滞留使长度 > 阈值时,输出的历史点里**不包含**用于初始化的那三个点。
|
||||
|
||||
**Trust_Track 字段构造(L573-608)**:
|
||||
|
||||
| 字段 | 赋值 |
|
||||
|---|---|
|
||||
| `X[6] / P[6][6]` | 三点初始化结果 |
|
||||
| `X1/X2/X3 = X`、`P1/P2/P3 = P` | 三个 IMM 子模型同状态、同协方差初始化 |
|
||||
| `u[0..2] = 0.3333` | 模型概率均分 |
|
||||
| `Track_Index` | `track_ind_get()` 分配的航迹号 |
|
||||
| `T_track` / `GNSS_time` / `Height` | 取**末点**的对应值 |
|
||||
| `Amplitude / snr_point / RCS / pitch_num / speed_dim / range_dim` | 末点透传 |
|
||||
| `point_flag = 1` | 实点 |
|
||||
| `Track_Mode = 0` | TWS |
|
||||
| `Target_Type = UNCONF_TARGET(0)` | 未确认目标 |
|
||||
| `Extrapolate_round = 0` | 外推计数清零 |
|
||||
| `approach_flag = -1` | 接近标志复位 |
|
||||
| `manual_delete_flag / manual_tracking_flag / associate_point_number / point_type = 0` | 各类标志复位 |
|
||||
|
||||
> L606-608 有一处**死代码**:`coor_trans::cart2polar(X[0], X[3], &r, &azi)`(把 `X[3]=vy` 当 y 用),计算结果 `r/azi` 从未被使用。
|
||||
|
||||
**航迹号分配**(`track_index_mangement.cpp`):空表返回 1;否则从 `lastest_index+1` 向上找第一个未被占用的号(含 500),找不到再从 1 回绕;全满返回 0(**不起批**,该候选被静默丢弃)。
|
||||
|
||||
### 8.4 输出更新航迹(L612-664)
|
||||
|
||||
- `*Trust_track_num_Output += 1`,在 `track_asso` 已写的行之后追加一行;
|
||||
- 对 `j = 0 .. L-1`(`L = track_start_point_num`)写入该航迹的**历史点**:`Track_Index`、`Point_Sum = L`,以及每个点的距离/方位/俯仰、速度、幅度/SNR/RCS、俯仰波位、十字星数据、航向角、时间、直角坐标、原始点信息等(完整映射见第 14 节);
|
||||
- 之后该航迹的后续更新由 `track_asso`(每次 `Point_Sum=1`)负责。
|
||||
|
||||
### 8.5 起批屏蔽区(L699-712)
|
||||
|
||||
```cpp
|
||||
for (i = 0; i < Work_Parameter.track_prohibite_area_num; i++)
|
||||
if (r < R_max_track_prohibited[i] && r > R_min_track_prohibited[i] &&
|
||||
azi < Azimuth_max_track_prohibited[i] && azi > Azimuth_min_track_prohibited[i])
|
||||
return 1; // 位于屏蔽区,禁止起批
|
||||
return 0;
|
||||
```
|
||||
|
||||
- 判据是**距离-方位矩形区域**(`azi` 已转成度),最多 30 个(`RadarPara` 数组长度;数量未做上限校验);
|
||||
- 采用**严格不等号**:恰好落在边界上的点不算屏蔽;
|
||||
- 判断用**末点**极坐标,不使用高度。
|
||||
|
||||
---
|
||||
|
||||
## 9. 阶段六:临时航迹消亡
|
||||
|
||||
函数:`tmp_track_die()`(L673-697):
|
||||
|
||||
```cpp
|
||||
if (n <= 0) erase; // 空航迹
|
||||
if ((*Iter)[n-1].buff_round > 2 || n >= 10) erase;
|
||||
```
|
||||
|
||||
| 消亡条件 | 含义 |
|
||||
|---|---|
|
||||
| 序列为空 | 防御性清理 |
|
||||
| 末点 `buff_round > 2` | 连续 2 圈没有新点(允许空 1 圈) |
|
||||
| 长度 `n >= 10` | 达到上限仍未完成起批(例如长期滞留屏蔽区) |
|
||||
|
||||
消亡**不输出任何信息**(与可靠航迹消亡不同,起批失败的临时航迹对宿主不可见)。
|
||||
|
||||
---
|
||||
|
||||
## 10. 起始条件汇总表
|
||||
|
||||
### 10.1 条件链(一条 3 点起批航迹的完整判据)
|
||||
|
||||
| 阶段 | 判据(源码位置) |
|
||||
|---|---|
|
||||
| **0. 输入资格** | 该点未被 `track_asso` 关联(已从 `point_recv` 删除);凝聚后 `100 m ≤ r ≤ 100000 m` |
|
||||
| **1. 生成航迹头** | 未被阶段二/三消费(`Use_Flag==0`);创建时 `buff_round=1`(L77-114) |
|
||||
| **2. 航迹头 → 2 点**(L325, L368-372) | 航迹头 `size==1` 且 `buff_round ≥ 2`;`dis ≤ V_MAX·ΔT`;`dis ≥ V_MIN·ΔT`;`dis ≥ abs(vr_p)·ΔT`;`dis ≥ abs(vr_h)·ΔT`;`vr_p·vr_h > 0`(隐含 `ΔT > 0`) |
|
||||
| **3. 2 点 → 3 点及以上**(L164, L191, L221) | 临时航迹 `size > 1` 且末点 `buff_round ≥ 2`;`abs(r_p − r_末点) ≥ 5 m`;`d ≤ 8`(代码写 `d² ≤ 64`);`α < 60°`;`vr_p·vr_末点 > 0` |
|
||||
| **4. 转可靠航迹**(L484, L556) | `track_start_point_num ≤ L ≤ 10`;末点不在起批屏蔽区;同源去重后保留;`trust_track.size() < 500` 且成功分配航迹号 |
|
||||
| **5. 消亡**(L686) | `末点.buff_round > 2`(连续 2 圈无新点)或 `长度 ≥ 10` |
|
||||
|
||||
### 10.2 门限汇总
|
||||
|
||||
| 判据 | 数值 | 来源 | 可调 |
|
||||
|---|---|---|---|
|
||||
| 最大速度 `V_MAX` | 运行时 | `RadarPara.V_MAX` | ✅ |
|
||||
| 最小速度 `V_MIN` | 运行时 | `RadarPara.V_MIN` | ✅ |
|
||||
| 距离差下限 `abs(Δr)` | **5 m(硬编码)** | `track_init.cpp` L191 | ❌ |
|
||||
| 起批波门 `d ≤ 8` | `TRACK_START_THRESHOLD=8` | `parameters.h` L49/L98 | 编译期 |
|
||||
| 起批折角 | `ALPHA_START=60°` | `parameters.h` L50/L99 | 编译期 |
|
||||
| 起批点数 | 3~9(典型 3~4) | `RadarPara.track_start_point_num` | ✅ |
|
||||
| 临时航迹长度上限 | 10(硬编码) | L484 / L686 | ❌ |
|
||||
| 允许丢圈数 | 1 圈(`buff_round > 2` 消亡) | L686 | ❌ |
|
||||
| 航迹头上限等待 | 2 圈 | L686(1 点航迹同判据) | ❌ |
|
||||
| 起批屏蔽区 | ≤30 个矩形 | `RadarPara.track_prohibite_area_*` | ✅ |
|
||||
| 航迹数上限 | 500 | `MAX_TRACK_INDEX` | 编译期 |
|
||||
|
||||
---
|
||||
|
||||
## 11. 关键算法细节
|
||||
|
||||
### 11.1 两点卡尔曼初始化(`kalman.cpp` L18-58)
|
||||
|
||||
输入两个直角坐标量测 `Z0, Z1` 与时间差 `T`(秒):
|
||||
|
||||
$$
|
||||
X=\begin{bmatrix}x_1\\ (x_1-x_0)/T\\ y_1\\ (y_1-y_0)/T\end{bmatrix},
|
||||
\qquad
|
||||
P_{axis}=\begin{bmatrix}R & R/T\\ R/T & 2R/T^2\end{bmatrix}
|
||||
$$
|
||||
|
||||
其中 `R` 取 `Z1` 处的**转换量测协方差**(极坐标→直角坐标,见 `README.md` 9.1 节),x/y 两轴独立填充 `P[4][4]`。
|
||||
|
||||
> 风险:`T → 0` 时速度与协方差同时发散,函数无除零保护(对应 BUG-18);临时航迹的两点初始化 `T` 来自相邻两圈,实际由时间戳决定,若宿主给出重复时间戳会异常。
|
||||
|
||||
### 11.2 EKF 统计距离(`kalman.cpp` L615-693)
|
||||
|
||||
对 4 维状态做 CV 一步预测(`F = [[1,T,0,0],[0,1,0,0],[0,0,1,T],[0,0,0,1]]`,过程噪声 `Q = 0.03²·I`,`G=[T²/2, T]ᵀ`):
|
||||
|
||||
$$
|
||||
\hat X_{k|k-1}=F X,\qquad P_{k|k-1}=FPF^{T}+GQG^{T}
|
||||
$$
|
||||
|
||||
非线性量测函数(距离-方位-径向速度):
|
||||
|
||||
$$
|
||||
h(\hat X)=\begin{bmatrix}\sqrt{x^2+y^2}\\ \operatorname{atan2}(y,x)\bmod 2\pi\\ -\dfrac{x v_x+y v_y}{\sqrt{x^2+y^2}}\end{bmatrix},
|
||||
\qquad H=\left.\frac{\partial h}{\partial X}\right|_{\hat X_{k|k-1}}
|
||||
$$
|
||||
|
||||
量测噪声 `R = diag(σ_r², σ_a², σ_v²) = diag(10², 0.02², 2²)`,新息协方差 `S = H P Hᵀ + R`,残差做两项修正:
|
||||
|
||||
- **方位环绕**:`ν_θ = wrapToPi(θ_p − θ_pred)`,归一化到 `[−π, π)`;
|
||||
- **多普勒盲速折叠**:`ν_v ← ν_v − round(ν_v / v_bind)·v_bind`,其中
|
||||
|
||||
$$
|
||||
v_{bind}=\frac{150000}{f_{GHz}\cdot PRI_{\mu s}}\ \text{(m/s)},
|
||||
\qquad f_{GHz}=9.2+0.02\cdot f_{ind}
|
||||
$$
|
||||
|
||||
(`PRI ≤ 0` 或 `f ≤ 0` 时返回安全值 1.0;调用方约定 PRI 单位 µs,不允许为 0。)
|
||||
|
||||
返回马氏距离平方:
|
||||
|
||||
$$
|
||||
d=\nu^{T}S^{-1}\nu,\qquad \nu=Z-h(\hat X_{k|k-1})
|
||||
$$
|
||||
|
||||
**调用参数来源**:`prt = point.PRF_index`(实际存的是 `DataRev.PRI`,单位 µs);`freq_ind = point.Freq_index`。
|
||||
|
||||
### 11.3 折角计算(L448-456)
|
||||
|
||||
$$
|
||||
\alpha=\arccos\!\left(\frac{\vec D_{12}\cdot\vec D_{23}}{\|\vec D_{12}\|\,\|\vec D_{23}\|}\right)\cdot\frac{180}{\pi}
|
||||
$$
|
||||
|
||||
其中 `D12 = (末点 − 前一点)`、`D23 = (新点 − 末点)`,三点均取 **Temp_track 的直角坐标**(`X[0]` 为 x、`X[2]` 为 y)。`α` 越小表示越接近直线;`ALPHA_START=60°` 表示允许的航向偏折。
|
||||
|
||||
> 无 `acos` 定义域钳位:当两点重合(分母为 0)或浮点误差使比值略超 ±1 时结果为 `NaN`,`NaN < 60` 恒为 false → 表现为「该点无法关联」,不崩溃。
|
||||
|
||||
---
|
||||
|
||||
## 12. 与其他文档/实现的差异
|
||||
|
||||
### 12.1 与 `README.md` 第 8.3 / 9.3 节的差异(README 已同步本节结论)
|
||||
|
||||
> 说明:下表为本文档成文时发现的差异,当时 README 尚未同步 7d5a56f 的改动;随后已按本节结论更新 README 第 2、7.2、8.1、8.3、9.3、9.6、10.1 等节,「README 描述」列对应的是更新前的旧表述,保留以说明改动缘由。
|
||||
|
||||
| 项 | README 描述(更新前) | 当前源码 |
|
||||
|---|---|---|
|
||||
| 点-航迹头判据 | 按距离分段:近程 `r≥1000` 全速 / `r<1000` 半速;中远程 `r≥2000` 全速 / `r<2000` 1/2~1/3 速 | 该分支已**整体注释停用**,改为统一 `V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT` **外加两条多普勒一致性条件** `dis ≥ abs(vr_p)·ΔT`、`dis ≥ abs(vr_h)·ΔT`(L361-372) |
|
||||
| 起批触发长度 | 长度满足 `track_start_point_num ≤ L ≤ 10` | 一致 |
|
||||
| 初始化用点 | 最近三点 | 不一致:取下标 `track_start_point_num-3 ~ -1` 的三个点,航迹长度大于起批点数时**不是**航迹真正的末三点(详见第 15 节第 2 条) |
|
||||
| 输出历史点 | "将临时航迹的全部 L 个点写入" | 实际只写**前 `track_start_point_num` 个点**(L614 的 `L` 是起批点数,不是航迹长度) |
|
||||
| use_flag 过滤 | "track_init 跳过 Use_Flag==1 的点" | 上游 `track_asso_process()` 已删除 `Use_Flag==1` 的点迹,`track_init` 入口(L59-70)也会再剔除一次,两道处理叠加,描述成立 |
|
||||
|
||||
### 12.2 与 `track_init_direct_tracking.cpp`(引导跟踪版,未启用)的差异
|
||||
|
||||
| 项 | `track_init.cpp`(TWS,启用) | `track_init_direct_tracking.cpp`(未启用) |
|
||||
|---|---|---|
|
||||
| 入口 | 完整实现 | `track_init_process_logic` 空实现(L11-23) |
|
||||
| 点-航迹头判据 | 统一门限 + 两条多普勒一致性条件 | 仍为分距离段门限(`V_MAX/5` 亦在其注释中) |
|
||||
| 起批长度 | `L ≥ track_start_point_num`(长度可大于阈值) | `L == track_start_point_num`(严格相等) |
|
||||
| 去重 | 有同源去重 | 无 |
|
||||
| 屏蔽区 | 有 | 无 |
|
||||
| 航迹号 | `track_ind_get()` 分配 | 外部传入 `track_ID` |
|
||||
| 消亡阈值 | `buff_round > 2` | `buff_round > 1`(更严格) |
|
||||
| 输出模式 | `Track_Mode = 0`(TWS) | `Track_Mode = 1`(TAS) |
|
||||
|
||||
---
|
||||
|
||||
## 13. 参数与调参
|
||||
|
||||
### 13.1 编译期宏(`parameters.h`)
|
||||
|
||||
| 宏 | 相扫/机扫 | 含义 | 调大效果 |
|
||||
|---|---|---|---|
|
||||
| `TRACK_START_THRESHOLD` | 8 / 8 | 起批波门,实际生效为 `d ≤ 8` | **更容易起批**(虚警增多) |
|
||||
| `ALPHA_START` | 60 / 60 | 起批折角(度) | 更容易起批(允许更急的转向) |
|
||||
| `SIGMA_R / SIGMA_A / SIGMA_V` | 10 / 0.02 / 2.0 | EKF 量测噪声 | 影响 `S` 与 `d`,进而影响关联难度 |
|
||||
| `MAX_TRACK_INDEX` | 500 | 航迹号/航迹数上限 | — |
|
||||
| `R_MIN / R_MAX` | 100 / 100000 | 有效量程(凝聚阶段过滤) | — |
|
||||
| `PI` | 3.1415926**f** | 角度换算(float 精度) | — |
|
||||
|
||||
### 13.2 运行期参数(`RadarPara`)
|
||||
|
||||
| 字段 | 影响 |
|
||||
|---|---|
|
||||
| `track_start_point_num` | 起批点数(接口校验 3~9):调大 → 起批更慢、更抗虚警,但机动目标捕获更迟 |
|
||||
| `V_MAX` / `V_MIN` | 头-点关联的速度区间;`V_MIN` 调大可滤除静止/慢速点 |
|
||||
| `work_mode` | 当前对起始流程**无实际影响**(近/远程分支取值相同) |
|
||||
| `track_prohibite_area_num` + 4×30 数组 | 起批屏蔽区(距离 m / 方位 度) |
|
||||
| `track_start_threshold` | **声明但未被实现读取**(实际用宏 `TRACK_START_THRESHOLD`) |
|
||||
|
||||
### 13.3 调参直觉
|
||||
|
||||
- **虚警多、起批太容易**:先降 `TRACK_START_THRESHOLD`(8 → 5 左右),再降 `ALPHA_START`;必要时提高 `track_start_point_num`。
|
||||
- **真实目标起批慢**:提高 `TRACK_START_THRESHOLD` / `ALPHA_START`,或降低 `track_start_point_num`(最小 3)。
|
||||
- **静止/低速杂波起批**:提高 `V_MIN`。
|
||||
- **某方位扇区/距离段禁止起批**(如地物、干扰区):配置起批屏蔽区,比调门限更精准。
|
||||
|
||||
---
|
||||
|
||||
## 14. 输出字段映射
|
||||
|
||||
对每条新起批航迹的每个历史点 `j = 0 .. track_start_point_num-1`(L614-664):
|
||||
|
||||
| 输出字段 | 来源 | 备注 |
|
||||
|---|---|---|
|
||||
| `Track_Index` | 新分配的航迹号 | 同一行内所有点相同 |
|
||||
| `Point_Sum` | `= track_start_point_num` | 与 `track_asso` 输出的 `Point_Sum=1` 可作区分 |
|
||||
| `Range` / `Azimuth` | 点的 `r` / `azi`(→度) | 极坐标 |
|
||||
| `Elevation` | `asin(h/r)`(度,钳位 ±1) | BUG-27 修复后的定义域钳位 |
|
||||
| `z` | 点的 `height` | |
|
||||
| `Range_V` | `sqrt(X[1]² + X[3]²)` = `sqrt(vx²+vy²)` | 速度**模值**(非径向速度) |
|
||||
| `Direction_Angle` | `atan2(vy, vx)`(→度, `[0,360)`) | 航向角 |
|
||||
| `x / v_x / y / v_y` | 点的 `X[0..3]` | 注意 4 维布局 |
|
||||
| `range_point / azi_point / elev_point / vr_point` | 点的原始测量 | `point_type=0` |
|
||||
| `Amplitude / track_snr / track_rcs` | 点的 `Amp / snr / RCS` | |
|
||||
| `pitch_num / speed_dim / range_dim` | 点透传 | |
|
||||
| `track_time` | `T/1000`(秒) | |
|
||||
| `GNSS_time` | 点透传 | |
|
||||
| `Flag_Point` | 恒为 **1**(实点) | |
|
||||
| `Track_Mode` | 恒为 **0**(TWS) | |
|
||||
| **未赋值** | `Flag_Direction`、`x_Predict/y_Predict/z_Predict`、`Range_Predict/Azimuth_Predict/Elevation_Predict`、`Target_Type`、`prf_point`、`v_z` | DLL 不清零该数组,宿主读到的是其自身缓存的旧值/未初始化值。注意 `Flag_Direction` 只在关联输出路径赋值(`track_asso.cpp` / `track_asso_tas.cpp`,f249fdc:`vr_point > 0 ? 1 : 0`),起批输出不写该字段 |
|
||||
|
||||
---
|
||||
|
||||
## 15. 已知问题与风险点
|
||||
|
||||
> 以下为阅读源码得到的观察,**不代表已确认的缺陷**,部分为有意保留的历史行为;修改前请结合实测数据评估。
|
||||
|
||||
1. **硬编码 5 m 距离差门限**(L191):`|r_p − r_末点| < 5` 的点永不参与关联。对切向飞行、远距离慢速目标可能造成漏关联(起批失败)。
|
||||
2. **初始化用点不是航迹真正的末三点**(L505、L563-565 vs L614):起批流程中 `L = track_start_point_num`(L505 覆盖了前面循环里的航迹长度变量),Z0/Z1/Z2 取下标 `L-3 / L-2 / L-1`,输出循环也只写到 `j = L-1`。因此当航迹长度 N 大于起批点数时,参与初始化的只是**前 L 个点中的最后三点**,而不是航迹真正的末三点;第 L 个点之后的尾部点既未参与初始化也不出现在输出中(这些点通常来自滞留在起批屏蔽区期间的积累)。
|
||||
3. **去重只比较前三个点**(L517-519 硬编码 0/1/2),且优度指标取 `[track_start_point_num-1].d`(长度大于阈值时非末点);若 `track_start_point_num > 3`,共享前三点但后续分叉的两条航迹会被误判为同源。
|
||||
4. **无一对一关联约束**:一个点可同时为多条临时航迹续点(阶段二/三),产生航迹分裂,虚警场景下临时航迹表可能快速膨胀(靠去重与消亡收敛)。
|
||||
5. **ΔT ≤ 0 无显式校验**(BUG-33 遗留):头-点关联靠 `dis ≤ V_MAX·ΔT` 隐式排除负 ΔT;EKF 关联路径 `d_cal_track_init_EKF(ΔT<0)` 会得到无物理意义的结果,只是通常会被 `d ≤ 8` 挡住。
|
||||
6. **判据对马氏距离平方二次平方**(L221):实际门限为 `d ≤ 8`(3 自由度约 95.4%)。若按字面 `d² ≤ 64` 理解会误判为 `d ≤ 64`(极宽松)。修改 `TRACK_START_THRESHOLD` 时务必记得这一层。
|
||||
7. **两点/三点初始化无 `T > 0` 保护**(BUG-18):`T` 极小(重复/乱序时间戳)时速度、加速度与协方差会爆炸性放大。
|
||||
8. **死代码**:L72-75 的 `point_recv` 回写随即被 L117-118 清空;L606-608 的 `cart2polar` 结果未使用;L348-356 的近/远程分支取值相同。
|
||||
9. **`acos` 无定义域钳位**(L452):浮点越界产生 `NaN`,表现为条件恒假(静默丢弃该关联),不报错也不崩溃。
|
||||
10. **航迹数饱和静默丢弃**:`trust_track.size() ≥ 500` 或 `track_ind_get()` 返回 0 时,候选被直接跳过且该临时航迹已从 `temp_track` 摘除 → 起批机会永久丢失。
|
||||
11. **未赋值输出字段**(第 14 节):宿主若直接使用 `x_Predict`、`Flag_Direction`、`Target_Type` 等字段,会读到非本次写入的数据。
|
||||
12. **`PI` 为 float 常量**(3.1415926f)参与双精度运算,角度换算存在约 1e-7 量级偏差,属既有实现风格。
|
||||
13. **精度/量纲提醒**:`Temp_track::vr` 直接取点的 `Velocity`(多普勒)作为径向速度,EKF 关联时用于残差折叠(盲速 `v_bind`)与符号判据;若宿主的 `Velocity` 存在模糊,符号判据 `vr_p·vr_末点 > 0` 可能失效。
|
||||
14. **输出数组行号可能越界(极端场景)**:容量判据用 `trust_track->size() < 500`,而 `Trust_Track_Output` 的行计数已在 `track_asso` 阶段按「消亡前」的航迹数推进,中间的 `track_die` 删除 D 条后腾出的配额会让起始流程多写 D 行;当 D ≥ 2 且当圈起批数占满配额时,写入行号可超过 `MAX_TRACK_NUM=500`(详见 2.3)。建议把判据改为同时校验 `*Trust_track_num_Output < MAX_TRACK_NUM` 与 `trust_track->size() < MAX_TRACK_INDEX`。
|
||||
|
||||
---
|
||||
|
||||
## 16. 代码索引
|
||||
|
||||
### 16.1 `track_init.cpp`
|
||||
|
||||
| 行号 | 内容 |
|
||||
|---|---|
|
||||
| L11-143 | `track_init_process_logic` 主流程 |
|
||||
| L36-38 | 装填点迹 |
|
||||
| L40-47 | `buff_round` 自增 |
|
||||
| L49 | `if(point_process.size()>0)` 主分支 |
|
||||
| L52 / L55 | 两阶段关联调用 |
|
||||
| L57-70 | 删除已消费点迹 |
|
||||
| L72-75 | 点迹回写(等效空操作) |
|
||||
| L77-114 | 剩余点 → 航迹头 |
|
||||
| L117-118 | 清空 `point_process` / `point_recv` |
|
||||
| L121 | 转可靠航迹调用 |
|
||||
| L126 | 临时航迹消亡调用 |
|
||||
| L145-304 | `point_temp_track_asso` |
|
||||
| L164 | ≥2 点 + `buff_round≥2` 资格 |
|
||||
| L191 | `abs(Δr) ≥ 5 m` 门限 |
|
||||
| L194-195 | EKF 统计距离 `d` |
|
||||
| L199-205 | 折角 `α` |
|
||||
| L221 | 三条件判据 |
|
||||
| L237-281 | 生成新临时航迹(两点初始化) |
|
||||
| L283-303 | 删除被关联的原航迹 |
|
||||
| L306-445 | `point_track_head_asso` |
|
||||
| L325 | 1 点 + `buff_round≥2` 资格 |
|
||||
| L361-372 | 门限判据(含停用的旧分支注释) |
|
||||
| L388-427 | 生成 2 点临时航迹 |
|
||||
| L429-443 | 删除被关联的航迹头 |
|
||||
| L448-456 | `alpha_cal_track_init` |
|
||||
| L458-671 | `tmp_track_to_trust_track` |
|
||||
| L484 | 起批长度 + 屏蔽区条件 |
|
||||
| L504-550 | 同源去重 |
|
||||
| L552-667 | 起批与输出 |
|
||||
| L563-571 | 三点卡尔曼初始化 |
|
||||
| L573-604 | `Trust_Track` 构造 |
|
||||
| L612-664 | 输出历史点 |
|
||||
| L673-697 | `tmp_track_die` |
|
||||
| L699-712 | `track_init_prohibit` |
|
||||
| L714-723 | 构造 / `reset()` |
|
||||
|
||||
### 16.2 依赖
|
||||
|
||||
| 文件 | 函数 | 用途 |
|
||||
|---|---|---|
|
||||
| `kalman.cpp` L18-58 | `kalman_filter_init_2dots` | 两点(4 维)初始化 |
|
||||
| `kalman.cpp` L125-194 | `kalman_filter_init_3dots` | 三点(6 维)初始化 |
|
||||
| `kalman.cpp` L615-693 | `d_cal_track_init_EKF` | 起始关联统计距离 |
|
||||
| `kalman.cpp` L780-789 | `Bind_speed` | 盲速计算 |
|
||||
| `coor_trans.cpp` L8-20 | `polar2cart` / `cart2polar` | 坐标转换 |
|
||||
| `track_index_mangement.cpp` L14-77 | `track_ind_get` | 航迹号分配 |
|
||||
| `data_process.cpp` L51 | `data_preprocess` | 一圈完成 → `model=1` |
|
||||
| `data_process.cpp` L154-159 | `track_process` | 起始流程调用点 |
|
||||
| `data_process.cpp` L204-222 | 参数初始化/修改 | 校验 `track_start_point_num ∈ [3,9]` |
|
||||
Reference in New Issue
Block a user