更新:1、修改参数配置方式,改为由配置文件输入;
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
+15
-1
@@ -9,6 +9,11 @@
|
||||
|
||||
### Added
|
||||
|
||||
- **参数加载器(参数配置方式改造,工作区未提交)**:新增内部模块 `data_process_class_dll/rdp_parameter_loader.h/.cpp`,把 TOML 配置文件解析到 `struct.h::RDP_parameter`(接口 `int rdp_parameter_load(const char* path, RDP_parameter& parameter, std::string* error_msg)`)。采用**严格校验**:11 个配置段(`radar_params` / `cohesion` / `track_associate` / `track_init` / `track_die` / `track_quality` / `track_index` / `beam_control` / `singer_model` / `track_init_prohibite_area` / `tas_prohibite_area`)全部必填;标量键全部必填且校验类型(数字/整数)与取值范围;拒绝未知配置段与未知键(拼错即报错);屏蔽区 4 个数组必须存在且元素个数等于对应 `*_num`(`*_num` 限 0~30,为 0 时数组可省略或为空数组),并逐区校验距离/方位范围。任一错误返回 -1 并给出首条可读原因,**不修改**调用方传入的参数(全部通过后才一次性赋值)。
|
||||
- **新增公开虚接口 `parameters_reload`(参数配置方式改造)**:`data_process_class_dll.h` 的 `Data_process_class_dll` 新增 `virtual int parameters_reload(const char* parameter_file_path)`(基类空实现返回 -1);`Data_Process` 实现为「加载成功后整体赋值 `rdp_parameter`,失败保留当前参数并经 `RDP_LOG` 打印原因」。宿主改为在 `GetB()` 之后调用一次即可完成全部工作参数配置,重连/换配置时再次调用可热重载;不再需要构造并下发 `struct RadarPara`。
|
||||
- **示例配置文件 `config/config_tmp.toml`(参数配置方式改造)**:11 个配置段与 `RDP_parameter` 的注释分组一一对应,键名与结构体成员名完全一致(`[radar_params]`、`[cohesion]`、`[track_associate]`、`[track_init]`、`[track_die]`、`[track_quality]`、`[track_index]`、`[beam_control]`、`[singer_model]` 为标量段,两个屏蔽区段用与成员同名的 TOML 数组,样例为 `*_num = 0` + 空数组);数值与改造前的宏/`RadarPara` 默认值一致。
|
||||
- **内置 toml++ v3.4.0 单头文件(参数配置方式改造)**:新增 `third_party/toml/toml.hpp`(MIT,上游单头文件原样保存);`CMakeLists.txt` 的 `SOURCES` 增加 `data_process_class_dll/rdp_parameter_loader.cpp`、`target_include_directories` 增加 `third_party/toml`;`rdp_parameter_loader.cpp` 用 `#pragma warning(push, 0)` / `(pop)` 包裹第三方头,维持 `/W3` 零告警约定。
|
||||
- 文档同步(参数配置方式改造):更新 `README.md`(1/2/3/4/5/6/7/8/9/10/11/12/13 节)与 `docs/data_process_flow_report.md` 第 9 节、`docs/track_init_readme.md` 第 13 节等——参数来源改为「TOML 配置文件 → `RDP_parameter`」、宿主调用改为 `parameters_reload`、新增旧参数→新字段对照表、编译期宏清单仅保留非配置常量、已知问题与版本历史同步。
|
||||
- 新增 `.vscode/scripts/config_paths.bat`:按「扫描体制 / 构建类型 / 记录模式」推导并校验构建目录 `build/<X256_PS|X256_MS>/<Debug|Release>/<REC_AUTO|REC_OFF|REC_ON>`(接受 `X256_PS`/`X256_MS` 别名),供所有构建脚本共用。
|
||||
- 新增 `.vscode/scripts/build_all.bat`:对 `PHASE_SCANNING` 与 `MECHANICAL_SCANNING` 依次「配置 + 构建」,任一配置失败即中止并返回非 0。
|
||||
- 新增 `.vscode/scripts/clean_all.bat`:删除全部配置的构建输出(含旧版单一构建目录;`build/output` 打包留档不动)。
|
||||
@@ -23,10 +28,16 @@
|
||||
|
||||
### Changed
|
||||
|
||||
- **参数来源改为「TOML 配置文件 → `RDP_parameter`」(参数配置方式改造,工作区未提交)**:原先「宿主下发 `struct RadarPara` + `parameters.h` 编译期宏」两条参数来源统一为运行期配置文件;`struct.h::RDP_parameter` 成为工作参数唯一来源,由 `Data_Process::rdp_parameter{}`(零初始化)持有,`parameters_reload()` 成功后才整体替换。改造只替换参数来源,不改变公式、阈值语义与处理流程,默认数值与改造前一致。
|
||||
- **各模块参数传递方式(参数配置方式改造)**:`kalman`(使用量测误差/频点的 11 个方法:`kalman_filter_init_2dots`、`d_cal_track_init`、`kalman_filter_init_3dots`、`d_cal`、`d_cal_EKF`、`d_cal_with_doppler`、`kalman_filter_EKF`、`kalman_filter`、`d_cal_track_init_EKF`、`d_cal_track_init_with_doppler`、`Bind_speed`)、`dot_coh`/`dot_coh_tas`、`track_asso`/`track_asso_tas`/`track_asso_direct_tracking`、`track_init`/`track_init_direct_tracking`、`track_die`/`track_die_tas`、`track_index_mangement`、`tas_ctrl` 的入口与内部函数新增 `const RDP_parameter& rdp_parameter` 形参(原按值传递的 `struct RadarPara Work_Parameter` 全部删除);无参数依赖的 `kalman_pred` 保持原签名。
|
||||
- **旧宏与旧 `RadarPara` 字段的等价替换(参数配置方式改造)**:如 `SIGMA_R/A/E/V`→`sigma_range/azimuth/pitch/velocity`、`DOT_COH_RANGE/V/AZI`→`point_cohesion_range/velocity/azimuth`、`R_MIN/R_MAX`→`point_cohesion_range_min/max`、`FREQ0`→`freq_0_GHz`、`ASSO_THORD`→`associate_threshold`、`MAX_EXTRAPOLATE_NUM(_TAS)`→`extrapolate_max(_tas)`、`H_F_WIN_LEN`→`height_smooth_win`、`TRACK_START_THRESHOLD/ALPHA_START`→`track_init_threshold/track_init_angle`、`TRACK_START_QUALITY_L3~L7`→`track_init_quality_L3~L7`、`TRACK_DIE_QUALITY(_PRAMARY)/TRACK_DIE_ROUND(_PRIMARY/_TAS)`→`track_die_quality(_pramary)/track_die_round(_pramary/_tas)`、`TRACK_QUALITY_*`→`track_quality_*`、`work_mode/Height/Sys_delay`→`radar_work_mode/radar_height/radar_system_delay`、`track_start_point_num`→`track_init_point_num_min`、`V_MAX/V_MIN`→`track_init_velocity_max/min`、`track_prohibite_area_*`→`track_init_prohibite_area_*`、`TAS_prohibite_area_*`→`tas_prohibite_area_*`、`Model1/2/3_Q_fast/slow`→`model_1/2/3_Q_fast/slow`、`DATA_RATE_SHORT/MIDDLE/FAR`→`data_rate_tws`(契约确认的等价改法)、`DATA_RATE_TAS`→`data_rate_tas`;完整对照见 `README.md` 10.3 节。
|
||||
- **航迹号、TAS 队列与跟踪波束类型改为按配置分配(参数配置方式改造)**:`track_index_mangement.cpp` 的定长数组 `int List[MAX_TRACK_INDEX]` 改为按 `rdp_parameter.track_index_max` 分配的 `std::vector<int>`;TAS 队列固定容量改用编译期常量 `RDP_TAS_QUEUE_CAPACITY`(30),运行期同时跟踪目标数上限改用配置项 `tas_track_num_max`(加载器校验 0~30,原 `MAX_TAS_NUM` 相扫 4 / 机扫 1);跟踪波束 `Tracking_beam->type` 由硬编码 1 改为配置项 `default_track_beam_type`。
|
||||
- **`parameters.h` 精简为编译期常量(参数配置方式改造)**:删除全部雷达工作参数宏,仅保留 `PI`、版本宏(`MAJOR_VERSION`/`MINOR_VERSION`/`PATCH_VERSION`/`VERSION_INFO`/`VERSION_STRING`)、日志宏(`RDP_LOG_*`)、记录宏(`RDP_RECORD_*`)、接口容量常量 `MAX_TRACK_NUM`(500)/`RDP_TAS_QUEUE_CAPACITY`(30)、`UNCONF_TARGET` 与 `Round`;`requirements.md` 同时改写为本次参数配置方式改造的需求说明。
|
||||
- **本次改造的验证(Lead 实测,2026-10-27)**:① 构建:8 个配置(X256_PS/X256_MS × Release/Debug × REC_OFF/REC_ON)全部成功、退出码 0、0 warning 0 error,每个配置均生成 `bin/data_process_class_dll.dll`;Release REC_OFF DLL 体积 331,776 字节(约 324 KB,较改造前约 178 KB 增大,原因是纯头文件的 toml++ 解析器编入 DLL),Debug REC_ON 为 4,694,016 字节。② 导出表(Release REC_OFF,共 31 个导出):新增 `parameters_reload`(`?parameters_reload@Data_process_class_dll@@UEAAHPEBD@Z`),`track_process_parameters_initial`/`track_process_parameters_modify` 与 `RadarPara` 相关符号已不存在,`Data_Process_Factory::GetB/Destroy` 等其余接口不变。③ 加载器单元测试(229 项断言全部通过):样例配置 62 个键逐字段取值正确(含整数/浮点与负的二次/一次衰减系数);文件不存在、空路径、nullptr、TOML 语法错误、缺标量键、类型错误、未知键、缺配置段、取值越界(起批点数 2、`data_rate_tas=0`)、区域数 31、区域方位 min≥max、数组长度与 `*_num` 不一致等失败路径均返回 -1 且不修改当前参数,失败后可重新加载成功;屏蔽区数组按 `*_num` 读取、未使用位置保持 0。④ 公开接口运行期测试(8 个配置逐一编译链接运行,各 29 项断言全部通过):`GetB()`/`version_info`(`V1.5.8.PS`/`V1.5.8.MS`)、`parameters_reload` 成功/失败返回值与「失败后再加载成功」、TWS/TAS 主流程冒烟、`Beam_Ctrl`、`track_clear_all`、`track_delete`/`tracking_start`/`tracking_stop`/`primary_target_add`/`primary_target_cancel`/`tracking_point`、`Destroy()` 后重新 `GetB()` 并再次加载配置均正常。⑤ 静态审计:源码无 `Work_Parameter`/`RadarPara`/旧参数宏残留(区分大小写整词审计 0 命中);62 个 `RDP_parameter` 成员 ↔ 62 个加载器键 ↔ 62 个 TOML 键一一对应,11 个配置段与加载器一致。验证程序与日志位于 git 忽略的构建目录中,未入库。
|
||||
- 构建目录布局由「每个扫描体制一个目录(Release/Debug 共用、重新配置互相覆盖)」改为按完整配置隔离:`build/<X256_PS|X256_MS>/<Debug|Release>/<REC_AUTO|REC_OFF|REC_ON>`,关闭/开启记录模块的产物可并存、互不覆盖。
|
||||
- `configure.bat` / `build.bat` / `clean.bat` 参数由 `<build-dir> ...` 改为 `<PHASE_SCANNING|MECHANICAL_SCANNING> <Debug|Release> [AUTO|ON|OFF]`,构建目录自动推导;`clean.bat` 由 `cmake --build --target clean` 改为删除该配置的构建目录(因此放在配置目录 `bin/` 下、CMake 不会重建的 `rdp_playback.exe`/回放数据会一并删除,需放到 `build/output/` 或仓库外)。
|
||||
- VS Code 配置同步:`settings.json` 的 `cmake.buildDirectory` 改为 `${workspaceFolder}/build/${variant:scanMode}/${buildType}/${variant:recordMode}`、`compileCommands` 指向 `build/X256_PS/Debug/REC_ON/compile_commands.json`;`cmake-variants.yaml` 新增 `recordMode` 变体(REC_AUTO / REC_OFF / REC_ON,映射 `RDP_DATA_RECORD`);`c_cpp_properties.json` 的 `compileCommands` 更新到新目录;`launch.json` 的 `preLaunchTask` 改为「配置并构建全部(Debug/REC_ON)」,`program`/`symbolSearchPath` 指向 `build/X256_PS/Debug/REC_ON/bin`。
|
||||
- 更新 README(3 节目录结构、4.2~4.4 节构建说明、12 节版本历史、13 节参考文档)与 `docs/data_record_debug.md` 第 2 节构建示例,同步新的构建目录布局、脚本签名与 5 个任务;`requirements.md` 更新为本次构建脚本/VS Code 任务调整需求。
|
||||
- 更新 README(3 节目录结构、4.2~4.4 节构建说明、12 节版本历史、13 节参考文档)与 `docs/data_record_debug.md` 第 2 节构建示例,同步新的构建目录布局、脚本签名与 5 个任务;`requirements.md` 当时更新为构建脚本/VS Code 任务调整需求(其后已由下方「参数配置方式改造」需求整体替换)。
|
||||
- 本次改动的构建验证:5 个任务全部实际执行(清理全部 → Release/REC_OFF → Release/REC_ON → Debug/REC_OFF → Debug/REC_ON),退出码均为 0;8 种配置(PS/MS × Release/Debug × REC_OFF/REC_ON)构建日志均 0 warning、0 error;每个配置目录均生成 `bin/data_process_class_dll.dll`、`lib/`(REC_ON 另有 `lib/rdp_record.lib`)与 `compile_commands.json`;编译命令中的宏与配置对应(PS 为 `-DPHASE_SCANNING`、MS 为 `-DMECHANICAL_SCANNING`,REC_ON 含 `RDP_RECORD_ENABLE`);「清理全部」删除 8 个配置目录及旧版单一构建目录而 `build/output` 未被删除;错误扫描体制/构建类型/记录模式与未配置即构建等非法路径均返回非 0。补充验证:已构建配置再次执行 `build_all.bat` 为 0 个编译/链接步骤(增量幂等);模拟 MSVC 环境无效时 `build_all.bat` 在第一个配置即中止(未尝试第二个扫描体制,退出码 1);`clean.bat` 单配置清理后目录消失且可正常重建。
|
||||
- **12 个业务记录点由「同步构造实体写库」改为「帧/圈内收集源结构体 → 一次入队」(数据记录性能优化)**:`data_process.cpp`(TWS/TAS 输入点迹)、`dot_coh.cpp`、`dot_coh_tas.cpp`、`track_init.cpp`(临时航迹)、`track_asso.cpp`(可靠航迹 + 关联信息按 `RDP_RECORD_ASSO_CHUNK_ROWS` 分块)、`track_asso_tas.cpp`(同)共 12 处;记录时机、条件与记录行数/表列均保持不变;6 个业务文件删除 `#include "rdp_record_entity.h"`,所有新增代码仍在 `#ifdef RDP_RECORD_ENABLE` 内,记录关闭构建零代码、零影响。
|
||||
- **记录线程模型与 Shutdown 语义(数据记录性能优化)**:由「处理线程单连接 + 互斥锁串行同步写」改为「处理线程只入队 + 独立写线程承担实体构造与全部写库」;`RDP_RECORD`/`RDP_RECORD_BATCH` 保留为遗留同步接口(在调用线程直接写库、会阻塞,仅供测试/特殊场景);`Shutdown()` 会**排空**队列与 pending 后再关库,并在关库前打印统计。
|
||||
@@ -37,11 +48,14 @@
|
||||
|
||||
### Fixed
|
||||
|
||||
- 修正文档中把参数来源写成「`parameters.h` 编译期宏 / 宿主下发 `RadarPara`」的过期描述(本次文档更新,对应参数配置方式改造):README 2、3、4.2、5.1、5.2、6.1、6.2、7.1、7.3、8.1~8.3、9.1~9.11、10、11、12、13 节与 `docs/data_process_flow_report.md` 第 9/10 节、`docs/track_init_readme.md` 第 1/2/4/6/8/10/11~16 节按源码同步——参数来源改为 TOML 配置文件 → `RDP_parameter`、旧宏改记为「原 …」并给出新字段名、新增 `parameters_reload` 调用方式与旧→新对照,数值未变。
|
||||
- 修正文档中 TAS 航迹消亡圈数与高度平滑窗口的过期数值(本次文档更新):README 2/5.2/9.7/9.9/10.1 节与 `docs/data_process_flow_report.md` 4.6、6.1、9.1 节按 5d105d2、6284f3f 同步为「相扫 TAS 20 圈 / 机扫 96」、「相扫 H_F_WIN_LEN=6、TAS 偏移 +6/+8/+10/+12」。
|
||||
- 修正 `docs/data_record_debug.md` 中「质量字段尚未加入记录实体」的过期描述(本次文档更新,对应 1ffb287):字段速查与记录点说明补齐 `quality` / `Quality` / `Unupdated_time` / `Point_sum` / `Valid_point_sum`。
|
||||
|
||||
### Removed
|
||||
|
||||
- **删除 `struct RadarPara` 与两个旧公开虚接口(参数配置方式改造)**:`data_process_class_dll.h` 中的 `struct RadarPara` 及 `track_process_parameters_initial(RadarPara)` / `track_process_parameters_modify(RadarPara)` 整体删除,由 `parameters_reload(const char*)` 取代;`data_process.h` 中的 `struct RadarPara Work_Parameter` 删除。旧 `RadarPara` 中实现未使用的字段(`R_TAS_max/min`、`V_TAS_max/min`、`TAS_Height_1~5`、`cfar_th`、`north_angle`、`Beam_num`)未进入新结构体(相应自动 TAS 逻辑仍保持停用)。
|
||||
- **删除 `parameters.h` 中的雷达工作参数宏(参数配置方式改造)**:`SIGMA_R/A/E/V`、`DOT_COH_RANGE/V/AZI`、`R_MIN`/`R_MAX`、`FREQ0`、`ASSO_THORD`、`MAX_EXTRAPOLATE_NUM(_TAS)`、`H_F_WIN_LEN`、`TRACK_START_THRESHOLD`、`ALPHA_START`、`TRACK_START_QUALITY_L3~L7`、`TRACK_DIE_*`、`TRACK_QUALITY_*`、`MAX_TRACK_INDEX`、`MAX_TAS_NUM`、`TAS_QUEUE_LENGTH`、`DATA_RATE_*`(含 `H_F_WIN_LEN`、`MAX_BEAM_NUM`、`ASSOCIATE_THRESHOLD_MIN/MID/MAX`、`DIRECT_TRACKING_QUEUE_LENGTH` 等)以及已注释的 `V_MAX`/`V_MIN` 残留;这些参数改由配置文件提供(见 `README.md` 10.1~10.3)。
|
||||
- 删除 `tasks.json` 中原有的 15 个任务(`CMake: 配置 X256_PS|X256_MS`、`CMake: 构建 ...`、`CMake: 清理 ...`、`CMake: 配置并构建 ...` 等),改为上述 5 个任务。
|
||||
|
||||
## [1.5.8] - 2026-10-20
|
||||
|
||||
@@ -93,6 +93,7 @@ set(SOURCES
|
||||
data_process_class_dll/rdp_log.cpp
|
||||
data_process_class_dll/data_process_class_dll.cpp
|
||||
data_process_class_dll/data_process.cpp
|
||||
data_process_class_dll/rdp_parameter_loader.cpp
|
||||
data_process_class_dll/dot_coh.cpp
|
||||
data_process_class_dll/kalman.cpp
|
||||
data_process_class_dll/coor_trans.cpp
|
||||
@@ -175,8 +176,10 @@ if(WIN32)
|
||||
endif()
|
||||
|
||||
# Eigen 头文件位于源码目录下(<Eigen/Dense>)。
|
||||
# toml++ 为单头文件第三方库(<toml.hpp>,仅 rdp_parameter_loader.cpp 使用)。
|
||||
target_include_directories(data_process_class_dll PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/data_process_class_dll
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/third_party/toml
|
||||
)
|
||||
|
||||
if(MSVC)
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
> 仓库 simianzhen256-v1.5.5-ms-vscode(目录名沿用旧版本,当前代码版本为 **V1.5.8**;已由 Qt/qmake 工程迁移为 VS Code + CMake 工程,编译器为 MSVC 2022 / v143,C++17,x64 目标)。
|
||||
>
|
||||
> 版本号由 `parameters.h` 的 `MAJOR_VERSION` / `MINOR_VERSION` / `PATCH_VERSION`(当前 1.5.8)与体制后缀拼装为 `VERSION_STRING`(相扫 `V1.5.8.PS` / 机扫 `V1.5.8.MS`),实例创建后可从 `Data_Process_Factory::GetB()` 返回对象的公开成员 `version_info`(`const char*`)读取。
|
||||
>
|
||||
> **工作参数来源(参数配置方式改造后)**:全部雷达工作参数由宿主调用 `parameters_reload(配置文件路径)` 从 **TOML 配置文件**加载到内部 `RDP_parameter`,不再由宿主下发 `struct RadarPara`、也不再由 `parameters.h` 编译期宏配置(见 10)。
|
||||
|
||||
---
|
||||
|
||||
@@ -48,6 +50,7 @@
|
||||
| 坐标 | 直角坐标系 x/y 为水平面投影,z = 目标高度 + 雷达平台高度 |
|
||||
| 点迹类型 | point_type = 0 为 TWS,point_type = 1 为 TAS |
|
||||
| 跟踪模式 | Track_Mode = 0 为 TWS,Track_Mode = 1 为 TAS |
|
||||
| 参数来源 | TOML 配置文件(`config/config_tmp.toml` 为样例)→ 内部 `struct RDP_parameter`,宿主通过 `parameters_reload()` 加载;参数对照见 10 |
|
||||
|
||||
---
|
||||
|
||||
@@ -57,17 +60,18 @@
|
||||
- **两帧滑窗点迹凝聚**:TWS 点迹先与上一帧缓存合并凝聚,再进入关联,兼顾跨波位(CPI)相邻点迹的分裂问题;近程模式增加速度维约束。
|
||||
- **逻辑法航迹起始**:每圈按固定顺序推进——① 未关联点迹先与 ≥2 点临时航迹做 EKF 波门 + 折角约束关联(延长临时航迹),② 再与 1 点航迹头做速度法关联(两点卡尔曼初始化出 2 点临时航迹),③ 两步都做完后删除被消费的点迹、剩余点迹转成新航迹头,④ 长度达标的临时航迹经三点卡尔曼初始化转可靠航迹;带起批屏蔽区与重复航迹去重。
|
||||
- **3 模型 IMM 滤波**:三个 Singer 模型(低/中/高过程噪声),量测为「距离-方位-径向速度」三维的 EKF,多普勒残差按盲速折叠,似然用 3×3 新息协方差。
|
||||
- **最近邻数据关联**:三模型统计距离波门 `ASSO_THORD = 4`(等效 d < 4,TWS 另按航迹距离分段收紧模型 3 波门),再叠加高度门限(≤1000 m 150 m / 2000 m 200 m / 3000 m 250 m / 5000 m 300 m,更远不约束)与 TWS 弦距-位移距离门限。ΔT ≤ 0(点迹时间戳不晚于航迹时间)的候选不更新航迹状态、仅消费该点迹(标记已使用),避免其继续起批而生成虚假 TWS 航迹。关联输出的航迹同时给出**靠近/远离方向标志**:按航迹 IMM 融合速度矢量与「雷达→目标」位置矢量的夹角判定——`acos((-x·cos(Direction_Angle) − y·sin(Direction_Angle)) / r) < π/2` 置 1(靠近),否则 0(远离),等价于「滤波速度的径向分量指向雷达」,不再使用受多普勒模糊影响的实测径向速度 `vr_point`;TWS、TAS 两通道一致(见 9.6)。
|
||||
- **重点目标与外推上限**:`primary_target_add` / `primary_target_cancel` 可按批号手动标记/取消重点目标,重点航迹的消亡轮数放宽到 `TRACK_DIE_ROUND_PRIMARY`(10);未关联航迹的外推圈数受限(TWS ≤ `MAX_EXTRAPOLATE_NUM`=3,TAS 相扫 5 / 机扫 48),超限后停止外推。**输出航迹只包含本圈有更新的航迹**(`Track_Update_Flag` 门控),宿主不能再用输出行数推断航迹总数(见 9.9)。
|
||||
- **最近邻数据关联**:三模型统计距离波门 `associate_threshold = 4`(配置项 `[track_associate] associate_threshold`,等效 d < 4,TWS 另按航迹距离分段收紧模型 3 波门),再叠加高度门限(≤1000 m 150 m / 2000 m 200 m / 3000 m 250 m / 5000 m 300 m,更远不约束)与 TWS 弦距-位移距离门限。ΔT ≤ 0(点迹时间戳不晚于航迹时间)的候选不更新航迹状态、仅消费该点迹(标记已使用),避免其继续起批而生成虚假 TWS 航迹。关联输出的航迹同时给出**靠近/远离方向标志**:按航迹 IMM 融合速度矢量与「雷达→目标」位置矢量的夹角判定——`acos((-x·cos(Direction_Angle) − y·sin(Direction_Angle)) / r) < π/2` 置 1(靠近),否则 0(远离),等价于「滤波速度的径向分量指向雷达」,不再使用受多普勒模糊影响的实测径向速度 `vr_point`;TWS、TAS 两通道一致(见 9.6)。
|
||||
- **重点目标与外推上限**:`primary_target_add` / `primary_target_cancel` 可按批号手动标记/取消重点目标,重点航迹的消亡轮数放宽到 `track_die_round_pramary`(默认 10);未关联航迹的外推圈数受限(TWS ≤ `extrapolate_max`=3,TAS 由 `extrapolate_max_tas` 控制,样例配置为相扫 5 / 机扫需配 48),超限后停止外推。**输出航迹只包含本圈有更新的航迹**(`Track_Update_Flag` 门控),宿主不能再用输出行数推断航迹总数(见 9.9)。
|
||||
- **航迹质量(V1.5.8)**:归一化航迹质量(0~1,内部)贯穿起始→关联→外推三阶段——临时航迹每延长一点按「统计距离 + SNR」重算,关联实点后按「统计距离 + SNR + 有效点占比」加权并指数平滑,未关联期间按未更新时长二次衰减;起批需达到**按长度分级**的最低质量,输出航迹给 0~100 的 `Quality` 字段;**普通 TWS 航迹的消亡改由质量决定**(`Quality < 0.3`),重点航迹与 TAS 航迹仍按外推圈数(见 9.11)。
|
||||
- **点迹附加信息字段扩展(需同步重编译宿主)**:`DataRev` 新增 `amp_asv` / `prn_fftn`,`Track` 新增 `amp_asv_point` / `prn_fftn_point`,结构体布局随之变化,宿主必须使用同一版本头文件重新编译对接;输入侧已打通(TWS 输出有效,TAS 输出仍未写回),详见 11.2。
|
||||
- **高度维独立平滑**:按距离分段选择滑动平均窗长,TWS 为 `H_F_WIN_LEN + {2,3,4,6}`、TAS 为 `H_F_WIN_LEN + {6,8,10,12}`(相扫 `H_F_WIN_LEN`=6、机扫 3),抑制俯仰测量跳变(见 9.7)。
|
||||
- **高度维独立平滑**:按距离分段选择滑动平均窗长,TWS 为 `height_smooth_win + {2,3,4,6}`、TAS 为 `height_smooth_win + {6,8,10,12}`(样例配置 `height_smooth_win`=6 对应相扫;机扫需配 3),抑制俯仰测量跳变(见 9.7)。
|
||||
- **参数配置外置(参数配置方式改造)**:全部工作参数(原 `parameters.h` 宏 + `RadarPara` 字段)改由 TOML 配置文件提供,宿主在 `GetB()` 后调用一次 `parameters_reload(路径)` 加载(重连/换配置可再次调用);加载器严格校验 11 个配置段、键名/类型/取值范围与屏蔽区数组长度,失败返回 -1 且不改变当前参数(见 10.1)。样例配置 `config/config_tmp.toml` 为**相扫**取值;`extrapolate_max_tas`、`height_smooth_win`、`track_die_round_tas`、`tas_track_num_max` 在改造前按扫描体制取不同宏值(机扫依次为 48 / 3 / 96 / 1),现在同一份配置对所有体制生效,机扫部署需改用对应取值的配置文件(相扫/机扫的**判圈逻辑**仍由编译期 `PHASE_SCANNING` / `MECHANICAL_SCANNING` 区分,见 4.2、7.2)。
|
||||
- **TAS 波束控制**:手动转 TAS 目标入队(队列长度与容量随扫描体制切换),按最新时间戳外推输出跟踪波束(距离/方位/俯仰/批号)。
|
||||
- **航迹生命周期管理**:外推轮数计数、超时消亡与质量消亡(普通 TWS 航迹)、手动删除、全清空(含各模块 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` 日志宏在每条日志前附加 `[时间戳][文件:行号]`。该宏在 `parameters.h` 中**当前为注释状态(默认不启用)**,即默认不做重定向、日志输出到控制台(见 10.1)。
|
||||
- **日志文件输出**:`RDP_LOG_ENABLE` 定义时,首次 `Data_Process_Factory::GetB()` 自动用 `rdbuf()` 将 `std::cout` 重定向到 `RDP_LOG_DIR`(`.\rdp_logs\`)下按日期+时间命名的日志文件,`Destroy()` 时恢复控制台并关闭文件;`RDP_LOG` 日志宏在每条日志前附加 `[时间戳][文件:行号]`。该宏在 `parameters.h` 中**当前为注释状态(默认不启用)**,即默认不做重定向、日志输出到控制台(见 10.4)。
|
||||
- **版本号输出**:`parameters.h` 的 `MAJOR_VERSION`/`MINOR_VERSION`/`PATCH_VERSION`(当前 1.5.8)经两级 `STRINGIFY` 宏拼成 `VERSION_STRING`(`V1.5.8.PS` / `V1.5.8.MS`);`Data_process_class_dll` 基类公开成员 `const char* version_info` 由 `Data_Process_Factory::GetB()` 在创建实例时赋值,宿主可直接读取以核对/上报 DLL 版本(b227ae1)。
|
||||
- **数据记录调试功能(可选编译、异步批量写入)**:`rdp_record` 模块把结构体按「一个记录实体一张表」写入 SQLite(一次运行一个 db 文件、多张表),结构体到表的映射由内置的 ormpp(`third_party/ormpp`,含 iguana、frozen 与 SQLite 3.49.1 amalgamation)完成;由 CMake 选项 `RDP_DATA_RECORD`(`AUTO`/`ON`/`OFF`)控制,**AUTO 时 Debug 构建启用、Release(交付)构建完全不编译**——记录模块源码不参与编译、记录宏(6 个异步入队宏与遗留 `RDP_RECORD`/`RDP_RECORD_BATCH`)展开为 `((void)0)`(实参不求值),DLL 不新增依赖与导出符号、无性能损失。**V1.5.8 优化后的运行模型**:`GetB()` 建目录、开库(`.\rdp_records\rdp_record_YYYYMMDD_HHMMSS.db`)并启动独立**写线程**;12 个记录点只做「帧/圈内收集源结构体 → 一次入队」(收集代码均在 `#ifdef RDP_RECORD_ENABLE` 内),实体构造(数组转文本)与 SQLite **批量事务**全部在写线程完成;入队队列**有界**(默认 64 MB / 4096 批,满则整批丢弃并计入 `stats().dropped_rows`,不阻塞处理线程),写线程按 4096 行或等待 50 ms 凑批提交,SQLite 使用 WAL + `synchronous=OFF` + 8 MB 缓存,建表后去掉 `AUTOINCREMENT`;`Destroy()` 时停止入队、排空、`wal_checkpoint(TRUNCATE)`、关库并打印统计,支持 `Destroy()`→`GetB()` 重连重新建库(同秒文件名冲突追加 `_1`/`_2` 后缀)。当前内置 **6 个记录实体定义、6 张在用表**(输入点迹、TWS/TAS 凝聚后点迹、临时航迹、可靠航迹、点航关联信息),**可靠航迹与关联信息两通道均覆盖 TWS 与 TAS**(TAS 侧由 5cb67cc 补齐,关联信息以 `tas_flag` 区分来源),临时航迹与可靠航迹表还记录**航迹质量**(1ffb287:`RdpRecTempTrack.quality`、`RdpRecTrustTrack.Quality` 及 `Unupdated_time`/`Point_sum`/`Valid_point_sum`),并新增 `point_id`/`frame_id`/`circle_id`/`temp_track_id`/`track_id` 等关联标识,可把一次处理的点迹→凝聚→临时航迹→可靠航迹→关联信息串成完整链路;新增记录数据按「加记录实体 + 注册 + 加异步入队接口/宏 + 写线程分支 + 业务侧收集调用」四步进行,详见 docs/data_record_debug.md(见 4.3、10.1、11.3)。
|
||||
- **数据记录调试功能(可选编译、异步批量写入)**:`rdp_record` 模块把结构体按「一个记录实体一张表」写入 SQLite(一次运行一个 db 文件、多张表),结构体到表的映射由内置的 ormpp(`third_party/ormpp`,含 iguana、frozen 与 SQLite 3.49.1 amalgamation)完成;由 CMake 选项 `RDP_DATA_RECORD`(`AUTO`/`ON`/`OFF`)控制,**AUTO 时 Debug 构建启用、Release(交付)构建完全不编译**——记录模块源码不参与编译、记录宏(6 个异步入队宏与遗留 `RDP_RECORD`/`RDP_RECORD_BATCH`)展开为 `((void)0)`(实参不求值),DLL 不新增依赖与导出符号、无性能损失。**V1.5.8 优化后的运行模型**:`GetB()` 建目录、开库(`.\rdp_records\rdp_record_YYYYMMDD_HHMMSS.db`)并启动独立**写线程**;12 个记录点只做「帧/圈内收集源结构体 → 一次入队」(收集代码均在 `#ifdef RDP_RECORD_ENABLE` 内),实体构造(数组转文本)与 SQLite **批量事务**全部在写线程完成;入队队列**有界**(默认 64 MB / 4096 批,满则整批丢弃并计入 `stats().dropped_rows`,不阻塞处理线程),写线程按 4096 行或等待 50 ms 凑批提交,SQLite 使用 WAL + `synchronous=OFF` + 8 MB 缓存,建表后去掉 `AUTOINCREMENT`;`Destroy()` 时停止入队、排空、`wal_checkpoint(TRUNCATE)`、关库并打印统计,支持 `Destroy()`→`GetB()` 重连重新建库(同秒文件名冲突追加 `_1`/`_2` 后缀)。当前内置 **6 个记录实体定义、6 张在用表**(输入点迹、TWS/TAS 凝聚后点迹、临时航迹、可靠航迹、点航关联信息),**可靠航迹与关联信息两通道均覆盖 TWS 与 TAS**(TAS 侧由 5cb67cc 补齐,关联信息以 `tas_flag` 区分来源),临时航迹与可靠航迹表还记录**航迹质量**(1ffb287:`RdpRecTempTrack.quality`、`RdpRecTrustTrack.Quality` 及 `Unupdated_time`/`Point_sum`/`Valid_point_sum`),并新增 `point_id`/`frame_id`/`circle_id`/`temp_track_id`/`track_id` 等关联标识,可把一次处理的点迹→凝聚→临时航迹→可靠航迹→关联信息串成完整链路;新增记录数据按「加记录实体 + 注册 + 加异步入队接口/宏 + 写线程分支 + 业务侧收集调用」四步进行,详见 docs/data_record_debug.md(见 4.3、10.4、11.3)。
|
||||
|
||||
---
|
||||
|
||||
@@ -78,7 +82,9 @@
|
||||
├── CMakeLists.txt # CMake 构建脚本(扫描模式切换、导出宏、输出目录)
|
||||
├── CHANGELOG.md # 变更日志(Keep a Changelog 格式,按 git 提交记录维护)
|
||||
├── README.md # 本文件:项目说明(架构、接口、流程、算法、参数)
|
||||
├── requirements.md # 需求说明:构建脚本与 VS Code 任务调整(Debug/Release × 扫描体制 × RDP_DATA_RECORD,5 个任务)
|
||||
├── requirements.md # 需求说明:雷达参数配置方式改造(TOML 配置文件 → RDP_parameter、parameters_reload 接口)
|
||||
├── config/ # 参数配置文件目录
|
||||
│ └── config_tmp.toml # 参数配置样例(TOML,11 个配置段,键名与 struct RDP_parameter 成员一致;样例为相扫取值)
|
||||
├── archived_docs/ # 过期文档留档(仅作历史参考;接口与流程以本 README 与 docs/ 专项说明为准)
|
||||
│ ├── BUG_REPORT.md # 全量逻辑 BUG 分析报告(P0~P3,37 项)
|
||||
│ └── BUG_FIX_REPORT.md # BUG 修复报告(修复明细 + 编译验证结果)
|
||||
@@ -104,17 +110,20 @@
|
||||
│ ├── data_process_class_dll.pro # 迁移前的 Qt/qmake 工程文件(备份)
|
||||
│ └── data_process_class_dll.pro.user
|
||||
├── third_party/ # 内置第三方源码(按上游原样保存,来源/版本/升级方式见其 UPSTREAM.md)
|
||||
│ └── ormpp/ # ormpp 0.2.0 + iguana 1.0.5 + frozen + SQLite 3.49.1 amalgamation(Apache-2.0 / 公有领域)
|
||||
│ ├── ormpp/ # ormpp 0.2.0 + iguana 1.0.5 + frozen + SQLite 3.49.1 amalgamation(Apache-2.0 / 公有领域)
|
||||
│ └── toml/
|
||||
│ └── toml.hpp # toml++ v3.4.0 单头文件(MIT,仅参数加载器 rdp_parameter_loader.cpp 使用)
|
||||
├── data_process_class_dll/ # 源码目录
|
||||
│ ├── data_process_class_dll.h # 对外接口:DataRev/Track/TrackingBeam/RadarPara 结构体、抽象基类(公开成员 version_info)、工厂类
|
||||
│ ├── data_process_class_dll.h # 对外接口:DataRev/Track/TrackingBeam/Target_direct_tracking 结构体、抽象基类(公开成员 version_info、虚接口 parameters_reload)、工厂类
|
||||
│ ├── data_process_class_dll_global.h # DLL 导入导出宏
|
||||
│ ├── data_process_class_dll.cpp # 工厂实现(GetB/Destroy 单例)
|
||||
│ ├── rdp_log.h / .cpp # 日志输出模块:rdbuf() 重定向 std::cout、RDP_LOG 宏(开关宏在 parameters.h)
|
||||
│ ├── rdp_record.h / .cpp # 数据记录调试模块:结构体→SQLite 表(内置 ormpp)、异步入队接口与宏(RDP_RECORD_POINTS 等 6 个)+ 遗留 RDP_RECORD/RDP_RECORD_BATCH、有界队列、异步写线程、批量事务、Stats 统计、失败降级/熔断(开关由 CMake 选项 RDP_DATA_RECORD 控制;实现以 C++20 独立编译)
|
||||
│ ├── rdp_record_entity.h # 记录实体(=数据表)定义区:6 张在用表(输入点迹、TWS/TAS 凝聚后点迹、临时航迹、可靠航迹、关联信息)+ RDP_RECORD_DECLARE 声明宏;新增需要记录的数据在此加结构体
|
||||
│ ├── data_process.h / .cpp # Data_Process 实现:接口分发、TWS/TAS 主流程、参数管理、清空/删除/转跟踪;含 point_id/frame_id/circle_id 自增计数(记录用)
|
||||
│ ├── struct.h # 内部结构体:PointRecv / Trust_Track / Temp_track / Asso_Info / Tracking_Target
|
||||
│ ├── parameters.h # 编译期宏参数(按 MECHANICAL_SCANNING / PHASE_SCANNING 分两组)
|
||||
│ ├── rdp_parameter_loader.h / .cpp # TOML 参数加载器(内部模块,不属于宿主接口):rdp_parameter_load() 严格校验 11 个配置段并写入 RDP_parameter,失败返回 -1 且不改变入参
|
||||
│ ├── data_process.h / .cpp # Data_Process 实现:接口分发、TWS/TAS 主流程、参数加载(parameters_reload)、清空/删除/转跟踪;含 point_id/frame_id/circle_id 自增计数(记录用)
|
||||
│ ├── struct.h # 内部结构体:PointRecv / Trust_Track / Temp_track / Asso_Info / Tracking_Target / RDP_parameter(工作参数,与 TOML 配置一一对应)
|
||||
│ ├── parameters.h # 编译期常量:PI、版本宏、RDP_LOG_*、RDP_RECORD_*、MAX_TRACK_NUM、RDP_TAS_QUEUE_CAPACITY、UNCONF_TARGET、Round(不再含雷达工作参数宏)
|
||||
│ ├── dot_coh.h / .cpp # TWS 点迹凝聚(含两帧滑窗缓冲)
|
||||
│ ├── dot_coh_tas.h / .cpp # TAS 点迹凝聚
|
||||
│ ├── coor_trans.h / .cpp # 极坐标 ↔ 直角坐标转换
|
||||
@@ -154,11 +163,12 @@
|
||||
| CMake | ≥ 3.21(VS2022 生成器自 3.21 起支持;本工程实际在 CMake 4.x 上验证) |
|
||||
| 生成器 | NMake Makefiles(VS Code 配置与脚本均使用该生成器) |
|
||||
| 线性代数 | 无需安装——Eigen 头文件已内置在源码目录 |
|
||||
| 参数配置 | 无需安装——配置文件为 TOML 格式,解析器 toml++ v3.4.0 已内置在 `third_party/toml/toml.hpp`(仅头文件);样例见 `config/config_tmp.toml`(见 10) |
|
||||
| 数据记录(可选调试功能) | 无需安装——ormpp / iguana / frozen / SQLite amalgamation 已内置在 `third_party/ormpp`(版本与升级方式见该目录 UPSTREAM.md);启用时记录模块以 C++20 独立编译,所需的 C 编译器由同一 VS 2022 工具集提供 |
|
||||
|
||||
### 4.2 扫描体制
|
||||
|
||||
通过 CMake 缓存变量 RADAR_SCAN_MODE 切换,对应 parameters.h 中的两组宏;每个「扫描体制 × 构建类型 × 记录模式」组合拥有独立构建目录(见 4.3):
|
||||
通过 CMake 缓存变量 RADAR_SCAN_MODE 切换,决定编译期扫描体制分支(`parameters.h` 中不再有按体制分组的雷达工作参数宏,参数统一由配置文件提供,见 10);每个「扫描体制 × 构建类型 × 记录模式」组合拥有独立构建目录(见 4.3):
|
||||
|
||||
| 变量值 | 对应宏 | 构建目录 |
|
||||
|---|---|---|
|
||||
@@ -197,7 +207,7 @@ build/<X256_PS|X256_MS>/<Debug|Release>/<REC_AUTO|REC_OFF|REC_ON>
|
||||
|
||||
数据记录调试功能由缓存变量 `RDP_DATA_RECORD` 控制(取值 `AUTO`/`ON`/`OFF`,缺省 `AUTO`:Debug 启用、Release 关闭)。显式取值时构建目录名随之不同(`REC_OFF`/`REC_ON`),因此关闭与开启记录模块的产物可以并存。启用时配置阶段打印 `数据记录调试功能:启用(C++20 静态库 rdp_record + 内置 SQLite)`,并额外构建静态库 `rdp_record`(记录模块 + SQLite,链接进 DLL);关闭时打印 `...:关闭(不编译记录模块,RDP_RECORD 为空操作)`,记录模块源码完全不参与编译。开关语义、表结构约定与新增记录点配方见 docs/data_record_debug.md。
|
||||
|
||||
记录功能的性能参数集中在 `parameters.h` 的「数据记录性能参数」段(均以 `#ifndef` 保护,便于用 `/DRDP_RECORD_...` 编译选项临时覆盖做压测):入队队列上限 `RDP_RECORD_QUEUE_CAP_BYTES`(默认 64 MB)与 `RDP_RECORD_QUEUE_MAX_BATCHES`(默认 4096 批)、写线程批提交 `RDP_RECORD_WRITE_BATCH_ROWS`(默认 4096 行)/ `RDP_RECORD_WRITE_MAX_WAIT_MS`(默认 50 ms)、关联信息分块 `RDP_RECORD_ASSO_CHUNK_ROWS`(默认 131072 行)、SQLite 缓存/忙等待/WAL 开关(`RDP_RECORD_SQLITE_CACHE_KB`=8192、`RDP_RECORD_SQLITE_BUSY_TIMEOUT_MS`=2000、`RDP_RECORD_SQLITE_WAL`=1)、连续错误熔断阈值 `RDP_RECORD_MAX_CONSECUTIVE_ERRORS`(默认 10)与丢弃日志限频 `RDP_RECORD_DROP_LOG_INTERVAL`(默认 1024 行);默认值、语义与运行统计(`rdp_record::Stats`/`stats()`)见 10.1 与 docs/data_record_debug.md 第 3 节。
|
||||
记录功能的性能参数集中在 `parameters.h` 的「数据记录性能参数」段(均以 `#ifndef` 保护,便于用 `/DRDP_RECORD_...` 编译选项临时覆盖做压测):入队队列上限 `RDP_RECORD_QUEUE_CAP_BYTES`(默认 64 MB)与 `RDP_RECORD_QUEUE_MAX_BATCHES`(默认 4096 批)、写线程批提交 `RDP_RECORD_WRITE_BATCH_ROWS`(默认 4096 行)/ `RDP_RECORD_WRITE_MAX_WAIT_MS`(默认 50 ms)、关联信息分块 `RDP_RECORD_ASSO_CHUNK_ROWS`(默认 131072 行)、SQLite 缓存/忙等待/WAL 开关(`RDP_RECORD_SQLITE_CACHE_KB`=8192、`RDP_RECORD_SQLITE_BUSY_TIMEOUT_MS`=2000、`RDP_RECORD_SQLITE_WAL`=1)、连续错误熔断阈值 `RDP_RECORD_MAX_CONSECUTIVE_ERRORS`(默认 10)与丢弃日志限频 `RDP_RECORD_DROP_LOG_INTERVAL`(默认 1024 行);默认值、语义与运行统计(`rdp_record::Stats`/`stats()`)见 10.4 与 docs/data_record_debug.md 第 3 节。
|
||||
|
||||
VS Code 任务(`.vscode/tasks.json`)只保留以下 5 个(原有任务已全部删除),可在「终端 → 运行任务」中选择,或按 `Ctrl+Shift+B` 运行默认构建任务「配置并构建全部(Release/REC_OFF)」:
|
||||
|
||||
@@ -225,6 +235,8 @@ VS Code 任务(`.vscode/tasks.json`)只保留以下 5 个(原有任务已
|
||||
|
||||
产物为 **x64(AMD64)** 目标。宿主程序必须是 64 位、且使用与本 DLL 同一版本头文件重新编译——MSVC 2013 → MSVC 2022 同时伴随 x86 → x64 的 ABI 变化,旧的 32 位宿主无法加载该 DLL。
|
||||
|
||||
> **本次参数配置改造后的体积/导出变化**:Release REC_OFF 的 `data_process_class_dll.dll` 为 331,776 字节(约 324 KB),较改造前约 178 KB 增大——原因是纯头文件的 toml++ 解析器随 `rdp_parameter_loader.cpp` 编入 DLL(Debug REC_ON 为 4,694,016 字节)。导出表(Release REC_OFF 共 31 个导出)新增 `parameters_reload`(`?parameters_reload@Data_process_class_dll@@UEAAHPEBD@Z`),`track_process_parameters_initial`/`track_process_parameters_modify` 与 `RadarPara` 相关符号已不存在,`Data_Process_Factory::GetB`/`Destroy` 等其余接口不变。
|
||||
|
||||
仓库 `build/X256_PS/<Debug|Release>/<REC_*>/bin/` 内原附带 **rdp_playback.exe**(回放测试工具,依赖 Qt5Core.dll、sqlite3.dll,配置样例 RadarConfigParam.ini,可回放 .db 记录的点迹数据验证 DLL 输出),该工具不在本仓库源码范围内、当前构建目录中亦不存在,需自行放置;由于 DLL 已改为 x64,配套的 rdp_playback.exe 必须是 **x64 版本**(同时其自带 Qt5Core.dll / sqlite3.dll 也需为 x64),32 位版本无法加载新版 DLL。
|
||||
|
||||
### 4.5 编译警告处理
|
||||
@@ -235,7 +247,7 @@ VS Code 任务(`.vscode/tasks.json`)只保留以下 5 个(原有任务已
|
||||
- MSVC 2022 在 `/W3` 下新增报出的 C4267(`size_t` → `int` 窄化)共 17 处已按同一约定修复(`track_asso.cpp`、`track_asso_tas.cpp`、`track_init.cpp`、`track_init_direct_tracking.cpp`、`tas_ctrl.cpp` 中改为 `static_cast<int>(...)`),同样不改变计算逻辑。
|
||||
- 当前 8 种显式配置(X256_PS / X256_MS × Release / Debug × REC_OFF / REC_ON)均以 **0 warning、0 error** 通过编译(REC_AUTO 与「Debug=ON / Release=OFF」等价,未重复单独验证)。
|
||||
- cl.exe 的诊断信息为中文(跟随 VS 安装语言,`VSLANG=1033` 对 VS2022 的 cl.exe 无效),构建日志中偶见 `注意: 包含文件:` 行泄漏,属 CMake 依赖扫描的显示噪声,头文件依赖(`CMakeFiles/*.dir/**/*.obj.d`)记录完整,不影响增量编译。
|
||||
- **第三方库告警例外(仅记录功能启用时)**:数据记录模块的第三方依赖按上游原样内置在 `third_party/ormpp`(保留上游字节,含换行符,不转 BOM/编码),其中 `third_party/ormpp/thirdparty/sqlite3/sqlite3.c` 以源文件级 `/w` 单独编译,`data_process_class_dll/rdp_record.cpp` 用 `#pragma warning(push, 0)` / `(pop)` 包裹第三方头;除此之外本工程源码仍按上一条的 `/W3` 零告警标准编译(X256_PS/X256_MS × Release/Debug × REC_OFF/REC_ON 八种配置实测 0 warning、0 error)。
|
||||
- **第三方库告警例外(仅记录功能启用时)**:数据记录模块的第三方依赖按上游原样内置在 `third_party/ormpp`(保留上游字节,含换行符,不转 BOM/编码),其中 `third_party/ormpp/thirdparty/sqlite3/sqlite3.c` 以源文件级 `/w` 单独编译,`data_process_class_dll/rdp_record.cpp` 用 `#pragma warning(push, 0)` / `(pop)` 包裹第三方头;参数加载器使用的 toml++ 单头文件(`third_party/toml/toml.hpp`)同样在 `rdp_parameter_loader.cpp` 中以 `#pragma warning(push, 0)` / `(pop)` 包裹。除此之外本工程源码仍按上一条的 `/W3` 零告警标准编译(X256_PS/X256_MS × Release/Debug × REC_OFF/REC_ON 八种配置实测 0 warning、0 error)。
|
||||
|
||||
### 4.6 编译器升级说明(MSVC 2013 → MSVC 2022)
|
||||
|
||||
@@ -276,15 +288,14 @@ classDiagram
|
||||
+tracking_start(int) int
|
||||
+tracking_stop(int) int
|
||||
+tracking_point(float) int
|
||||
+track_process_parameters_initial(RadarPara) int
|
||||
+track_process_parameters_modify(RadarPara) int
|
||||
+parameters_reload(const char*) int
|
||||
}
|
||||
class Data_Process {
|
||||
-Data_buffer / Data_buffer_tas : vector PointRecv
|
||||
-point_recv / point_recv_tas : vector PointRecv
|
||||
-trust_track : vector Trust_Track
|
||||
-temp_track : vector Temp_track
|
||||
-Work_Parameter : RadarPara
|
||||
-rdp_parameter : RDP_parameter
|
||||
-dot_coh / dot_coh_tas / track_asso / track_asso_tas
|
||||
-track_init / track_die / track_die_tas / tas_ctrl
|
||||
}
|
||||
@@ -326,15 +337,16 @@ classDiagram
|
||||
|
||||
| 类 / 文件 | 职责 |
|
||||
|---|---|
|
||||
| Data_Process(data_process.cpp) | 对外接口实现与流程编排:输入缓存、按 model 分发 TWS/TAS 流水线、参数管理、航迹清空/删除、手动转/停 TAS、手动标记/取消重点目标(primary_target_add/cancel)、波束控制转发 |
|
||||
| Data_Process(data_process.cpp) | 对外接口实现与流程编排:输入缓存、按 model 分发 TWS/TAS 流水线、参数加载(`parameters_reload`:TOML → `rdp_parameter`)、航迹清空/删除、手动转/停 TAS、手动标记/取消重点目标(primary_target_add/cancel)、波束控制转发 |
|
||||
| rdp_parameter_loader(rdp_parameter_loader.h/.cpp) | 参数加载器(内部模块):解析 TOML 并严格校验 11 个配置段/键集合/类型/取值范围/屏蔽区数组长度,全部通过后一次性写入 `RDP_parameter`;失败返回 -1 并给出首条原因、不改变入参(见 10.1) |
|
||||
| Dot_Coh | TWS 点迹凝聚:两帧滑窗合并凝聚(dot_coh_process_buff) |
|
||||
| Dot_Coh_TAS | TAS 点迹凝聚(单帧,距离/方位/速度三维门限) |
|
||||
| Track_Asso | TWS 点航关联 + IMM 滤波:模型交互 → 门限筛选(NN)→ 各模型 EKF → 概率更新 → 模型输出(只输出 `Track_Update_Flag==1` 的航迹);未关联航迹按上限外推;高度平滑;按滤波速度矢量写 `Flag_Direction`;ΔT≤0(乱序/重复时间戳)的候选取出后不更新滤波、不刷新航迹,仅消费该点迹 |
|
||||
| Track_Asso_Tas | TAS 航迹关联(只处理指定批号 tas_track_idx,门限与 TWS 略有差异;输出同样受 `Track_Update_Flag` 门控,外推上限 `MAX_EXTRAPOLATE_NUM_TAS` 的判定方向与 TWS 相反) |
|
||||
| Track_Asso_Tas | TAS 航迹关联(只处理指定批号 tas_track_idx,门限与 TWS 略有差异;输出同样受 `Track_Update_Flag` 门控,外推上限 `extrapolate_max_tas` 的判定方向与 TWS 相反) |
|
||||
| Track_Init | 逻辑法航迹起始,每圈顺序固定:点-临时航迹关联(延长 ≥2 点临时航迹)→ 点-航迹头关联(两点起始)→ 剩余点转新航迹头 → 长度达标者起批(三点卡尔曼初始化);另含起批屏蔽区判断与临时航迹消亡 |
|
||||
| Track_Die / Track_Die_Tas | 可靠航迹消亡:**TWS 普通航迹按航迹质量**(V1.5.8:`Quality < 0.3`)、TWS 重点航迹按未关联轮数(10)、TAS 按未关联轮数(相扫 20、机扫 96),或手动删除标志置位时输出消亡批号并删除 |
|
||||
| TAS_Ctrl | TAS 波束控制:手动目标入队(tas_target_add)、失效目标出队(tas_target_del)、跟踪波束预测输出(tas_beam_output:数据率门控 + 队内选择最早 CPI 目标)、调度时刻记录(last_track_time) |
|
||||
| Track_Ind_Mangement | 航迹号 1~500 的顺序分配与回绕复用 |
|
||||
| Track_Ind_Mangement | 航迹号 1~`track_index_max`(默认 500)的顺序分配与回绕复用 |
|
||||
| kalman | 两点/三点滤波初始化、线性卡尔曼预测/滤波、EKF(3 维量测含多普勒)、马氏统计距离 d、盲速 Bind_speed |
|
||||
| coor_trans | 极坐标 ↔ 直角坐标转换 |
|
||||
| rdp_log(rdp_log.h/.cpp) | 日志输出:按 parameters.h 的 `RDP_LOG_ENABLE` 开关,在 `Init()` 中用 `rdbuf()` 将 `std::cout` 重定向到日期+时间命名的日志文件、`Shutdown()` 恢复控制台并关闭;提供 `RDP_LOG` 宏自动附加 `[时间戳][文件:行号]` 前缀 |
|
||||
@@ -353,12 +365,14 @@ classDiagram
|
||||
| Track | 输出航迹 | 位置/速度、极坐标量、**航迹质量 `Quality`(0~100,V1.5.8 新增;插在 `Amplitude` 之后,宿主需同步重编译)**、航迹号、更新点数、方向标志 Flag_Direction(仅关联/外推输出路径赋值:按滤波速度矢量判定,1 靠近 / 0 远离,见 9.6;起批输出不写该字段)、实虚点标志 Flag_Point、一步预测、跟踪模式、航向角 Direction_Angle、目标类型、关联点信息、归一化幅度 `amp_asv_point` / 速度维 FFT 点数 `prn_fftn_point`、航迹时间(s)/GNSS、SNR/RCS、十字星数据透传 |
|
||||
| TrackingBeam | TAS 跟踪波束指令 | open_flag、type、Range、Azi、Elev、TAS_track_index |
|
||||
| Target_direct_tracking | 引导跟踪信息 | 批号、距离/方位/俯仰、开关(接口未启用) |
|
||||
| RadarPara | 工作参数 | 见 [参数说明](#10-参数说明) |
|
||||
|
||||
> 工作参数结构体 `RDP_parameter` 定义在内部头 `struct.h`,**不在公开头中导出**:宿主不再构造/下发参数结构体,改为通过 `parameters_reload(配置文件路径)` 把 TOML 配置加载进 DLL(参数含义见 10)。
|
||||
|
||||
### 6.2 内部结构体(struct.h)
|
||||
|
||||
| 结构体 | 用途 | 关键字段 |
|
||||
|---|---|---|
|
||||
| RDP_parameter | 工作参数(唯一来源) | 按注释分组:雷达工作参数(`radar_work_mode`/`freq_0_GHz`/`radar_height`/`radar_system_delay`/`data_rate_tws`/`data_rate_tas`/`data_rate_track_only`)、点迹凝聚参数、航迹关联参数(`sigma_*`/`associate_threshold`/`extrapolate_max(_tas)`/`height_smooth_win`)、航迹起始参数(`track_init_*`)、航迹消亡参数(`track_die_*`)、航迹质量参数(`track_quality_*`)、航迹批号(`track_index_max`)、波束调度(`default_track_beam_type`/`tas_track_num_max`)、singer 模型参数(`model_*_Q_*`)、两个屏蔽区(各 30 组距离/方位区间);与 TOML 配置段/键一一对应(见 10.2) |
|
||||
| PointRecv | 内部点迹 | 距离/方位(弧度)/速度/幅度/高度/SNR/RCS/CPI/GNSS 时间、点迹号 `point_id` / 帧号 `frame_id` / 圈号 `circle_id`(记录链路用,c3c7e66 新增)、使用标志、波位/PRF/频点、跟踪模式、点迹区号、归一化幅度 `amp_asv_point` / 速度维 FFT 点数 `prn_fftn_point`、十字星数据 |
|
||||
| Trust_Track | 可靠航迹(IMM 全状态) | 航迹号、标识字段 `track_id` / `point_id` / `circle_id`(b227ae1 新增,起批时取临时航迹末点的 `temp_track_id`/`point_id`/`circle_id`,关联成功时刷新为被关联点迹的 ID)、高度、极坐标量快照 `Range` / `Azimuth` / `Elevation` / `Velocity`(b227ae1 新增,仅在本圈有输出时回填)、**航迹质量相关状态(V1.5.8 新增)`Quality`(0~1)/ `Point_sum`(累计点数,含外推)/ `Valid_point_sum`(累计实点关联次数)/ `Unupdated_time`(连续未关联实点的累计毫秒)**、航迹时间 `T_track`(CPI 时间)/ `GNSS_time`、更新标志 Track_Update_Flag(**输出门控**:仅置 1 的航迹当圈被输出,输出后清零)、外推轮数 Extrapolate_round、实虚点标志、手动删除/手动 TAS/重点目标(primary_target_flag,决定消亡走质量还是 10 圈轮数)标志、关联点数、高度平滑缓冲 Hight_smooth、关联点信息、归一化幅度 `amp_asv_point` / 速度维 FFT 点数 `prn_fftn_point`、融合状态 X[6]/P[6][6]、三个子模型 X1/X2/X3、P1/P2/P3、模型概率 u[3] |
|
||||
| Temp_track | 临时航迹(航迹起始阶段) | 4 维状态 X[4]/P[4][4]、极坐标点信息(`r`/`azi`/`vr`/`height`/`SNR`/`RCS`/`amplitude`)、**航迹质量 `quality`(0~1,V1.5.8 新增,起批时带给 Trust_Track)**、时间戳 `CPI_time` / `GNSS_time`、`buff_round`(已缓存圈数)、标识字段 `temp_track_id` / `point_id` / `circle_id`、区号 `temp_track_section_idx`、俯仰波位号 `pitch_num`、关联标志(c3c7e66 起字段统一更名,如原 `snr`/`Amp`/`T`) |
|
||||
@@ -368,7 +382,7 @@ classDiagram
|
||||
|
||||
> 说明:Trust_Track 的状态布局为 [x, vx, ax, y, vy, ay],x/y 两轴解耦,每轴为「位置-速度-加速度」。
|
||||
>
|
||||
> ⚠ **结构体布局变更**:`DataRev` 新增 `amp_asv` / `prn_fftn`,`Track` 新增 `amp_asv_point` / `prn_fftn_point`;**V1.5.8 起 `Track` 又在 `Amplitude` 之后插入 `Quality`(之后字段整体后移)**,内部 `Trust_Track` / `Temp_track` 新增质量相关字段。宿主与 DLL 必须使用同一版本头文件并**同步重新编译**,否则字段错位。对外结构体中 `DataRev` / `RadarPara` 布局自 1.5.5 起未变化。
|
||||
> ⚠ **结构体布局变更**:`DataRev` 新增 `amp_asv` / `prn_fftn`,`Track` 新增 `amp_asv_point` / `prn_fftn_point`;**V1.5.8 起 `Track` 又在 `Amplitude` 之后插入 `Quality`(之后字段整体后移)**,内部 `Trust_Track` / `Temp_track` 新增质量相关字段。宿主与 DLL 必须使用同一版本头文件并**同步重新编译**,否则字段错位。对外结构体中 `DataRev` 布局自 1.5.5 起未变化;原对外 `RadarPara` 已删除,`RDP_parameter` 仅在内部使用(宿主无需包含其定义)。
|
||||
|
||||
---
|
||||
|
||||
@@ -381,8 +395,7 @@ classDiagram
|
||||
| Data_Process_Factory::GetB() | 实例指针 | 获取单例(首次调用时创建,并写入 `version_info`) |
|
||||
| Data_Process_Factory::Destroy() | — | 销毁实例并将指针置空 |
|
||||
| version_info(基类公开成员) | `const char*` | DLL 版本字符串,由 `GetB()` 赋值为 `VERSION_STRING`(相扫 `V1.5.8.PS` / 机扫 `V1.5.8.MS`);只读使用,不要释放(b227ae1) |
|
||||
| track_process_parameters_initial(RadarPara) | 0 成功 / -1 失败 | 初始化工作参数;校验 3 ≤ track_start_point_num ≤ 9 |
|
||||
| track_process_parameters_modify(RadarPara) | 0 / -1 | 运行时修改参数(校验同上) |
|
||||
| parameters_reload(const char* path) | 0 成功 / -1 失败 | **加载全部工作参数**:解析 TOML 配置文件并校验,成功后整体替换内部 `RDP_parameter`;失败时当前参数保持不变并打印原因。建议 `GetB()` 后立即调用一次,重连/换配置时再次调用即可热重载(见 10.1) |
|
||||
| data_preprocess(DataRev[150]) | 0/1/2/3 | 输入一帧点迹并缓存,返回处理模式(见下表) |
|
||||
| track_process(Track[][10], &num, dieIdx[], &dieNum, model) | 1 | 按 data_preprocess 的返回值执行 TWS/TAS 处理 |
|
||||
| Beam_Ctrl(TrackingBeam*, Track[][10], &num) | — | TAS 波束控制(内部清零并重填航迹输出计数) |
|
||||
@@ -397,7 +410,7 @@ classDiagram
|
||||
|
||||
> **输出语义(50e0b5c 起)**:`track_process(model=1/3)` 只把本圈**有更新**的航迹写入 `Trust_Track_Output`(关联到实点、外推成功,或 TAS 指定批号被更新),行数由 `Trust_track_num_Output` 给出;未更新(含外推超限)的航迹当圈不出现在输出中,宿主不能据输出行数推断航迹总数。输出数组内容由 DLL 覆盖写入指定行,其余行保持宿主原值(DLL 不清零整块数组)。
|
||||
|
||||
> 日志输出为 DLL 内部功能(不对外导出,接口见 10.1 的宏开关):首次 `GetB()` 创建实例时自动初始化(`RDP_LOG_ENABLE` 定义则将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件,该宏当前为注释状态,见 10.1),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭日志文件。
|
||||
> 日志输出为 DLL 内部功能(不对外导出,接口见 10.4 的宏开关):首次 `GetB()` 创建实例时自动初始化(`RDP_LOG_ENABLE` 定义则将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件,该宏当前为注释状态,见 10.4),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭日志文件。
|
||||
|
||||
> 数据记录同样为 DLL 内部功能(不对外导出):记录功能启用时,`data_preprocess` 在帧内收集输入点迹(判圈 return 之前入队)、`dot_coh`/`dot_coh_tas` 收集凝聚后点迹、`track_init` 收集临时航迹、`track_asso`/`track_asso_tas` 收集可靠航迹与关联信息(关联信息按 `RDP_RECORD_ASSO_CHUNK_ROWS` 分块、函数返回前 flush),收集完成后各调用一次**异步入队**宏;实体构造与写库由记录写线程异步完成,落盘为 `.\rdp_records\rdp_record_YYYYMMDD_HHMMSS.db` 的 6 张在用表(表清单与字段见 docs/data_record_debug.md 第 7 节)。处理线程不会被记录 I/O 阻塞,但队列满时记录会整批丢弃(`stats().dropped_rows` 可查),进程异常退出时队列尾部与 WAL 尾部可能丢失。
|
||||
|
||||
@@ -416,7 +429,7 @@ classDiagram
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["Data_Process_Factory::GetB()"] --> B["track_process_parameters_initial(RadarPara)"]
|
||||
A["Data_Process_Factory::GetB()"] --> B["parameters_reload(参数配置文件路径)"]
|
||||
B --> C{"接收一帧点迹"}
|
||||
C --> D["model = data_preprocess(DataRev[150])"]
|
||||
D --> E{"model ?"}
|
||||
@@ -454,7 +467,7 @@ flowchart TD
|
||||
|
||||
其中 model=2(未满一圈)只执行滑窗凝聚 dot_coh_process_buff,把上一帧凝聚完成的点输出到 point_recv 供下一圈的关联/起批使用,当前帧未凝聚点作为下一次的缓存——即「凝聚在前、关联在后、缓存一帧」的两帧滑窗结构。
|
||||
|
||||
输出环节(`track_asso_process()` 末尾)只写 `Track_Update_Flag==1` 的航迹:关联到实点、或外推成功(`Extrapolate_round <= MAX_EXTRAPOLATE_NUM` = 3)时置 1;外推超限的航迹不再推进位置与航迹时间,当圈也不输出,直到重新关联上实点才恢复(详见 9.9)。
|
||||
输出环节(`track_asso_process()` 末尾)只写 `Track_Update_Flag==1` 的航迹:关联到实点、或外推成功(`Extrapolate_round <= extrapolate_max`,样例 3)时置 1;外推超限的航迹不再推进位置与航迹时间,当圈也不输出,直到重新关联上实点才恢复(详见 9.9)。
|
||||
|
||||
### 8.2 TAS 流程(model=3)
|
||||
|
||||
@@ -469,7 +482,7 @@ flowchart TD
|
||||
G --> H["Beam_Ctrl: 数据率门控 + 队内最早 CPI 目标 + 预测波束输出"]
|
||||
```
|
||||
|
||||
TAS 输出同样受 `Track_Update_Flag` 门控:只有 `Track_Index == tas_track_idx` 且本圈有更新(关联到实点、或外推成功)时才向 `Trust_Track_Output` 写一行(`point_type=1`)。TAS 外推上限为 `MAX_EXTRAPOLATE_NUM_TAS`(相扫 5 / 机扫 48),判定方向与 TWS 相反(`Extrapolate_round >= 上限` 时才外推并输出),详见 9.9 与 11.2。
|
||||
TAS 输出同样受 `Track_Update_Flag` 门控:只有 `Track_Index == tas_track_idx` 且本圈有更新(关联到实点、或外推成功)时才向 `Trust_Track_Output` 写一行(`point_type=1`)。TAS 外推上限为配置项 `extrapolate_max_tas`(样例配置为相扫 5,机扫需配 48),判定方向与 TWS 相反(`Extrapolate_round >= 上限` 时才外推并输出),详见 9.9 与 11.2。
|
||||
|
||||
### 8.3 航迹起始流程(逻辑法)
|
||||
|
||||
@@ -477,12 +490,12 @@ TAS 输出同样受 `Track_Update_Flag` 门控:只有 `Track_Index == tas_trac
|
||||
flowchart TD
|
||||
A["本圈未关联点迹(point_recv → point_process)"] --> A1["临时航迹所有点的 buff_round +1"]
|
||||
A1 --> B["① 点-临时航迹关联(≥2 点临时航迹、末点 buff_round≥2):EKF 统计距离波门 d²≤64、折角<60°、径向速度同号、|Δr|≥5m → 复制原航迹并追加新点"]
|
||||
B --> C["② 点-航迹头关联(1 点航迹头、buff_round≥2):V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT、径向速度同号、dis ≥ |vr_point|·ΔT 且 dis ≥ |v_track_head|·ΔT → 两点卡尔曼初始化(航迹头 + 新点 → 2 点临时航迹)"]
|
||||
B --> C["② 点-航迹头关联(1 点航迹头、buff_round≥2):track_init_velocity_min·ΔT ≤ dis ≤ track_init_velocity_max·ΔT、径向速度同号、dis ≥ |vr_point|·ΔT 且 dis ≥ |v_track_head|·ΔT → 两点卡尔曼初始化(航迹头 + 新点 → 2 点临时航迹)"]
|
||||
C --> D["③ 删除 Use_Flag==1 的点迹,剩余点全部转为新航迹头 Temp_track(1 点、buff_round=1)"]
|
||||
D --> E{"④ 长度 ≥ track_start_point_num(3~9) 且 ≤10 且不在起批屏蔽区 ?"}
|
||||
D --> E{"④ 长度 ≥ track_init_point_num_min(3~9) 且 ≤10 且不在起批屏蔽区 ?"}
|
||||
E -- 是 --> F["重复航迹去重(保留末点 d 更小者)→ 三点卡尔曼初始化"]
|
||||
F --> G["转可靠航迹:分配航迹号 1~500,IMM 三模型同状态初始化 u=1/3"]
|
||||
G --> H["输出前 track_start_point_num 个历史点 Trust_Track_Output[i][0..L-1]"]
|
||||
F --> G["转可靠航迹:分配航迹号 1~track_index_max(默认 500),IMM 三模型同状态初始化 u=1/3"]
|
||||
G --> H["输出前 track_init_point_num_min 个历史点 Trust_Track_Output[i][0..L-1]"]
|
||||
E -- 否 --> I["留在临时航迹表,下一圈继续关联"]
|
||||
H --> J["⑤ 临时航迹消亡:末点 buff_round>2 或 长度≥10"]
|
||||
I --> J
|
||||
@@ -522,7 +535,7 @@ $$
|
||||
R_{xy}=\left(\lambda_\theta^{-2}-2\right)r^2\sin\theta\cos\theta+\tfrac12\left(r^2+\sigma_r^2\right)\lambda'_\theta\sin 2\theta
|
||||
$$
|
||||
|
||||
其中 σr、σa 由 SIGMA_R / SIGMA_A 宏给定(10 m / 0.02 rad)。
|
||||
其中 σr、σa 取自配置项 `sigma_range` / `sigma_azimuth`(默认 10 m / 0.02 rad,见 10.2)。
|
||||
|
||||
### 9.2 点迹凝聚
|
||||
|
||||
@@ -530,8 +543,8 @@ $$
|
||||
|
||||
| 模式 | 距离门限 | 方位门限(环绕最小角差) | 速度门限 |
|
||||
|---|---|---|---|
|
||||
| work_mode==0(近程) | ≤ 80 m(DOT_COH_RANGE) | ≤ 6°(DOT_COH_AZI) | ≤ 2 m/s(DOT_COH_V) |
|
||||
| work_mode!=0(中/远程) | ≤ 80 m | ≤ 6° | 不约束 |
|
||||
| radar_work_mode==0(近程) | ≤ 80 m(point_cohesion_range) | ≤ 6°(point_cohesion_azimuth) | ≤ 2 m/s(point_cohesion_velocity) |
|
||||
| radar_work_mode!=0(中/远程) | ≤ 80 m | ≤ 6° | 不约束 |
|
||||
|
||||
方位差按环绕角处理:
|
||||
|
||||
@@ -539,7 +552,7 @@ $$
|
||||
\Delta\theta = \min\left(|\theta_0-\theta_1|,\ 2\pi-|\theta_0-\theta_1|\right)
|
||||
$$
|
||||
|
||||
凝聚后删除 r<100 m 或 r>100000 m 的量程外点。TWS 的 dot_coh_process_buff 将上一帧缓存点标记区号 1、当前帧点标记区号 2,合并凝聚后:区号 1 的点输出到 point_recv(进入关联),区号 2 的点存入 data_input_buff 供下一帧凝聚——形成两帧滑窗。
|
||||
凝聚后删除量程外的点(下限/上限由配置项 `point_cohesion_range_min` / `point_cohesion_range_max` 给定,默认 100 m / 100000 m)。TWS 的 dot_coh_process_buff 将上一帧缓存点标记区号 1、当前帧点标记区号 2,合并凝聚后:区号 1 的点输出到 point_recv(进入关联),区号 2 的点存入 data_input_buff 供下一帧凝聚——形成两帧滑窗。
|
||||
|
||||
### 9.3 航迹起始(逻辑法)
|
||||
|
||||
@@ -557,8 +570,8 @@ $$
|
||||
对 ≥ 2 点的临时航迹(末点 buff_round ≥ 2),用航迹末点状态做 EKF 一步预测,计算三维(距离-方位-径向速度)马氏统计距离 d(见 9.5.3),并要求 |r_point − r_temp| ≥ 5 m,同时满足:
|
||||
|
||||
$$
|
||||
d^2 \le TRACK\_START\_THRESHOLD^2 = 64,\qquad
|
||||
\alpha < ALPHA\_START = 60°,\qquad
|
||||
d^2 \le track\_init\_threshold^2 = 64,\qquad
|
||||
\alpha < track\_init\_angle = 60°,\qquad
|
||||
v_{r,point}\cdot v_{r,track}>0
|
||||
$$
|
||||
|
||||
@@ -581,9 +594,9 @@ d_{12} \ge \left|v_{r,point}\right|\Delta T,\qquad
|
||||
d_{12} \ge \left|v_{r,head}\right|\Delta T
|
||||
$$
|
||||
|
||||
其中 d12 为两点间平面距离,ΔT 为时间差(秒),V_min/V_max 取 `RadarPara.V_MIN / V_MAX`(代码中 vmax 的近程/中远程分支取值相同,均为 V_MAX)。后两条为**多普勒-几何一致性约束**:切向飞行目标在 ΔT 内的实际位移远小于 |vr|·ΔT,据此被排除,用于限制切向高速点迹起批出的短航迹。
|
||||
其中 d12 为两点间平面距离,ΔT 为时间差(秒),V_min/V_max 取配置项 `track_init_velocity_min` / `track_init_velocity_max`(默认 1 / 50 m/s;代码中 vmax 的近程/中远程分支取值相同)。后两条为**多普勒-几何一致性约束**:切向飞行目标在 ΔT 内的实际位移远小于 |vr|·ΔT,据此被排除,用于限制切向高速点迹起批出的短航迹。
|
||||
|
||||
> 历史行为(已注释停用):原先按扫描体制与距离分段收紧速度门限——近程 r≥1000 用全速、r<1000 用半速;中远程 r≥2000 用全速、r<2000 取下限 1/2·V_min·ΔT / 上限 1/3·V_max·ΔT。
|
||||
> 历史行为(已注释停用):原先按扫描体制与距离分段收紧速度门限——近程 r≥1000 用全速、r<1000 用半速;中远程 r≥2000 用全速、r<2000 取下限 1/2·最小速度·ΔT / 上限 1/3·最大速度·ΔT。
|
||||
|
||||
关联成功用 **两点卡尔曼初始化**(见 9.5.2)生成 2 点临时航迹:第 1 点为原航迹头,第 2 点为该点迹。
|
||||
|
||||
@@ -593,7 +606,7 @@ $$
|
||||
|
||||
#### (4) 转可靠航迹(起批)
|
||||
|
||||
临时航迹长度 N 满足 track_start_point_num ≤ N ≤ 10(起批点数由参数给定,典型 3~4,接口层校验 3~9)、不在起批屏蔽区内(屏蔽区为距离-方位矩形区域,最多 30 个)**且航迹质量达到该长度对应的门限**(V1.5.8 新增,见 9.11)时进入起批队列;同源(前三点时间/距离相同)的重复航迹只保留 d 较小者(比较下标 `track_start_point_num-1` 处的 d)。起批时令 L = track_start_point_num,用航迹中下标 L-3、L-2、L-1 的三个点做 **三点卡尔曼初始化** 得到 6 维状态(航迹长度恰为 L 时即最近三点),构造 Trust_Track:
|
||||
临时航迹长度 N 满足 track_init_point_num_min ≤ N ≤ 10(起批点数由配置项 `track_init_point_num_min` 给定,默认 3,加载器校验 3~9)、不在起批屏蔽区内(屏蔽区为距离-方位矩形区域,最多 30 个)**且航迹质量达到该长度对应的门限**(V1.5.8 新增,见 9.11)时进入起批队列;同源(前三点时间/距离相同)的重复航迹只保留 d 较小者(比较下标 `track_init_point_num_min-1` 处的 d)。起批时令 L = track_init_point_num_min,用航迹中下标 L-3、L-2、L-1 的三个点做 **三点卡尔曼初始化** 得到 6 维状态(航迹长度恰为 L 时即最近三点),构造 Trust_Track:
|
||||
|
||||
- 三个 IMM 子模型用同一状态/协方差初始化,模型概率 u = [1/3, 1/3, 1/3];
|
||||
- 质量状态初始化:`Quality` = 临时航迹末点质量,`Point_sum` = `Valid_point_sum` = L,`Unupdated_time` = 0;
|
||||
@@ -682,11 +695,11 @@ $$
|
||||
|
||||
| 模型 | TWS 版(track_asso.cpp) | TAS 版(track_asso_tas.cpp) |
|
||||
|---|---|---|
|
||||
| 模型 1(低机动) | q1 = Model1_Q_slow(恒定) | 同左 |
|
||||
| 模型 1(低机动) | q1 = model_1_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 字段当前未参与计算(模型 1 只用 slow);各档系数以源码为准。
|
||||
> 注意:`model_1_Q_fast` 配置项当前未参与计算(模型 1 只用 slow);各档系数以源码为准,对应配置文件 `[singer_model]` 段(见 10.2)。
|
||||
|
||||
#### 9.5.2 两点初始化(临时航迹,4 维)
|
||||
|
||||
@@ -787,7 +800,7 @@ $$
|
||||
|
||||
#### TWS(track_asso.cpp)
|
||||
|
||||
对每条航迹 × 每个点计算三模型 d1, d2, d3,波门按航迹距离分段(`ASSO_THORD = 4`,判据写作 `d*d < ASSO_THORD*ASSO_THORD`,而 d 本身已是马氏距离平方,故下表的等价门限为 d < 4):
|
||||
对每条航迹 × 每个点计算三模型 d1, d2, d3,波门按航迹距离分段(`associate_threshold = 4`,配置项 `[track_associate] associate_threshold`,判据写作 `d*d < associate_threshold*associate_threshold`,而 d 本身已是马氏距离平方,故下表的等价门限为 d < 4):
|
||||
|
||||
| 航迹距离 | 模型 1/2 门限 | 模型 3 门限 |
|
||||
|---|---|---|
|
||||
@@ -816,7 +829,7 @@ D=\operatorname{atan2}(X[4],X[1])\ \text{(速度方向角)},\qquad
|
||||
Flag\_Direction=\begin{cases}1,& \arccos\!\left(\dfrac{-x\cos D-y\sin D}{r}\right)<\dfrac{\pi}{2}\\[2mm]0,&\text{否则}\end{cases}
|
||||
$$
|
||||
|
||||
其中 x、y 为经 `Sys_delay` 补偿后的航迹位置,r = √(x²+y²),`Direction_Angle` 输出字段即 D 的度数形式。该判据等价于「目标速度方向与『雷达→目标』位置方向的夹角大于 90°」,即**滤波速度矢量的径向分量指向雷达(径向接近)时判为靠近(1)**,否则远离(0);中间版本曾按合速度归一化并把门限取为 π/4(45° 锥角),42b598e 去掉归一化并放宽到 π/2。外推(虚点)输出同样按该判据写 `Flag_Direction`。
|
||||
其中 x、y 为经 `radar_system_delay` 补偿后的航迹位置,r = √(x²+y²),`Direction_Angle` 输出字段即 D 的度数形式。该判据等价于「目标速度方向与『雷达→目标』位置方向的夹角大于 90°」,即**滤波速度矢量的径向分量指向雷达(径向接近)时判为靠近(1)**,否则远离(0);中间版本曾按合速度归一化并把门限取为 π/4(45° 锥角),42b598e 去掉归一化并放宽到 π/2。外推(虚点)输出同样按该判据写 `Flag_Direction`。
|
||||
|
||||
**ΔT ≤ 0 的候选(604f158 起的行为)**:ΔT = (点迹 CPI 时间 − 航迹 T_track)/1000,ΔT ≤ 0 表示该点迹时间戳不晚于航迹时间(典型场景:航迹已按 TAS 点迹更新过,本圈 TWS 点迹的时间戳更早,或存在重复时间戳)。这种候选**不再被剔除**,仍参与最近邻最小 d 的比较与选取,但选中后:
|
||||
|
||||
@@ -833,22 +846,22 @@ $$
|
||||
只处理 tas_track_idx 指定的航迹,只有统计距离与高度两个门限,两者同时通过(`thd_d && thd_h`)才记为候选:
|
||||
|
||||
$$
|
||||
thd\_d:\ d_i^2 < ASSO\_THORD^2 = 16 \qquad\text{且}\qquad thd\_h:\ \text{高度门限通过}
|
||||
thd\_d:\ d_i^2 < associate\_threshold^2 = 16 \qquad\text{且}\qquad thd\_h:\ \text{高度门限通过}
|
||||
$$
|
||||
|
||||
高度门限按航迹距离分段:≤1000 m:|h_t−h_p| ≤ 150 m;1000~2000 m:≤ 200 m;2000~3000 m:≤ 250 m;3000~5000 m:≤ 300 m;>5000 m:不约束(与 TWS 通道 9.6 的取值相同)。
|
||||
|
||||
输出航迹同样写入**方向标志** `Flag_Direction`,判据与 TWS 通道完全一致(fc49707/42b598e:按 IMM 融合速度矢量与位置矢量的夹角判定,见上文 TWS 小节)。
|
||||
|
||||
> ⚠ **波门语义提示**:代码中 d 本身已是马氏距离平方,但门限写作 d*d < ASSO_THORD*ASSO_THORD,等效于 d < 4(而非 √d < 4)。该行为在 BUG-20 中被识别,**有意保留未改**;ASSO_THORD/1000、/100 已改为浮点除法(BUG-21 修复)。TAS 不区分距离段,`ASSO_THORD` 原值直接作为门限。
|
||||
> ⚠ **波门语义提示**:代码中 d 本身已是马氏距离平方,但门限写作 d*d < associate_threshold*associate_threshold,`associate_threshold = 4` 时等效于 d < 4(而非 √d < 4)。该行为在 BUG-20 中被识别,**有意保留未改**;associate_threshold/1000、/100 已改为浮点除法(BUG-21 修复)。TAS 不区分距离段,`associate_threshold` 原值直接作为门限。
|
||||
|
||||
### 9.7 高度平滑
|
||||
|
||||
关联实点后,点迹高度入滑窗 Hight_smooth,输出高度取最近 w 个值的平均(不足 w 个则全平均)。
|
||||
|
||||
按距离分段取窗长 `w`,通道与体制不同(`H_F_WIN_LEN` 相扫 6 / 机扫 3;TAS 的偏移段于 6284f3f 由 +2/+3/+4/+6 改为 +6/+8/+10/+12):
|
||||
按距离分段取窗长 `w`,通道与体制不同(窗长基数取配置项 `height_smooth_win`,样例配置为相扫 6;机扫需配 3。TAS 的偏移段于 6284f3f 由 +2/+3/+4/+6 改为 +6/+8/+10/+12):
|
||||
|
||||
| 距离段 | TWS(`H_F_WIN_LEN + 2/3/4/6`) | TAS(`H_F_WIN_LEN + 6/8/10/12`) |
|
||||
| 距离段 | TWS(`height_smooth_win + 2/3/4/6`) | TAS(`height_smooth_win + 6/8/10/12`) |
|
||||
|---|---|---|
|
||||
| r ≤ 1000 m | 8 / 5 | 12 / 9 |
|
||||
| 1000 < r ≤ 2000 m | 9 / 6 | 14 / 11 |
|
||||
@@ -872,33 +885,33 @@ $$
|
||||
|
||||
#### 外推
|
||||
|
||||
- **TWS**:未关联到实点的航迹(`manual_tracking_flag==0`)按工作模式数据率 DATA_RATE_SHORT/MIDDLE/FAR 预测一帧(ΔT≤0 的候选虽被消费但未刷新航迹状态,其 `point_flag` 仍为 0,故同样在本环节外推,见 9.6)。预测用各子模型 Singer F/Qj 推进,`Extrapolate_round+1`,`point_flag=0`(虚点/补点),航迹时间同步推进,并置 `Track_Update_Flag=1`(本圈输出)。
|
||||
- **TWS**:未关联到实点的航迹(`manual_tracking_flag==0`)按 TWS 数据率配置项 `data_rate_tws` 预测一帧(原实现按 `radar_work_mode` 在 `DATA_RATE_SHORT/MIDDLE/FAR` 三档中三选一,新结构体统一为单字段 `data_rate_tws`,见 10.3;ΔT≤0 的候选虽被消费但未刷新航迹状态,其 `point_flag` 仍为 0,故同样在本环节外推,见 9.6)。预测用各子模型 Singer F/Qj 推进,`Extrapolate_round+1`,`point_flag=0`(虚点/补点),航迹时间同步推进,并置 `Track_Update_Flag=1`(本圈输出)。
|
||||
> **时间差取值(56b5449 → b4aed8a)**:56b5449 曾把 TWS 外推的 ΔT 改为按最新时间戳实时计算(`(latest_timestamp − T_track)/1000`,并对 `ΔT ≤ 0` 跳过),b4aed8a 已**回调**为按 `work_mode` 取固定数据率(原因:按最新时间戳推进会在 TAS 工作时同步外推出 TWS 航迹)。因此 `track_asso_process()` 与 `model_filter()` 虽接收 `latest_timestamp` 形参,但当前不参与 ΔT 计算(实时计算的两行代码以注释保留在源码中)。
|
||||
- **TWS 外推上限(50e0b5c)**:仅当 `Extrapolate_round <= MAX_EXTRAPOLATE_NUM`(=3)时才做上述预测;超过上限后不再推进位置与航迹时间,只执行 `Extrapolate_round+1`、`point_flag=0`、`Track_Update_Flag=0`——该航迹当圈不输出、位置冻结在最后一次预测处,直到重新关联到实点(`Extrapolate_round` 清零)才恢复。目的是避免长时间未关联的航迹被反复外推到远处。
|
||||
- **TWS 外推上限(50e0b5c)**:仅当 `Extrapolate_round <= extrapolate_max`(默认 3)时才做上述预测;超过上限后不再推进位置与航迹时间,只执行 `Extrapolate_round+1`、`point_flag=0`、`Track_Update_Flag=0`——该航迹当圈不输出、位置冻结在最后一次预测处,直到重新关联到实点(`Extrapolate_round` 清零)才恢复。目的是避免长时间未关联的航迹被反复外推到远处。
|
||||
- **质量衰减(V1.5.8)**:本环节对每条未关联航迹(不论是否已超外推上限)按未更新时长降低 `Quality`,`Unupdated_time` 同步累加,因此**超限后航迹虽不再输出,质量仍在下降**,直到触发质量消亡(见下)。因为 ΔT 已移到上限判断之外计算,这一衰减在上限内/外都按 `work_mode` 的数据率推进。
|
||||
- **TAS**:按 `latest_timestamp − T_track` 预测(ΔT ≤ 0 跳过),航迹时间推进到最新时间戳。上限判定为 `Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS`(相扫 5 / 机扫 48)时才预测并输出,未达该轮数时只累加 `Extrapolate_round` 且不输出(位置与航迹时间停在最后一次关联处)。⚠ 判定方向与 TWS 的 `<=` 相反,见 11.2。TAS 通道**不维护航迹质量**(`track_asso_tas.cpp` 未更新 `Quality`),消亡仍按外推圈数。
|
||||
- **TAS**:按 `latest_timestamp − T_track` 预测(ΔT ≤ 0 跳过),航迹时间推进到最新时间戳。上限判定为 `Extrapolate_round >= extrapolate_max_tas`(配置项,样例为相扫 5;机扫需配 48)时才预测并输出,未达该轮数时只累加 `Extrapolate_round` 且不输出(位置与航迹时间停在最后一次关联处)。⚠ 判定方向与 TWS 的 `<=` 相反,见 11.2。TAS 通道**不维护航迹质量**(`track_asso_tas.cpp` 未更新 `Quality`),消亡仍按外推圈数。
|
||||
- **输出门控**:`track_asso_process()` / `track_asso_process_tas()` 的输出循环只写 `Track_Update_Flag==1` 的航迹,写完即清零;因此「本圈未更新」的航迹(外推超限等)不会出现在 `Trust_Track_Output` 中。
|
||||
|
||||
#### 消亡(track_die.cpp / track_die_tas.cpp)
|
||||
|
||||
**TWS 普通航迹自 V1.5.8 起改为按航迹质量消亡**(`Quality < TRACK_DIE_QUALITY` = 0.3),其余通道仍按外推圈数(判据 `Extrapolate_round > 阈值`,50e0b5c 起由 `>=` 改为 `>`),满足其一即消亡:
|
||||
**TWS 普通航迹自 V1.5.8 起改为按航迹质量消亡**(`Quality < track_die_quality`,默认 0.3),其余通道仍按外推圈数(判据 `Extrapolate_round > 阈值`,50e0b5c 起由 `>=` 改为 `>`),满足其一即消亡:
|
||||
|
||||
| 通道 | 条件 |
|
||||
|---|---|
|
||||
| TWS 普通航迹 | Track_Mode==0 且 primary_target_flag==0 且 **Quality < TRACK_DIE_QUALITY(=0.3)**(V1.5.8,782bff5) |
|
||||
| TWS 重点航迹 | Track_Mode==0 且 primary_target_flag==1 且 Extrapolate_round > TRACK_DIE_ROUND_PRIMARY(=10) |
|
||||
| TAS 航迹 | Track_Mode==1 且 Extrapolate_round > TRACK_DIE_ROUND_TAS(相扫 20 / 机扫 96) |
|
||||
| TWS 普通航迹 | Track_Mode==0 且 primary_target_flag==0 且 **Quality < track_die_quality(样例 0.3)**(V1.5.8,782bff5) |
|
||||
| TWS 重点航迹 | Track_Mode==0 且 primary_target_flag==1 且 Extrapolate_round > track_die_round_pramary(样例 10) |
|
||||
| TAS 航迹 | Track_Mode==1 且 Extrapolate_round > track_die_round_tas(样例配置为相扫 20;机扫需配 96) |
|
||||
| 任意航迹 | manual_delete_flag==1(手动删除标记,不区分通道) |
|
||||
|
||||
消亡批号写入 Track_die_Index_Output 并删除本地航迹。重点航迹实际连续 11 圈无实点关联才消亡(TAS 相扫 21 圈 / 机扫 97 圈,因 `Extrapolate_round` 每圈未关联只自增 1;相扫 TAS 阈值于 5d105d2 由 10 提高到 20);普通 TWS 航迹的存活时长改为由质量衰减速度决定——未关联时间越长衰减越快,`Quality` 降到 0.3 以下即消亡,不再有固定圈数上限(`TRACK_DIE_ROUND` 仍定义但已不被引用,见 10.1)。`track_process()` 中的调用顺序为 `track_asso → track_die → track_init`(data_process.cpp),即消亡先于航迹起始执行。
|
||||
消亡批号写入 Track_die_Index_Output 并删除本地航迹。重点航迹实际连续 11 圈无实点关联才消亡(TAS 相扫 21 圈 / 机扫 97 圈,因 `Extrapolate_round` 每圈未关联只自增 1;相扫 TAS 阈值于 5d105d2 由 10 提高到 20);普通 TWS 航迹的存活时长改为由质量衰减速度决定——未关联时间越长衰减越快,`Quality` 降到 `track_die_quality` 以下即消亡,不再有固定圈数上限(`track_die_round`(原 6 圈)仍可配置但已不被引用,见 10.2)。`track_process()` 中的调用顺序为 `track_asso → track_die → track_init`(data_process.cpp),即消亡先于航迹起始执行。
|
||||
|
||||
> 重点目标标记(`primary_target_add` / `primary_target_cancel`)在 TWS 上决定「按质量消亡(普通)」还是「按 10 圈轮数消亡(重点)」,不改变关联、滤波、波束控制等其他环节,也不体现在输出航迹字段中。
|
||||
|
||||
### 9.10 TAS 波束控制(tas_ctrl.cpp)
|
||||
|
||||
- **入队条件**(仅手动):manual_tracking_flag==1 且 Track_Mode==0 且队列未满(MAX_TAS_NUM,相扫 4 / 机扫 1)且不在 TAS 禁止区内。入队即置 Track_Mode=1、以航迹时间初始化 last_track_time,并输出一次航迹更新。
|
||||
- **入队条件**(仅手动):manual_tracking_flag==1 且 Track_Mode==0 且队列未满(配置项 `tas_track_num_max`,样例为相扫 4;机扫需配 1,上限 `RDP_TAS_QUEUE_CAPACITY`=30)且不在 TAS 禁止区内。入队即置 Track_Mode=1、以航迹时间初始化 last_track_time,并输出一次航迹更新。
|
||||
- **出队**:队列中目标在航迹表中不存在、或不再处于手动 TAS 状态时清除。(自动转 TAS / 自动退出的 tas_auto_start/tas_auto_end 逻辑全部注释停用——只支持手动。)
|
||||
- **波束输出**:在 TAS 队列内遍历,逐个目标先按**数据率门控**筛选——仅要求目标距上次波束输出(`last_track_time`)的时间 `delta_T_last_track = (latest_timestamp − last_track_time)/1000` 大于运行时参数 `Work_Parameter.DATA_RATE_TAS` 秒(单位 s,1521592 起由 Hz 改为 s,需为 >0 的有效值),保证 TAS 目标输出间隔不随波束时间(Beam_Ctrl 调用频率)变化;对通过门控的目标再比较航迹时间 `T_track`,取 CPI 时间最早者,选中后按其 CPI 时间与最新时间戳之差外推到 latest_timestamp:
|
||||
- **波束输出**:在 TAS 队列内遍历,逐个目标先按**数据率门控**筛选——仅要求目标距上次波束输出(`last_track_time`)的时间 `delta_T_last_track = (latest_timestamp − last_track_time)/1000` 大于配置项 `data_rate_tas` 秒(单位 s,1521592 起由 Hz 改为 s,加载器校验须 >0),保证 TAS 目标输出间隔不随波束时间(Beam_Ctrl 调用频率)变化;对通过门控的目标再比较航迹时间 `T_track`,取 CPI 时间最早者,选中后按其 CPI 时间与最新时间戳之差外推到 latest_timestamp:
|
||||
|
||||
$$
|
||||
\Delta T=(latest\_timestamp-CPI\_time)/1000,\qquad
|
||||
@@ -906,15 +919,15 @@ x_p=x+v_x\Delta T,\quad y_p=y+v_y\Delta T,\quad
|
||||
r_p=\sqrt{x_p^2+y_p^2},\quad \theta_p=\operatorname{atan2}(y_p,x_p)
|
||||
$$
|
||||
|
||||
输出 TrackingBeam{open_flag=1, type=1, Range=rp, Azi=θp(°), Elev=asin(h/rp)(°), TAS_track_index};队空、目标不在航迹表或数据率门控未通过时 open_flag=0。输出后更新该目标队列项的 last_track_time;原每次 Beam_Ctrl 末尾的整队循环移位(队列轮转)已停用,改为按数据率门控在队列内轮换目标。
|
||||
输出 TrackingBeam{open_flag=1, type=default_track_beam_type(配置项,样例 1), Range=rp, Azi=θp(°), Elev=asin(h/rp)(°), TAS_track_index};队空、目标不在航迹表或数据率门控未通过时 open_flag=0。输出后更新该目标队列项的 last_track_time;原每次 Beam_Ctrl 末尾的整队循环移位(队列轮转)已停用,改为按数据率门控在队列内轮换目标。
|
||||
|
||||
> e0bf227 修正:数据率门控原先同时要求「距上次波束输出」与「航迹时间 T_track 距最新时间戳」都超过 DATA_RATE_TAS,第二个条件与「选 CPI 最早目标」的语义冲突(航迹时间越早反倒越容易被抑制),会漏掉本应输出的波束;现已只保留前者。
|
||||
> e0bf227 修正:数据率门控原先同时要求「距上次波束输出」与「航迹时间 T_track 距最新时间戳」都超过 `data_rate_tas`,第二个条件与「选 CPI 最早目标」的语义冲突(航迹时间越早反倒越容易被抑制),会漏掉本应输出的波束;现已只保留前者。
|
||||
|
||||
- tas_ctrl_process 入口强制 *Trust_track_num_Output=0 并在写入前检查容量,防止输出数组越界(BUG-09 修复)。
|
||||
|
||||
### 9.11 航迹质量(V1.5.8,1ae0cf2 / d9a7bd0 / 4b70b24 / 782bff5)
|
||||
|
||||
内部质量 `Quality` 为 **0~1** 的归一化值(`Temp_track::quality`、`Trust_Track::Quality`),输出时乘 `TRACK_QUALITY_MAX`(100)折算为 **0~100** 写入 `Track.Quality`。它贯穿「临时航迹 → 起批 → 关联 → 外推 → 消亡」全过程,是普通 TWS 航迹**自动**消亡的唯一依据。
|
||||
内部质量 `Quality` 为 **0~1** 的归一化值(`Temp_track::quality`、`Trust_Track::Quality`),输出时乘 `track_quality_max`(配置项,100)折算为 **0~100** 写入 `Track.Quality`。它贯穿「临时航迹 → 起批 → 关联 → 外推 → 消亡」全过程,是普通 TWS 航迹**自动**消亡的唯一依据。
|
||||
|
||||
**三个公共分量**
|
||||
|
||||
@@ -927,14 +940,14 @@ $$
|
||||
**① 临时航迹阶段(`track_init.cpp::point_temp_track_asso()`)**:每成功延长一个点后重算——`quality_tmp = (0.5·d_tmp + 0.3·snr_tmp)/(0.5+0.3)`(只用距离与 SNR 两项,不含长度项)。
|
||||
|
||||
- 长度 L = 3:SNR 取三点平均,`quality_tmp` **写满三个点**;
|
||||
- 长度 L > 3:SNR 只取末点,末点质量 = `前一点质量×0.8 + quality_tmp×0.2`(指数平滑,`TRACK_QUALITY_ITER_COEFF`)。
|
||||
- 长度 L > 3:SNR 只取末点,末点质量 = `前一点质量×0.8 + quality_tmp×0.2`(指数平滑,系数为配置项 `track_quality_iter_coeff`,默认 0.8)。
|
||||
|
||||
**② 关联更新(`track_asso.cpp::model_filter()`)**:关联到实点并完成 IMM/EKF 更新后,用 `tq = 0.5·d_tmp + 0.3·snr_tmp + 0.2·len_tmp` 求即时质量,再 `Quality = Quality×0.8 + tq×0.2`;随后 `Point_sum++`、`Valid_point_sum++`(**质量用的是自增前的计数**),`Unupdated_time` 清零。注意 `Valid_point_sum/Point_sum` 为**整数除法**(先截断再乘 0.5)。
|
||||
**② 关联更新(`track_asso.cpp::model_filter()`)**:关联到实点并完成 IMM/EKF 更新后,用 `tq = track_quality_d_weight·d_tmp + track_quality_snr_weight·snr_tmp + track_quality_len_weight·len_tmp`(权重默认 0.5/0.3/0.2)求即时质量,再 `Quality = Quality×track_quality_iter_coeff + tq×(1−track_quality_iter_coeff)`(默认 0.8/0.2);随后 `Point_sum++`、`Valid_point_sum++`(**质量用的是自增前的计数**),`Unupdated_time` 清零。注意 `Valid_point_sum/Point_sum` 为**整数除法**(先截断再乘 0.5)。
|
||||
|
||||
**③ 外推衰减(`track_asso.cpp::model_filter()` 外推环节)**:按累计未更新时长做二次衰减。记 `t = Unupdated_time/1000`(秒),每次先 `Quality -= f(t_old)`、执行 `Unupdated_time += ΔT`、再 `Quality += f(t_new)`,净效果为 `Quality += [f(t_new) − f(t_old)]`(即 `Quality -= [f(t_old) − f(t_new)]`),其中
|
||||
|
||||
$$
|
||||
f(t)=O_2\,t^2+O_1\,t,\qquad O_2=-0.0008,\; O_1=-0.0025
|
||||
f(t)=track\_quality\_decrease\_coeff\_o2\,t^2+track\_quality\_decrease\_coeff\_o1\,t,\qquad O_2=-0.0008,\; O_1=-0.0025
|
||||
$$
|
||||
|
||||
因两系数为负,`f` 单调递减,故每圈质量下降 `0.0008\,(t_{new}^2-t_{old}^2)+0.0025\,(t_{new}-t_{old})`——**未关联越久、衰减越快**;结果钳位到 ≥0。该衰减在上限内/外都会执行(见 9.9)。
|
||||
@@ -947,83 +960,194 @@ $$
|
||||
|
||||
不达标的本圈不起批,临时航迹继续留在表中积累(受 `buff_round`/长度 10 的临时航迹消亡约束)。
|
||||
|
||||
**⑤ 消亡(`track_die.cpp`)**:普通 TWS 航迹 `Quality < TRACK_DIE_QUALITY`(0.3)即消亡;重点航迹与 TAS 航迹仍按外推圈数(见 9.9)。
|
||||
**⑤ 消亡(`track_die.cpp`)**:普通 TWS 航迹 `Quality < track_die_quality`(默认 0.3)即消亡;重点航迹与 TAS 航迹仍按外推圈数(见 9.9)。
|
||||
|
||||
**⑥ 调试入库(1ffb287)**:记录功能启用时,临时航迹表 `RdpRecTempTrack` 增加 `quality`、可靠航迹表 `RdpRecTrustTrack` 增加 `Quality` / `Unupdated_time` / `Point_sum` / `Valid_point_sum`,可用 SQL 直接观察质量从起批、关联平滑到未关联衰减的全过程(字段顺序见 docs/data_record_debug.md 第 7 节)。
|
||||
|
||||
> 参数全部集中在 `parameters.h` 的「通用数据处理参数」段(两种扫描体制共用):`TRACK_QUALITY_MAX/MIN`、`TRACK_QUALITY_D_WEIGHT`(0.5) / `SNR_WEIGHT`(0.3) / `LEN_WEIGHT`(0.2)、`TRACK_QUALITY_ITER_COEFF`(0.8)、`TRACK_QUALITY_DECREASE_COEFF_O2/O1`、`TRACK_START_QUALITY_L3~L7`、`TRACK_DIE_QUALITY(_PRAMARY)`。
|
||||
> 参数来源:质量相关参数位于配置文件 `[track_quality]` 段(`track_quality_max`/`min`、`track_quality_d_weight`(0.5) / `snr_weight`(0.3) / `len_weight`(0.2)、`track_quality_iter_coeff`(0.8)、`track_quality_decrease_coeff_o2/o1`),起批质量门限位于 `[track_init]` 段(`track_init_quality_L3~L7`),消亡质量门限位于 `[track_die]` 段(`track_die_quality(_pramary)`);全部经 `parameters_reload()` 加载(见 10)。
|
||||
>
|
||||
> 已知局限(详见 11.2):TAS 通道不维护质量;`TRACK_QUALITY_MIN`、`TRACK_DIE_QUALITY_PRAMARY`、`TRACK_DIE_ROUND` 当前定义但未参与计算;起批质量门限按**临时航迹长度**取值,而实际起批只用末点质量。
|
||||
> 已知局限(详见 11.2):TAS 通道不维护质量;`track_quality_min`、`track_die_quality_pramary`、`track_die_round` 当前可配置但未参与计算;起批质量门限按**临时航迹长度**取值,而实际起批只用末点质量。
|
||||
|
||||
---
|
||||
|
||||
## 10. 参数说明
|
||||
|
||||
### 10.1 编译期宏(parameters.h)
|
||||
> 自「参数配置方式改造」起,工作参数的唯一来源是 **TOML 配置文件 → `struct.h::RDP_parameter`**:`parameters.h` 不再含雷达工作参数宏,公开头中也不再有 `struct RadarPara`。宿主只需在 `GetB()` 后调用 `parameters_reload(配置文件路径)`。数值与改造前一致,默认值见 `config/config_tmp.toml`。
|
||||
|
||||
`parameters.h` 分为「通用数据处理参数」段(两种体制共用,V1.5.8 的航迹质量参数即在此段)与 `#ifdef MECHANICAL_SCANNING` / `#ifdef PHASE_SCANNING` 两段(随体制取不同值)。下表「同左」表示两体制取值相同。
|
||||
### 10.1 参数加载机制与校验规则
|
||||
|
||||
| 宏 | 相扫 PHASE_SCANNING | 机扫 MECHANICAL_SCANNING | 含义 |
|
||||
```text
|
||||
宿主:GetB() → parameters_reload("config/config_tmp.toml") → data_preprocess / track_process / Beam_Ctrl ...
|
||||
(重连或切换配置时再次 parameters_reload 即可热重载;成功返回 0,失败返回 -1)
|
||||
DLL :rdp_parameter_load() 解析 TOML → 严格校验 → 全部通过后一次性赋值给 Data_Process::rdp_parameter
|
||||
```
|
||||
|
||||
配置段与 `RDP_parameter` 的注释分组一一对应,**键名 = 结构体成员名**:
|
||||
|
||||
| 配置段 | 内容 |
|
||||
|---|---|
|
||||
| `[radar_params]` | 雷达工作参数:工作模式、频点、平台高度、系统延迟、TWS/TAS/纯跟踪数据率 |
|
||||
| `[cohesion]` | 点迹凝聚参数:距离/速度/方位门限、有效量程上下限 |
|
||||
| `[track_associate]` | 航迹关联参数:量测误差 `sigma_*`、关联门限 `associate_threshold`、外推上限、高度平滑窗长 |
|
||||
| `[track_init]` | 航迹起始参数:起批波门/折角/点数、起批质量门限 L3~L7、起始速度范围 |
|
||||
| `[track_die]` | 航迹消亡参数:消亡圈数(普通/重点/TAS)、质量消亡门限 |
|
||||
| `[track_quality]` | 航迹质量参数:折算上下限、三项权重、平滑与衰减系数 |
|
||||
| `[track_index]` | 航迹批号管理:`track_index_max` |
|
||||
| `[beam_control]` | 波束调度:默认跟踪波束类型、最大同时跟踪 TAS 目标数 |
|
||||
| `[singer_model]` | 三个 Singer 模型的过程噪声强度(fast/slow) |
|
||||
| `[track_init_prohibite_area]` | 起批屏蔽区:`*_num` + 4 个与成员同名的 TOML 数组 |
|
||||
| `[tas_prohibite_area]` | 禁止 TAS 跟踪区:`*_num` + 4 个与成员同名的 TOML 数组 |
|
||||
|
||||
**加载器严格校验规则**(`rdp_parameter_loader.cpp`,任一不满足即失败):
|
||||
|
||||
1. 11 个配置段**全部必填**,且必须是 TOML 表;顶层出现未知配置段直接拒绝;
|
||||
2. 所有**标量键必填**,类型必须匹配(浮点参数接受整数或浮点,整数参数只接受整数),且不得超出 C++ `float`/`int` 可表示范围;
|
||||
3. 每个段内只允许登记过的键,**拼错的键按未知参数拒绝**(不会静默忽略);
|
||||
4. 取值范围校验:如 `radar_work_mode∈[0,2]`、`freq_0_GHz>0`、`data_rate_tws/data_rate_tas>0`、`sigma_*>0`、`associate_threshold>0`、`height_smooth_win≥1`、`track_init_point_num_min∈[3,9]`、`track_init_angle∈(0,180]`、`track_init_quality_L3~L7∈[0,1]`、`track_init_velocity_max>min≥0`、消亡圈数≥0、质量门限∈[0,1]、质量衰减系数≤0、`track_index_max∈[1, MAX_TRACK_NUM]`、`tas_track_num_max∈[0, RDP_TAS_QUEUE_CAPACITY]`、Singer 模型 Q>0 等(完整清单以源码为准);
|
||||
5. 屏蔽区:`*_num∈[0,30]`;4 个数组的元素个数必须**等于** `*_num`(`*_num=0` 时数组可省略,存在时必须为空数组);并逐区校验距离 `0≤min<max`、方位 `0≤min<max≤360`;
|
||||
6. 失败语义:返回 -1,并经可选 `std::string* error_msg` 给出**首条**可读原因(`Data_Process::parameters_reload` 会打印到 `RDP_LOG`),**当前已加载参数保持不变**(加载过程先写入临时对象,全部成功后才整体赋值)。
|
||||
|
||||
> 兼容性提示:改造前 `TRACK_DIE_ROUND_TAS`、`MAX_EXTRAPOLATE_NUM_TAS`、`H_F_WIN_LEN`、`TAS_QUEUE_LENGTH`/`MAX_TAS_NUM` 按扫描体制取不同宏值;现在同一份配置对所有体制生效。`config/config_tmp.toml` 是**相扫**取值,机扫部署需把这 4 项分别配为 `track_die_round_tas=96`、`extrapolate_max_tas=48`、`height_smooth_win=3`、`tas_track_num_max=1`(其余参数两体制相同)。注意「相扫/机扫判圈逻辑」不是配置项,仍由编译期 `PHASE_SCANNING` / `MECHANICAL_SCANNING` 区分(见 7.2)。
|
||||
|
||||
### 10.2 参数表(`RDP_parameter` 字段 ↔ TOML 键 ↔ 样例值)
|
||||
|
||||
「样例值」为 `config/config_tmp.toml` 的取值(相扫);数值含义与改造前相同。
|
||||
|
||||
| 配置段 | 键(=结构体字段) | 样例值 | 含义 / 影响 |
|
||||
|---|---|---|---|
|
||||
| RDP_LOG_ENABLE | 注释停用(默认不启用) | 同左 | 日志文件输出开关:定义时将 `std::cout` 重定向到日志文件;**当前源码中该宏为注释状态**(parameters.h 的注释文字仍写「定义(默认)」,以代码实际状态为准),不重定向、日志输出到标准输出/控制台 |
|
||||
| RDP_LOG_DIR | `.\rdp_logs\` | 同左 | 日志文件输出目录(相对宿主进程当前工作目录,目录不存在时自动创建) |
|
||||
| RDP_RECORD_ENABLE | 由 CMake 选项 `RDP_DATA_RECORD` 定义(AUTO 时 Debug 定义) | 同左 | 数据记录调试开关:**不要在 parameters.h 中手工定义**(由构建系统传入);定义时记录模块参与编译,未定义时全部记录宏(6 个异步入队宏 + 遗留 `RDP_RECORD`/`RDP_RECORD_BATCH`)展开为空操作,见 4.3 与 11.3 |
|
||||
| RDP_RECORD_DB_DIR | `.\rdp_records\` | 同左 | 数据记录库输出目录(相对宿主进程当前工作目录,目录不存在时自动创建;仅记录功能启用时产生) |
|
||||
| RDP_RECORD_POINTS / RDP_RECORD_COHESION_TWS / RDP_RECORD_COHESION_TAS / RDP_RECORD_TEMP_TRACKS / RDP_RECORD_TRUST_TRACKS / RDP_RECORD_ASSO_INFOS | 由 `RDP_RECORD_ENABLE` 条件定义(记录开启时展开为 `std::move` + 异步入队,关闭时 `((void)0)`) | 同左 | 6 个异步入队记录点宏,参数为对应源结构体的 `std::vector`(`PointRecv` / `Temp_track` / `Trust_Track` / `Asso_Info`),宏内部 `std::move`、调用后 vector 为空;宏与表的对应关系见 docs/data_record_debug.md 第 4、7 节。遗留的 `RDP_RECORD`/`RDP_RECORD_BATCH` 为同步接口(会阻塞) |
|
||||
| RDP_RECORD_QUEUE_CAP_BYTES | `64*1024*1024`(64 MB) | 同左 | 入队队列字节上限;超过该值整批丢弃并计入 `stats().dropped_rows`,不阻塞处理线程 |
|
||||
| RDP_RECORD_QUEUE_MAX_BATCHES | 4096 | 同左 | 入队队列批数上限(与字节上限同时生效) |
|
||||
| RDP_RECORD_WRITE_BATCH_ROWS | 4096 | 同左 | 写线程每个事务的目标提交行数(按表累积到该行数即提交) |
|
||||
| RDP_RECORD_WRITE_MAX_WAIT_MS | 50 | 同左 | 写线程为凑批等待的最长时间(ms),超时后提交现有数据 |
|
||||
| RDP_RECORD_ASSO_CHUNK_ROWS | 131072 | 同左 | 关联信息(⑥)单次入队分块上限,限制处理线程收集 `Asso_Info` 的内存峰值 |
|
||||
| RDP_RECORD_SQLITE_CACHE_KB | 8192 | 同左 | SQLite 页缓存大小(KB,执行 `PRAGMA cache_size=-8192`,即 8 MB) |
|
||||
| RDP_RECORD_SQLITE_BUSY_TIMEOUT_MS | 2000 | 同左 | SQLite 忙等待超时(ms) |
|
||||
| RDP_RECORD_SQLITE_WAL | 1 | 同左 | 是否尝试开启 WAL(1=尝试;失败/不支持时回退默认 journal 模式并提示) |
|
||||
| RDP_RECORD_MAX_CONSECUTIVE_ERRORS | 10 | 同左 | 异步批量写入连续失败达到该次数后全局熔断(停止记录并丢弃队列) |
|
||||
| RDP_RECORD_DROP_LOG_INTERVAL | 1024 | 同左 | 队列丢弃日志限频:每累计丢弃该行数提示一次 |
|
||||
| rdp_record::Stats / stats()(非宏) | — | — | 运行统计结构体(定义于 `rdp_record.h`,任意时刻线程安全读取、`Shutdown()` 时自动打印):`enqueued_rows`/`dropped_rows`/`written_rows`/`transactions`/`write_errors`/`queue_peak_bytes`/`queue_peak_rows` |
|
||||
| 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 |
|
||||
| DOT_COH_RANGE / DOT_COH_V / DOT_COH_AZI | 80 / 2 / 6 | 同左 | 凝聚门限:距离 m / 速度 m/s / 方位 ° |
|
||||
| MAX_BEAM_NUM | 100 | 同左 | 最大波位数 |
|
||||
| MAX_TRACK_NUM / MAX_TRACK_INDEX | 500 / 500 | 同左 | 航迹数上限 / 航迹号上限 |
|
||||
| R_MIN / R_MAX | 100 / 100000 | 同左 | 有效量程(m) |
|
||||
| TRACK_START_THRESHOLD | 8 | 同左 | 起批波门(d² 门限 64;越大越容易起批) |
|
||||
| ALPHA_START | 60 | 同左 | 起批折角(°) |
|
||||
| TRACK_DIE_ROUND | 6 | 6 | **已不被引用**:V1.5.8 起 TWS 普通航迹改按质量消亡(见下行),该宏仍定义但代码中无使用 |
|
||||
| TRACK_DIE_QUALITY | 0.3 | 0.3 | TWS **普通**航迹消亡质量门限(`Quality < 0.3` 即消亡;V1.5.8 新增,与体制无关) |
|
||||
| TRACK_START_QUALITY_L3 / L4 / L5 / L6 / L7 | 0.7 / 0.6 / 0.5 / 0.4 / 0.3 | 同左 | 起批最低质量(按临时航迹长度 3/4/5/6/≥7 取值;V1.5.8 新增) |
|
||||
| TRACK_QUALITY_MAX / MIN | 100 / 0 | 同左 | 质量折算上限(输出 `Track.Quality` = 内部质量×100)/ 下限(**当前未使用**) |
|
||||
| TRACK_QUALITY_D_WEIGHT / SNR_WEIGHT / LEN_WEIGHT | 0.5 / 0.3 / 0.2 | 同左 | 质量三要素权重:统计距离 / SNR / 有效点占比(V1.5.8 新增) |
|
||||
| TRACK_QUALITY_ITER_COEFF | 0.8 | 同左 | 质量指数平滑系数(`新 = 旧×0.8 + 即时×0.2`) |
|
||||
| TRACK_QUALITY_DECREASE_COEFF_O2 / O1 | −0.0008 / −0.0025 | 同左 | 质量衰减二次项 / 一次项系数(负值;随未更新时间 `t` 累计,见 9.11) |
|
||||
| TRACK_DIE_ROUND_PRIMARY | 10 | 10 | TWS **重点航迹**消亡圈数(primary_target_flag==1 时套用;50e0b5c 新增) |
|
||||
| TRACK_DIE_ROUND_TAS | 20 | 96 | TAS 航迹消亡圈数(判据 Extrapolate_round > 阈值;相扫 50e0b5c 由 5→10、5d105d2 再调整为 20,机扫保持 96) |
|
||||
| MAX_EXTRAPOLATE_NUM | 3 | 3 | TWS 未关联航迹最大外推圈数(Extrapolate_round ≤ 3 才继续外推;50e0b5c 新增) |
|
||||
| MAX_EXTRAPOLATE_NUM_TAS | 5 | 48 | TAS 最大外推圈数(判据为 Extrapolate_round ≥ 该值时才外推,方向与 TWS 相反,见 9.9 与 11.2;50e0b5c 新增) |
|
||||
| ASSO_THORD | 4 | 同左 | 关联波门(判据 d*d < ASSO_THORD²,d 已是马氏距离平方,等效 d < 4;e0bf227 起由 3 调大至 4) |
|
||||
| H_F_WIN_LEN | 6 | 3 | 高度平滑窗长基数:TWS 实际取 `+2/+3/+4/+6`、TAS 取 `+6/+8/+10/+12`(相扫基数 6284f3f 起由 3 调整为 6;TAS 偏移同提交由 +2/+3/+4/+6 改为 +6/+8/+10/+12) |
|
||||
| TAS_QUEUE_LENGTH / MAX_TAS_NUM | 4 / 4 | 1 / 1 | TAS 队列长度 / 最大 TAS 目标数 |
|
||||
| FREQ0 | 9.2 GHz | 同左 | 载波频点(原 FREQ0~FREQ20 一组宏已精简为单个 FREQ0,e729c5b 起由 16.8 GHz 改为 9.2 GHz) |
|
||||
| radar_params | `radar_work_mode` | 0 | 工作模式:0 近程 / 1 中程 / 2 远程(当前影响凝聚是否带速度维约束;TWS 外推已改为固定 `data_rate_tws`、不再按模式三选一;原 `RadarPara.work_mode`) |
|
||||
| radar_params | `freq_0_GHz` | 9.2 | 载波频点(GHz,盲速计算基准;原 `FREQ0`) |
|
||||
| radar_params | `radar_height` | 0 | 雷达平台高度(m,输出 z 叠加;原 `Height`) |
|
||||
| radar_params | `radar_system_delay` | 0 | 系统延迟(s,输出位置按 `x + vx·delay` 补偿;原 `Sys_delay`) |
|
||||
| radar_params | `data_rate_tws` | 3 | TWS 数据率/外推步长(s;原按 `work_mode` 在 `DATA_RATE_SHORT/MIDDLE/FAR` 中三选一) |
|
||||
| radar_params | `data_rate_tas` | 1 | TAS 波束输出间隔(s,校验 >0;原 `DATA_RATE_TAS`,1521592 起单位由 Hz 改为 s) |
|
||||
| radar_params | `data_rate_track_only` | 0.2 | 纯跟踪模式数据率(s;当前实现保留未用) |
|
||||
| cohesion | `point_cohesion_range` | 80 | 距离凝聚门限(m;原 `DOT_COH_RANGE`) |
|
||||
| cohesion | `point_cohesion_velocity` | 2 | 速度凝聚门限(m/s;原 `DOT_COH_V`) |
|
||||
| cohesion | `point_cohesion_azimuth` | 6 | 方位凝聚门限(°;原 `DOT_COH_AZI`) |
|
||||
| cohesion | `point_cohesion_range_min` | 100 | 有效量程下限(m;原 `R_MIN`) |
|
||||
| cohesion | `point_cohesion_range_max` | 100000 | 有效量程上限(m;原 `R_MAX`,校验 > 下限) |
|
||||
| track_associate | `sigma_range` | 10.0 | 距离量测误差(m;原 `SIGMA_R`) |
|
||||
| track_associate | `sigma_azimuth` | 0.02 | 方位量测误差(rad;原 `SIGMA_A`) |
|
||||
| track_associate | `sigma_pitch` | 0.2 | 俯仰量测误差(rad;原 `SIGMA_E`) |
|
||||
| track_associate | `sigma_velocity` | 2.0 | 速度量测误差(m/s;原 `SIGMA_V`) |
|
||||
| track_associate | `associate_threshold` | 4.0 | 关联波门(原 `ASSO_THORD`,判据 `d*d < 该值²`,语义见 9.6) |
|
||||
| track_associate | `extrapolate_max` | 3 | TWS 最大外推圈数(原 `MAX_EXTRAPOLATE_NUM`) |
|
||||
| track_associate | `extrapolate_max_tas` | 5 | TAS 外推轮数阈值(原 `MAX_EXTRAPOLATE_NUM_TAS`;机扫需配 48) |
|
||||
| track_associate | `height_smooth_win` | 6 | 高度平滑窗长基数(原 `H_F_WIN_LEN`;机扫需配 3,见 9.7) |
|
||||
| track_init | `track_init_threshold` | 8 | 起批统计距离波门(原 `TRACK_START_THRESHOLD`;实际生效 `d ≤ 8`) |
|
||||
| track_init | `track_init_angle` | 60 | 起批折角上限(°;原 `ALPHA_START`) |
|
||||
| track_init | `track_init_point_num_min` | 3 | 起批所需点数(3~9;原 `RadarPara.track_start_point_num`) |
|
||||
| track_init | `track_init_quality_L3`~`L7` | 0.7 / 0.6 / 0.5 / 0.4 / 0.3 | 按临时航迹长度 3/4/5/6/≥7 的起批最低质量(原 `TRACK_START_QUALITY_L3~L7`) |
|
||||
| track_init | `track_init_velocity_min` | 1 | 航迹头关联速度下限(m/s;原 `V_MIN`,校验 < 上限) |
|
||||
| track_init | `track_init_velocity_max` | 50 | 航迹头关联速度上限(m/s;原 `V_MAX`) |
|
||||
| track_die | `track_die_round` | 6 | 普通 TWS 消亡圈数(V1.5.8 起改按质量消亡,**当前未被引用**,保留) |
|
||||
| track_die | `track_die_round_pramary` | 10 | 重点航迹消亡圈数(原 `TRACK_DIE_ROUND_PRIMARY`) |
|
||||
| track_die | `track_die_round_tas` | 20 | TAS 航迹消亡圈数(原 `TRACK_DIE_ROUND_TAS`;机扫需配 96) |
|
||||
| track_die | `track_die_quality` | 0.3 | TWS 普通航迹质量消亡门限(原 `TRACK_DIE_QUALITY`) |
|
||||
| track_die | `track_die_quality_pramary` | 0.3 | 重点航迹质量门限(保留,当前未参与计算) |
|
||||
| track_quality | `track_quality_max` | 100 | 质量折算上限(输出 `Track.Quality` = 内部质量 × 该值) |
|
||||
| track_quality | `track_quality_min` | 0 | 质量下限(保留,当前未使用) |
|
||||
| track_quality | `track_quality_d_weight` | 0.5 | 质量三要素权重:统计距离 |
|
||||
| track_quality | `track_quality_snr_weight` | 0.3 | 质量三要素权重:SNR |
|
||||
| track_quality | `track_quality_len_weight` | 0.2 | 质量三要素权重:有效点占比 |
|
||||
| track_quality | `track_quality_iter_coeff` | 0.8 | 质量指数平滑系数(`新 = 旧×c + 即时×(1−c)`) |
|
||||
| track_quality | `track_quality_decrease_coeff_o2` | −0.0008 | 质量衰减二次项系数(须 ≤0,见 9.11) |
|
||||
| track_quality | `track_quality_decrease_coeff_o1` | −0.0025 | 质量衰减一次项系数(须 ≤0) |
|
||||
| track_index | `track_index_max` | 500 | 航迹号上限(1~500;原 `MAX_TRACK_INDEX`,内部航迹号表按该值分配) |
|
||||
| beam_control | `default_track_beam_type` | 1 | 跟踪波束 `TrackingBeam.type` 取值(原硬编码 1) |
|
||||
| beam_control | `tas_track_num_max` | 4 | 最大同时跟踪 TAS 目标数(0~30;原 `MAX_TAS_NUM`,机扫需配 1) |
|
||||
| singer_model | `model_1_Q_fast` | 0.000001 | 模型 1 过程噪声(**当前未参与计算**,模型 1 只用 slow) |
|
||||
| singer_model | `model_1_Q_slow` | 0.000001 | 模型 1 过程噪声(恒定,见 9.5.1) |
|
||||
| singer_model | `model_2_Q_fast` | 0.01 | 模型 2 过程噪声 fast 档 |
|
||||
| singer_model | `model_2_Q_slow` | 0.01 | 模型 2 过程噪声 slow 档 |
|
||||
| singer_model | `model_3_Q_fast` | 1.0 | 模型 3 过程噪声 fast 档 |
|
||||
| singer_model | `model_3_Q_slow` | 1.0 | 模型 3 过程噪声 slow 档 |
|
||||
| track_init_prohibite_area | `track_init_prohibite_area_num` | 0 | 起批屏蔽区数(0~30;原 `RadarPara.track_prohibite_area_num`) |
|
||||
| track_init_prohibite_area | `track_init_prohibite_area_range_min/max`、`_azimuth_min/max` | `[]` | 4 个 TOML 数组,长度必须等于 `*_num`(距离 m / 方位 °;原 `R_min/max_track_prohibited`、`Azimuth_min/max_track_prohibited`) |
|
||||
| tas_prohibite_area | `tas_prohibite_area_num` | 0 | 禁止 TAS 跟踪区数(0~30;原 `RadarPara.TAS_prohibite_area_num`) |
|
||||
| tas_prohibite_area | `tas_prohibite_area_range_min/max`、`_azimuth_min/max` | `[]` | 4 个 TOML 数组,长度必须等于 `*_num`(原 `R_min/max_TAS_prohibited`、`Azimuth_min/max_TAS_prohibited`) |
|
||||
|
||||
### 10.2 运行时参数(RadarPara)
|
||||
> ⚠ 结构体成员名 `track_die_round_pramary`、`track_die_quality_pramary` 沿用源码既有拼写(pramary),配置键必须与之一致,不是笔误修正对象。
|
||||
|
||||
| 字段 | 含义 | 备注 |
|
||||
### 10.3 旧参数 → 新字段对照(宿主/调参迁移用)
|
||||
|
||||
**(a)旧 `struct RadarPara` 字段**
|
||||
|
||||
| 旧字段 | 新字段(配置键) | 说明 |
|
||||
|---|---|---|
|
||||
| Beam_num | 波位数 | |
|
||||
| Height | 雷达平台高度(m) | 输出 z 叠加 |
|
||||
| Sys_delay | 系统延迟(s) | 输出航迹位置做 x + vx·Sys_delay 补偿 |
|
||||
| R_TAS_max/min、V_TAS_max/min、TAS_Height_1~5 | TAS 自动开启/退出条件 | 自动逻辑已停用,字段保留 |
|
||||
| track_prohibite_area_num + 4×30 数组 | 起批屏蔽区(最多 30 个) | 数量未做上限校验(BUG-07) |
|
||||
| TAS_prohibite_area_num + 4×30 数组 | TAS 禁止区(最多 30 个) | 同上 |
|
||||
| cfar_th | CFAR 门限 | 当前未参与处理 |
|
||||
| work_mode | 0 近程 / 1 中程 / 2 远程 | 决定数据率与门限分支 |
|
||||
| north_angle | 北偏角 | 保留 |
|
||||
| V_MAX / V_MIN | 目标速度上下限(m/s) | 航迹头关联速度区间门限 |
|
||||
| DATA_RATE_SHORT / MIDDLE / FAR | 三模式数据率(s) | TWS 外推步长 |
|
||||
| DATA_RATE_TAS | TAS 波束输出间隔(s) | 由调用者经 track_process_parameters_initial 输入(须为 >0 的有效值);`tas_beam_output()` 门控阈值为 DATA_RATE_TAS 秒(1521592 起单位由 Hz 改为 s),引导跟踪外推同样使用该运行时参数 |
|
||||
| track_start_point_num | 起批点数 | 校验范围 3~9 |
|
||||
| track_start_threshold / track_asso_threshold / track_asso_threshold_tas | 起批/关联波门 | **⚠ 声明但当前实现未使用**,实际门限为宏 TRACK_START_THRESHOLD / ASSO_THORD |
|
||||
| Model1/2/3_Q_fast/slow | 三个 Singer 模型过程噪声强度 | Model1_Q_fast 未使用;分档规则见 9.5.1 |
|
||||
| `work_mode` | `radar_work_mode` | 工作模式 |
|
||||
| `Height` | `radar_height` | 雷达平台高度 |
|
||||
| `Sys_delay` | `radar_system_delay` | 系统延迟 |
|
||||
| `DATA_RATE_SHORT/MIDDLE/FAR` | `data_rate_tws` | 原按 `work_mode` 三选一,现为单字段(契约确认的等价改法) |
|
||||
| `DATA_RATE_TAS` | `data_rate_tas` | TAS 数据率 |
|
||||
| `track_start_point_num` | `track_init_point_num_min` | 起批点数 |
|
||||
| `track_start_threshold`(原实现未用) | `track_init_threshold` | 起批波门 |
|
||||
| `track_asso_threshold`(原实现未用) | `associate_threshold` | 关联波门 |
|
||||
| `track_prohibite_area_num` | `track_init_prohibite_area_num` | 起批屏蔽区数 |
|
||||
| `R_max_track_prohibited` / `R_min_track_prohibited` | `track_init_prohibite_area_range_max/min` | 起批屏蔽区距离 |
|
||||
| `Azimuth_max_track_prohibited` / `Azimuth_min_track_prohibited` | `track_init_prohibite_area_azimuth_max/min` | 起批屏蔽区方位 |
|
||||
| `TAS_prohibite_area_num` | `tas_prohibite_area_num` | TAS 禁止区数 |
|
||||
| `R_max_TAS_prohibited` / `R_min_TAS_prohibited` | `tas_prohibite_area_range_max/min` | TAS 禁止区距离 |
|
||||
| `Azimuth_max_TAS_prohibited` / `Azimuth_min_TAS_prohibited` | `tas_prohibite_area_azimuth_max/min` | TAS 禁止区方位 |
|
||||
| `V_MAX` / `V_MIN` | `track_init_velocity_max/min` | 航迹头关联速度范围 |
|
||||
| `Model1_Q_fast/slow`、`Model2_Q_*`、`Model3_Q_*` | `model_1/2/3_Q_fast/slow` | Singer 模型过程噪声 |
|
||||
| `R_TAS_max/min`、`V_TAS_max/min`、`TAS_Height_1~5`、`cfar_th`、`north_angle`、`Beam_num` | 无 | 实现未使用,新结构体已排除(自动 TAS 逻辑保持停用) |
|
||||
|
||||
**(b)旧 `parameters.h` 宏**
|
||||
|
||||
| 旧宏 | 新字段(配置键) | 说明 |
|
||||
|---|---|---|
|
||||
| `FREQ0` | `freq_0_GHz` | 载波频点 |
|
||||
| `SIGMA_R` / `SIGMA_A` / `SIGMA_E` / `SIGMA_V` | `sigma_range` / `sigma_azimuth` / `sigma_pitch` / `sigma_velocity` | 量测误差 |
|
||||
| `DOT_COH_RANGE` / `DOT_COH_V` / `DOT_COH_AZI` | `point_cohesion_range` / `point_cohesion_velocity` / `point_cohesion_azimuth` | 凝聚门限 |
|
||||
| `R_MIN` / `R_MAX` | `point_cohesion_range_min` / `point_cohesion_range_max` | 有效量程 |
|
||||
| `ASSO_THORD` | `associate_threshold` | 关联波门 |
|
||||
| `MAX_EXTRAPOLATE_NUM` / `MAX_EXTRAPOLATE_NUM_TAS` | `extrapolate_max` / `extrapolate_max_tas` | 外推上限 |
|
||||
| `H_F_WIN_LEN` | `height_smooth_win` | 高度平滑窗长基数 |
|
||||
| `TRACK_START_THRESHOLD` / `ALPHA_START` | `track_init_threshold` / `track_init_angle` | 起批波门/折角 |
|
||||
| `TRACK_START_QUALITY_L3~L7` | `track_init_quality_L3~L7` | 起批最低质量 |
|
||||
| `TRACK_DIE_QUALITY` / `TRACK_DIE_QUALITY_PRAMARY` | `track_die_quality` / `track_die_quality_pramary` | 消亡质量门限 |
|
||||
| `TRACK_DIE_ROUND` / `TRACK_DIE_ROUND_PRIMARY` / `TRACK_DIE_ROUND_TAS` | `track_die_round` / `track_die_round_pramary` / `track_die_round_tas` | 消亡圈数(`track_die_round` 当前未被引用) |
|
||||
| `TRACK_QUALITY_MAX` / `MIN` | `track_quality_max` / `track_quality_min` | 质量折算上下限(min 当前未使用) |
|
||||
| `TRACK_QUALITY_D_WEIGHT` / `SNR_WEIGHT` / `LEN_WEIGHT` | `track_quality_d_weight` / `snr_weight` / `len_weight` | 质量权重 |
|
||||
| `TRACK_QUALITY_ITER_COEFF` | `track_quality_iter_coeff` | 质量平滑系数 |
|
||||
| `TRACK_QUALITY_DECREASE_COEFF_O2` / `O1` | `track_quality_decrease_coeff_o2` / `o1` | 质量衰减系数 |
|
||||
| `MAX_TRACK_INDEX` | `track_index_max` | 航迹号上限(原定长占用表改为按该值分配的 `std::vector`) |
|
||||
| `MAX_TAS_NUM` | `tas_track_num_max` | 最大同时跟踪 TAS 目标数 |
|
||||
| `TAS_QUEUE_LENGTH` | 编译期常量 `RDP_TAS_QUEUE_CAPACITY`(30,见 10.4) | 队列容量仍为编译期常量,运行期上限用 `tas_track_num_max` |
|
||||
| 硬编码 `Tracking_beam->type = 1` | `default_track_beam_type` | 跟踪波束类型 |
|
||||
| `PI`、`MAX_TRACK_NUM`、`UNCONF_TARGET`、版本/日志/记录宏、`Round` | 保留不变 | 见 10.4 |
|
||||
|
||||
### 10.4 编译期常量(parameters.h 保留宏)
|
||||
|
||||
`parameters.h` 已删除全部雷达工作参数宏(改为配置文件,见 10.1~10.3),仅保留接口/内存布局所需的编译期常量、版本/日志/记录宏:
|
||||
|
||||
| 宏 / 常量 | 取值 | 含义 |
|
||||
|---|---|---|
|
||||
| `MAX_TRACK_NUM` | 500 | 输出航迹数组 `Track[500][10]` 的固定容量(宿主接口维度) |
|
||||
| `RDP_TAS_QUEUE_CAPACITY` | 30 | TAS 跟踪队列容量上限;运行期配置项 `tas_track_num_max` 不得超过该值 |
|
||||
| `UNCONF_TARGET` | 0 | 未确认目标类型(`Track::Target_Type` 的缺省语义值) |
|
||||
| `PI` | 3.1415926f | 圆周率常量 |
|
||||
| `MAJOR_VERSION` / `MINOR_VERSION` / `PATCH_VERSION` | 1 / 5 / 8 | 版本号(拼装为 `VERSION_STRING`,见 7.1) |
|
||||
| `VERSION_INFO` / `VERSION_STRING` | `PS`/`MS`、`V1.5.8.PS`/`V1.5.8.MS` | 扫描体制后缀与完整版本串(由 `GetB()` 写入 `version_info`) |
|
||||
| `Round(x)` | 宏 | 四舍五入取整(盲速折叠用) |
|
||||
| RDP_LOG_ENABLE | 注释停用(默认不启用) | 日志文件输出开关:定义时将 `std::cout` 重定向到日志文件;**当前源码中该宏为注释状态**(parameters.h 注释文字仍写「定义(默认)」,以代码实际状态为准),不重定向、日志输出到标准输出/控制台 |
|
||||
| RDP_LOG_DIR | `.\rdp_logs\` | 日志文件输出目录(相对宿主进程当前工作目录,目录不存在时自动创建) |
|
||||
| RDP_RECORD_ENABLE | 由 CMake 选项 `RDP_DATA_RECORD` 定义(AUTO 时 Debug 定义) | 数据记录调试开关:**不要在 parameters.h 中手工定义**(由构建系统传入);定义时记录模块参与编译,未定义时全部记录宏(6 个异步入队宏 + 遗留 `RDP_RECORD`/`RDP_RECORD_BATCH`)展开为空操作,见 4.3 与 11.3 |
|
||||
| RDP_RECORD_DB_DIR | `.\rdp_records\` | 数据记录库输出目录(相对宿主进程当前工作目录,目录不存在时自动创建;仅记录功能启用时产生) |
|
||||
| RDP_RECORD_POINTS / RDP_RECORD_COHESION_TWS / RDP_RECORD_COHESION_TAS / RDP_RECORD_TEMP_TRACKS / RDP_RECORD_TRUST_TRACKS / RDP_RECORD_ASSO_INFOS | 由 `RDP_RECORD_ENABLE` 条件定义(记录开启时展开为 `std::move` + 异步入队,关闭时 `((void)0)`) | 6 个异步入队记录点宏,参数为对应源结构体的 `std::vector`(`PointRecv` / `Temp_track` / `Trust_Track` / `Asso_Info`),宏内部 `std::move`、调用后 vector 为空;宏与表的对应关系见 docs/data_record_debug.md 第 4、7 节。遗留的 `RDP_RECORD`/`RDP_RECORD_BATCH` 为同步接口(会阻塞) |
|
||||
| RDP_RECORD_QUEUE_CAP_BYTES | `64*1024*1024`(64 MB) | 入队队列字节上限;超过该值整批丢弃并计入 `stats().dropped_rows`,不阻塞处理线程 |
|
||||
| RDP_RECORD_QUEUE_MAX_BATCHES | 4096 | 入队队列批数上限(与字节上限同时生效) |
|
||||
| RDP_RECORD_WRITE_BATCH_ROWS | 4096 | 写线程每个事务的目标提交行数(按表累积到该行数即提交) |
|
||||
| RDP_RECORD_WRITE_MAX_WAIT_MS | 50 | 写线程为凑批等待的最长时间(ms),超时后提交现有数据 |
|
||||
| RDP_RECORD_ASSO_CHUNK_ROWS | 131072 | 关联信息(⑥)单次入队分块上限,限制处理线程收集 `Asso_Info` 的内存峰值 |
|
||||
| RDP_RECORD_SQLITE_CACHE_KB | 8192 | SQLite 页缓存大小(KB,执行 `PRAGMA cache_size=-8192`,即 8 MB) |
|
||||
| RDP_RECORD_SQLITE_BUSY_TIMEOUT_MS | 2000 | SQLite 忙等待超时(ms) |
|
||||
| RDP_RECORD_SQLITE_WAL | 1 | 是否尝试开启 WAL(1=尝试;失败/不支持时回退默认 journal 模式并提示) |
|
||||
| RDP_RECORD_MAX_CONSECUTIVE_ERRORS | 10 | 异步批量写入连续失败达到该次数后全局熔断(停止记录并丢弃队列) |
|
||||
| RDP_RECORD_DROP_LOG_INTERVAL | 1024 | 队列丢弃日志限频:每累计丢弃该行数提示一次 |
|
||||
| rdp_record::Stats / stats()(非宏) | — | 运行统计结构体(定义于 `rdp_record.h`,任意时刻线程安全读取、`Shutdown()` 时自动打印):`enqueued_rows`/`dropped_rows`/`written_rows`/`transactions`/`write_errors`/`queue_peak_bytes`/`queue_peak_rows` |
|
||||
|
||||
---
|
||||
|
||||
@@ -1036,28 +1160,28 @@ $$
|
||||
- **越界/防御**:TAS Point_Sum 限幅 150(BUG-04);size()-1 下溢(BUG-05);起批点数校验(BUG-06);航迹号下标校验(BUG-10);Beam_Ctrl 输出计数清零+容量检查(BUG-09);消亡空向量保护(BUG-34);航迹号 500 可复用(BUG-35);局部结构体值初始化(BUG-37)。
|
||||
- **时间戳/时序**:TWS 关联 ΔT≤0 的候选不参与滤波更新、不刷新航迹状态并消费该点迹(604f158 起由「剔除候选、点迹保持未使用」改为当前行为,仍属 BUG-15 处置范围);TAS 关联 ΔT≤0 拒绝(BUG-16);TAS 外推负时间差跳过(BUG-17);Bind_speed 除零保护(BUG-19)。
|
||||
- **数值稳定性**:方位残差 ±π 环绕(BUG-22);IMM 概率零分母/奇异保护(BUG-23);EKF 似然 3×3 新息协方差(BUG-24);航向角 atan2(BUG-26);asin 定义域钳位(BUG-27);近程模型 3 门限浮点除法(BUG-21);track_clear_all 全模块 reset(BUG-28);未初始化变量修复(BUG-11/12/14)。
|
||||
- **后修订(e0bf227,BUG_FIX_REPORT.md 成文之后)**:TWS 关联高度门限由注释状态改为实际生效(`track_asso.cpp`),统计距离/高度/距离三类判据拆为独立布尔量,`ASSO_THORD` 由 3 调大至 4;TAS 关联高度门限 2000~3000 m 段放宽至 250 m;修复 `tas_beam_output()` 数据率门控误判(详见 CHANGELOG 的 [1.5.8] 与 9.6、9.10)。
|
||||
- **后修订(50e0b5c、fc49707、42b598e、e8c56f9,BUG_FIX_REPORT.md 成文之后)**:输出航迹增加 `Track_Update_Flag` 门控(只输出本圈有更新的航迹);未关联航迹外推增加圈数上限;消亡判据改为 `>`,正常/重点航迹分别定为 6 / 10 圈(TAS 相扫 10、机扫 96;**其中 TAS 相扫其后于 5d105d2 调整为 20,且普通 TWS 航迹的 6 圈判据已改为质量消亡,见 10.1 与 11.2**);`Flag_Direction` 改用滤波速度矢量判定,修复速度模糊时的向站/背站误判;手动操作接口补 `break`;`DataRev`/`Track` 新增归一化幅度与速度维 FFT 点数字段(输入侧其后已打通,TAS 输出仍未写回,见 11.2)。
|
||||
- **后修订(e0bf227,BUG_FIX_REPORT.md 成文之后)**:TWS 关联高度门限由注释状态改为实际生效(`track_asso.cpp`),统计距离/高度/距离三类判据拆为独立布尔量,关联门限 `associate_threshold` 由 3 调大至 4;TAS 关联高度门限 2000~3000 m 段放宽至 250 m;修复 `tas_beam_output()` 数据率门控误判(详见 CHANGELOG 的 [1.5.8] 与 9.6、9.10)。
|
||||
- **后修订(50e0b5c、fc49707、42b598e、e8c56f9,BUG_FIX_REPORT.md 成文之后)**:输出航迹增加 `Track_Update_Flag` 门控(只输出本圈有更新的航迹);未关联航迹外推增加圈数上限;消亡判据改为 `>`,正常/重点航迹分别定为 6 / 10 圈(TAS 相扫 10、机扫 96;**其中 TAS 相扫其后于 5d105d2 调整为 20,且普通 TWS 航迹的 6 圈判据已改为质量消亡,见 10.2 与 11.2**);`Flag_Direction` 改用滤波速度矢量判定,修复速度模糊时的向站/背站误判;手动操作接口补 `break`;`DataRev`/`Track` 新增归一化幅度与速度维 FFT 点数字段(输入侧其后已打通,TAS 输出仍未写回,见 11.2)。
|
||||
|
||||
### 11.2 有意保留 / 未修复(archived_docs/BUG_REPORT.md 标注项 + 后续源码观察)
|
||||
|
||||
- **BUG-01**:基类无虚析构,Destroy() 按基类指针 delete。
|
||||
- **BUG-02**:Hight_smooth 只增不减,长时运行内存持续增长。
|
||||
- **BUG-03**:工厂 GetB/Destroy 非线程安全,接口线程模型未定义(宿主应单线程调用)。
|
||||
- **BUG-07**:禁止区个数未限制在 30 内,配置过大越界读。
|
||||
- **BUG-07**:禁止区个数在实现内部仍未二次校验,配置过大仍可能越界读;但**配置入口已加防护**——加载器强制 `track_init_prohibite_area_num`/`tas_prohibite_area_num` 在 0~30 且 4 个数组长度等于 `*_num`,非法配置在 `parameters_reload` 阶段即返回 -1、不会进入处理流程(见 10.1)。
|
||||
- **BUG-08**:消亡航迹输出数组容量未传入,仅约定 ≤500。
|
||||
- **BUG-13**:Work_Parameter 等成员构造后未初始化,必须先调 track_process_parameters_initial 再处理。
|
||||
- **BUG-13(参数初始化,已被本次改造改变)**:`Data_Process::rdp_parameter{}` 为零初始化,未加载配置时 `data_rate_tws`/`sigma_*` 等均为 0(非法)。宿主必须在**首次 `data_preprocess` 之前**调用 `parameters_reload()` 且确认返回 0;加载失败时保留的是上一次成功参数(首次失败则为全 0),DLL 不会自动纠正。
|
||||
- **BUG-18**:卡尔曼两点/三点初始化对零/负时间差无保护(起批时间戳必须严格递增)。
|
||||
- **BUG-20**:门限对马氏距离平方再次平方(e0bf227 把 `ASSO_THORD` 调为 4 后等效 d < 4),有意保留。
|
||||
- **BUG-20**:门限对马氏距离平方再次平方(e0bf227 把 `associate_threshold` 调为 4 后等效 d < 4),有意保留。
|
||||
- **BUG-25**:TWS 关联/输出未排除 Track_Mode==1 的 TAS 航迹(原 `manual_tracking_flag==0` 过滤代码块已于 fc49707 删除,未恢复过滤)。
|
||||
- **BUG-29**:tracking_stop 后 TAS 队列项延迟到下一次 Beam_Ctrl 才移除。
|
||||
- **BUG-33**:起批关联未显式校验 ΔT>0。
|
||||
- **BUG-36**:TAS 波束关闭时只清 open_flag,其余字段保留旧值。
|
||||
- **归一化幅度 / 速度维 FFT 点数(e8c56f9 新增,输入侧已打通)**:`data_preprocess()` 现已在 TWS、TAS 两条分支都把 `DataRev.amp_asv` / `DataRev.prn_fftn` 拷入内部 `PointRecv`(`data_process.cpp`,并随 `RdpRecPoint` 落库),因此 TWS 关联路径(`track_asso.cpp` 写航迹、输出时写回 `Track`)取到的是有效数据。**TAS 通道仍未打通**:`track_asso_tas.cpp` 的关联与输出路径没有涉及这两个字段,TAS 输出航迹中 `amp_asv_point` / `prn_fftn_point` 仍保持宿主原值,宿主暂不应依赖 TAS 侧的这两个字段。
|
||||
- **TAS 外推上限判据方向与 TWS 相反(50e0b5c)**:TWS 用 `Extrapolate_round <= MAX_EXTRAPOLATE_NUM` 外推,TAS 用 `Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS`,实际效果是 TAS 航迹在未达上限的若干圈内既不外推也不输出(位置与航迹时间停在最后一次关联处),达到上限后才恢复逐帧外推输出;如属笔误,宜与 TWS 侧统一。
|
||||
- **TAS 外推上限判据方向与 TWS 相反(50e0b5c)**:TWS 用 `Extrapolate_round <= extrapolate_max` 外推,TAS 用 `Extrapolate_round >= extrapolate_max_tas`,实际效果是 TAS 航迹在未达上限的若干圈内既不外推也不输出(位置与航迹时间停在最后一次关联处),达到上限后才恢复逐帧外推输出;如属笔误,宜与 TWS 侧统一。
|
||||
- **Flag_Direction 判据的 acos 无定义域钳位(fc49707/42b598e)**:对 `(-x·cos D − y·sin D)/r` 直接取 `acos`(与 BUG-27 同类问题),浮点舍入使参数略微越过 ±1 时返回 NaN,比较为假、按「远离 0」输出;`r` 为 0 时还存在除零。
|
||||
- **航迹质量仅覆盖 TWS 通道(V1.5.8)**:`track_asso_tas.cpp` 既不更新 `Quality` 也不在 `model_output()` 写 `Track.Quality`,因此 TAS 航迹的 `Quality` 停留在起批时的初值(手动转 TAS 后不再变化),TAS 消亡仍按外推圈数。宿主若要按质量过滤,需自行区分 `Track_Mode`。
|
||||
- **质量相关宏存在未使用项(V1.5.8)**:`TRACK_QUALITY_MIN`、`TRACK_DIE_QUALITY_PRAMARY` 定义但未参与任何计算;`TRACK_DIE_ROUND` 在 TWS 普通航迹改为质量消亡后也不再被引用(保留备用)。调参时不要误以为三者生效。
|
||||
- **质量相关参数存在未使用项(V1.5.8)**:配置项 `track_quality_min`、`track_die_quality_pramary` 可配但未参与任何计算;`track_die_round` 在 TWS 普通航迹改为质量消亡后也不再被引用(保留备用)。调参时不要误以为三者生效。
|
||||
- **起批质量门限取的是临时航迹「末点」质量**:`tmp_track_to_trust_track()` 用 `(*Iter)[L-1].quality` 与按长度取的门限比较,而临时航迹质量只在 L=3 时写过所有点、L>3 时只更新末点,因此门限表实际是针对「最新一点的质量」,不是整条航迹的平均质量。
|
||||
- **质量长度项含整数除法**:`1 − Valid_point_sum/Point_sum` 中两操作数均为 `int`,先做整数除法截断再乘 0.5,`Valid_point_sum/Point_sum` 的精度损失最大约 1/Point_sum;对点数很多的航迹影响可忽略,但精确复现公式时需注意。
|
||||
- **数据记录未采用关联信息裁剪与记录策略(P0-a / P2,V1.5.8 优化时明确不做)**:⑥ 关联信息仍按「每个候选一行」写入(单圈 ≈ 点迹数 × 航迹数),未裁剪为只记录取中候选,也未做按容量/时长切库与旧库清理;因此高航迹密度场景下判圈帧仍有可观记录开销(200 目标/150 点压测 p99 ≈ 13~24 ms),记录库也可能很大(60 圈约 1.06 GB)。属本轮刻意取舍,评价实时性时需区分普通帧(p99 ≈ 0.1 ms)与判圈帧。
|
||||
@@ -1067,7 +1191,7 @@ $$
|
||||
- direct_tracking_process(引导跟踪)、tracking_point(手动打波束)为**空实现**;Track_Init_Direct_Tracking::track_init_process_logic 同样为空。
|
||||
- track_asso_direct_tracking.cpp **未加入 CMake 源列表**(与旧 .pro 一致)。
|
||||
- TAS **自动**开启/退出逻辑(tas_auto_start/tas_auto_end)注释停用,仅支持手动 tracking_start/tracking_stop。
|
||||
- RadarPara 中的起批/关联波门字段未被实现读取(见 10.2)。
|
||||
- 新结构体 `RDP_parameter` 已排除旧 `RadarPara` 中实现未使用的字段(`R_TAS_max/min`、`V_TAS_max/min`、`TAS_Height_1~5`、`cfar_th`、`north_angle`、`Beam_num`;其中前两组用于已停用的 TAS 自动开启/退出逻辑),这些功能仍保持停用(见 10.3)。
|
||||
- build/output/ 下为早期打包导出物,其 data_process_class_dll.h 时间戳仍为 32 位 int,与当前 64 位接口不一致,勿作为对接依据。
|
||||
- **数据记录调试功能默认只在 Debug 启用**:CMake 选项 `RDP_DATA_RECORD` 的 AUTO 默认值为「Debug 启用、Release 关闭」,交付/Release 构建下记录模块(`rdp_record.cpp`、`sqlite3.c`)不参与编译、所有记录宏为空操作(导出符号与依赖均无变化);如需在 Release 构建中临时排查可显式 `-DRDP_DATA_RECORD=ON`,但正式交付应保持 AUTO/OFF(见 docs/data_record_debug.md)。记录点中的字段错误只在启用记录功能的构建中才会被检查(关闭时宏丢弃实参)。
|
||||
- **记录表的三个观察**:① 可靠航迹表 `RdpRecTrustTrack` 在输出分支内写库,而该分支入口已把 `Track_Update_Flag` 清零(`track_asso.cpp` / `track_asso_tas.cpp` 均先置 0 再输出),因此表中的 `Track_Update_Flag` 恒为 0,判断「本圈是否更新」应改用该表是否出现对应 `track_id`/`circle_id` 的记录;② `Trust_Track` 的极坐标量快照 `Range`/`Azimuth`/`Elevation`/`Velocity` 只在航迹当圈有输出时回填,长期未输出(外推超限)的航迹在表中该项为 0 或旧值;③ TAS 关联信息行中 `R`/`VT` 为占位 0、`thd_r` 恒为 true、`track_index` 存的是 TAS 目标批号(TWS 行才是航迹表下标),跨通道分析时不要按 TWS 语义解读。
|
||||
@@ -1078,21 +1202,22 @@ $$
|
||||
以下为阅读源码得到的观察,**未在本轮文档更新中改动代码**;修改前请结合实测数据评估:
|
||||
|
||||
- 点-临时航迹关联硬编码 `|Δr| ≥ 5 m` 门限,切向/远距离慢速目标可能漏关联导致起批失败。
|
||||
- 起批判据对马氏距离平方再次平方(`d*d ≤ TRACK_START_THRESHOLD²`),实际门限为 `d ≤ 8`,按字面 `d² ≤ 64` 理解会严重放宽;调 `TRACK_START_THRESHOLD` 时需注意这一层。
|
||||
- 起批判据对马氏距离平方再次平方(`d*d ≤ track_init_threshold²`),`track_init_threshold=8` 时实际门限为 `d ≤ 8`,按字面 `d² ≤ 64` 理解会严重放宽;调 `track_init_threshold` 时需注意这一层。
|
||||
- 折角计算 `acos` 无定义域钳位,浮点越界产生 NaN 时条件恒假(静默丢弃该关联)。
|
||||
- 起批输出数组行号可能越过容量:容量判据用 `trust_track->size() < MAX_TRACK_INDEX`,而输出行计数在 track_asso 阶段(track_die 删除航迹之前)已推进,极端场景(当圈消亡 ≥ 2 条且起批占满配额)可写到 `MAX_TRACK_NUM` 之外。
|
||||
- 起批输出数组行号可能越过容量:容量判据用 `trust_track->size() < track_index_max`,而输出行计数在 track_asso 阶段(track_die 删除航迹之前)已推进,极端场景(当圈消亡 ≥ 2 条且起批占满配额)可写到 `MAX_TRACK_NUM` 之外。
|
||||
- 起批输出有多个字段未赋值(见 9.3(4) 与 docs/track_init_readme.md 第 14 节),宿主不应依赖这些字段。
|
||||
|
||||
---
|
||||
|
||||
## 12. 版本历史
|
||||
|
||||
当前代码版本:**V1.5.8**(`parameters.h` 的 `PATCH_VERSION`;版本号提交为 3088ce4,2026-10-20)。版本号自 1.5.6 起可运行时读取(`version_info`,见 7.1)。中间状态 b4aed8a 曾把版本号置为 7,但未单独发布/记录,本文档按 1.5.8 汇总。**1.5.8 之后又有若干未发版的代码改动**(构建脚本重构、记录性能优化、TAS 消亡圈数与高度平滑调整、质量入库),已记入 CHANGELOG 的 `[Unreleased]`,下表前 5 行为其提交。
|
||||
当前代码版本:**V1.5.8**(`parameters.h` 的 `PATCH_VERSION`;版本号提交为 3088ce4,2026-10-20)。版本号自 1.5.6 起可运行时读取(`version_info`,见 7.1)。中间状态 b4aed8a 曾把版本号置为 7,但未单独发布/记录,本文档按 1.5.8 汇总。**1.5.8 之后又有若干未发版的代码改动**(构建脚本重构、记录性能优化、TAS 消亡圈数与高度平滑调整、质量入库、**参数配置方式改造**),版本号未升级(仍为 1.5.8),已记入 CHANGELOG 的 `[Unreleased]`,下表前 6 行为其提交/工作区状态。
|
||||
|
||||
近期提交(git log --oneline,新→旧;仅列代码改动,纯文档提交略):
|
||||
|
||||
| 提交 | 内容 |
|
||||
|---|---|
|
||||
| 工作区(本次改造,未提交) | **参数配置方式改造:TOML 配置文件 → `RDP_parameter`**。新增 `rdp_parameter_loader.h/.cpp`(严格校验 11 个配置段、标量键必填、类型/范围校验、未知段/键拒绝、屏蔽区数组长度等于 `*_num`,失败 -1 且不改变当前参数)与示例 `config/config_tmp.toml`;内置 toml++ v3.4.0 单头文件 `third_party/toml/toml.hpp`;公开头删除 `struct RadarPara` 与 `track_process_parameters_initial/modify`,新增 `virtual int parameters_reload(const char*)`(`Data_Process` 实现为加载成功后整体替换 `rdp_parameter`);`data_process.h` 删除 `Work_Parameter`,各模块(kalman 11 个方法、dot_coh、dot_coh_tas、track_asso(_tas/_direct_tracking)、track_init(_direct_tracking)、track_die(_tas)、track_index_mangement、tas_ctrl)改为 `const RDP_parameter& rdp_parameter`,宏与旧字段按映射等价替换;`parameters.h` 仅保留 PI、版本/日志/记录宏、`MAX_TRACK_NUM`、`RDP_TAS_QUEUE_CAPACITY`、`UNCONF_TARGET`、`Round`;`track_index_mangement` 航迹号表改按 `track_index_max` 分配,TAS 队列容量用 `RDP_TAS_QUEUE_CAPACITY`、运行期上限用 `tas_track_num_max`,波束类型用 `default_track_beam_type`。验证(Lead 实测,2026-10-27):8 配置构建成功且 0 warning 0 error;Release REC_OFF DLL 331,776 字节、导出表 31 项(新增 `parameters_reload`,旧参数接口与 `RadarPara` 符号消失);加载器单测 229 项断言、公开接口运行期测试(8 配置各 29 项断言)全部通过;62 个结构体成员 ↔ 62 个加载器键 ↔ 62 个 TOML 键一一对应。详见 CHANGELOG `[Unreleased]` |
|
||||
| 1ffb287 | 增加航迹质量记录到数据库:`RdpRecTempTrack` 增 `quality`,`RdpRecTrustTrack` 增 `Quality`/`Unupdated_time`/`Point_sum`/`Valid_point_sum` 并同步注册 |
|
||||
| 6284f3f | 修改 TAS 模式高度平滑点数:`track_asso_tas.cpp` 分段偏移 `+2/+3/+4/+6` → `+6/+8/+10/+12`,相扫 `H_F_WIN_LEN` 3→6(相扫 TAS 窗口 12/14/16/18、机扫 9/11/13/15;TWS 相扫随之 8/9/10/12) |
|
||||
| 5d105d2 | 修改相扫模式下 TAS 航迹消亡圈数:`TRACK_DIE_ROUND_TAS` 10→20(机扫保持 96),相扫 TAS 实际连续 21 圈无实点才消亡 |
|
||||
@@ -1151,13 +1276,15 @@ $$
|
||||
| 文档 | 内容 |
|
||||
|---|---|
|
||||
| CHANGELOG.md | 变更日志(Keep a Changelog 格式,按 git 提交记录维护) |
|
||||
| docs/data_process_flow_report.md | 数据处理流程与关键逻辑说明(按当前源码整理:前处理与判圈、凝聚、TWS/TAS 关联与滤波、航迹起始、外推消亡、TAS 波束控制、参数脉络;含与本文档的历史差异提示)——原同内容 HTML 渲染版(`data_process_flow_report.html`)已于 e0bf227 删除,HTML 版本在 git 忽略的 `design_docs/` 内留档 |
|
||||
| docs/data_process_flow_report.md | 数据处理流程与关键逻辑说明(按当前源码整理:前处理与判圈、凝聚、TWS/TAS 关联与滤波、航迹起始、外推消亡、TAS 波束控制、参数脉络;参数来源已同步为 TOML → `RDP_parameter`;含与本文档的历史差异提示)——原同内容 HTML 渲染版(`data_process_flow_report.html`)已于 e0bf227 删除,HTML 版本在 git 忽略的 `design_docs/` 内留档 |
|
||||
| docs/track_init_readme.md | 航迹起始(Track Init)专项说明:分阶段流程、起始条件汇总、调参直觉、输出字段映射(含未赋值字段)、已知风险与代码索引 |
|
||||
| docs/data_record_debug.md | 数据记录调试功能说明:开关与构建(`RDP_DATA_RECORD`、C++20 隔离与 C 编译器要求、关闭时零代码依据)、运行期行为(异步写线程、有界队列与丢弃、批事务参数、WAL/PRAGMA、去 AUTOINCREMENT、失败降级与熔断、Shutdown 排空与统计、Destroy→GetB 重连)、记录实体(表)约定、**新增记录点四步配方**(实体→异步入队接口/宏→写线程分支→业务侧收集调用)、当前 6 张在用记录表与记录点清单(含新宏与 file:line)、查询示例、已知限制与验收参考 |
|
||||
| third_party/ormpp/UPSTREAM.md | 内置第三方源码的来源说明:ormpp / iguana / frozen / SQLite 版本、许可证、复现与升级步骤(含 blob 校验值) |
|
||||
| archived_docs/BUG_REPORT.md | 全量逻辑 BUG 分析(P0~P3 共 37 项,含位置、说明、建议、处置结论)——过期留档,仅作历史参考 |
|
||||
| archived_docs/BUG_FIX_REPORT.md | 修复明细、修复状态汇总表与 MSVC 2013 x86 编译验证记录——过期留档(对应 01d28e0 时期) |
|
||||
| requirements.md | 修改需求说明:构建脚本与 VS Code 任务调整(Debug/Release × PHASE_SCANNING/MECHANICAL_SCANNING × `RDP_DATA_RECORD`,原有任务删除后重建 5 个任务)——已按该说明实现(见 4.3);此前同名的数据记录调试功能与日志文件输出需求亦已实现,分别见 docs/data_record_debug.md 与 10.1 |
|
||||
| requirements.md | 修改需求说明:**雷达参数配置方式改造**(删除 `RadarPara`/旧参数接口、新增 `parameters_reload(const char*)`、TOML 配置文件 → `RDP_parameter`)——已按该说明实现(见 10);此前同名的数据记录调试功能与日志文件输出需求亦已实现,分别见 docs/data_record_debug.md 与 10.4 |
|
||||
| config/config_tmp.toml | 参数配置样例(TOML,11 个配置段、62 个键与 `RDP_parameter` 成员一一对应,样例为相扫取值;机扫差异项见 10.1 兼容性提示)——参数表见 10.2 |
|
||||
| third_party/toml/toml.hpp | 内置 toml++ v3.4.0 单头文件(MIT,上游原样保存);仅参数加载器 `rdp_parameter_loader.cpp` 以 `#pragma warning(push, 0)` 包裹使用,来源与许可证信息见文件头 |
|
||||
| backup/data_process_class_dll/data_process_class_dll.pro | 迁移前的 Qt/qmake 工程文件(源文件清单参考) |
|
||||
| .vscode/tasks.json、.vscode/cmake-variants.yaml、.vscode/scripts/*.bat | VS Code 任务、构建变体与构建脚本(构建目录布局见 4.3) |
|
||||
| build/X256_PS/<Debug\|Release>/<REC_*>/bin/RadarConfigParam.ini | 回放测试工具(rdp_playback.exe)配置样例(需自行放置) |
|
||||
|
||||
@@ -0,0 +1,161 @@
|
||||
# 雷达数据处理配置文件(TOML 格式)
|
||||
#
|
||||
# 说明:
|
||||
# 1) 配置段与 struct.h::RDP_parameter 的注释分组一一对应,配置键名与结构体成员名完全一致;
|
||||
# 2) 所有标量键均为必填项,且会做类型与取值范围校验(详见 rdp_parameter_loader.cpp);
|
||||
# 3) 屏蔽区数组使用与结构体成员同名的 TOML 数组,元素个数必须等于对应 *_num(0~30);
|
||||
# *_num = 0 时数组可省略,存在时必须为空数组。
|
||||
|
||||
## 雷达工作参数
|
||||
[radar_params]
|
||||
# 工作模式:0 近程 / 1 中程 / 2 远程
|
||||
radar_work_mode = 0
|
||||
# 频点0频率(GHz)
|
||||
freq_0_GHz = 9.2
|
||||
# 雷达平台高度(m)
|
||||
radar_height = 0
|
||||
# 系统延迟(s)
|
||||
radar_system_delay = 0
|
||||
# TWS模式数据率(s)
|
||||
data_rate_tws = 3
|
||||
# TAS模式数据率(s)
|
||||
data_rate_tas = 1
|
||||
# 纯跟踪模式数据率(s)
|
||||
data_rate_track_only = 0.2
|
||||
|
||||
## 点迹凝聚参数
|
||||
[cohesion]
|
||||
# 距离凝聚范围(m)
|
||||
point_cohesion_range = 80
|
||||
# 速度凝聚范围(m/s)
|
||||
point_cohesion_velocity = 2
|
||||
# 方位凝聚范围(度)
|
||||
point_cohesion_azimuth = 6
|
||||
# 最小凝聚距离(m)
|
||||
point_cohesion_range_min = 100
|
||||
# 最大凝聚距离(m)
|
||||
point_cohesion_range_max = 100000
|
||||
|
||||
## 航迹关联参数
|
||||
[track_associate]
|
||||
# 距离量测精度(m)
|
||||
sigma_range = 10.0
|
||||
# 方位量测精度(弧度)
|
||||
sigma_azimuth = 0.02
|
||||
# 俯仰量测精度(弧度)
|
||||
sigma_pitch = 0.2
|
||||
# 速度量测精度(m/s)
|
||||
sigma_velocity = 2.0
|
||||
# 关联门限
|
||||
associate_threshold = 4.0
|
||||
# 最大外推点数
|
||||
extrapolate_max = 3
|
||||
# TAS最大外推点数
|
||||
extrapolate_max_tas = 5
|
||||
# 高度平滑窗长
|
||||
height_smooth_win = 6
|
||||
|
||||
## 航迹起始参数
|
||||
[track_init]
|
||||
# 起航波门
|
||||
track_init_threshold = 8
|
||||
# 起航夹角(度)
|
||||
track_init_angle = 60
|
||||
# 最少起批点数(3~9)
|
||||
track_init_point_num_min = 3
|
||||
# 3点起批质量门限
|
||||
track_init_quality_L3 = 0.7
|
||||
# 4点起批质量门限
|
||||
track_init_quality_L4 = 0.6
|
||||
# 5点起批质量门限
|
||||
track_init_quality_L5 = 0.5
|
||||
# 6点起批质量门限
|
||||
track_init_quality_L6 = 0.4
|
||||
# 7点起批质量门限
|
||||
track_init_quality_L7 = 0.3
|
||||
# 航迹起始最小速度(m/s)
|
||||
track_init_velocity_min = 1
|
||||
# 航迹起始最大速度(m/s)
|
||||
track_init_velocity_max = 50
|
||||
|
||||
## 航迹消亡参数
|
||||
[track_die]
|
||||
# 航迹消亡圈数
|
||||
track_die_round = 6
|
||||
# 重点航迹消亡圈数
|
||||
track_die_round_pramary = 10
|
||||
# TAS航迹消亡圈数
|
||||
track_die_round_tas = 20
|
||||
# 航迹消批质量条件
|
||||
track_die_quality = 0.3
|
||||
# 重点航迹消批质量条件
|
||||
track_die_quality_pramary = 0.3
|
||||
|
||||
## 航迹质量管理参数
|
||||
[track_quality]
|
||||
# 航迹质量最大值
|
||||
track_quality_max = 100
|
||||
# 航迹质量最小值
|
||||
track_quality_min = 0
|
||||
# 马氏距离权重
|
||||
track_quality_d_weight = 0.5
|
||||
# 信噪比权重
|
||||
track_quality_snr_weight = 0.3
|
||||
# 航迹长度权重
|
||||
track_quality_len_weight = 0.2
|
||||
# 航迹质量迭代系数
|
||||
track_quality_iter_coeff = 0.8
|
||||
# 航迹质量下降系数2次项(必须 <= 0)
|
||||
track_quality_decrease_coeff_o2 = -0.0008
|
||||
# 航迹质量下降系数1次项(必须 <= 0)
|
||||
track_quality_decrease_coeff_o1 = -0.0025
|
||||
|
||||
## 航迹批号管理
|
||||
[track_index]
|
||||
# 最大航迹批号(1~500)
|
||||
track_index_max = 500
|
||||
|
||||
## 波束调度
|
||||
[beam_control]
|
||||
# 默认跟踪波束类型
|
||||
default_track_beam_type = 1
|
||||
# 最大同时跟踪目标数(TAS 队列容量上限 30)
|
||||
tas_track_num_max = 4
|
||||
|
||||
## singer模型机动参数(均必须 > 0)
|
||||
[singer_model]
|
||||
# 模型1
|
||||
model_1_Q_fast = 0.000001
|
||||
model_1_Q_slow = 0.000001
|
||||
# 模型2
|
||||
model_2_Q_fast = 0.01
|
||||
model_2_Q_slow = 0.01
|
||||
# 模型3
|
||||
model_3_Q_fast = 1.0
|
||||
model_3_Q_slow = 1.0
|
||||
|
||||
## 航迹起始屏蔽区(数组元素个数必须等于 track_init_prohibite_area_num)
|
||||
[track_init_prohibite_area]
|
||||
# 航迹起始屏蔽区数(0~30)
|
||||
track_init_prohibite_area_num = 0
|
||||
# 最小距离 (m)
|
||||
track_init_prohibite_area_range_min = []
|
||||
# 最大距离 (m)
|
||||
track_init_prohibite_area_range_max = []
|
||||
# 最小方位角 (度)
|
||||
track_init_prohibite_area_azimuth_min = []
|
||||
# 最大方位角 (度)
|
||||
track_init_prohibite_area_azimuth_max = []
|
||||
|
||||
## 禁止TAS跟踪区域(数组元素个数必须等于 tas_prohibite_area_num)
|
||||
[tas_prohibite_area]
|
||||
# 禁止TAS的区域数(0~30)
|
||||
tas_prohibite_area_num = 0
|
||||
# 最小距离 (m)
|
||||
tas_prohibite_area_range_min = []
|
||||
# 最大距离 (m)
|
||||
tas_prohibite_area_range_max = []
|
||||
# 最小方位角 (度)
|
||||
tas_prohibite_area_azimuth_min = []
|
||||
# 最大方位角 (度)
|
||||
tas_prohibite_area_azimuth_max = []
|
||||
@@ -1,7 +1,9 @@
|
||||
#include "data_process.h"
|
||||
#include "rdp_log.h"
|
||||
#include "rdp_record.h"
|
||||
#include "rdp_parameter_loader.h"
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
@@ -182,7 +184,7 @@ int model)
|
||||
if(model==2)
|
||||
{
|
||||
|
||||
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,Work_Parameter);
|
||||
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv,rdp_parameter);
|
||||
std::vector<PointRecv>().swap(Data_buffer);
|
||||
}
|
||||
|
||||
@@ -196,19 +198,20 @@ int model)
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
latest_timestamp,
|
||||
Work_Parameter
|
||||
rdp_parameter
|
||||
);
|
||||
|
||||
track_die.track_die_process( &trust_track,
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
Track_die_num_Output,
|
||||
rdp_parameter);
|
||||
|
||||
track_init.track_init_process_logic( &point_recv,
|
||||
&trust_track,
|
||||
&temp_track,
|
||||
Trust_Track_Output,
|
||||
Trust_track_num_Output,
|
||||
Work_Parameter);
|
||||
rdp_parameter);
|
||||
|
||||
}
|
||||
|
||||
@@ -218,7 +221,7 @@ int model)
|
||||
*Trust_track_num_Output=0; //航迹更新数
|
||||
*Track_die_num_Output=0; //航迹消亡数目
|
||||
|
||||
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
|
||||
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas,rdp_parameter); //点迹凝聚
|
||||
|
||||
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
|
||||
|
||||
@@ -226,7 +229,7 @@ int model)
|
||||
&trust_track, //航迹文件
|
||||
Trust_Track_Output, //更新航迹信息
|
||||
Trust_track_num_Output, //更新航迹数
|
||||
Work_Parameter, //工作参数
|
||||
rdp_parameter, //工作参数
|
||||
TAS_track_idx,
|
||||
latest_timestamp //最新时间戳
|
||||
);
|
||||
@@ -235,7 +238,8 @@ int model)
|
||||
|
||||
track_die_tas.track_die_process_tas( &trust_track,
|
||||
Track_die_Index_Output,
|
||||
Track_die_num_Output);
|
||||
Track_die_num_Output,
|
||||
rdp_parameter);
|
||||
|
||||
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
|
||||
}
|
||||
@@ -249,27 +253,22 @@ void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
|
||||
int *Trust_track_num_Output)
|
||||
{
|
||||
|
||||
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,Work_Parameter, latest_timestamp);
|
||||
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output,rdp_parameter, latest_timestamp);
|
||||
}
|
||||
|
||||
//数据处理参数初始化
|
||||
int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parameter)
|
||||
//数据处理参数加载(从 TOML 配置文件加载全部工作参数)
|
||||
int Data_Process::parameters_reload(const char* parameter_file_path)
|
||||
{
|
||||
|
||||
//工作参数设置
|
||||
if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
|
||||
RDP_parameter loaded{};
|
||||
std::string error;
|
||||
if (rdp_parameter_load(parameter_file_path, loaded, &error) != 0)
|
||||
{
|
||||
RDP_LOG << "parameters_reload failed: " << error << std::endl;
|
||||
return -1;
|
||||
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
//数据处理参数修改
|
||||
int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter)
|
||||
{
|
||||
if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9)
|
||||
return -1;
|
||||
memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara));
|
||||
rdp_parameter = loaded;
|
||||
RDP_LOG << "parameters_reload ok: " << parameter_file_path << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -68,11 +68,10 @@ public:
|
||||
//手动打跟踪波束
|
||||
int tracking_point(float Azimuth); //方位角
|
||||
|
||||
//参数设置初始化
|
||||
int track_process_parameters_initial(struct RadarPara Radar_Parameter);
|
||||
|
||||
//参数设置修改函数
|
||||
int track_process_parameters_modify(struct RadarPara Radar_Parameter);
|
||||
//参数加载函数(从 TOML 配置文件加载全部工作参数到 rdp_parameter)
|
||||
//参数:parameter_file_path - 配置文件路径
|
||||
//返回:成功返回 0;失败返回 -1(失败时不改变当前参数)
|
||||
int parameters_reload(const char* parameter_file_path) override;
|
||||
|
||||
private:
|
||||
|
||||
@@ -84,6 +83,8 @@ private:
|
||||
std::vector <Trust_Track> trust_track; //可靠航迹 按航迹区存入
|
||||
std::vector <std::vector<Temp_track>> temp_track; //临时航迹 按临时航迹区存入
|
||||
|
||||
RDP_parameter rdp_parameter{}; //工作参数 由 parameters_reload() 从配置文件加载
|
||||
|
||||
int Beam_num; //波位数
|
||||
int beam_count; //波位计数
|
||||
int data_num; //波位1点数
|
||||
@@ -104,8 +105,6 @@ private:
|
||||
|
||||
long long circle_id = 0; //圈id
|
||||
|
||||
struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入
|
||||
|
||||
Dot_Coh dot_coh; //点迹凝聚
|
||||
|
||||
Dot_Coh_TAS dot_coh_tas;
|
||||
|
||||
@@ -109,70 +109,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking
|
||||
int open_flag; // 1 引导跟踪 0 结束引导跟踪
|
||||
};
|
||||
|
||||
// 雷达参数结构体
|
||||
struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
|
||||
{
|
||||
int Beam_num; //波位数
|
||||
|
||||
float Height; //雷达平台高度 (m)
|
||||
|
||||
float Sys_delay; //系统延迟 (s)
|
||||
float R_TAS_max; //TAS最大距离 (m)
|
||||
float R_TAS_min; //TAS最小距离 (m)
|
||||
|
||||
float V_TAS_max; //TAS最大速度 (m/s)
|
||||
float V_TAS_min; //TAS最小速度 (m/s)
|
||||
|
||||
float TAS_Height_1; //TAS跟踪高度1 (m)
|
||||
float TAS_Height_2; //TAS跟踪高度2 (m)
|
||||
float TAS_Height_3; //TAS跟踪高度3 (m)
|
||||
float TAS_Height_4; //TAS跟踪高度4 (m)
|
||||
float TAS_Height_5; //TAS跟踪高度5 (m)
|
||||
|
||||
int track_prohibite_area_num; //禁止航迹起始的区域数目
|
||||
float R_max_track_prohibited[30]; //最大距离 (m)
|
||||
float R_min_track_prohibited[30]; //最小距离 (m)
|
||||
float Azimuth_max_track_prohibited[30]; //最大方位角 (度)
|
||||
float Azimuth_min_track_prohibited[30]; //最小方位角 (度)
|
||||
|
||||
int TAS_prohibite_area_num; //禁止TAS的区域数目
|
||||
float R_max_TAS_prohibited[30]; //最大距离 (m)
|
||||
float R_min_TAS_prohibited[30]; //最小距离 (m)
|
||||
float Azimuth_max_TAS_prohibited[30]; //最大方位角 (度)
|
||||
float Azimuth_min_TAS_prohibited[30]; //最小方位角 (度)
|
||||
|
||||
int cfar_th; //cfar门限
|
||||
|
||||
int work_mode; //工作模式 0进程 1中程 2远程
|
||||
int north_angle; //北偏角
|
||||
|
||||
float V_MAX; //最大速度
|
||||
float V_MIN; //最小速度
|
||||
|
||||
float DATA_RATE_SHORT; //三种不同模式下的 数据率
|
||||
float DATA_RATE_MIDDLE;
|
||||
float DATA_RATE_FAR;
|
||||
|
||||
float DATA_RATE_TAS; //TAS模式数据率,由dll调用者输入;TAS波束输出间隔阈值为 DATA_RATE_TAS 秒
|
||||
|
||||
//数据关联参数
|
||||
int track_start_point_num; //起批点数(典型值 3或4)
|
||||
float track_start_threshold; //起航波门大小(典型值 3)
|
||||
float track_asso_threshold; //关联波门大小(典型值 3)
|
||||
float track_asso_threshold_tas; //关联波门大小tas (典型值 3)
|
||||
|
||||
//singer模型机动参数
|
||||
float Model1_Q_fast; //0.00001
|
||||
float Model1_Q_slow; //0.00001
|
||||
|
||||
float Model2_Q_fast; //0.1
|
||||
float Model2_Q_slow; //0.001
|
||||
|
||||
float Model3_Q_fast; //0.8
|
||||
float Model3_Q_slow; //0.4
|
||||
|
||||
};
|
||||
|
||||
class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll
|
||||
{
|
||||
|
||||
@@ -261,16 +197,12 @@ public:
|
||||
return 0;
|
||||
}
|
||||
|
||||
//参数设置初始化函数
|
||||
virtual int track_process_parameters_initial(struct RadarPara Radar_Parameter)
|
||||
//数据处理参数加载函数(从 TOML 配置文件加载全部工作参数)
|
||||
//参数:parameter_file_path - 配置文件路径(宿主进程当前工作目录可达的路径)
|
||||
//返回:成功返回 0;失败返回 -1(失败时不改变当前参数)
|
||||
virtual int parameters_reload(const char*)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
//参数设置修改函数
|
||||
virtual int track_process_parameters_modify(struct RadarPara Radar_Parameter)
|
||||
{
|
||||
return 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -8,7 +8,7 @@ using namespace std;
|
||||
|
||||
int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
std::vector <PointRecv> *point_recv,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
if(data_input->size()>1)
|
||||
@@ -32,10 +32,10 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
float point_1_A=static_cast<float>((*data_input)[i].amplitude);
|
||||
|
||||
//凝聚条件: 距离、方位接近
|
||||
if(Work_Parameter.work_mode == 0)
|
||||
if(rdp_parameter.radar_work_mode == 0)
|
||||
{
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=static_cast<float>((*data_input)[i].range);
|
||||
@@ -44,7 +44,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
point_0_F=static_cast<float>((*data_input)[i].azimuth);
|
||||
(*data_input)[loop_of_point].use_flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
(*data_input)[i].use_flag=1;
|
||||
@@ -53,7 +53,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
else
|
||||
{
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=static_cast<float>((*data_input)[i].range);
|
||||
@@ -62,7 +62,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
point_0_F=static_cast<float>((*data_input)[i].azimuth);
|
||||
(*data_input)[loop_of_point].use_flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
(*data_input)[i].use_flag=1;
|
||||
@@ -79,7 +79,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
std::vector <PointRecv>::iterator Iter;
|
||||
for (Iter=data_input->begin(); Iter!=data_input->end();)
|
||||
{
|
||||
if((*Iter).use_flag==1 || (*Iter).range <R_MIN || (*Iter).range > R_MAX )
|
||||
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
|
||||
{
|
||||
data_input->erase(Iter);
|
||||
Iter=data_input->begin();
|
||||
@@ -100,7 +100,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
||||
|
||||
int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
|
||||
std::vector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
@@ -148,10 +148,10 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
|
||||
float point_1_A=static_cast<float>(data_tmp[i].amplitude);
|
||||
|
||||
//凝聚条件: 距离、方位接近
|
||||
if(Work_Parameter.work_mode == 0)
|
||||
if(rdp_parameter.radar_work_mode == 0)
|
||||
{
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=static_cast<float>(data_tmp[i].range);
|
||||
@@ -160,7 +160,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
|
||||
point_0_F=static_cast<float>(data_tmp[i].azimuth);
|
||||
data_tmp[loop_of_point].use_flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
||||
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_tmp[i].use_flag=1;
|
||||
@@ -169,7 +169,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
|
||||
else
|
||||
{
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
|
||||
&& point_0_A <point_1_A )//
|
||||
{
|
||||
point_0_R=static_cast<float>(data_tmp[i].range);
|
||||
@@ -178,7 +178,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
|
||||
point_0_F=static_cast<float>(data_tmp[i].azimuth);
|
||||
data_tmp[loop_of_point].use_flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
||||
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI )//
|
||||
&& point_0_A >= point_1_A)//
|
||||
{
|
||||
data_tmp[i].use_flag=1;
|
||||
@@ -195,7 +195,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
|
||||
std::vector <PointRecv>::iterator Iter;
|
||||
for (Iter=data_tmp.begin(); Iter!=data_tmp.end();)
|
||||
{
|
||||
if((*Iter).use_flag==1 || (*Iter).range <R_MIN || (*Iter).range > R_MAX )
|
||||
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
|
||||
{
|
||||
data_tmp.erase(Iter);
|
||||
Iter=data_tmp.begin();
|
||||
|
||||
@@ -15,13 +15,13 @@ public:
|
||||
// 点迹凝聚函数
|
||||
int dot_coh_process( std::vector <PointRecv> *data_input, //输入的点迹
|
||||
std::vector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
|
||||
//点迹凝聚函数,缓存一帧,一边输出,一边进行滑窗凝聚
|
||||
int dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
|
||||
std::vector <PointRecv> *point_recv, //输出点迹
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
|
||||
//构造函数
|
||||
|
||||
@@ -6,8 +6,8 @@
|
||||
using namespace std;
|
||||
|
||||
int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
|
||||
std::vector <PointRecv> *point_recv_tas //输出点迹
|
||||
)
|
||||
std::vector <PointRecv> *point_recv_tas, //输出点迹
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
if (data_input->size() == 0) {
|
||||
return 1;
|
||||
@@ -32,7 +32,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
|
||||
|
||||
//凝聚条件: 距离、方位接近
|
||||
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
||||
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)
|
||||
if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)
|
||||
&& point_0_A <point_1_A )
|
||||
{
|
||||
point_0_R=static_cast<float>((*data_input)[i].range);
|
||||
@@ -41,7 +41,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
|
||||
point_0_F=static_cast<float>((*data_input)[i].azimuth);
|
||||
(*data_input)[loop_of_point].use_flag=1;
|
||||
}
|
||||
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)
|
||||
else if( (fabs(point_0_R-point_1_R)<=rdp_parameter.point_cohesion_range && delta_F <=rdp_parameter.point_cohesion_azimuth/180.0*PI && fabs(point_0_V-point_1_V)<=rdp_parameter.point_cohesion_velocity)
|
||||
&& point_0_A >= point_1_A)
|
||||
{
|
||||
(*data_input)[i].use_flag=1;
|
||||
@@ -55,7 +55,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
|
||||
std::vector <PointRecv>::iterator Iter;
|
||||
for (Iter=data_input->begin(); Iter!=data_input->end();)
|
||||
{
|
||||
if((*Iter).use_flag==1 || (*Iter).range <R_MIN || (*Iter).range > R_MAX )
|
||||
if((*Iter).use_flag==1 || (*Iter).range <rdp_parameter.point_cohesion_range_min || (*Iter).range > rdp_parameter.point_cohesion_range_max )
|
||||
{
|
||||
data_input->erase(Iter);
|
||||
Iter=data_input->begin();
|
||||
|
||||
@@ -9,8 +9,8 @@ public:
|
||||
|
||||
// 点迹凝聚函数
|
||||
int dot_coh_tas_process(std::vector <PointRecv> *data_input, //输入点迹
|
||||
std::vector <PointRecv> *point_recv_tas //输出点迹
|
||||
);
|
||||
std::vector <PointRecv> *point_recv_tas, //输出点迹
|
||||
const RDP_parameter& rdp_parameter);
|
||||
};
|
||||
|
||||
#endif // DOT_COH_TAS_H
|
||||
|
||||
@@ -15,7 +15,7 @@ static double wrapAnglePi(double a)
|
||||
}
|
||||
|
||||
|
||||
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4])
|
||||
void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4], const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
X[0]=Z1[0];
|
||||
@@ -26,13 +26,13 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
double R[2][2];
|
||||
R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R[1][0]=R[0][1];
|
||||
|
||||
P[0][0]=R[0][0];
|
||||
@@ -57,7 +57,7 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub
|
||||
|
||||
}
|
||||
|
||||
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
|
||||
double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
@@ -92,13 +92,13 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
Matrix4d P_present;
|
||||
@@ -122,7 +122,7 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T)
|
||||
|
||||
}
|
||||
|
||||
void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6])
|
||||
void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6], const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
double x0=Z0[0];
|
||||
double y0=Z0[1];
|
||||
@@ -145,23 +145,23 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z0[0],Z0[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
R0[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R0[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R0[1][0]=R0[0][1];
|
||||
|
||||
Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta);
|
||||
R1[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R1[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R1[1][0]=R1[0][1];
|
||||
|
||||
Coor_trans.cart2polar(Z2[0],Z2[1],&rho,&theta);
|
||||
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R2[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R2[1][0]=R2[0][1];
|
||||
|
||||
double P11[3][3]={{R2[0][0], R2[0][0]/T2, (R2[0][0]/T2)/((T2+T1)/2)} ,
|
||||
@@ -193,7 +193,7 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d
|
||||
|
||||
};
|
||||
|
||||
double kalman::d_cal(double Z[2],double X[6],double P[6][6])
|
||||
double kalman::d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
VectorXd X_pred(6);
|
||||
MatrixXd P_pred(6,6);
|
||||
@@ -218,13 +218,13 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
//S
|
||||
@@ -240,7 +240,7 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6])
|
||||
|
||||
};
|
||||
|
||||
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind)
|
||||
double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
MatrixXd F1(6,6);
|
||||
@@ -284,9 +284,9 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
|
||||
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
|
||||
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
|
||||
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
|
||||
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
|
||||
R(0,1)=0;
|
||||
R(1,0)=0;
|
||||
R(2,0)=0;
|
||||
@@ -299,7 +299,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
|
||||
|
||||
//d
|
||||
Vector3d Z_mea(Z[0],Z[1],Z[2]);
|
||||
@@ -313,7 +313,7 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6]
|
||||
|
||||
}
|
||||
|
||||
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind)
|
||||
double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
MatrixXd F1(6,6);
|
||||
@@ -362,15 +362,15 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
Matrix3d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
|
||||
R(2,0)=0;
|
||||
R(2,1)=0;
|
||||
R(0,2)=0;
|
||||
@@ -381,7 +381,7 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
// bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
|
||||
|
||||
//d
|
||||
Vector3d delta_z;
|
||||
@@ -431,7 +431,7 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6]
|
||||
|
||||
void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],double P_cur[6][6],double Z[3],
|
||||
double X_filter[6],double P_filter[6][6],double S_filter[3][3],
|
||||
double prt,double freq_ind )
|
||||
double prt,double freq_ind, const RDP_parameter& rdp_parameter )
|
||||
{
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
@@ -477,9 +477,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
|
||||
H(2,3) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,4)=-y/sqrt(x*x+y*y); H(2,5)=0;
|
||||
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
|
||||
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
|
||||
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
|
||||
R(0,1)=0;
|
||||
R(1,0)=0;
|
||||
R(2,0)=0;
|
||||
@@ -496,7 +496,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
|
||||
K=P_pred*H.transpose()*S.inverse();
|
||||
|
||||
//bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
|
||||
|
||||
//d
|
||||
Vector3d delta_z;
|
||||
@@ -530,7 +530,7 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d
|
||||
|
||||
void kalman::kalman_filter(double F[6][6], double Q[6][6],
|
||||
double X_cur[6],double P_cur[6][6],
|
||||
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2])
|
||||
double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
MatrixXd F1(6,6);
|
||||
MatrixXd Q1(6,6);
|
||||
@@ -570,13 +570,13 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
Matrix2d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
|
||||
//S
|
||||
@@ -612,7 +612,7 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6],
|
||||
|
||||
};
|
||||
|
||||
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind)
|
||||
double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//X(k+1|k)
|
||||
@@ -663,9 +663,9 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
|
||||
H(2,2) = -x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); H(2,3)=-y/sqrt(x*x+y*y);
|
||||
|
||||
Matrix3d R;
|
||||
R(0,0)=SIGMA_R*SIGMA_R;
|
||||
R(1,1)=SIGMA_A*SIGMA_A;
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
R(0,0)=rdp_parameter.sigma_range*rdp_parameter.sigma_range;
|
||||
R(1,1)=rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth;
|
||||
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
|
||||
R(0,1)=0;
|
||||
R(1,0)=0;
|
||||
R(2,0)=0;
|
||||
@@ -678,7 +678,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
|
||||
|
||||
//d
|
||||
Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]);
|
||||
@@ -692,7 +692,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl
|
||||
|
||||
}
|
||||
|
||||
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind)
|
||||
double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
//X(k+1|k)
|
||||
Matrix4d F;
|
||||
@@ -744,15 +744,15 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
|
||||
double rho,theta;
|
||||
coor_trans Coor_trans;
|
||||
Coor_trans.cart2polar(Z[0],Z[1],&rho,&theta);
|
||||
double lambda_theta=exp(-SIGMA_A*SIGMA_A/2);
|
||||
double lambda_theta1=exp(-2*SIGMA_A*SIGMA_A);
|
||||
double lambda_theta=exp(-rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth/2);
|
||||
double lambda_theta1=exp(-2*rdp_parameter.sigma_azimuth*rdp_parameter.sigma_azimuth);
|
||||
|
||||
Matrix3d R;
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta);
|
||||
R(0,0)=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1+lambda_theta1*cos(2*theta));
|
||||
R(1,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*(1-lambda_theta1*cos(2*theta));
|
||||
R(0,1)=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+rdp_parameter.sigma_range*rdp_parameter.sigma_range)*lambda_theta1*sin(2*theta);
|
||||
R(1,0)=R(0,1);
|
||||
R(2,2)=SIGMA_V*SIGMA_V;
|
||||
R(2,2)=rdp_parameter.sigma_velocity*rdp_parameter.sigma_velocity;
|
||||
R(2,0)=0;
|
||||
R(2,1)=0;
|
||||
R(0,2)=0;
|
||||
@@ -763,7 +763,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
|
||||
S=H*P_pred*H.transpose()+R;
|
||||
|
||||
//bind_speed
|
||||
double v_bind=Bind_speed(prt, freq_ind);
|
||||
double v_bind=Bind_speed(prt, freq_ind, rdp_parameter);
|
||||
|
||||
//d
|
||||
Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr);
|
||||
@@ -777,9 +777,9 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4]
|
||||
|
||||
};
|
||||
|
||||
double kalman::Bind_speed(double prt,double freq_ind)
|
||||
double kalman::Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
double freq=FREQ0+freq_ind*0.02;
|
||||
double freq=rdp_parameter.freq_0_GHz+freq_ind*0.02;
|
||||
|
||||
if (prt <= 0.0 || freq <= 0.0)
|
||||
return 1.0; // 避免除零;正常流程 PRI 不允许为 0
|
||||
|
||||
@@ -13,29 +13,29 @@ public:
|
||||
|
||||
void kalman_pred(double F[6][6], double Q[6][6], double X[6], double P[6][6], double X_pred[6], double P_pred[6][6]);
|
||||
|
||||
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器
|
||||
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4], const RDP_parameter& rdp_parameter);//两点初始化卡尔曼滤波器
|
||||
|
||||
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d
|
||||
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T, const RDP_parameter& rdp_parameter); //计算点 临时航迹的d
|
||||
|
||||
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
|
||||
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
|
||||
|
||||
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind);
|
||||
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
|
||||
|
||||
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]);
|
||||
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6], const RDP_parameter& rdp_parameter);
|
||||
|
||||
double d_cal(double Z[2],double X[6],double P[6][6]);
|
||||
double d_cal(double Z[2],double X[6],double P[6][6], const RDP_parameter& rdp_parameter);
|
||||
|
||||
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind);
|
||||
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind, const RDP_parameter& rdp_parameter);
|
||||
|
||||
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind);
|
||||
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind, const RDP_parameter& rdp_parameter);
|
||||
|
||||
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]);
|
||||
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2], const RDP_parameter& rdp_parameter);
|
||||
|
||||
void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3],
|
||||
double X_filter[6],double P_filter[6][6],double S_filter[3][3],
|
||||
double prt,double freq_ind );
|
||||
double prt,double freq_ind, const RDP_parameter& rdp_parameter );
|
||||
|
||||
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
|
||||
double Bind_speed(double prt,double freq_ind, const RDP_parameter& rdp_parameter); //根据PRT 和 频率 计算不模糊速度
|
||||
|
||||
};
|
||||
#endif // KALMAN_H
|
||||
|
||||
@@ -4,9 +4,6 @@
|
||||
/**********************************雷达参数*************************************************/
|
||||
#define PI 3.1415926f
|
||||
|
||||
//频点(GHz)
|
||||
#define FREQ0 9.2
|
||||
|
||||
/**********************************版本信息*************************************************/
|
||||
#define MAJOR_VERSION 1
|
||||
#define MINOR_VERSION 5
|
||||
@@ -90,134 +87,17 @@
|
||||
#define RDP_RECORD_DROP_LOG_INTERVAL 1024u
|
||||
#endif
|
||||
|
||||
//#define MECHANICAL_SCANNING
|
||||
//#define PHASE_SCANNING
|
||||
|
||||
/******************************通用数据处理参数**************************************/
|
||||
//航迹质量管理参数
|
||||
#define TRACK_QUALITY_MAX 100
|
||||
#define TRACK_QUALITY_MIN 0
|
||||
#define TRACK_QUALITY_D_WEIGHT 0.5
|
||||
#define TRACK_QUALITY_SNR_WEIGHT 0.3
|
||||
#define TRACK_QUALITY_LEN_WEIGHT 0.2
|
||||
#define TRACK_QUALITY_ITER_COEFF 0.8
|
||||
#define TRACK_QUALITY_DECREASE_COEFF_O2 -0.0008
|
||||
#define TRACK_QUALITY_DECREASE_COEFF_O1 -0.0025
|
||||
|
||||
//航迹起批质量条件
|
||||
#define TRACK_START_QUALITY_L3 0.7
|
||||
#define TRACK_START_QUALITY_L4 0.6
|
||||
#define TRACK_START_QUALITY_L5 0.5
|
||||
#define TRACK_START_QUALITY_L6 0.4
|
||||
#define TRACK_START_QUALITY_L7 0.3
|
||||
|
||||
//航迹消批质量条件
|
||||
#define TRACK_DIE_QUALITY 0.3
|
||||
#define TRACK_DIE_QUALITY_PRAMARY 0.3
|
||||
|
||||
#ifdef MECHANICAL_SCANNING
|
||||
/******************************机扫数据处理参数**************************************/
|
||||
// #define DATA_RATE_TAS 0.0625
|
||||
|
||||
#define SIGMA_R 10.0 //测量误差
|
||||
#define SIGMA_A 0.02
|
||||
#define SIGMA_E 0.2
|
||||
#define SIGMA_V 2.0
|
||||
|
||||
#define DOT_COH_RANGE 80 //点迹凝聚
|
||||
#define DOT_COH_V 2
|
||||
#define DOT_COH_AZI 6
|
||||
|
||||
#define MAX_BEAM_NUM 100 //最大TWS波位数
|
||||
/******************************编译期容量常量***********************************************/
|
||||
// 说明:下列常量是「接口/内存布局」需要的编译期容量,不是雷达工作参数,
|
||||
// 雷达工作参数统一由 TOML 配置文件经 parameters_reload() 载入 struct.h::RDP_parameter。
|
||||
|
||||
//输出航迹数组的容量(宿主接口固定维度,见 data_process_class_dll.h)
|
||||
#define MAX_TRACK_NUM 500
|
||||
#define MAX_TRACK_INDEX 500 //最大航迹批号
|
||||
|
||||
#define ASSOCIATE_THRESHOLD_MIN 1
|
||||
#define ASSOCIATE_THRESHOLD_MID 3 //关联波门
|
||||
#define ASSOCIATE_THRESHOLD_MAX 10
|
||||
|
||||
#define R_MIN 100
|
||||
#define R_MAX 100000
|
||||
|
||||
#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批
|
||||
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
|
||||
|
||||
#define MAX_EXTRAPOLATE_NUM 3 //最大外推点数
|
||||
#define MAX_EXTRAPOLATE_NUM_TAS 48 //TAS最大外推点数
|
||||
|
||||
#define TRACK_DIE_ROUND 6 // 航迹消亡圈数
|
||||
#define TRACK_DIE_ROUND_PRIMARY 10 // 重点航迹消亡圈数
|
||||
#define TRACK_DIE_ROUND_TAS 96 //TAS航迹消亡圈数
|
||||
|
||||
#define ASSO_THORD 4
|
||||
|
||||
#define H_F_WIN_LEN 3
|
||||
|
||||
#define TAS_QUEUE_LENGTH 1 //Tas调用的cpi数
|
||||
#define MAX_TAS_NUM 1
|
||||
|
||||
//TAS 跟踪队列容量上限(运行期配置项 tas_track_num_max 不得超过该值)
|
||||
#define RDP_TAS_QUEUE_CAPACITY 30
|
||||
//未确认目标类型(Track::Target_Type 的缺省语义值)
|
||||
#define UNCONF_TARGET 0
|
||||
|
||||
#define DIRECT_TRACKING_QUEUE_LENGTH 10
|
||||
|
||||
#pragma message("MECHANICAL_SCANNING")
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef PHASE_SCANNING
|
||||
|
||||
/******************************相扫数据处理参数**************************************/
|
||||
// #define DATA_RATE_TAS 0.3
|
||||
|
||||
#define SIGMA_R 10.0 //测量误差
|
||||
#define SIGMA_A 0.02
|
||||
#define SIGMA_E 0.2
|
||||
#define SIGMA_V 2.0
|
||||
|
||||
#define DOT_COH_RANGE 80 //点迹凝聚
|
||||
#define DOT_COH_V 2
|
||||
#define DOT_COH_AZI 6
|
||||
|
||||
#define MAX_BEAM_NUM 100 //最大TWS波位数
|
||||
|
||||
#define MAX_TRACK_NUM 500
|
||||
#define MAX_TRACK_INDEX 500 //最大航迹批号
|
||||
|
||||
#define ASSOCIATE_THRESHOLD_MIN 1
|
||||
#define ASSOCIATE_THRESHOLD_MID 3 //关联波门
|
||||
#define ASSOCIATE_THRESHOLD_MAX 10
|
||||
|
||||
#define R_MIN 100
|
||||
#define R_MAX 100000
|
||||
|
||||
//#define V_MAX 500 //最大速度
|
||||
//#define V_MIN 1 //速度最小
|
||||
#define TRACK_START_THRESHOLD 8 //起航波门 3 越大越容易起批
|
||||
#define ALPHA_START 60 //起航夹角 120 越大越容易起批
|
||||
|
||||
#define MAX_EXTRAPOLATE_NUM 3 //最大外推点数
|
||||
#define MAX_EXTRAPOLATE_NUM_TAS 5 //TAS最大外推点数
|
||||
|
||||
#define TRACK_DIE_ROUND 6 // 航迹消亡圈数
|
||||
#define TRACK_DIE_ROUND_PRIMARY 10 // 重点航迹消亡圈数
|
||||
#define TRACK_DIE_ROUND_TAS 20 //TAS航迹消亡圈数
|
||||
|
||||
#define ASSO_THORD 4
|
||||
|
||||
#define H_F_WIN_LEN 6
|
||||
|
||||
#define TAS_QUEUE_LENGTH 4 //Tas调用的cpi数
|
||||
#define MAX_TAS_NUM 4
|
||||
|
||||
#define UNCONF_TARGET 0
|
||||
|
||||
#define DIRECT_TRACKING_QUEUE_LENGTH 10
|
||||
|
||||
#pragma message("PHASE_SCANNING")
|
||||
|
||||
#endif
|
||||
|
||||
#define Round(x) (((x) > 0) \
|
||||
? (((int)((x) + 0.5) > (int)(x)) ? ((int)(x) + 1) : ((int)(x))) \
|
||||
: (((int)((x) - 0.5) < (int)(x)) ? ((int)(x) - 1) : ((int)(x))))
|
||||
|
||||
@@ -0,0 +1,614 @@
|
||||
#include "rdp_parameter_loader.h"
|
||||
#include "parameters.h"
|
||||
#include <cfloat>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <vector>
|
||||
|
||||
// toml++ 为纯头文件第三方库(third_party/toml/toml.hpp,v3.4.0)。
|
||||
// 第三方头文件不参与本工程的告警治理:包含期间临时屏蔽告警(/W3 下仍要求 0 告警)。
|
||||
#if defined(_MSC_VER)
|
||||
#pragma warning(push, 0)
|
||||
#endif
|
||||
#include <toml.hpp>
|
||||
#if defined(_MSC_VER)
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
|
||||
/************************************************************************************************
|
||||
* 实现说明
|
||||
*
|
||||
* 配置文件结构与 RDP_parameter 一一对应:
|
||||
* [radar_params] 雷达工作参数
|
||||
* [cohesion] 点迹凝聚参数
|
||||
* [track_associate] 航迹关联参数
|
||||
* [track_init] 航迹起始参数
|
||||
* [track_die] 航迹消亡参数
|
||||
* [track_quality] 航迹质量管理参数
|
||||
* [track_index] 航迹批号管理
|
||||
* [beam_control] 波束调度参数
|
||||
* [singer_model] singer 模型机动参数
|
||||
* [track_init_prohibite_area] 航迹起始屏蔽区(数组)
|
||||
* [tas_prohibite_area] 禁止 TAS 跟踪区(数组)
|
||||
*
|
||||
* 标量参数用「字段描述表」集中登记(段名 + 键名 + 成员指针),避免逐项重复代码;
|
||||
* 屏蔽区数组因为「长度由 *_num 决定」单独处理。加载流程:
|
||||
* 解析 TOML -> 校验段/键集合 -> 读取标量与数组到临时对象 -> 取值校验 -> 一次性赋值。
|
||||
* 任一步失败都不会修改调用方传入的 parameter。
|
||||
************************************************************************************************/
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// 屏蔽区数组容量,必须与 struct.h::RDP_parameter 中的数组长度一致。
|
||||
constexpr int kProhibiteAreaCapacity = 30;
|
||||
|
||||
// 带上下文的错误收集:只记录第一条错误,统一返回 false 便于链式返回。
|
||||
class LoadContext
|
||||
{
|
||||
public:
|
||||
explicit LoadContext(std::string* error_msg) : error_msg_(error_msg) {}
|
||||
|
||||
bool fail(const std::string& message)
|
||||
{
|
||||
if (!failed_)
|
||||
{
|
||||
failed_ = true;
|
||||
if (error_msg_ != nullptr)
|
||||
*error_msg_ = message;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool failed() const { return failed_; }
|
||||
|
||||
private:
|
||||
std::string* error_msg_;
|
||||
bool failed_ = false;
|
||||
};
|
||||
|
||||
// 标量字段描述:TOML 段名、键名、RDP_parameter 成员指针。
|
||||
struct FloatField
|
||||
{
|
||||
const char* section;
|
||||
const char* key;
|
||||
float RDP_parameter::*member;
|
||||
};
|
||||
|
||||
struct IntField
|
||||
{
|
||||
const char* section;
|
||||
const char* key;
|
||||
int RDP_parameter::*member;
|
||||
};
|
||||
|
||||
// ------------------------------ 标量字段登记表 ------------------------------
|
||||
const FloatField kFloatFields[] = {
|
||||
// [radar_params] 雷达工作参数
|
||||
{"radar_params", "freq_0_GHz", &RDP_parameter::freq_0_GHz},
|
||||
{"radar_params", "radar_height", &RDP_parameter::radar_height},
|
||||
{"radar_params", "radar_system_delay", &RDP_parameter::radar_system_delay},
|
||||
{"radar_params", "data_rate_tws", &RDP_parameter::data_rate_tws},
|
||||
{"radar_params", "data_rate_tas", &RDP_parameter::data_rate_tas},
|
||||
{"radar_params", "data_rate_track_only", &RDP_parameter::data_rate_track_only},
|
||||
// [cohesion] 点迹凝聚参数
|
||||
{"cohesion", "point_cohesion_range", &RDP_parameter::point_cohesion_range},
|
||||
{"cohesion", "point_cohesion_velocity", &RDP_parameter::point_cohesion_velocity},
|
||||
{"cohesion", "point_cohesion_azimuth", &RDP_parameter::point_cohesion_azimuth},
|
||||
{"cohesion", "point_cohesion_range_min", &RDP_parameter::point_cohesion_range_min},
|
||||
{"cohesion", "point_cohesion_range_max", &RDP_parameter::point_cohesion_range_max},
|
||||
// [track_associate] 航迹关联参数
|
||||
{"track_associate", "sigma_range", &RDP_parameter::sigma_range},
|
||||
{"track_associate", "sigma_azimuth", &RDP_parameter::sigma_azimuth},
|
||||
{"track_associate", "sigma_pitch", &RDP_parameter::sigma_pitch},
|
||||
{"track_associate", "sigma_velocity", &RDP_parameter::sigma_velocity},
|
||||
{"track_associate", "associate_threshold", &RDP_parameter::associate_threshold},
|
||||
// [track_init] 航迹起始参数
|
||||
{"track_init", "track_init_threshold", &RDP_parameter::track_init_threshold},
|
||||
{"track_init", "track_init_angle", &RDP_parameter::track_init_angle},
|
||||
{"track_init", "track_init_quality_L3", &RDP_parameter::track_init_quality_L3},
|
||||
{"track_init", "track_init_quality_L4", &RDP_parameter::track_init_quality_L4},
|
||||
{"track_init", "track_init_quality_L5", &RDP_parameter::track_init_quality_L5},
|
||||
{"track_init", "track_init_quality_L6", &RDP_parameter::track_init_quality_L6},
|
||||
{"track_init", "track_init_quality_L7", &RDP_parameter::track_init_quality_L7},
|
||||
{"track_init", "track_init_velocity_min", &RDP_parameter::track_init_velocity_min},
|
||||
{"track_init", "track_init_velocity_max", &RDP_parameter::track_init_velocity_max},
|
||||
// [track_die] 航迹消亡参数
|
||||
{"track_die", "track_die_quality", &RDP_parameter::track_die_quality},
|
||||
{"track_die", "track_die_quality_pramary", &RDP_parameter::track_die_quality_pramary},
|
||||
// [track_quality] 航迹质量管理参数
|
||||
{"track_quality", "track_quality_max", &RDP_parameter::track_quality_max},
|
||||
{"track_quality", "track_quality_min", &RDP_parameter::track_quality_min},
|
||||
{"track_quality", "track_quality_d_weight", &RDP_parameter::track_quality_d_weight},
|
||||
{"track_quality", "track_quality_snr_weight", &RDP_parameter::track_quality_snr_weight},
|
||||
{"track_quality", "track_quality_len_weight", &RDP_parameter::track_quality_len_weight},
|
||||
{"track_quality", "track_quality_iter_coeff", &RDP_parameter::track_quality_iter_coeff},
|
||||
{"track_quality", "track_quality_decrease_coeff_o2", &RDP_parameter::track_quality_decrease_coeff_o2},
|
||||
{"track_quality", "track_quality_decrease_coeff_o1", &RDP_parameter::track_quality_decrease_coeff_o1},
|
||||
// [singer_model] singer 模型机动参数
|
||||
{"singer_model", "model_1_Q_fast", &RDP_parameter::model_1_Q_fast},
|
||||
{"singer_model", "model_1_Q_slow", &RDP_parameter::model_1_Q_slow},
|
||||
{"singer_model", "model_2_Q_fast", &RDP_parameter::model_2_Q_fast},
|
||||
{"singer_model", "model_2_Q_slow", &RDP_parameter::model_2_Q_slow},
|
||||
{"singer_model", "model_3_Q_fast", &RDP_parameter::model_3_Q_fast},
|
||||
{"singer_model", "model_3_Q_slow", &RDP_parameter::model_3_Q_slow},
|
||||
};
|
||||
|
||||
const IntField kIntFields[] = {
|
||||
// [radar_params] 雷达工作参数
|
||||
{"radar_params", "radar_work_mode", &RDP_parameter::radar_work_mode},
|
||||
// [track_associate] 航迹关联参数
|
||||
{"track_associate", "extrapolate_max", &RDP_parameter::extrapolate_max},
|
||||
{"track_associate", "extrapolate_max_tas", &RDP_parameter::extrapolate_max_tas},
|
||||
{"track_associate", "height_smooth_win", &RDP_parameter::height_smooth_win},
|
||||
// [track_init] 航迹起始参数
|
||||
{"track_init", "track_init_point_num_min", &RDP_parameter::track_init_point_num_min},
|
||||
// [track_die] 航迹消亡参数
|
||||
{"track_die", "track_die_round", &RDP_parameter::track_die_round},
|
||||
{"track_die", "track_die_round_pramary", &RDP_parameter::track_die_round_pramary},
|
||||
{"track_die", "track_die_round_tas", &RDP_parameter::track_die_round_tas},
|
||||
// [track_index] 航迹批号管理
|
||||
{"track_index", "track_index_max", &RDP_parameter::track_index_max},
|
||||
// [beam_control] 波束调度参数
|
||||
{"beam_control", "default_track_beam_type", &RDP_parameter::default_track_beam_type},
|
||||
{"beam_control", "tas_track_num_max", &RDP_parameter::tas_track_num_max},
|
||||
// 屏蔽区数目(对应数组长度由它决定)
|
||||
{"track_init_prohibite_area", "track_init_prohibite_area_num", &RDP_parameter::track_init_prohibite_area_num},
|
||||
{"tas_prohibite_area", "tas_prohibite_area_num", &RDP_parameter::tas_prohibite_area_num},
|
||||
};
|
||||
|
||||
// 屏蔽区数组描述:段名 + 键名 + 成员指针(数组长度为 kProhibiteAreaCapacity)。
|
||||
struct AreaArrayField
|
||||
{
|
||||
const char* section;
|
||||
const char* key;
|
||||
float (RDP_parameter::*member)[kProhibiteAreaCapacity];
|
||||
};
|
||||
|
||||
const AreaArrayField kAreaArrayFields[] = {
|
||||
{"track_init_prohibite_area", "track_init_prohibite_area_range_min", &RDP_parameter::track_init_prohibite_area_range_min},
|
||||
{"track_init_prohibite_area", "track_init_prohibite_area_range_max", &RDP_parameter::track_init_prohibite_area_range_max},
|
||||
{"track_init_prohibite_area", "track_init_prohibite_area_azimuth_min", &RDP_parameter::track_init_prohibite_area_azimuth_min},
|
||||
{"track_init_prohibite_area", "track_init_prohibite_area_azimuth_max", &RDP_parameter::track_init_prohibite_area_azimuth_max},
|
||||
{"tas_prohibite_area", "tas_prohibite_area_range_min", &RDP_parameter::tas_prohibite_area_range_min},
|
||||
{"tas_prohibite_area", "tas_prohibite_area_range_max", &RDP_parameter::tas_prohibite_area_range_max},
|
||||
{"tas_prohibite_area", "tas_prohibite_area_azimuth_min", &RDP_parameter::tas_prohibite_area_azimuth_min},
|
||||
{"tas_prohibite_area", "tas_prohibite_area_azimuth_max", &RDP_parameter::tas_prohibite_area_azimuth_max},
|
||||
};
|
||||
|
||||
const char* const kAllSections[] = {
|
||||
"radar_params",
|
||||
"cohesion",
|
||||
"track_associate",
|
||||
"track_init",
|
||||
"track_die",
|
||||
"track_quality",
|
||||
"track_index",
|
||||
"beam_control",
|
||||
"singer_model",
|
||||
"track_init_prohibite_area",
|
||||
"tas_prohibite_area",
|
||||
};
|
||||
|
||||
// ------------------------------ 基础读取工具 ------------------------------
|
||||
bool node_to_double(const toml::node& node, double& value)
|
||||
{
|
||||
if (node.is_integer())
|
||||
{
|
||||
value = static_cast<double>(*node.value<std::int64_t>());
|
||||
return true;
|
||||
}
|
||||
if (node.is_floating_point())
|
||||
{
|
||||
value = *node.value<double>();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool read_float(const toml::table& section, const char* section_name, const char* key, float& out, LoadContext& ctx)
|
||||
{
|
||||
const toml::node* node = section.get(key);
|
||||
if (node == nullptr)
|
||||
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + key);
|
||||
|
||||
double value = 0.0;
|
||||
if (!node_to_double(*node, value))
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 必须是数字");
|
||||
|
||||
if (!std::isfinite(value) || std::fabs(value) > static_cast<double>(FLT_MAX))
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 超出 float 可表示范围");
|
||||
|
||||
out = static_cast<float>(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool read_int(const toml::table& section, const char* section_name, const char* key, int& out, LoadContext& ctx)
|
||||
{
|
||||
const toml::node* node = section.get(key);
|
||||
if (node == nullptr)
|
||||
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + key);
|
||||
|
||||
if (!node->is_integer())
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 必须是整数");
|
||||
|
||||
const std::int64_t value = *node->value<std::int64_t>();
|
||||
if (value < std::numeric_limits<int>::min() || value > std::numeric_limits<int>::max())
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + key + " 超出 int 可表示范围");
|
||||
|
||||
out = static_cast<int>(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
// 取配置段;缺失或不是表都算错误(严格模式:11 个配置段全部必填)。
|
||||
const toml::table* get_section(const toml::table& root, const char* section_name, LoadContext& ctx)
|
||||
{
|
||||
const toml::node* node = root.get(section_name);
|
||||
if (node == nullptr)
|
||||
{
|
||||
ctx.fail("缺少配置段 [" + std::string(section_name) + "]");
|
||||
return nullptr;
|
||||
}
|
||||
const toml::table* table = node->as_table();
|
||||
if (table == nullptr)
|
||||
{
|
||||
ctx.fail("配置项 [" + std::string(section_name) + "] 必须是 TOML 表");
|
||||
return nullptr;
|
||||
}
|
||||
return table;
|
||||
}
|
||||
|
||||
// 收集某个段允许出现的全部键(标量 + 数组)。
|
||||
std::vector<std::string_view> allowed_keys_of(const std::string_view section)
|
||||
{
|
||||
std::vector<std::string_view> keys;
|
||||
for (const FloatField& field : kFloatFields)
|
||||
{
|
||||
if (section == field.section)
|
||||
keys.emplace_back(field.key);
|
||||
}
|
||||
for (const IntField& field : kIntFields)
|
||||
{
|
||||
if (section == field.section)
|
||||
keys.emplace_back(field.key);
|
||||
}
|
||||
for (const AreaArrayField& field : kAreaArrayFields)
|
||||
{
|
||||
if (section == field.section)
|
||||
keys.emplace_back(field.key);
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
// 严格校验某个段中只出现允许的键(拼错的键按未知参数拒绝)。
|
||||
bool check_keys_of_section(const toml::table& section, const char* section_name, LoadContext& ctx)
|
||||
{
|
||||
const std::vector<std::string_view> allowed = allowed_keys_of(section_name);
|
||||
for (const auto& [key, value] : section)
|
||||
{
|
||||
(void)value;
|
||||
bool known = false;
|
||||
for (const std::string_view item : allowed)
|
||||
{
|
||||
if (key.str() == item)
|
||||
{
|
||||
known = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!known)
|
||||
return ctx.fail("配置段 [" + std::string(section_name) + "] 中存在未知配置项: " + std::string(key.str()));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// 读取一个屏蔽区段的 4 个数组。
|
||||
// 约定:*_num > 0 时数组必须存在且元素个数等于 *_num;*_num == 0 时数组可省略,
|
||||
// 存在时必须是空数组。数组元素写入 RDP_parameter 的定长数组(未使用位置保持 0)。
|
||||
bool read_area_arrays(const toml::table& section, const char* section_name, int area_num,
|
||||
RDP_parameter& parameter, LoadContext& ctx)
|
||||
{
|
||||
for (const AreaArrayField& field : kAreaArrayFields)
|
||||
{
|
||||
if (std::string_view(field.section) != section_name)
|
||||
continue;
|
||||
|
||||
const toml::node* node = section.get(field.key);
|
||||
if (node == nullptr)
|
||||
{
|
||||
if (area_num == 0)
|
||||
continue;
|
||||
return ctx.fail("缺少配置项 [" + std::string(section_name) + "] " + field.key);
|
||||
}
|
||||
|
||||
const toml::array* array = node->as_array();
|
||||
if (array == nullptr)
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 必须是数组");
|
||||
|
||||
if (static_cast<int>(array->size()) != area_num)
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的元素个数(" +
|
||||
std::to_string(array->size()) + ")与 " + std::string(section_name) + "_num(" +
|
||||
std::to_string(area_num) + ")不一致");
|
||||
|
||||
float* destination = parameter.*(field.member);
|
||||
int index = 0;
|
||||
for (const toml::node& element : *array)
|
||||
{
|
||||
double value = 0.0;
|
||||
if (!node_to_double(element, value))
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的第 " +
|
||||
std::to_string(index + 1) + " 个元素必须是数字");
|
||||
if (!std::isfinite(value) || std::fabs(value) > static_cast<double>(FLT_MAX))
|
||||
return ctx.fail("配置项 [" + std::string(section_name) + "] " + field.key + " 的第 " +
|
||||
std::to_string(index + 1) + " 个元素超出 float 可表示范围");
|
||||
destination[index] = static_cast<float>(value);
|
||||
++index;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ------------------------------ 取值校验 ------------------------------
|
||||
bool validate_parameter(const RDP_parameter& p, LoadContext& ctx)
|
||||
{
|
||||
// 雷达工作参数
|
||||
if (p.radar_work_mode < 0 || p.radar_work_mode > 2)
|
||||
return ctx.fail("参数 radar_work_mode 必须在 0~2 之间(实际 " + std::to_string(p.radar_work_mode) + ")");
|
||||
if (!(p.freq_0_GHz > 0.0f))
|
||||
return ctx.fail("参数 freq_0_GHz 必须大于 0");
|
||||
if (p.radar_system_delay < 0.0f)
|
||||
return ctx.fail("参数 radar_system_delay 不能为负");
|
||||
if (!(p.data_rate_tws > 0.0f))
|
||||
return ctx.fail("参数 data_rate_tws 必须大于 0");
|
||||
if (!(p.data_rate_tas > 0.0f))
|
||||
return ctx.fail("参数 data_rate_tas 必须大于 0");
|
||||
if (p.data_rate_track_only < 0.0f)
|
||||
return ctx.fail("参数 data_rate_track_only 不能为负");
|
||||
|
||||
// 点迹凝聚参数
|
||||
if (!(p.point_cohesion_range > 0.0f))
|
||||
return ctx.fail("参数 point_cohesion_range 必须大于 0");
|
||||
if (!(p.point_cohesion_velocity > 0.0f))
|
||||
return ctx.fail("参数 point_cohesion_velocity 必须大于 0");
|
||||
if (!(p.point_cohesion_azimuth > 0.0f))
|
||||
return ctx.fail("参数 point_cohesion_azimuth 必须大于 0");
|
||||
if (p.point_cohesion_range_min < 0.0f)
|
||||
return ctx.fail("参数 point_cohesion_range_min 不能为负");
|
||||
if (!(p.point_cohesion_range_max > p.point_cohesion_range_min))
|
||||
return ctx.fail("参数 point_cohesion_range_max 必须大于 point_cohesion_range_min");
|
||||
|
||||
// 航迹关联参数
|
||||
if (!(p.sigma_range > 0.0f))
|
||||
return ctx.fail("参数 sigma_range 必须大于 0");
|
||||
if (!(p.sigma_azimuth > 0.0f))
|
||||
return ctx.fail("参数 sigma_azimuth 必须大于 0");
|
||||
if (!(p.sigma_pitch > 0.0f))
|
||||
return ctx.fail("参数 sigma_pitch 必须大于 0");
|
||||
if (!(p.sigma_velocity > 0.0f))
|
||||
return ctx.fail("参数 sigma_velocity 必须大于 0");
|
||||
if (!(p.associate_threshold > 0.0f))
|
||||
return ctx.fail("参数 associate_threshold 必须大于 0");
|
||||
if (p.extrapolate_max < 0)
|
||||
return ctx.fail("参数 extrapolate_max 不能为负");
|
||||
if (p.extrapolate_max_tas < 0)
|
||||
return ctx.fail("参数 extrapolate_max_tas 不能为负");
|
||||
if (p.height_smooth_win < 1)
|
||||
return ctx.fail("参数 height_smooth_win 必须大于等于 1");
|
||||
|
||||
// 航迹起始参数
|
||||
if (!(p.track_init_threshold > 0.0f))
|
||||
return ctx.fail("参数 track_init_threshold 必须大于 0");
|
||||
if (!(p.track_init_angle > 0.0f) || p.track_init_angle > 180.0f)
|
||||
return ctx.fail("参数 track_init_angle 必须在 (0, 180] 度范围内");
|
||||
if (p.track_init_point_num_min < 3 || p.track_init_point_num_min > 9)
|
||||
return ctx.fail("参数 track_init_point_num_min 必须在 3~9 之间(实际 " +
|
||||
std::to_string(p.track_init_point_num_min) + ")");
|
||||
const float init_quality[] = {
|
||||
p.track_init_quality_L3, p.track_init_quality_L4, p.track_init_quality_L5,
|
||||
p.track_init_quality_L6, p.track_init_quality_L7};
|
||||
for (int i = 0; i < 5; ++i)
|
||||
{
|
||||
if (init_quality[i] < 0.0f || init_quality[i] > 1.0f)
|
||||
return ctx.fail("参数 track_init_quality_L" + std::to_string(i + 3) + " 必须在 0~1 之间");
|
||||
}
|
||||
if (p.track_init_velocity_min < 0.0f)
|
||||
return ctx.fail("参数 track_init_velocity_min 不能为负");
|
||||
if (!(p.track_init_velocity_max > p.track_init_velocity_min))
|
||||
return ctx.fail("参数 track_init_velocity_max 必须大于 track_init_velocity_min");
|
||||
|
||||
// 航迹消亡参数
|
||||
if (p.track_die_round < 0)
|
||||
return ctx.fail("参数 track_die_round 不能为负");
|
||||
if (p.track_die_round_pramary < 0)
|
||||
return ctx.fail("参数 track_die_round_pramary 不能为负");
|
||||
if (p.track_die_round_tas < 0)
|
||||
return ctx.fail("参数 track_die_round_tas 不能为负");
|
||||
if (p.track_die_quality < 0.0f || p.track_die_quality > 1.0f)
|
||||
return ctx.fail("参数 track_die_quality 必须在 0~1 之间");
|
||||
if (p.track_die_quality_pramary < 0.0f || p.track_die_quality_pramary > 1.0f)
|
||||
return ctx.fail("参数 track_die_quality_pramary 必须在 0~1 之间");
|
||||
|
||||
// 航迹质量管理参数
|
||||
if (p.track_quality_min < 0.0f)
|
||||
return ctx.fail("参数 track_quality_min 不能为负");
|
||||
if (!(p.track_quality_max > p.track_quality_min))
|
||||
return ctx.fail("参数 track_quality_max 必须大于 track_quality_min");
|
||||
if (p.track_quality_d_weight < 0.0f || p.track_quality_snr_weight < 0.0f || p.track_quality_len_weight < 0.0f)
|
||||
return ctx.fail("参数 track_quality_d_weight / track_quality_snr_weight / track_quality_len_weight 不能为负");
|
||||
if (p.track_quality_iter_coeff < 0.0f || p.track_quality_iter_coeff > 1.0f)
|
||||
return ctx.fail("参数 track_quality_iter_coeff 必须在 0~1 之间");
|
||||
if (p.track_quality_decrease_coeff_o2 > 0.0f || p.track_quality_decrease_coeff_o1 > 0.0f)
|
||||
return ctx.fail("参数 track_quality_decrease_coeff_o2 / track_quality_decrease_coeff_o1 必须小于等于 0");
|
||||
|
||||
// 航迹批号管理
|
||||
if (p.track_index_max < 1 || p.track_index_max > MAX_TRACK_NUM)
|
||||
return ctx.fail("参数 track_index_max 必须在 1~" + std::to_string(MAX_TRACK_NUM) + " 之间(实际 " +
|
||||
std::to_string(p.track_index_max) + ")");
|
||||
|
||||
// 波束调度参数
|
||||
if (p.default_track_beam_type < 0)
|
||||
return ctx.fail("参数 default_track_beam_type 不能为负");
|
||||
if (p.tas_track_num_max < 0 || p.tas_track_num_max > RDP_TAS_QUEUE_CAPACITY)
|
||||
return ctx.fail("参数 tas_track_num_max 必须在 0~" + std::to_string(RDP_TAS_QUEUE_CAPACITY) +
|
||||
" 之间(实际 " + std::to_string(p.tas_track_num_max) + ")");
|
||||
|
||||
// singer 模型机动参数
|
||||
const float model_q[] = {
|
||||
p.model_1_Q_fast, p.model_1_Q_slow, p.model_2_Q_fast,
|
||||
p.model_2_Q_slow, p.model_3_Q_fast, p.model_3_Q_slow};
|
||||
const char* const model_q_names[] = {
|
||||
"model_1_Q_fast", "model_1_Q_slow", "model_2_Q_fast",
|
||||
"model_2_Q_slow", "model_3_Q_fast", "model_3_Q_slow"};
|
||||
for (int i = 0; i < 6; ++i)
|
||||
{
|
||||
if (!(model_q[i] > 0.0f))
|
||||
return ctx.fail(std::string("参数 ") + model_q_names[i] + " 必须大于 0");
|
||||
}
|
||||
|
||||
// 屏蔽区
|
||||
if (p.track_init_prohibite_area_num < 0 || p.track_init_prohibite_area_num > kProhibiteAreaCapacity)
|
||||
return ctx.fail("参数 track_init_prohibite_area_num 必须在 0~" +
|
||||
std::to_string(kProhibiteAreaCapacity) + " 之间");
|
||||
if (p.tas_prohibite_area_num < 0 || p.tas_prohibite_area_num > kProhibiteAreaCapacity)
|
||||
return ctx.fail("参数 tas_prohibite_area_num 必须在 0~" + std::to_string(kProhibiteAreaCapacity) + " 之间");
|
||||
|
||||
for (int i = 0; i < p.track_init_prohibite_area_num; ++i)
|
||||
{
|
||||
const std::string prefix = "track_init_prohibite_area 第 " + std::to_string(i + 1) + " 区";
|
||||
if (p.track_init_prohibite_area_range_min[i] < 0.0f ||
|
||||
!(p.track_init_prohibite_area_range_max[i] > p.track_init_prohibite_area_range_min[i]))
|
||||
return ctx.fail(prefix + " 的距离范围非法(要求 max > min >= 0)");
|
||||
if (p.track_init_prohibite_area_azimuth_min[i] < 0.0f ||
|
||||
p.track_init_prohibite_area_azimuth_max[i] > 360.0f ||
|
||||
!(p.track_init_prohibite_area_azimuth_max[i] > p.track_init_prohibite_area_azimuth_min[i]))
|
||||
return ctx.fail(prefix + " 的方位范围非法(要求 0 =< min < max <= 360)");
|
||||
}
|
||||
for (int i = 0; i < p.tas_prohibite_area_num; ++i)
|
||||
{
|
||||
const std::string prefix = "tas_prohibite_area 第 " + std::to_string(i + 1) + " 区";
|
||||
if (p.tas_prohibite_area_range_min[i] < 0.0f ||
|
||||
!(p.tas_prohibite_area_range_max[i] > p.tas_prohibite_area_range_min[i]))
|
||||
return ctx.fail(prefix + " 的距离范围非法(要求 max > min >= 0)");
|
||||
if (p.tas_prohibite_area_azimuth_min[i] < 0.0f ||
|
||||
p.tas_prohibite_area_azimuth_max[i] > 360.0f ||
|
||||
!(p.tas_prohibite_area_azimuth_max[i] > p.tas_prohibite_area_azimuth_min[i]))
|
||||
return ctx.fail(prefix + " 的方位范围非法(要求 0 =< min < max <= 360)");
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ------------------------------ 加载主流程 ------------------------------
|
||||
bool load_all_sections(const toml::table& root, RDP_parameter& parameter, LoadContext& ctx)
|
||||
{
|
||||
// 1) 顶层只允许已知配置段
|
||||
for (const auto& [key, value] : root)
|
||||
{
|
||||
(void)value;
|
||||
bool known = false;
|
||||
for (const char* name : kAllSections)
|
||||
{
|
||||
if (key.str() == name)
|
||||
{
|
||||
known = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!known)
|
||||
return ctx.fail("配置文件中存在未知配置段: [" + std::string(key.str()) + "]");
|
||||
}
|
||||
|
||||
// 2) 先确认 11 个配置段都存在且为表(缺段优先报错,便于定位)
|
||||
for (const char* name : kAllSections)
|
||||
{
|
||||
if (get_section(root, name, ctx) == nullptr)
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3) 再校验每段内只允许登记过的键
|
||||
for (const char* name : kAllSections)
|
||||
{
|
||||
const toml::table* section = get_section(root, name, ctx);
|
||||
if (section == nullptr)
|
||||
return false;
|
||||
if (!check_keys_of_section(*section, name, ctx))
|
||||
return false;
|
||||
}
|
||||
|
||||
// 4) 读取全部标量
|
||||
for (const FloatField& field : kFloatFields)
|
||||
{
|
||||
const toml::table* section = get_section(root, field.section, ctx);
|
||||
if (section == nullptr)
|
||||
return false;
|
||||
if (!read_float(*section, field.section, field.key, parameter.*(field.member), ctx))
|
||||
return false;
|
||||
}
|
||||
for (const IntField& field : kIntFields)
|
||||
{
|
||||
const toml::table* section = get_section(root, field.section, ctx);
|
||||
if (section == nullptr)
|
||||
return false;
|
||||
if (!read_int(*section, field.section, field.key, parameter.*(field.member), ctx))
|
||||
return false;
|
||||
}
|
||||
|
||||
// 4) 读取屏蔽区数组(长度依赖已读入的 *_num)
|
||||
const toml::table* init_area = get_section(root, "track_init_prohibite_area", ctx);
|
||||
if (init_area == nullptr)
|
||||
return false;
|
||||
if (!read_area_arrays(*init_area, "track_init_prohibite_area", parameter.track_init_prohibite_area_num, parameter, ctx))
|
||||
return false;
|
||||
|
||||
const toml::table* tas_area = get_section(root, "tas_prohibite_area", ctx);
|
||||
if (tas_area == nullptr)
|
||||
return false;
|
||||
if (!read_area_arrays(*tas_area, "tas_prohibite_area", parameter.tas_prohibite_area_num, parameter, ctx))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int rdp_parameter_load(const char* parameter_file_path, RDP_parameter& parameter, std::string* error_msg)
|
||||
{
|
||||
LoadContext ctx(error_msg);
|
||||
|
||||
if (parameter_file_path == nullptr || parameter_file_path[0] == '\0')
|
||||
{
|
||||
ctx.fail("配置文件路径为空");
|
||||
return -1;
|
||||
}
|
||||
if (error_msg != nullptr)
|
||||
error_msg->clear();
|
||||
|
||||
toml::table root;
|
||||
try
|
||||
{
|
||||
root = toml::parse_file(parameter_file_path);
|
||||
}
|
||||
catch (const toml::parse_error& err)
|
||||
{
|
||||
ctx.fail(std::string("TOML 解析失败: ") + std::string(err.what()));
|
||||
return -1;
|
||||
}
|
||||
catch (const std::exception& err)
|
||||
{
|
||||
ctx.fail(std::string("读取配置文件失败: ") + err.what());
|
||||
return -1;
|
||||
}
|
||||
|
||||
RDP_parameter loaded{};
|
||||
if (!load_all_sections(root, loaded, ctx))
|
||||
return -1;
|
||||
if (!validate_parameter(loaded, ctx))
|
||||
return -1;
|
||||
|
||||
// 全部校验通过后才一次性写入,保证失败时调用方参数不变。
|
||||
parameter = loaded;
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef RDP_PARAMETER_LOADER_H
|
||||
#define RDP_PARAMETER_LOADER_H
|
||||
#pragma once
|
||||
#include "struct.h"
|
||||
#include <string>
|
||||
|
||||
/************************************************************************************************
|
||||
* 雷达数据处理参数加载器(内部模块,不属于宿主接口)
|
||||
*
|
||||
* 功能:把 TOML 配置文件加载到 struct.h::RDP_parameter。
|
||||
* 约定:
|
||||
* 1) 配置文件的每个配置段与 RDP_parameter 的注释分组对应,配置键名与结构体成员名完全一致;
|
||||
* 2) 屏蔽区(track_init_prohibite_area / tas_prohibite_area)的数组使用与成员同名的 TOML 数组,
|
||||
* 数组长度必须等于对应的 *_num,且 *_num 不超过 30;
|
||||
* 3) 所有标量键均必须存在且类型/取值合法(严格校验),任一错误返回 -1 且不修改 parameter;
|
||||
* 4) 数组键在 *_num == 0 时可以省略,存在时必须为空数组;*_num > 0 时必须存在且长度等于 *_num。
|
||||
*
|
||||
* 参数:
|
||||
* parameter_file_path - 配置文件路径(UTF-8,宿主进程当前工作目录可达)
|
||||
* parameter - 输出参数;仅在成功时被写入
|
||||
* error_msg - 可选输出;失败时写入可读的错误原因(UTF-8)
|
||||
* 返回:成功返回 0;失败返回 -1
|
||||
************************************************************************************************/
|
||||
int rdp_parameter_load(const char* parameter_file_path, RDP_parameter& parameter, std::string* error_msg);
|
||||
|
||||
#endif // RDP_PARAMETER_LOADER_H
|
||||
@@ -189,4 +189,92 @@ struct Direct_Tracking_Target
|
||||
|
||||
};
|
||||
|
||||
// 系统工作参数结构体
|
||||
struct RDP_parameter
|
||||
{
|
||||
// ******************* 雷达工作参数 ************************//
|
||||
int radar_work_mode; //工作模式
|
||||
float freq_0_GHz; //频点0频率
|
||||
float radar_height; //雷达平台高度 (m)
|
||||
float radar_system_delay; //系统延迟 (s)
|
||||
float data_rate_tws; //TWS模式数据率
|
||||
float data_rate_tas; //TAS模式数据率
|
||||
float data_rate_track_only; //纯跟踪模式数据率
|
||||
|
||||
// ******************* 点迹凝聚参数 ************************//
|
||||
float point_cohesion_range; //距离凝聚范围
|
||||
float point_cohesion_velocity; //速度凝聚范围
|
||||
float point_cohesion_azimuth; //方位凝聚范围
|
||||
float point_cohesion_range_min; //最小凝聚距离
|
||||
float point_cohesion_range_max; //最大凝聚距离
|
||||
|
||||
// ******************* 航迹关联参数 ************************//
|
||||
float sigma_range; //距离量测精度
|
||||
float sigma_azimuth; //方位量测精度
|
||||
float sigma_pitch; //俯仰量测精度
|
||||
float sigma_velocity; //速度量测精度
|
||||
float associate_threshold; //关联门限
|
||||
int extrapolate_max; //最大外推点数
|
||||
int extrapolate_max_tas; //TAS最大外推点数
|
||||
int height_smooth_win; //高度平滑窗长
|
||||
|
||||
// ******************* 航迹起始参数 ************************//
|
||||
float track_init_threshold; //起航门限
|
||||
float track_init_angle; //起航夹角
|
||||
int track_init_point_num_min; //最少起批点数
|
||||
float track_init_quality_L3; //3点起批质量门限
|
||||
float track_init_quality_L4; //4点起批质量门限
|
||||
float track_init_quality_L5; //5点起批质量门限
|
||||
float track_init_quality_L6; //6点起批质量门限
|
||||
float track_init_quality_L7; //7点起批质量门限
|
||||
float track_init_velocity_min; //航迹起始最小速度
|
||||
float track_init_velocity_max; //航迹起始最大速度
|
||||
|
||||
// ******************* 航迹消亡参数 ************************//
|
||||
int track_die_round; //航迹消亡圈数
|
||||
int track_die_round_pramary; //重点航迹消亡圈数
|
||||
int track_die_round_tas; //TAS航迹消亡圈数
|
||||
float track_die_quality; //航迹消批质量条件
|
||||
float track_die_quality_pramary; //重点航迹消批质量条件
|
||||
|
||||
// ******************* 航迹质量管理参数 ************************//
|
||||
float track_quality_max; //航迹质量最大值
|
||||
float track_quality_min; //航迹质量最小值
|
||||
float track_quality_d_weight; //马氏距离权重
|
||||
float track_quality_snr_weight; //信噪比权重
|
||||
float track_quality_len_weight; //航迹长度权重
|
||||
float track_quality_iter_coeff; //航迹质量迭代系数
|
||||
float track_quality_decrease_coeff_o2; //航迹质量下降系数2次项
|
||||
float track_quality_decrease_coeff_o1; //航迹质量下降系数1次项
|
||||
|
||||
// ******************* 航迹批号管理 ************************//
|
||||
int track_index_max; //最大航迹批号
|
||||
|
||||
// ******************* 波束调度参数 ************************//
|
||||
int default_track_beam_type; //默认跟踪波束类型
|
||||
int tas_track_num_max; //最大同时跟踪目标数
|
||||
|
||||
// ******************* singer模型机动参数 ************************//
|
||||
float model_1_Q_fast; //模型1
|
||||
float model_1_Q_slow; //模型1
|
||||
float model_2_Q_fast; //模型2
|
||||
float model_2_Q_slow; //模型2
|
||||
float model_3_Q_fast; //模型3
|
||||
float model_3_Q_slow; //模型3
|
||||
|
||||
// ******************* 航迹起始屏蔽区 ************************//
|
||||
int track_init_prohibite_area_num; //航迹起始屏蔽区数
|
||||
float track_init_prohibite_area_range_min[30]; //最小距离 (m)
|
||||
float track_init_prohibite_area_range_max[30]; //最大距离 (m)
|
||||
float track_init_prohibite_area_azimuth_min[30]; //最小方位角 (度)
|
||||
float track_init_prohibite_area_azimuth_max[30]; //最大方位角 (度)
|
||||
|
||||
// ******************* 禁止TAS跟踪区 ************************//
|
||||
int tas_prohibite_area_num; //禁止TAS的区域数
|
||||
float tas_prohibite_area_range_min[30]; //最小距离 (m)
|
||||
float tas_prohibite_area_range_max[30]; //最大距离 (m)
|
||||
float tas_prohibite_area_azimuth_min[30]; //最小方位角 (度)
|
||||
float tas_prohibite_area_azimuth_max[30]; //最大方位角 (度)
|
||||
};
|
||||
|
||||
#endif // STRUCT_H
|
||||
|
||||
@@ -9,15 +9,9 @@
|
||||
|
||||
using namespace std;
|
||||
|
||||
// parameters.h 中定义了 DATA_RATE_TAS 宏(相扫0.3/机扫0.0625),
|
||||
// 而本模块的波束门控必须使用 RadarPara 结构体成员 Work_Parameter.DATA_RATE_TAS
|
||||
// (由dll调用者经 track_process_parameters_initial 输入),故取消该宏定义,
|
||||
// 否则 Work_Parameter.DATA_RATE_TAS 会被预处理器展开为非法常量表达式。
|
||||
#undef DATA_RATE_TAS
|
||||
|
||||
TAS_Ctrl::TAS_Ctrl()
|
||||
{
|
||||
memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target));
|
||||
memset(tas_target_queue,0,RDP_TAS_QUEUE_CAPACITY*sizeof(Tracking_Target));
|
||||
|
||||
tas_target_num=0;
|
||||
}
|
||||
@@ -33,19 +27,19 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track
|
||||
struct TrackingBeam *Tracking_beam,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
long long latest_timestamp)
|
||||
{
|
||||
if (Trust_track_num_Output == 0) return;
|
||||
*Trust_track_num_Output = 0;
|
||||
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
|
||||
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
|
||||
tas_beam_output(trust_track,Tracking_beam,Work_Parameter,latest_timestamp);
|
||||
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
|
||||
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
|
||||
tas_beam_output(trust_track,Tracking_beam,rdp_parameter,latest_timestamp);
|
||||
|
||||
// //跟踪队列移位
|
||||
// struct Tracking_Target tas_target_tmp;
|
||||
// memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
|
||||
// for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
|
||||
// memcpy(&tas_target_tmp, &tas_target_queue[RDP_TAS_QUEUE_CAPACITY-1], sizeof(Tracking_Target));
|
||||
// for (int i=RDP_TAS_QUEUE_CAPACITY-1;i>0;i--)
|
||||
// {
|
||||
// memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
|
||||
// }
|
||||
@@ -55,26 +49,26 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track
|
||||
void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
for (size_t i=0;i<trust_track->size();i++)
|
||||
{
|
||||
double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
|
||||
double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
||||
double h=(*trust_track)[i].Height+Work_Parameter.Height;
|
||||
double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
|
||||
double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
|
||||
if(azi<0)
|
||||
azi=azi+2*PI;
|
||||
azi=azi/PI*180;
|
||||
|
||||
//进入跟踪的条件: 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
|
||||
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
|
||||
&& tas_target_num < MAX_TAS_NUM
|
||||
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, rdp_parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
|
||||
&& tas_target_num < rdp_parameter.tas_track_num_max
|
||||
&& (*trust_track)[i].Track_Mode == 0
|
||||
&& tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0)
|
||||
&& tas_prohibite_area( r, azi, v, h, rdp_parameter) == 0)
|
||||
{
|
||||
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
|
||||
for(int j=0;j<RDP_TAS_QUEUE_CAPACITY;j++)
|
||||
{
|
||||
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
|
||||
{
|
||||
@@ -107,7 +101,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
|
||||
break;
|
||||
@@ -119,32 +113,16 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
|
||||
|
||||
};
|
||||
|
||||
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
|
||||
//{
|
||||
// 说明:TAS 自动开始(tas_auto_start)的原实现依赖改造前雷达参数结构体中的
|
||||
// TAS 距离/速度/高度门限,这些参数未纳入新的 RDP_parameter,故原实现保持停用;
|
||||
// 具体逻辑不再以注释代码形式保留(tas_ctrl.h 中保留声明,后续如需启用再按新参数补齐)。
|
||||
|
||||
//// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
//// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
|
||||
//// )
|
||||
// if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
|
||||
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
|
||||
// {
|
||||
// return 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
//// return 0;
|
||||
|
||||
//};
|
||||
|
||||
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
|
||||
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
|
||||
for (int i=0;i<rdp_parameter.tas_prohibite_area_num;i++)
|
||||
{
|
||||
if(r<Work_Parameter.R_max_TAS_prohibited[i] && r>Work_Parameter.R_min_TAS_prohibited[i] && azi<Work_Parameter.Azimuth_max_TAS_prohibited[i] && azi>Work_Parameter.Azimuth_min_TAS_prohibited[i])
|
||||
if(r<rdp_parameter.tas_prohibite_area_range_max[i] && r>rdp_parameter.tas_prohibite_area_range_min[i] && azi<rdp_parameter.tas_prohibite_area_azimuth_max[i] && azi>rdp_parameter.tas_prohibite_area_azimuth_min[i])
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
@@ -157,11 +135,11 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct
|
||||
void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//查找TAS目标是否已经消批 若消批则移出TAS队列
|
||||
for (int i=0;i<TAS_QUEUE_LENGTH;i++)
|
||||
for (int i=0;i<RDP_TAS_QUEUE_CAPACITY;i++)
|
||||
{
|
||||
if(tas_target_queue[i].empty_flag == 1)
|
||||
{
|
||||
@@ -189,14 +167,14 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
|
||||
// {
|
||||
// double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
|
||||
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
||||
// double h=(*trust_track)[i].Height+Work_Parameter.Height;
|
||||
// double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
|
||||
|
||||
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
|
||||
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,rdp_parameter) == 1) //满足结束跟踪条件
|
||||
// {
|
||||
|
||||
// int track_index=(*trust_track)[i].Track_Index;
|
||||
|
||||
// for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
|
||||
// for (int ii=0;ii<RDP_TAS_QUEUE_CAPACITY;ii++)
|
||||
// {
|
||||
// if(tas_target_queue[ii].Index == track_index)
|
||||
// {
|
||||
@@ -224,7 +202,7 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
|
||||
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
|
||||
// }
|
||||
@@ -235,28 +213,12 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
|
||||
|
||||
};
|
||||
|
||||
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
|
||||
//{
|
||||
// if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
|
||||
// || v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
|
||||
// )
|
||||
// {
|
||||
// return 1;
|
||||
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
|
||||
//};
|
||||
// 说明:TAS 自动结束(tas_auto_end)的原实现同样依赖未纳入 RDP_parameter 的
|
||||
// TAS 距离/速度/高度门限,故保持停用(tas_ctrl.h 中保留声明,实现待后续补齐)。
|
||||
|
||||
void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track,
|
||||
struct TrackingBeam *Tracking_beam,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
long long latest_timestamp)
|
||||
{
|
||||
|
||||
@@ -270,7 +232,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
|
||||
double X_now[6] = {0};
|
||||
|
||||
//查找TAS队列中CPI时间最早的目标且满足TAS数据率的目标
|
||||
for(int tas_idx=0;tas_idx<TAS_QUEUE_LENGTH;tas_idx++)
|
||||
for(int tas_idx=0;tas_idx<RDP_TAS_QUEUE_CAPACITY;tas_idx++)
|
||||
{
|
||||
if(tas_target_queue[tas_idx].empty_flag==0)
|
||||
{
|
||||
@@ -285,7 +247,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
|
||||
delta_T_last_track = (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f;
|
||||
|
||||
//查找CPI时间最早,且满足TAS数据率的目标
|
||||
if(CPI_time < earliest_CPI_time && (delta_T_last_track > Work_Parameter.DATA_RATE_TAS))
|
||||
if(CPI_time < earliest_CPI_time && (delta_T_last_track > rdp_parameter.data_rate_tas))
|
||||
{
|
||||
earliest_CPI_time = CPI_time;
|
||||
H_track = (*trust_track)[track_idx].Height;
|
||||
@@ -339,7 +301,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
|
||||
Tracking_beam->Elev=(float)elev;
|
||||
|
||||
//跟踪波束类型
|
||||
Tracking_beam->type=1;
|
||||
Tracking_beam->type=rdp_parameter.default_track_beam_type;
|
||||
|
||||
//跟踪目标批号
|
||||
Tracking_beam->TAS_track_index = (*trust_track)[earliest_track_idx].Track_Index;
|
||||
|
||||
@@ -18,7 +18,7 @@ public:
|
||||
struct TrackingBeam *Tracking_beam,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
long long latest_timestamp);
|
||||
|
||||
private:
|
||||
@@ -27,30 +27,30 @@ private:
|
||||
void tas_target_add(std::vector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
//删除TAS目标
|
||||
void tas_target_del(std::vector<Trust_Track> *trust_track,
|
||||
struct Track Trust_Track_Output[][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
//TAS队列信息输出
|
||||
void tas_beam_output(std::vector<Trust_Track> *trust_track,
|
||||
struct TrackingBeam *Tracking_beam,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
long long latest_timestamp);
|
||||
|
||||
//TAS 自动开启条件
|
||||
int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter);
|
||||
int tas_auto_start(double v, double r, double h, const RDP_parameter& rdp_parameter);
|
||||
|
||||
//TAS 自动退出条件
|
||||
int tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter);
|
||||
int tas_auto_end(double v, double r, double h, const RDP_parameter& rdp_parameter);
|
||||
|
||||
// 禁止跟踪区域判断
|
||||
int tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter);
|
||||
int tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter);
|
||||
|
||||
struct Tracking_Target tas_target_queue[TAS_QUEUE_LENGTH]; //跟踪目标信息队列
|
||||
struct Tracking_Target tas_target_queue[RDP_TAS_QUEUE_CAPACITY]; //跟踪目标信息队列
|
||||
|
||||
int tas_target_num; //跟踪目标数量
|
||||
|
||||
|
||||
@@ -20,7 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
long long latest_timestamp, //最新时间戳
|
||||
struct RadarPara Work_Parameter //雷达参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
@@ -35,7 +35,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
//IMM算法
|
||||
model_interaction(trust_track);
|
||||
|
||||
model_filter(trust_track, Work_Parameter, latest_timestamp);
|
||||
model_filter(trust_track, rdp_parameter, latest_timestamp);
|
||||
|
||||
model_output(trust_track);
|
||||
|
||||
@@ -80,8 +80,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
double x,y;
|
||||
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
|
||||
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
|
||||
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
|
||||
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
|
||||
coor_trans Coor_trans;
|
||||
double r, azi;
|
||||
Coor_trans.cart2polar(x, y, &r, &azi);
|
||||
@@ -103,8 +103,8 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*trust_track)[i].T_track/1000.0);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * TRACK_QUALITY_MAX);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * rdp_parameter.track_quality_max);
|
||||
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
|
||||
@@ -130,7 +130,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
||||
//直接输出点迹高度-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);
|
||||
//输出平滑后的高度
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z = static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
|
||||
|
||||
@@ -288,7 +288,7 @@ void Track_Asso:: model_interaction(std::vector <Trust_Track> *trust_track)
|
||||
}
|
||||
|
||||
// 产生 F Q 矩阵
|
||||
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
||||
void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//F
|
||||
@@ -316,7 +316,7 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
|
||||
|
||||
//模型1
|
||||
//Q
|
||||
double q1=Work_Parameter.Model1_Q_slow;
|
||||
double q1=rdp_parameter.model_1_Q_slow;
|
||||
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -337,24 +337,24 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
|
||||
double q2;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow/20;
|
||||
q2=rdp_parameter.model_2_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=15&&v_track>5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow;
|
||||
q2=rdp_parameter.model_2_Q_slow;
|
||||
}
|
||||
else if(v_track<=30&&v_track>15)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow;
|
||||
q2=rdp_parameter.model_2_Q_slow;
|
||||
}
|
||||
else if(v_track<=100&&v_track>30)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_fast;
|
||||
q2=rdp_parameter.model_2_Q_fast;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_fast;
|
||||
q2=rdp_parameter.model_2_Q_fast;
|
||||
}
|
||||
|
||||
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -378,29 +378,29 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub
|
||||
double q3;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow/20;
|
||||
q3=rdp_parameter.model_3_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=15&&v_track>5)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow;
|
||||
q3=rdp_parameter.model_3_Q_slow;
|
||||
}
|
||||
else if(v_track<=20&&v_track>15)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow;
|
||||
q3=rdp_parameter.model_3_Q_slow;
|
||||
}
|
||||
else if(v_track<=50&&v_track>20)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_fast;
|
||||
q3=rdp_parameter.model_3_Q_fast;
|
||||
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q3=2*Work_Parameter.Model3_Q_fast;
|
||||
q3=2*rdp_parameter.model_3_Q_fast;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
q3=10*Work_Parameter.Model3_Q_fast;
|
||||
q3=10*rdp_parameter.model_3_Q_fast;
|
||||
}
|
||||
|
||||
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -429,19 +429,19 @@ void Track_Asso::IMM_d_cal(double v_track,
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
kalman Kalman;
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
|
||||
|
||||
}
|
||||
|
||||
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct RadarPara Work_Parameter, long long latest_timestamp)
|
||||
void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,const RDP_parameter& rdp_parameter, long long latest_timestamp)
|
||||
{
|
||||
|
||||
|
||||
@@ -492,17 +492,17 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
|
||||
|
||||
//判定统计距离门限,thd_d
|
||||
bool thd_d = false;
|
||||
|
||||
if( (r_track <= 500 && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \
|
||||
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 1000.0)) || \
|
||||
((r_track > 500 && r_track < 1000) && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 100.0)) || \
|
||||
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold / 100.0)) || \
|
||||
(r_track >= 1000 && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\
|
||||
(d1*d1 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3 < rdp_parameter.associate_threshold*rdp_parameter.associate_threshold))\
|
||||
){
|
||||
thd_d = true;
|
||||
}
|
||||
@@ -623,7 +623,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
double v_track = sqrt(pow((*trust_track)[track_index-1].X[1],2)+pow((*trust_track)[track_index-1].X[4],2));
|
||||
double delta_T = (point_process[point_index-1].CPI_time - (*trust_track)[track_index-1].T_track)/1000.0;
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
if (delta_T > 0)
|
||||
{
|
||||
@@ -632,9 +632,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
|
||||
|
||||
Matrix3f S1_out,S2_out,S3_out;
|
||||
for (int ii=0;ii<3;ii++)
|
||||
@@ -690,9 +690,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
double d_tmp = exp(-associated_info[min_index-1].d_min);
|
||||
double snr_tmp = 1/(1+exp(-(point_process[point_index-1].SNR - 16) / 1.2));
|
||||
double len_tmp = 1/(1+exp(-((*trust_track)[track_index-1].Valid_point_sum - 4) / 1.2)) * 0.5 + (1 - (*trust_track)[track_index-1].Valid_point_sum / (*trust_track)[track_index-1].Point_sum) * 0.5;
|
||||
double tq_tmp = TRACK_QUALITY_D_WEIGHT * d_tmp + TRACK_QUALITY_SNR_WEIGHT * snr_tmp + TRACK_QUALITY_LEN_WEIGHT * len_tmp;
|
||||
double tq_tmp = rdp_parameter.track_quality_d_weight * d_tmp + rdp_parameter.track_quality_snr_weight * snr_tmp + rdp_parameter.track_quality_len_weight * len_tmp;
|
||||
|
||||
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * TRACK_QUALITY_ITER_COEFF + tq_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
|
||||
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * rdp_parameter.track_quality_iter_coeff + tq_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
|
||||
|
||||
//更新航迹时间
|
||||
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_time;
|
||||
@@ -721,7 +721,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
std::memcpy((*trust_track)[track_index-1].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[track_index-1].range_dim));
|
||||
|
||||
//更新高度
|
||||
track_hight_update(track_index,point_index,trust_track);
|
||||
track_hight_update(track_index,point_index,trust_track,rdp_parameter);
|
||||
|
||||
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
|
||||
(*trust_track)[track_index-1].point_flag=1; //实点
|
||||
@@ -780,34 +780,16 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0) //未关联上点迹的航迹进行外推
|
||||
{
|
||||
double delta_T = 0;
|
||||
// double delta_T = (latest_timestamp - (*trust_track)[i].T_track)/1000.0;
|
||||
// if (delta_T <= 0)
|
||||
// continue;
|
||||
|
||||
if(Work_Parameter.work_mode == 0)
|
||||
{
|
||||
delta_T = Work_Parameter.DATA_RATE_SHORT;
|
||||
}
|
||||
else if(Work_Parameter.work_mode == 1)
|
||||
{
|
||||
delta_T = Work_Parameter.DATA_RATE_MIDDLE;
|
||||
}
|
||||
else if(Work_Parameter.work_mode == 2)
|
||||
{
|
||||
delta_T = Work_Parameter.DATA_RATE_FAR;
|
||||
}
|
||||
else
|
||||
{
|
||||
delta_T = Work_Parameter.DATA_RATE_SHORT;
|
||||
}
|
||||
|
||||
delta_T = rdp_parameter.data_rate_tws;
|
||||
|
||||
//外推点数未到上限,则继续外推,否则不外推
|
||||
if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM)
|
||||
if((*trust_track)[i].Extrapolate_round <= rdp_parameter.extrapolate_max)
|
||||
{
|
||||
double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
|
||||
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
//航迹X P
|
||||
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
@@ -845,10 +827,10 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
|
||||
//航迹质量递减
|
||||
double t = (*trust_track)[i].Unupdated_time / 1000.0;
|
||||
(*trust_track)[i].Quality -= t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
|
||||
(*trust_track)[i].Quality -= t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
|
||||
(*trust_track)[i].Unupdated_time += static_cast<long long>(delta_T*1000.0);
|
||||
t = (*trust_track)[i].Unupdated_time / 1000.0;
|
||||
(*trust_track)[i].Quality += t * t * TRACK_QUALITY_DECREASE_COEFF_O2 + t * TRACK_QUALITY_DECREASE_COEFF_O1;
|
||||
(*trust_track)[i].Quality += t * t * rdp_parameter.track_quality_decrease_coeff_o2 + t * rdp_parameter.track_quality_decrease_coeff_o1;
|
||||
(*trust_track)[i].Quality = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality;
|
||||
(*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1;
|
||||
|
||||
@@ -965,7 +947,8 @@ void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
|
||||
//高度维更新
|
||||
void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
std::vector <Trust_Track> *trust_track //航迹
|
||||
std::vector <Trust_Track> *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
@@ -978,19 +961,19 @@ void Track_Asso:: track_hight_update(int updata_
|
||||
|
||||
if((*trust_track)[updata_track_index-1].range_point <= 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+2;
|
||||
height_win_length = rdp_parameter.height_smooth_win+2;
|
||||
}
|
||||
else if((*trust_track)[updata_track_index-1].range_point <= 2000 && (*trust_track)[updata_track_index-1].range_point > 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+3;
|
||||
height_win_length = rdp_parameter.height_smooth_win+3;
|
||||
}
|
||||
else if((*trust_track)[updata_track_index-1].range_point <= 4000 && (*trust_track)[updata_track_index-1].range_point > 2000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+4;
|
||||
height_win_length = rdp_parameter.height_smooth_win+4;
|
||||
}
|
||||
else
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+6;
|
||||
height_win_length = rdp_parameter.height_smooth_win+6;
|
||||
}
|
||||
|
||||
double sum=0;
|
||||
|
||||
@@ -17,7 +17,7 @@ public:
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
long long latest_timestamp, //最新时间戳
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
|
||||
Track_Asso();
|
||||
@@ -31,7 +31,7 @@ private:
|
||||
void model_interaction(std::vector <Trust_Track> *trust_track); //模型交互
|
||||
|
||||
void model_filter(std::vector <Trust_Track> *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
long long latest_timestamp
|
||||
);
|
||||
|
||||
@@ -44,16 +44,17 @@ private:
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
|
||||
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
//高度维更新
|
||||
void track_hight_update(int updata_track_index, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
std::vector <Trust_Track> *trust_track //航迹
|
||||
std::vector <Trust_Track> *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
};
|
||||
#endif // TRACK_ASSO_H
|
||||
|
||||
@@ -12,7 +12,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
|
||||
Trust_Track *trust_track, //航迹
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
@@ -25,7 +25,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
|
||||
//IMM算法
|
||||
model_interaction(trust_track);
|
||||
|
||||
model_filter(trust_track, Work_Parameter);
|
||||
model_filter(trust_track, rdp_parameter);
|
||||
|
||||
model_output(trust_track);
|
||||
|
||||
@@ -38,8 +38,8 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
double x,y;
|
||||
x=(*trust_track).X[0]+(*trust_track).X[1]*Work_Parameter.Sys_delay;
|
||||
y=(*trust_track).X[3]+(*trust_track).X[4]*Work_Parameter.Sys_delay;
|
||||
x=(*trust_track).X[0]+(*trust_track).X[1]*rdp_parameter.radar_system_delay;
|
||||
y=(*trust_track).X[3]+(*trust_track).X[4]*rdp_parameter.radar_system_delay;
|
||||
coor_trans Coor_trans;
|
||||
double r, azi;
|
||||
Coor_trans.cart2polar(x, y, &r, &azi);
|
||||
@@ -58,7 +58,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector<P
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track).Track_Mode;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=(*trust_track).T_track/1000.0;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track).point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+Work_Parameter.Height;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track).Height+rdp_parameter.radar_height;
|
||||
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan2((*trust_track).X[4], (*trust_track).X[1]);
|
||||
@@ -186,7 +186,7 @@ void Track_Asso_Direct_Tracking::model_interaction(Trust_Track *trust_track)
|
||||
}
|
||||
|
||||
void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
//存储关联信息
|
||||
struct asso_info
|
||||
@@ -234,7 +234,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
|
||||
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
|
||||
|
||||
//计算高度门限
|
||||
double d_h;
|
||||
@@ -290,7 +290,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
|
||||
}
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if((d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD)
|
||||
if((d1*d1<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold)
|
||||
&& d_h == 1 )
|
||||
{
|
||||
struct asso_info associated_info_tmp;
|
||||
@@ -337,7 +337,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
|
||||
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
|
||||
double delta_T = (point_process[point_index-1].CPI_time - T_track)/1000.0;
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
|
||||
@@ -345,9 +345,9 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
|
||||
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
|
||||
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
|
||||
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
|
||||
Matrix3f S1_out,S2_out,S3_out;
|
||||
for (int ii=0;ii<3;ii++)
|
||||
for (int jj=0;jj<3;jj++)
|
||||
@@ -418,15 +418,15 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
|
||||
(*trust_track).RCS = point_process[point_index-1].RCS;
|
||||
|
||||
//更新高度
|
||||
track_hight_update(point_index,trust_track);
|
||||
track_hight_update(point_index,trust_track,rdp_parameter);
|
||||
}
|
||||
|
||||
//未关联上,航迹外推
|
||||
else
|
||||
{
|
||||
double delta_T = Work_Parameter.DATA_RATE_TAS/1000.0;
|
||||
double delta_T = rdp_parameter.data_rate_tas/1000.0;
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
||||
kalman Kalman;
|
||||
@@ -510,7 +510,7 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track)
|
||||
}
|
||||
|
||||
// 产生 F Q 矩阵
|
||||
void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
||||
void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//F
|
||||
@@ -538,7 +538,7 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
|
||||
|
||||
//模型1
|
||||
//Q
|
||||
double q1=Work_Parameter.Model1_Q_slow;
|
||||
double q1=rdp_parameter.model_1_Q_slow;
|
||||
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -559,15 +559,15 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
|
||||
double q2;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow/20;
|
||||
q2=rdp_parameter.model_2_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q2=10.0*Work_Parameter.Model2_Q_fast;
|
||||
q2=10.0*rdp_parameter.model_2_Q_fast;
|
||||
}
|
||||
else
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_fast;
|
||||
q2=rdp_parameter.model_2_Q_fast;
|
||||
}
|
||||
|
||||
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -591,15 +591,15 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub
|
||||
double q3;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow/20;
|
||||
q3=rdp_parameter.model_3_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q3=10.0*Work_Parameter.Model3_Q_fast;
|
||||
q3=10.0*rdp_parameter.model_3_Q_fast;
|
||||
}
|
||||
else
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_fast;
|
||||
q3=rdp_parameter.model_3_Q_fast;
|
||||
}
|
||||
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
@@ -627,24 +627,25 @@ void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track,
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
kalman Kalman;
|
||||
// *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind);
|
||||
// *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind);
|
||||
// *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind);
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
|
||||
|
||||
}
|
||||
|
||||
//高度维更新
|
||||
void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号
|
||||
Trust_Track *trust_track //航迹
|
||||
Trust_Track *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
|
||||
@@ -653,19 +654,19 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point
|
||||
int height_win_length;
|
||||
if((*trust_track).range_point <= 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+2;
|
||||
height_win_length = rdp_parameter.height_smooth_win+2;
|
||||
}
|
||||
else if((*trust_track).range_point <= 2000 && (*trust_track).range_point > 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+3;
|
||||
height_win_length = rdp_parameter.height_smooth_win+3;
|
||||
}
|
||||
else if((*trust_track).range_point <= 4000 && (*trust_track).range_point > 2000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+4;
|
||||
height_win_length = rdp_parameter.height_smooth_win+4;
|
||||
}
|
||||
else
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+6;
|
||||
height_win_length = rdp_parameter.height_smooth_win+6;
|
||||
}
|
||||
|
||||
double sum=0;
|
||||
|
||||
@@ -13,7 +13,7 @@ class Track_Asso_Direct_Tracking
|
||||
Trust_Track *trust_track, //航迹
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter //工作参数
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
|
||||
Track_Asso_Direct_Tracking();
|
||||
@@ -28,7 +28,7 @@ class Track_Asso_Direct_Tracking
|
||||
void model_interaction(Trust_Track *trust_track); //模型交互
|
||||
|
||||
void model_filter(Trust_Track *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter
|
||||
const RDP_parameter& rdp_parameter
|
||||
);
|
||||
|
||||
void model_output(Trust_Track *trust_track); //模型输出
|
||||
@@ -40,15 +40,16 @@ class Track_Asso_Direct_Tracking
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
|
||||
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
//高度维更新
|
||||
void track_hight_update(int asso_point_index, //点迹号
|
||||
Trust_Track *trust_track //航迹
|
||||
Trust_Track *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
};
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ int Track_Asso_Tas::track_asso_process_tas(std::vector<PointRecv>
|
||||
std::vector<Trust_Track> *trust_track, //航迹文件
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter, //工作参数
|
||||
const RDP_parameter& rdp_parameter, //工作参数
|
||||
int tas_track_idx,
|
||||
long long latest_timestamp //最新时间戳
|
||||
)
|
||||
@@ -32,7 +32,7 @@ long long latest_timestamp //最新时间戳
|
||||
//IMM算法
|
||||
model_interaction(trust_track,tas_track_idx);
|
||||
|
||||
model_filter(trust_track, Work_Parameter,tas_track_idx, latest_timestamp);
|
||||
model_filter(trust_track, rdp_parameter,tas_track_idx, latest_timestamp);
|
||||
|
||||
model_output(trust_track,tas_track_idx);
|
||||
|
||||
@@ -54,8 +54,8 @@ long long latest_timestamp //最新时间戳
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
||||
double x,y;
|
||||
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*Work_Parameter.Sys_delay;
|
||||
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*Work_Parameter.Sys_delay;
|
||||
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
|
||||
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
|
||||
coor_trans Coor_trans;
|
||||
double r, azi;
|
||||
Coor_trans.cart2polar(x, y, &r, &azi);
|
||||
@@ -75,7 +75,7 @@ long long latest_timestamp //最新时间戳
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_time=static_cast<float>((*trust_track)[i].T_track/1000.0);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
||||
|
||||
double Direction_Angle;
|
||||
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
|
||||
@@ -256,7 +256,7 @@ void Track_Asso_Tas:: model_interaction( std::vector<Trust_Track> *trust_track
|
||||
}
|
||||
|
||||
void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int tas_track_idx,
|
||||
long long latest_timestamp //最新时间戳
|
||||
)
|
||||
@@ -322,12 +322,12 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
|
||||
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,rdp_parameter);
|
||||
|
||||
//判定统计距离门限,thd_d
|
||||
bool thd_d = false;
|
||||
|
||||
if(d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD){
|
||||
if(d1*d1<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d2*d2<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold || d3*d3<rdp_parameter.associate_threshold*rdp_parameter.associate_threshold){
|
||||
thd_d = true;
|
||||
}
|
||||
|
||||
@@ -418,7 +418,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
|
||||
// Z[1]=point_process[point_index-1].Range*sin(point_process[point_index-1].Azimuth);
|
||||
double delta_T = (point_process[point_index-1].CPI_time - T_track)/1000.0;
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
|
||||
@@ -426,9 +426,9 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
|
||||
// Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1);
|
||||
// Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2);
|
||||
// Kalman.kalman_filter(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3);
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind );
|
||||
Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind, rdp_parameter);
|
||||
Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind, rdp_parameter);
|
||||
Matrix3f S1_out,S2_out,S3_out;
|
||||
for (int ii=0;ii<3;ii++)
|
||||
for (int jj=0;jj<3;jj++)
|
||||
@@ -511,7 +511,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
|
||||
std::memcpy((*trust_track)[i].range_dim, point_process[point_index-1].range_dim, sizeof((*trust_track)[i].range_dim));
|
||||
|
||||
//更新高度
|
||||
track_hight_update(tas_track_idx,point_index,trust_track);
|
||||
track_hight_update(tas_track_idx,point_index,trust_track,rdp_parameter);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -533,13 +533,13 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
|
||||
for(size_t i=0;i<trust_track->size();i++){
|
||||
if((*trust_track)[i].Track_Index == tas_track_idx)
|
||||
{
|
||||
if((*trust_track)[i].Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS)
|
||||
if((*trust_track)[i].Extrapolate_round >= rdp_parameter.extrapolate_max_tas)
|
||||
{
|
||||
double delta_T = (latest_timestamp - T_track)/1000.0;
|
||||
if (delta_T <= 0)
|
||||
continue;
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
double X1_pred[6],X2_pred[6],X3_pred[6],P1_pred[6][6],P2_pred[6][6],P3_pred[6][6];
|
||||
kalman Kalman;
|
||||
@@ -648,7 +648,7 @@ void Track_Asso_Tas::model_output(std::vector<Trust_Track> *trust_track, int
|
||||
}
|
||||
|
||||
// 产生 F Q 矩阵
|
||||
void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter)
|
||||
void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//F
|
||||
@@ -676,7 +676,7 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
|
||||
|
||||
//模型1
|
||||
//Q
|
||||
double q1=Work_Parameter.Model1_Q_slow;
|
||||
double q1=rdp_parameter.model_1_Q_slow;
|
||||
double q111=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q121=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
double q131=1/(2*pow(alpha,3))*(1-exp(-2*alpha*delta_T)-2*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -697,15 +697,15 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6],
|
||||
double q2;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_slow/20;
|
||||
q2=rdp_parameter.model_2_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q2=10.0*Work_Parameter.Model2_Q_fast;
|
||||
q2=10.0*rdp_parameter.model_2_Q_fast;
|
||||
}
|
||||
else
|
||||
{
|
||||
q2=Work_Parameter.Model2_Q_fast;
|
||||
q2=rdp_parameter.model_2_Q_fast;
|
||||
}
|
||||
|
||||
double q112=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
@@ -729,15 +729,15 @@ Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332;
|
||||
double q3;
|
||||
if(v_track<=5)
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_slow/20;
|
||||
q3=rdp_parameter.model_3_Q_slow/20;
|
||||
}
|
||||
else if(v_track<=100&&v_track>50)
|
||||
{
|
||||
q3=10.0*Work_Parameter.Model3_Q_fast;
|
||||
q3=10.0*rdp_parameter.model_3_Q_fast;
|
||||
}
|
||||
else
|
||||
{
|
||||
q3=Work_Parameter.Model3_Q_fast;
|
||||
q3=rdp_parameter.model_3_Q_fast;
|
||||
}
|
||||
double q113=1/(2*pow(alpha,5))*(1-exp(-2*alpha*delta_T)+2*alpha*delta_T+2*pow(alpha,3)*pow(delta_T,3)/3-2*pow(alpha,2)*pow(delta_T,2)-4*alpha*delta_T*exp(-alpha*delta_T));
|
||||
double q123=1/(2*pow(alpha,4))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)+2*alpha*delta_T*exp(-alpha*delta_T)-2*alpha*delta_T+pow(alpha,2)*pow(delta_T,2));
|
||||
@@ -765,25 +765,26 @@ double X3[6], double P3[6][6],
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,rdp_parameter);
|
||||
|
||||
kalman Kalman;
|
||||
// *d1=Kalman.d_cal_with_doppler(F,Q1,Z,X1,P1,v_point,prt,freq_ind);
|
||||
// *d2=Kalman.d_cal_with_doppler(F,Q2,Z,X2,P2,v_point,prt,freq_ind);
|
||||
// *d3=Kalman.d_cal_with_doppler(F,Q3,Z,X3,P3,v_point,prt,freq_ind);
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind);
|
||||
*d1=Kalman.d_cal_EKF(F,Q1,Z,X1,P1,prt,freq_ind, rdp_parameter);
|
||||
*d2=Kalman.d_cal_EKF(F,Q2,Z,X2,P2,prt,freq_ind, rdp_parameter);
|
||||
*d3=Kalman.d_cal_EKF(F,Q3,Z,X3,P3,prt,freq_ind, rdp_parameter);
|
||||
|
||||
}
|
||||
|
||||
//高度维更新
|
||||
void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
std::vector<Trust_Track> *trust_track //航迹
|
||||
std::vector<Trust_Track> *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
)
|
||||
{
|
||||
for(size_t i=0; i<trust_track->size(); i++)
|
||||
@@ -794,19 +795,19 @@ void Track_Asso_Tas::track_hight_update(int tas_
|
||||
int height_win_length;
|
||||
if((*trust_track)[i].range_point <= 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+6;
|
||||
height_win_length = rdp_parameter.height_smooth_win+6;
|
||||
}
|
||||
else if((*trust_track)[i].range_point <= 2000 && (*trust_track)[i].range_point > 1000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+8;
|
||||
height_win_length = rdp_parameter.height_smooth_win+8;
|
||||
}
|
||||
else if((*trust_track)[i].range_point <= 4000 && (*trust_track)[i].range_point > 2000)
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+10;
|
||||
height_win_length = rdp_parameter.height_smooth_win+10;
|
||||
}
|
||||
else
|
||||
{
|
||||
height_win_length = H_F_WIN_LEN+12;
|
||||
height_win_length = rdp_parameter.height_smooth_win+12;
|
||||
}
|
||||
|
||||
double sum=0;
|
||||
|
||||
@@ -15,7 +15,7 @@ public:
|
||||
std::vector<Trust_Track> *trust_track, //航迹文件
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息
|
||||
int *Trust_track_num_Output, //更新航迹数
|
||||
struct RadarPara Work_Parameter, //工作参数
|
||||
const RDP_parameter& rdp_parameter, //工作参数
|
||||
int tas_track_idx,
|
||||
long long latest_timestamp //最新时间戳
|
||||
);
|
||||
@@ -31,7 +31,7 @@ private:
|
||||
void model_interaction(std::vector<Trust_Track> *trust_track, int tas_track_idx); //模型交互
|
||||
|
||||
void model_filter(std::vector<Trust_Track> *trust_track, //滤波
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int tas_track_idx,
|
||||
long long latest_timestamp //最新时间戳
|
||||
);
|
||||
@@ -45,16 +45,17 @@ private:
|
||||
double Z[3], double prt,double freq_ind,
|
||||
double delta_T,
|
||||
double *d1,double *d2,double *d3,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter);
|
||||
void IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],const RDP_parameter& rdp_parameter);
|
||||
|
||||
double Pt[3][3]; //模型转移概率
|
||||
|
||||
//高度维更新
|
||||
void track_hight_update(int tas_track_index, //更新的航迹号
|
||||
void track_hight_update(int tas_track_idx, //更新的航迹号
|
||||
int asso_point_index, //点迹号
|
||||
std::vector<Trust_Track> *trust_track //航迹
|
||||
std::vector<Trust_Track> *trust_track, //航迹
|
||||
const RDP_parameter& rdp_parameter //工作参数
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
@@ -8,7 +8,8 @@ using namespace std;
|
||||
|
||||
void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
|
||||
int Track_die_Index_Output[],
|
||||
int *Track_die_num_Output)
|
||||
int *Track_die_num_Output,
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
std::vector <Trust_Track>::iterator Iter;
|
||||
@@ -23,13 +24,13 @@ void Track_Die::track_die_process( std::vector <Trust_Track> *trust_track,
|
||||
}
|
||||
|
||||
//非重点航迹自动消亡
|
||||
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < TRACK_DIE_QUALITY)
|
||||
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 0 && (*Iter).Quality < rdp_parameter.track_die_quality)
|
||||
{
|
||||
die_flag = true;
|
||||
}
|
||||
|
||||
//重点航迹自动消亡
|
||||
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_PRIMARY)
|
||||
if((*Iter).Track_Mode == 0 && (*Iter).primary_target_flag == 1 && (*Iter).Extrapolate_round > rdp_parameter.track_die_round_pramary)
|
||||
{
|
||||
die_flag = true;
|
||||
}
|
||||
|
||||
@@ -11,7 +11,8 @@ class Track_Die
|
||||
public:
|
||||
void track_die_process( std::vector <Trust_Track> *trust_track,
|
||||
int Track_die_Index_Output[],
|
||||
int *Track_die_num_Output);
|
||||
int *Track_die_num_Output,
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
};
|
||||
#endif // TRACK_DIE_H
|
||||
|
||||
@@ -11,7 +11,8 @@ using namespace std;
|
||||
|
||||
void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_track,
|
||||
int Track_die_Index_Output[],
|
||||
int *Track_die_num_Output)
|
||||
int *Track_die_num_Output,
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
std::vector <Trust_Track>::iterator Iter;
|
||||
@@ -24,7 +25,7 @@ void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_trac
|
||||
die_flag = true;
|
||||
}
|
||||
|
||||
if((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round > TRACK_DIE_ROUND_TAS)
|
||||
if((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round > rdp_parameter.track_die_round_tas)
|
||||
{
|
||||
die_flag = true;
|
||||
}
|
||||
|
||||
@@ -11,7 +11,8 @@ class Track_Die_Tas
|
||||
public:
|
||||
void track_die_process_tas( std::vector <Trust_Track> *trust_track,
|
||||
int Track_die_Index_Output[],
|
||||
int *Track_die_num_Output);
|
||||
int *Track_die_num_Output,
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
};
|
||||
#endif // TRACK_DIE_TAS_H
|
||||
|
||||
@@ -11,7 +11,8 @@ void Track_Ind_Mangement::reset()
|
||||
lastest_index = 0;
|
||||
}
|
||||
|
||||
int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track)
|
||||
int Track_Ind_Mangement::track_ind_get( std::vector<Trust_Track> *trust_track,
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
int isempty = 1;
|
||||
@@ -29,18 +30,19 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
|
||||
else
|
||||
{
|
||||
//建立航迹号列表
|
||||
int List[MAX_TRACK_INDEX]={0};
|
||||
const int track_index_max = rdp_parameter.track_index_max;
|
||||
std::vector<int> List(static_cast<std::size_t>(track_index_max), 0);
|
||||
for (size_t i=0;i<trust_track->size();i++)
|
||||
{
|
||||
int ti = (*trust_track)[i].Track_Index;
|
||||
if (ti >= 1 && ti <= MAX_TRACK_INDEX)
|
||||
if (ti >= 1 && ti <= track_index_max)
|
||||
List[ti-1]=1;
|
||||
}
|
||||
|
||||
//分配航迹号
|
||||
if(lastest_index<MAX_TRACK_INDEX)
|
||||
if(lastest_index<track_index_max)
|
||||
{
|
||||
for (int i=lastest_index+1;i<=MAX_TRACK_INDEX;i++)
|
||||
for (int i=lastest_index+1;i<=track_index_max;i++)
|
||||
{
|
||||
if(List[i-1]==0)
|
||||
{
|
||||
@@ -49,7 +51,7 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
|
||||
}
|
||||
}
|
||||
|
||||
for (int i=1;i<=MAX_TRACK_INDEX;i++)
|
||||
for (int i=1;i<=track_index_max;i++)
|
||||
{
|
||||
if(List[i-1]==0)
|
||||
{
|
||||
@@ -59,9 +61,9 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
|
||||
}
|
||||
|
||||
}
|
||||
else if(lastest_index == MAX_TRACK_INDEX)
|
||||
else if(lastest_index == track_index_max)
|
||||
{
|
||||
for (int i=1;i<=MAX_TRACK_INDEX;i++)
|
||||
for (int i=1;i<=track_index_max;i++)
|
||||
{
|
||||
if(List[i-1]==0)
|
||||
{
|
||||
|
||||
@@ -10,7 +10,7 @@ class Track_Ind_Mangement
|
||||
{
|
||||
public:
|
||||
|
||||
int track_ind_get( std::vector<Trust_Track> *trust_track);
|
||||
int track_ind_get( std::vector<Trust_Track> *trust_track, const RDP_parameter& rdp_parameter);
|
||||
void reset();
|
||||
|
||||
private:
|
||||
|
||||
@@ -14,7 +14,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter
|
||||
const RDP_parameter& rdp_parameter
|
||||
)
|
||||
{
|
||||
|
||||
@@ -35,10 +35,10 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
|
||||
if(point_process.size()>0)
|
||||
{
|
||||
//点迹与临时航迹关联
|
||||
point_temp_track_asso(temp_track,Work_Parameter);
|
||||
point_temp_track_asso(temp_track,rdp_parameter);
|
||||
|
||||
//点迹与航迹头关联
|
||||
point_track_head_asso(temp_track,Work_Parameter);
|
||||
point_track_head_asso(temp_track,rdp_parameter);
|
||||
|
||||
//删除关联上的点迹
|
||||
std::vector <PointRecv>::iterator Iter;
|
||||
@@ -118,7 +118,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
|
||||
#endif
|
||||
|
||||
//临时航迹满足起始长度 转为可靠航迹
|
||||
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
|
||||
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
|
||||
|
||||
}
|
||||
|
||||
@@ -130,7 +130,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
|
||||
}
|
||||
|
||||
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//关联信息
|
||||
@@ -179,7 +179,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
{
|
||||
//计算d
|
||||
kalman Kalman;
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind,rdp_parameter);
|
||||
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
||||
//计算夹角
|
||||
@@ -191,7 +191,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
double y2=Z[0]*sin(Z[1]);
|
||||
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
|
||||
|
||||
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
|
||||
if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
|
||||
{
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = static_cast<int>(i)+1;
|
||||
@@ -251,7 +251,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
kalman Kalman;
|
||||
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
@@ -278,8 +278,8 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
snr_tmp = 1/(1+exp(-(temp_track_ref.back().SNR - 16) / 1.2));
|
||||
}
|
||||
|
||||
quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
|
||||
quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
|
||||
quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
|
||||
quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
|
||||
|
||||
if(L == 3){
|
||||
for(int i=0;i<L;i++)
|
||||
@@ -287,7 +287,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
temp_track_ref[i].quality = quality_tmp;
|
||||
}
|
||||
}else{
|
||||
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * TRACK_QUALITY_ITER_COEFF + quality_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
|
||||
temp_track_ref[L-1].quality = temp_track_ref[L-2].quality * rdp_parameter.track_quality_iter_coeff + quality_tmp * (1 - rdp_parameter.track_quality_iter_coeff);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -315,7 +315,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
|
||||
}
|
||||
|
||||
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
//关联上的信息
|
||||
@@ -357,26 +357,26 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
|
||||
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
||||
|
||||
double vmax;
|
||||
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
|
||||
if(rdp_parameter.radar_work_mode == 0) //近程模式 最大速度减小一点
|
||||
{
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
vmax = rdp_parameter.track_init_velocity_max;
|
||||
}
|
||||
else //中远程模式 最大速度正常用
|
||||
{
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
vmax = rdp_parameter.track_init_velocity_max;
|
||||
}
|
||||
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
|
||||
// if(
|
||||
// ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
|
||||
// || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ))
|
||||
// ( (rdp_parameter.radar_work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
|
||||
// || (rdp_parameter.radar_work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) ))
|
||||
// )
|
||||
// && vr_point*v_track_head>0 )
|
||||
|
||||
//点航关联条件
|
||||
if( dis <= vmax* delta_T && dis >= Work_Parameter.V_MIN * delta_T && \
|
||||
if( dis <= vmax* delta_T && dis >= rdp_parameter.track_init_velocity_min * delta_T && \
|
||||
vr_point*v_track_head>0 && \
|
||||
dis >= abs(vr_point) * delta_T && \
|
||||
dis >= abs(v_track_head) * delta_T
|
||||
@@ -434,7 +434,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
|
||||
kalman Kalman;
|
||||
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
@@ -474,7 +474,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter)
|
||||
const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
std::vector <std::vector<Temp_track>> Track_to_start;
|
||||
@@ -498,8 +498,8 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
// double snr_tmp = 1/(1+exp(-(average_snr - 16) / 1.2));
|
||||
// double &d = (*Iter)[L-1].d;
|
||||
// double d_tmp = exp(-d);
|
||||
// double quality_tmp = d_tmp * TRACK_QUALITY_D_WEIGHT + snr_tmp * TRACK_QUALITY_SNR_WEIGHT;
|
||||
// quality_tmp /= (TRACK_QUALITY_D_WEIGHT + TRACK_QUALITY_SNR_WEIGHT);
|
||||
// double quality_tmp = d_tmp * rdp_parameter.track_quality_d_weight + snr_tmp * rdp_parameter.track_quality_snr_weight;
|
||||
// quality_tmp /= (rdp_parameter.track_quality_d_weight + rdp_parameter.track_quality_snr_weight);
|
||||
// for(int i=0;i<L;i++)
|
||||
// {
|
||||
// (*Iter)[i].quality = quality_tmp;
|
||||
@@ -514,12 +514,12 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
}
|
||||
|
||||
//航迹长度小于起始点数,不起批
|
||||
if(L < Work_Parameter.track_start_point_num){
|
||||
if(L < rdp_parameter.track_init_point_num_min){
|
||||
track_init_flag = false;
|
||||
}
|
||||
|
||||
//航迹起批禁止区域判断
|
||||
if(track_init_prohibit(range,azi,Work_Parameter)!=0)
|
||||
if(track_init_prohibit(range,azi,rdp_parameter)!=0)
|
||||
{
|
||||
track_init_flag = false;
|
||||
}
|
||||
@@ -527,32 +527,32 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
//航迹质量起批条件判断
|
||||
switch(L){
|
||||
case 3:
|
||||
if(quality < TRACK_START_QUALITY_L3){
|
||||
if(quality < rdp_parameter.track_init_quality_L3){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
if(quality < TRACK_START_QUALITY_L4){
|
||||
if(quality < rdp_parameter.track_init_quality_L4){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
if(quality < TRACK_START_QUALITY_L5){
|
||||
if(quality < rdp_parameter.track_init_quality_L5){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
case 6:
|
||||
if(quality < TRACK_START_QUALITY_L6){
|
||||
if(quality < rdp_parameter.track_init_quality_L6){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
case 7:
|
||||
if(quality < TRACK_START_QUALITY_L7){
|
||||
if(quality < rdp_parameter.track_init_quality_L7){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
if(quality < TRACK_START_QUALITY_L7){
|
||||
if(quality < rdp_parameter.track_init_quality_L7){
|
||||
track_init_flag = false;
|
||||
}
|
||||
break;
|
||||
@@ -581,7 +581,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
return;
|
||||
|
||||
//2.两两比较Track_to_start中的航迹信息,删除重复的航迹
|
||||
int L = Work_Parameter.track_start_point_num;
|
||||
int L = rdp_parameter.track_init_point_num_min;
|
||||
for (size_t i=0; i+1<Track_to_start.size();i++)
|
||||
{
|
||||
if(Track_to_start[i][0].asso_flag!=2)
|
||||
@@ -632,9 +632,9 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
for (size_t i=0; i<Track_to_start.size();i++)
|
||||
{
|
||||
if (Track_to_start[i].size() < 3) continue;
|
||||
if(trust_track->size()<MAX_TRACK_INDEX)
|
||||
if(trust_track->size() < static_cast<std::size_t>(rdp_parameter.track_index_max))
|
||||
{
|
||||
int index = track_index_mangement.track_ind_get(trust_track);
|
||||
int index = track_index_mangement.track_ind_get(trust_track, rdp_parameter);
|
||||
if(index>0)
|
||||
{
|
||||
|
||||
@@ -647,7 +647,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
double T1=(Track_to_start[i][L-2].CPI_time-Track_to_start[i][L-3].CPI_time)/1000.0;
|
||||
double T2=(Track_to_start[i][L-1].CPI_time-Track_to_start[i][L-2].CPI_time)/1000.0;
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P,rdp_parameter);
|
||||
|
||||
Trust_Track trust_track_tmp = {};
|
||||
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
||||
@@ -715,7 +715,7 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].amplitude);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>(Track_to_start[i][j].SNR);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>(Track_to_start[i][j].RCS);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * TRACK_QUALITY_MAX);
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].Quality = static_cast<float>(Track_to_start[i][j].quality * rdp_parameter.track_quality_max);
|
||||
|
||||
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
|
||||
|
||||
@@ -783,12 +783,12 @@ void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_t
|
||||
}
|
||||
};
|
||||
|
||||
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
|
||||
int Track_Init::track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter)
|
||||
{
|
||||
|
||||
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
|
||||
for (int i=0;i<rdp_parameter.track_init_prohibite_area_num;i++)
|
||||
{
|
||||
if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
|
||||
if(r<rdp_parameter.track_init_prohibite_area_range_max[i] && r>rdp_parameter.track_init_prohibite_area_range_min[i] && azi<rdp_parameter.track_init_prohibite_area_azimuth_max[i] && azi>rdp_parameter.track_init_prohibite_area_azimuth_min[i])
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@ public:
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter
|
||||
const RDP_parameter& rdp_parameter
|
||||
);
|
||||
|
||||
Track_Init();
|
||||
@@ -32,22 +32,22 @@ private:
|
||||
std::vector<PointRecv> point_process; //要处理的点迹
|
||||
|
||||
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //临时航迹与点迹关联
|
||||
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
|
||||
|
||||
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //航迹头与点迹关联
|
||||
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
|
||||
|
||||
void tmp_track_to_trust_track( std::vector <Trust_Track> *trust_track, //临时航迹转可靠航迹
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter);
|
||||
const RDP_parameter& rdp_parameter);
|
||||
|
||||
void tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track);//临时航迹消亡
|
||||
|
||||
double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角
|
||||
|
||||
int track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
|
||||
int track_init_prohibit(double r, double azi, const RDP_parameter& rdp_parameter); //判断目标是否位于航迹起始屏蔽区 是返回1 否返回0
|
||||
|
||||
Track_Ind_Mangement track_index_mangement;
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@ int Track_Init_Direct_Tracking::track_init_process_logic( std::vector<PointRecv>
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int track_ID
|
||||
)
|
||||
{
|
||||
@@ -23,7 +23,7 @@ int Track_Init_Direct_Tracking::track_init_process_logic( std::vector<PointRecv>
|
||||
}
|
||||
|
||||
void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter) //临时航迹与点迹关联
|
||||
const RDP_parameter& rdp_parameter) //临时航迹与点迹关联
|
||||
{
|
||||
//关联信息
|
||||
struct Asso_info
|
||||
@@ -70,7 +70,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
|
||||
{
|
||||
//计算d
|
||||
kalman Kalman;
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind);
|
||||
double d = Kalman.d_cal_track_init_EKF(Z,X,P,delta_T,prt,freq_ind,rdp_parameter);
|
||||
//double d = Kalman.d_cal_track_init_with_doppler(Z,X,P,delta_T,vr_point,prt,freq_ind);
|
||||
//double d = Kalman.d_cal_track_init(Z,X,P,delta_T);
|
||||
//计算夹角
|
||||
@@ -82,7 +82,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
|
||||
double y2=Z[0]*sin(Z[1]);
|
||||
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
|
||||
|
||||
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
|
||||
if(d*d<=rdp_parameter.track_init_threshold*rdp_parameter.track_init_threshold && alpha<rdp_parameter.track_init_angle && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*rdp_parameter.sigma_pitch&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
|
||||
{
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = static_cast<int>(i)+1;
|
||||
@@ -134,7 +134,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
|
||||
kalman Kalman;
|
||||
double T = ( point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][L-1].CPI_time)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
@@ -164,7 +164,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
|
||||
}
|
||||
|
||||
void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter) //航迹头与点迹关联
|
||||
const RDP_parameter& rdp_parameter) //航迹头与点迹关联
|
||||
{
|
||||
//关联上的信息
|
||||
struct Asso_info
|
||||
@@ -205,24 +205,24 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
|
||||
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
|
||||
|
||||
double vmax;
|
||||
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
|
||||
if(rdp_parameter.radar_work_mode == 0) //近程模式 最大速度减小一点
|
||||
{
|
||||
//vmax = V_MAX/5;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
//vmax = rdp_parameter.track_init_velocity_max/5;
|
||||
vmax = rdp_parameter.track_init_velocity_max;
|
||||
|
||||
}
|
||||
else //中远程模式 最大速度正常用
|
||||
{
|
||||
// vmax = V_MAX;
|
||||
vmax = Work_Parameter.V_MAX;
|
||||
// vmax = rdp_parameter.track_init_velocity_max;
|
||||
vmax = rdp_parameter.track_init_velocity_max;
|
||||
|
||||
}
|
||||
|
||||
double delta_T = (T_point - T_track_head)/1000.0;
|
||||
|
||||
//满足关联条件的点航
|
||||
if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )
|
||||
&& vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
|
||||
if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=rdp_parameter.track_init_velocity_min*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=rdp_parameter.track_init_velocity_min*delta_T/2.0) )
|
||||
&& vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*rdp_parameter.sigma_pitch
|
||||
{
|
||||
struct Asso_info asso_info_tmp;
|
||||
asso_info_tmp.point_idx = static_cast<int>(i)+1;
|
||||
@@ -270,7 +270,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
|
||||
kalman Kalman;
|
||||
double T = (point_process[asso_info[i].point_idx-1].CPI_time - (*temp_track)[asso_info[i].track_idx-1][0].CPI_time)/1000.0;
|
||||
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P);
|
||||
Kalman.kalman_filter_init_2dots(Z0,Z1,T,X,P,rdp_parameter);
|
||||
memcpy(asso_track_info_tmp.X, X, 4*sizeof(double));
|
||||
memcpy(asso_track_info_tmp.P, P, 4*4*sizeof(double));
|
||||
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
|
||||
@@ -299,7 +299,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int track_ID)
|
||||
{
|
||||
//temp_track中的临时航迹转为可靠航迹
|
||||
@@ -314,7 +314,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
|
||||
}
|
||||
double range=(*Iter)[L-1].r;
|
||||
double azi=(*Iter)[L-1].azi/PI*180;
|
||||
if( L==Work_Parameter.track_start_point_num) //按长度查找TRUST_TRACK_POINT
|
||||
if( L==rdp_parameter.track_init_point_num_min) //按长度查找TRUST_TRACK_POINT
|
||||
{
|
||||
|
||||
//加入本地航迹文件
|
||||
@@ -326,7 +326,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
|
||||
double T1=((*Iter)[L-2].CPI_time-(*Iter)[L-3].CPI_time)/1000.0;
|
||||
double T2=((*Iter)[L-1].CPI_time-(*Iter)[L-2].CPI_time)/1000.0;
|
||||
kalman Kalman;
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P,rdp_parameter);
|
||||
Trust_Track trust_track_tmp = {};
|
||||
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
||||
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
|
||||
|
||||
@@ -14,7 +14,7 @@ public:
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int track_ID
|
||||
);
|
||||
|
||||
@@ -26,16 +26,16 @@ private:
|
||||
std::vector<PointRecv> point_process; //要处理的点迹
|
||||
|
||||
void point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //临时航迹与点迹关联
|
||||
const RDP_parameter& rdp_parameter); //临时航迹与点迹关联
|
||||
|
||||
void point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct RadarPara Work_Parameter); //航迹头与点迹关联
|
||||
const RDP_parameter& rdp_parameter); //航迹头与点迹关联
|
||||
|
||||
void tmp_track_to_trust_track( std::vector<Trust_Track> *trust_track, //临时航迹转可靠航迹
|
||||
std::vector <std::vector<Temp_track>> *temp_track,
|
||||
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||
int *Trust_track_num_Output,
|
||||
struct RadarPara Work_Parameter,
|
||||
const RDP_parameter& rdp_parameter,
|
||||
int track_ID
|
||||
);
|
||||
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
|
||||
1. **时间主线**:所有时间戳为 64 位毫秒(CPI/GNSS),参与运算时除 1000 转秒;ΔT = 点迹时间 − 航迹时间,ΔT 的正负决定"有效更新 / 只消费不更新 / 跳过"。
|
||||
2. **状态主线**:每条可靠航迹维护一套 IMM(3 个 Singer 模型)状态 [x, vx, ax, y, vy, ay],x/y 两轴解耦,量测为「距离-方位-径向速度」三维 EKF。
|
||||
3. **生命周期主线**:可靠航迹的消亡分两种判据——**TWS 普通航迹自 V1.5.8 起按航迹质量**(`Quality < TRACK_DIE_QUALITY` = 0.3),**重点目标按外推轮数**(判据 `> 10`)、**TAS 按外推轮数**(判据 `> 阈值`:相扫 20 / 机扫 96 圈);临时航迹靠"缓存圈数 buff_round"消亡。
|
||||
3. **生命周期主线**:可靠航迹的消亡分两种判据——**TWS 普通航迹自 V1.5.8 起按航迹质量**(`Quality < track_die_quality`,配置项 0.3),**重点目标按外推轮数**(判据 `> 10`)、**TAS 按外推轮数**(判据 `> 阈值`:相扫 20 / 机扫 96 圈);临时航迹靠"缓存圈数 buff_round"消亡。
|
||||
|
||||
---
|
||||
|
||||
@@ -177,7 +177,7 @@ flowchart TD
|
||||
J --> L{"还有候选?"}
|
||||
K --> L
|
||||
L -- 有 --> G
|
||||
L -- 无 --> M{"未关联航迹(point_flag==0 且非手动TAS):<br/>Extrapolate_round ≤ MAX_EXTRAPOLATE_NUM ?"}
|
||||
L -- 无 --> M{"未关联航迹(point_flag==0 且非手动TAS):<br/>Extrapolate_round ≤ extrapolate_max ?"}
|
||||
M -- 是 --> M1["按数据率 Singer 预测一帧<br/>推进航迹时间,Track_Update_Flag=1"]
|
||||
M -- 否 --> M2["不预测、不推进时间<br/>仅 Extrapolate_round+1,Track_Update_Flag=0"]
|
||||
M1 --> N["只输出 Track_Update_Flag==1 的航迹行并清零该标志<br/>(其后由 track_init 追加起批历史点)"]
|
||||
@@ -186,7 +186,7 @@ flowchart TD
|
||||
|
||||
### 4.2 波门(统计距离、高度、距离三类都通过才算候选)
|
||||
|
||||
**(1)统计距离波门 thd_d**——按航迹距离分段收紧高机动模型(`ASSO_THORD = 4`):
|
||||
**(1)统计距离波门 thd_d**——按航迹距离分段收紧高机动模型(`associate_threshold`,样例配置 4):
|
||||
|
||||
| 航迹距离 r_track | 模型 1 / 2 | 模型 3 |
|
||||
|---|---|---|
|
||||
@@ -194,7 +194,7 @@ flowchart TD
|
||||
| 500 ~ 1000 m | d² < 16 | d² < 16/100(收紧) |
|
||||
| ≥ 1000 m | d² < 16 | d² < 16 |
|
||||
|
||||
> ⚠ **门限语义**:代码里 d 已经是马氏距离平方,判据又写作 `d*d < ASSO_THORD*ASSO_THORD`,`ASSO_THORD = 4` 时等效于 **d < 4**(而非 √d < 4)。调整 ASSO_THORD 时务必记住这一层"二次方"(该行为在 BUG-20 中被识别,有意保留)。
|
||||
> ⚠ **门限语义**:代码里 d 已经是马氏距离平方,判据又写作 `d*d < associate_threshold*associate_threshold`,样例配置 4 时等效于 **d < 4**(而非 √d < 4)。调整 `associate_threshold` 时务必记住这一层"二次方"(该行为在 BUG-20 中被识别,有意保留)。
|
||||
|
||||
**(2)高度门限 thd_h**(e0bf227 起在 TWS 侧生效)——|h_track − h_point| 按航迹距离分段:
|
||||
|
||||
@@ -250,7 +250,7 @@ VT > 300 且 R > 1200 → 拒绝(高速目标给固定 1200 m 上限)
|
||||
|
||||
### 4.6 高度平滑
|
||||
|
||||
关联实点后把点迹高度压入滑窗,输出高度取最近 w 个点的均值(不足 w 个则全平均)。窗口按距离分段,且 TWS/TAS 与扫描体制取值不同(`H_F_WIN_LEN` 相扫 6 / 机扫 3;TAS 偏移于 6284f3f 由 +2/+3/+4/+6 改为 +6/+8/+10/+12):
|
||||
关联实点后把点迹高度压入滑窗,输出高度取最近 w 个点的均值(不足 w 个则全平均)。窗口按距离分段,TWS/TAS 偏移不同、窗长基数取配置项 `height_smooth_win`(样例配置为相扫 6;机扫需配 3。TAS 偏移于 6284f3f 由 +2/+3/+4/+6 改为 +6/+8/+10/+12):
|
||||
|
||||
| 点迹距离 | TWS 窗口(相扫/机扫) | TAS 窗口(相扫/机扫) |
|
||||
|---|---|---|
|
||||
@@ -265,9 +265,9 @@ VT > 300 且 R > 1200 → 拒绝(高速目标给固定 1200 m 上限)
|
||||
|
||||
对 `point_flag == 0` 且 `manual_tracking_flag == 0` 的航迹(含 ΔT≤0 被消费的情形),按 `Extrapolate_round` 分两支(50e0b5c):
|
||||
|
||||
**(1)未到上限**:`Extrapolate_round <= MAX_EXTRAPOLATE_NUM`(机扫/相扫均为 3)时照常外推:
|
||||
**(1)未到上限**:`Extrapolate_round <= extrapolate_max`(配置项,默认 3)时照常外推:
|
||||
|
||||
- 推算步长取运行时数据率:`work_mode` 0/1/2 → `DATA_RATE_SHORT / MIDDLE / FAR`(秒);
|
||||
- 推算步长取配置项 `data_rate_tws`(秒;原实现按 `work_mode` 在 `DATA_RATE_SHORT/MIDDLE/FAR` 三档中三选一,改造后统一为单字段);
|
||||
- 用各子模型的 Singer F、Q 推进一帧(**不做量测更新**);
|
||||
- `T_track`、`GNSS_time` 各推进 ΔT×1000 毫秒;
|
||||
- `Extrapolate_round += 1`,`point_flag = 0`(补点/虚点),`Track_Update_Flag = 1` → 本圈作为补点行输出。
|
||||
@@ -278,11 +278,11 @@ VT > 300 且 R > 1200 → 拒绝(高速目标给固定 1200 m 上限)
|
||||
|
||||
**输出门控(50e0b5c)**:TWS 输出航迹的循环改为「`Track_Update_Flag == 1` 才输出,输出后清零」,即每圈只输出**本圈关联到实点或成功外推**的航迹;此前是每圈把航迹表中全部可靠航迹各输出一行。门控只判更新标志、不判 `manual_tracking_flag`,因此手动转 TAS 的航迹若本圈在 TWS 侧关联到实点,仍会经 TWS 通道输出。
|
||||
|
||||
> **普通 TWS 航迹的消亡改由质量决定(V1.5.8)**:`track_die.cpp` 对 `Track_Mode==0 && primary_target_flag==0` 的航迹改用 `Quality < TRACK_DIE_QUALITY`(0.3)判消亡,因此外推超限后航迹仍会驻留,直到质量衰减到门限以下;`Extrapolate_round` 继续每圈 +1 但不再决定普通 TWS 航迹的存活。重点航迹(10 圈)与 TAS(相扫 20 / 机扫 96)不变。
|
||||
> **普通 TWS 航迹的消亡改由质量决定(V1.5.8)**:`track_die.cpp` 对 `Track_Mode==0 && primary_target_flag==0` 的航迹改用 `Quality < track_die_quality`(配置项 0.3)判消亡,因此外推超限后航迹仍会驻留,直到质量衰减到门限以下;`Extrapolate_round` 继续每圈 +1 但不再决定普通 TWS 航迹的存活。重点航迹(10 圈)与 TAS(相扫 20 / 机扫 96)不变。
|
||||
>
|
||||
> 质量在外推环节按未更新时长二次衰减(`Unupdated_time` 累计,ΔT 由 `work_mode` 数据率决定),且该计算位于外推上限判断之外,故上限内、外都会衰减。
|
||||
>
|
||||
> **TAS 通道的判据方向相反**(`track_asso_tas.cpp`):`Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS` 时才做预测、推进航迹时间并置 `Track_Update_Flag = 1`;未达该轮数时只累加圈数(不预测、不输出)。即 TWS 是"先外推、到上限后停",TAS 是"先不预测、到轮数后才开始外推",详见 7.2 与第 10 节;TAS 不维护航迹质量。
|
||||
> **TAS 通道的判据方向相反**(`track_asso_tas.cpp`):`Extrapolate_round >= extrapolate_max_tas`(配置项,样例相扫 5 / 机扫需配 48)时才做预测、推进航迹时间并置 `Track_Update_Flag = 1`;未达该轮数时只累加圈数(不预测、不输出)。即 TWS 是"先外推、到上限后停",TAS 是"先不预测、到轮数后才开始外推",详见 7.2 与第 10 节;TAS 不维护航迹质量。
|
||||
|
||||
---
|
||||
|
||||
@@ -321,14 +321,14 @@ flowchart TD
|
||||
|
||||
```
|
||||
|r_point − r_末点| ≥ 5 m 距离差硬门限(近距点不参与)
|
||||
d ≤ 8(代码写作 d² ≤ TRACK_START_THRESHOLD²=64) 3 维 EKF 统计距离波门
|
||||
α < ALPHA_START = 60° 折角:最近三点连线夹角
|
||||
d ≤ 8(代码写作 d² ≤ track_init_threshold²=64) 3 维 EKF 统计距离波门
|
||||
α < track_init_angle = 60° 折角:最近三点连线夹角
|
||||
v_r,point · v_r,末点 > 0 多普勒同号
|
||||
```
|
||||
|
||||
- d 由 4 维 CV 状态一步预测 + 「距离-方位-径向速度」EKF 得到(过程噪声 0.03²、同样做方位环绕与盲速折叠);
|
||||
- 折角 α 为向量 D12(前一点→末点)与 D23(末点→新点)的夹角;α 越小越接近直线;
|
||||
- 门限同样写作 `d² ≤ TRACK_START_THRESHOLD²`,而 d 本身已是马氏距离平方,实际生效为 d ≤ 8(与可靠航迹关联门限的语义一致)。
|
||||
- 门限同样写作 `d² ≤ track_init_threshold²`,而 d 本身已是马氏距离平方,`track_init_threshold=8` 时实际生效为 d ≤ 8(与可靠航迹关联门限的语义一致)。
|
||||
|
||||
**阶段②:点 ↔ 航迹头(两点起始)**
|
||||
|
||||
@@ -377,7 +377,7 @@ dis ≥ |v_r,head| · ΔT 同上,用航迹头的多普勒约
|
||||
|
||||
| 条件 | 说明 |
|
||||
|---|---|
|
||||
| 长度 | `track_start_point_num ≤ L ≤ 10`(接口层校验 3~9,典型 3 或 4) |
|
||||
| 长度 | `track_init_point_num_min ≤ L ≤ 10`(配置加载器校验 3~9,典型 3 或 4) |
|
||||
| 位置 | 末点不落在**起批屏蔽区**(距离-方位矩形,最多 30 个,用严格不等号判断) |
|
||||
| 容量 | 可靠航迹数 < 500 且能分配到一个未占用航迹号 |
|
||||
| 初始化 | 用**最后三个点**做三点卡尔曼初始化 → 6 维状态;三模型同状态、u=1/3 |
|
||||
@@ -393,14 +393,14 @@ dis ≥ |v_r,head| · ΔT 同上,用航迹头的多普勒约
|
||||
|
||||
| 通道 | 条件(满足其一即消亡) |
|
||||
|---|---|
|
||||
| TWS 普通航迹 | `Track_Mode==0` 且 `primary_target_flag==0` 且 **`Quality < TRACK_DIE_QUALITY`(0.3)** —— V1.5.8(782bff5)起改按航迹质量,不再看外推圈数 |
|
||||
| TWS 重点航迹 | `Track_Mode==0` 且 `primary_target_flag==1` 且 `Extrapolate_round > TRACK_DIE_ROUND_PRIMARY`(10) |
|
||||
| TAS 航迹 | `Track_Mode==1` 且 `Extrapolate_round > TRACK_DIE_ROUND_TAS`(相扫 20 / 机扫 96) |
|
||||
| TWS 普通航迹 | `Track_Mode==0` 且 `primary_target_flag==0` 且 **`Quality < track_die_quality`(配置项 0.3)** —— V1.5.8(782bff5)起改按航迹质量,不再看外推圈数 |
|
||||
| TWS 重点航迹 | `Track_Mode==0` 且 `primary_target_flag==1` 且 `Extrapolate_round > track_die_round_pramary`(配置项 10) |
|
||||
| TAS 航迹 | `Track_Mode==1` 且 `Extrapolate_round > track_die_round_tas`(配置项;样例相扫 20 / 机扫需配 96) |
|
||||
| 手动删除 | `manual_delete_flag == 1`(各通道单独判定,置位即消亡,由外部删除接口 `track_delete` 置位) |
|
||||
|
||||
轮数类判据都改为 `die_flag` 组合("或"关系),且比较符由 `>=` 改为 `>`(50e0b5c):即允许的外推圈数比旧版各多 1 圈,且**重点航迹与普通航迹走不同门限**。由于每圈未关联只自增 1,判据为 `>` 时实际是"连续 N+1 圈没有实点关联"才消亡:重点航迹 11 圈、TAS 相扫 21 圈(阈值于 5d105d2 由 10 提高到 20)/ 机扫 97 圈。
|
||||
|
||||
> **普通 TWS 航迹的消亡时长(V1.5.8)**:改按质量后不再有固定圈数,存活时长由质量衰减速度决定——未关联期间按 `Unupdated_time` 的二次函数加速衰减(见 9.9 / README 9.11),`Quality` 跌破 0.3 即消亡;`TRACK_DIE_ROUND`(6)已不再被引用。
|
||||
> **普通 TWS 航迹的消亡时长(V1.5.8)**:改按质量后不再有固定圈数,存活时长由质量衰减速度决定——未关联期间按 `Unupdated_time` 的二次函数加速衰减(见 9.9 / README 9.11),`Quality` 跌破 `track_die_quality`(配置项 0.3)即消亡;`track_die_round`(配置项 6)已不再被引用。
|
||||
|
||||
> `primary_target_flag`(重点目标标记)由外部接口 `Data_Process::primary_target_add / primary_target_cancel` 按批号置 1 / 清 0(`data_process.cpp`);新起批航迹在 `track_init.cpp` 中以 `Trust_Track trust_track_tmp = {}` 零初始化,故恒为 0。该标记在 TWS 上决定走「质量消亡(普通)」还是「10 圈消亡(重点)」。
|
||||
|
||||
@@ -437,7 +437,7 @@ flowchart TD
|
||||
| 俯仰/高度门限 | 俯仰门限已注释停用;**高度门限生效**(按距离分段,e0bf227 起) | **高度门限生效**(按距离分段,取值与 TWS 相同) |
|
||||
| ΔT ≤ 0 | 参与选取但只消费点迹、不更新 | **直接跳过该点**(并记告警日志) |
|
||||
| 外推时间 | 按 work_mode 数据率固定步长 | 按 `最新时间戳 − 航迹时间` |
|
||||
| 外推触发轮数 | `Extrapolate_round <= MAX_EXTRAPOLATE_NUM`(3)才预测 | `Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS`(相扫 5 / 机扫 48)才预测(方向与 TWS 相反) |
|
||||
| 外推触发轮数 | `Extrapolate_round <= extrapolate_max`(配置项 3)才预测 | `Extrapolate_round >= extrapolate_max_tas`(配置项,样例相扫 5 / 机扫需配 48)才预测(方向与 TWS 相反) |
|
||||
| 输出 | 本圈有更新的航迹(关联到实点或成功外推,输出后清 `Track_Update_Flag`) | 该批号航迹且本圈有更新 |
|
||||
|
||||
### 7.2 TAS 关联与更新
|
||||
@@ -456,7 +456,7 @@ flowchart TD
|
||||
I --> J["三模型 EKF 更新 → 概率更新 → 融合<br/>刷新时间/点信息/高度平滑"]
|
||||
J --> K["输出该批号航迹(Track_Update_Flag==1 才输出,输出后清零)"]
|
||||
E --> L{"候选表为空?"}
|
||||
L -- 是 --> L1{"Extrapolate_round ≥ MAX_EXTRAPOLATE_NUM_TAS ?"}
|
||||
L -- 是 --> L1{"Extrapolate_round ≥ extrapolate_max_tas ?"}
|
||||
L1 -- 是 --> M["按 ΔT = 最新时间戳 − 航迹时间 外推一帧<br/>(ΔT ≤ 0 则跳过),Track_Update_Flag=1"]
|
||||
L1 -- 否 --> M2["不预测、不推进时间<br/>仅 Extrapolate_round+1,Track_Update_Flag=0"]
|
||||
M --> K
|
||||
@@ -475,7 +475,7 @@ flowchart TD
|
||||
|
||||
**TAS 后续处理**:外推不受 work_mode 数据率约束,而是直接推进到"本帧最新时间戳",保证 TAS 航迹时间与波束调度时刻同步。
|
||||
|
||||
**TAS 外推轮数门控(50e0b5c)**:与 TWS **方向相反**——只有 `Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS`(相扫 5 / 机扫 48)时才做预测、推进航迹时间并置 `Track_Update_Flag = 1`;未达该轮数时只 `Extrapolate_round += 1`、`point_flag = 0`、`Track_Update_Flag = 0`,既不预测也不输出。因此 TAS 航迹在刚丢失点迹时并不会立刻按"最新时间戳"外推:相扫体制下前 5 次未关联不产生任何输出行,第 6 次起才开始外推输出;机扫体制下前 48 次不输出。该不对称仅如实记录(成因不明,不作缺陷判定),连同 TWS/TAS 输出门控一并见第 10 节。
|
||||
**TAS 外推轮数门控(50e0b5c)**:与 TWS **方向相反**——只有 `Extrapolate_round >= extrapolate_max_tas`(配置项,样例相扫 5 / 机扫需配 48)时才做预测、推进航迹时间并置 `Track_Update_Flag = 1`;未达该轮数时只 `Extrapolate_round += 1`、`point_flag = 0`、`Track_Update_Flag = 0`,既不预测也不输出。因此 TAS 航迹在刚丢失点迹时并不会立刻按"最新时间戳"外推:相扫配置下前 5 次未关联不产生任何输出行,第 6 次起才开始外推输出;机扫配置(48)下前 48 次不输出。该不对称仅如实记录(成因不明,不作缺陷判定),连同 TWS/TAS 输出门控一并见第 10 节。
|
||||
|
||||
---
|
||||
|
||||
@@ -510,7 +510,7 @@ flowchart TD
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A["遍历 TAS 队列"] --> B["按批号在航迹表中找到该航迹"]
|
||||
B --> C{"距上次波束输出 > DATA_RATE_TAS 秒<br/>且 CPI 时间早于已选中的最早 CPI?"}
|
||||
B --> C{"距上次波束输出 > data_rate_tas 秒<br/>且 CPI 时间早于已选中的最早 CPI?"}
|
||||
C -- 否 --> A
|
||||
C -- 是 --> D["选中该目标(CPI 时间最早者)"]
|
||||
D --> E["位置外推:ΔT = (最新时间戳 − CPI 时间)/1000<br/>x + vx·ΔT, y + vy·ΔT"]
|
||||
@@ -521,7 +521,7 @@ flowchart TD
|
||||
|
||||
**要点**
|
||||
|
||||
- 门控只用一个条件(e0bf227 修正):目标「距上次波束输出的时间」(`latest_timestamp − last_track_time`)要大于 `DATA_RATE_TAS` 秒(单位是秒,由宿主经参数初始化传入,须为 > 0 的有效值)→ 保证 TAS 目标的数据率**不随 Beam_Ctrl 的调用频率变化**;修正前还要求「航迹时间与最新时间戳之差」也大于该门限,与"取最早 CPI 目标"的语义冲突,会漏掉本应输出的波束;
|
||||
- 门控只用一个条件(e0bf227 修正):目标「距上次波束输出的时间」(`latest_timestamp − last_track_time`)要大于配置项 `data_rate_tas` 秒(单位是秒,由 TOML 配置文件提供、加载器校验须 > 0)→ 保证 TAS 目标的数据率**不随 Beam_Ctrl 的调用频率变化**;修正前还要求「航迹时间与最新时间戳之差」也大于该门限,与"取最早 CPI 目标"的语义冲突,会漏掉本应输出的波束;
|
||||
- 多目标时选**队列中 CPI 时间最早**的目标(同时要求其 CPI 时间早于当前已选者),等价于"按最久未更新者优先"的轮转;
|
||||
- 选中后才按该目标的 CPI 时间算外推步长 `ΔT = (最新时间戳 − CPI 时间)/1000`;
|
||||
- 原来的"每次调用整队循环移位"已停用;
|
||||
@@ -531,46 +531,52 @@ flowchart TD
|
||||
|
||||
## 9. 参数脉络(决定行为的旋钮)
|
||||
|
||||
### 9.1 编译期宏(随扫描体制切换)
|
||||
> **参数配置方式改造后**:雷达工作参数不再来自 `parameters.h` 编译期宏或宿主下发的 `RadarPara`,而是由 TOML 配置文件(样例 `config/config_tmp.toml`)加载到 `struct.h::RDP_parameter`,经公开接口 `parameters_reload(配置文件路径)` 在运行期一次性加载(失败返回 -1 且不改变当前参数;严格校验规则与完整参数表见 README 第 10 节)。下表「样例值」为相扫配置;**机扫需在配置中改**的 4 项已单独标注。
|
||||
|
||||
| 宏 | 相扫 | 机扫 | 作用点 |
|
||||
|---|---|---|---|
|
||||
| `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` | 4 | 同左 | 关联波门(判据 d*d < ASSO_THORD²,d 已是马氏距离平方,等效 d < 4) |
|
||||
| `MAX_EXTRAPOLATE_NUM` | 3 | 同左 | TWS 未关联航迹最大外推轮数(`Extrapolate_round <= 该值` 才预测并输出) |
|
||||
| `MAX_EXTRAPOLATE_NUM_TAS` | 5 | 48 | TAS 未关联航迹开始外推的轮数(判据 `>=`,方向与 TWS 相反) |
|
||||
| `TRACK_DIE_ROUND` | 6 | 6 | **已不被引用**:V1.5.8 起 TWS 普通航迹改按质量消亡(宏保留备用) |
|
||||
| `TRACK_DIE_ROUND_PRIMARY` | 10 | 10 | TWS 重点航迹(`primary_target_flag==1`)消亡轮数 |
|
||||
| `TRACK_DIE_ROUND_TAS` | 20 | 96 | TAS 航迹消亡轮数(判据 `Extrapolate_round > 该值`;相扫 50e0b5c 由 5→10、5d105d2 再调整为 20) |
|
||||
| `TRACK_DIE_QUALITY` | 0.3 | 0.3 | TWS 普通航迹质量消亡门限(`Quality < 该值`;V1.5.8 新增) |
|
||||
| `TRACK_START_QUALITY_L3~L7` | 0.7 / 0.6 / 0.5 / 0.4 / 0.3 | 同左 | 起批最低质量(按临时航迹长度 3/4/5/6/≥7;V1.5.8 新增) |
|
||||
| `TRACK_QUALITY_*` | 见 README 10.1 | 同左 | 质量权重(D/SNR/LEN:0.5/0.3/0.2)、平滑系数 0.8、衰减系数 O2/O1 = −0.0008/−0.0025、折算上限 100(V1.5.8 新增) |
|
||||
| `MAX_TRACK_NUM / MAX_TRACK_INDEX` | 500 / 500 | 同左 | 航迹数上限 / 航迹号上限 |
|
||||
| `TAS_QUEUE_LENGTH / MAX_TAS_NUM` | 4 / 4 | 1 / 1 | TAS 队列长度 / 最大 TAS 目标数 |
|
||||
| `H_F_WIN_LEN` | 6 | 3 | 高度平滑窗长基数:TWS 取 `+2/+3/+4/+6`、TAS 取 `+6/+8/+10/+12`(相扫基数 6284f3f 起由 3 调整为 6,TAS 偏移同提交加长) |
|
||||
| `FREQ0` | 9.2 GHz | 同左 | 盲速计算的基准频率 |
|
||||
| `RDP_LOG_ENABLE / RDP_LOG_DIR` | — | — | 日志落盘开关与目录 |
|
||||
### 9.1 配置文件参数(`[段] 键`,样例值 = 相扫)
|
||||
|
||||
### 9.2 运行时参数(宿主传入,随任务变化)
|
||||
| 配置键 | 样例值 | 作用点 |
|
||||
|---|---|---|
|
||||
| `[radar_params] radar_work_mode` | 0 | 凝聚是否带速度维约束(当前唯一生效点;航迹起始的远近程分支取值相同,属死分支;TWS 外推已改为固定 `data_rate_tws`,不再按模式三选一) |
|
||||
| `[radar_params] freq_0_GHz` | 9.2 | 盲速计算基准频率(原 `FREQ0`) |
|
||||
| `[radar_params] radar_height` / `radar_system_delay` | 0 / 0 | 输出 z 叠加平台高度 / 输出位置系统延迟补偿(原 `Height` / `Sys_delay`) |
|
||||
| `[radar_params] data_rate_tws` | 3 | TWS 未关联航迹外推步长(秒;原按 `work_mode` 在 `DATA_RATE_SHORT/MIDDLE/FAR` 中三选一,现统一单字段) |
|
||||
| `[radar_params] data_rate_tas` | 1 | TAS 波束输出间隔门控(秒,须 > 0) |
|
||||
| `[cohesion] point_cohesion_range/velocity/azimuth` | 80 / 2 / 6 | 凝聚门限:距离 m / 速度 m/s / 方位 °(原 `DOT_COH_*`) |
|
||||
| `[cohesion] point_cohesion_range_min/max` | 100 / 100000 | 有效量程(凝聚后过滤;原 `R_MIN/R_MAX`) |
|
||||
| `[track_associate] sigma_range/azimuth/pitch/velocity` | 10 / 0.02 / 0.2 / 2.0 | 量测噪声、EKF 的 R、转换量测协方差(原 `SIGMA_*`) |
|
||||
| `[track_associate] associate_threshold` | 4 | 关联波门(判据 `d*d < 该值²`,d 已是马氏距离平方 → 等效 d < 4;原 `ASSO_THORD`) |
|
||||
| `[track_associate] extrapolate_max` | 3 | TWS 未关联航迹最大外推轮数(`Extrapolate_round <= 该值` 才预测并输出;原 `MAX_EXTRAPOLATE_NUM`) |
|
||||
| `[track_associate] extrapolate_max_tas` | 5(**机扫配 48**) | TAS 开始外推的轮数(判据 `>=`,方向与 TWS 相反;原 `MAX_EXTRAPOLATE_NUM_TAS`) |
|
||||
| `[track_associate] height_smooth_win` | 6(**机扫配 3**) | 高度平滑窗长基数:TWS 取 `+2/+3/+4/+6`、TAS 取 `+6/+8/+10/+12`(原 `H_F_WIN_LEN`) |
|
||||
| `[track_init] track_init_threshold` / `track_init_angle` | 8 / 60 | 起始统计距离波门(d ≤ 8)/ 起始折角上限(°)(原 `TRACK_START_THRESHOLD` / `ALPHA_START`) |
|
||||
| `[track_init] track_init_point_num_min` | 3 | 起批所需点数(校验 3~9;原 `RadarPara.track_start_point_num`) |
|
||||
| `[track_init] track_init_quality_L3~L7` | 0.7 / 0.6 / 0.5 / 0.4 / 0.3 | 起批最低质量(按临时航迹长度 3/4/5/6/≥7;原 `TRACK_START_QUALITY_L3~L7`) |
|
||||
| `[track_init] track_init_velocity_min/max` | 1 / 50 | 航迹头-点关联的速度区间(原 `V_MIN/V_MAX`) |
|
||||
| `[track_die] track_die_round` | 6 | **已不被引用**:V1.5.8 起 TWS 普通航迹改按质量消亡(保留备用;原 `TRACK_DIE_ROUND`) |
|
||||
| `[track_die] track_die_round_pramary` | 10 | TWS 重点航迹(`primary_target_flag==1`)消亡轮数(原 `TRACK_DIE_ROUND_PRIMARY`) |
|
||||
| `[track_die] track_die_round_tas` | 20(**机扫配 96**) | TAS 航迹消亡轮数(判据 `Extrapolate_round > 该值`;原 `TRACK_DIE_ROUND_TAS`) |
|
||||
| `[track_die] track_die_quality` | 0.3 | TWS 普通航迹质量消亡门限(`Quality < 该值`;原 `TRACK_DIE_QUALITY`) |
|
||||
| `[track_quality] track_quality_*` | 100 / 0 / 0.5 / 0.3 / 0.2 / 0.8 / −0.0008 / −0.0025 | 质量折算上限/下限、三项权重(D/SNR/LEN)、平滑系数、衰减系数 O2/O1(原 `TRACK_QUALITY_*`) |
|
||||
| `[track_index] track_index_max` | 500 | 航迹号上限(原 `MAX_TRACK_INDEX`,内部航迹号表按该值分配) |
|
||||
| `[beam_control] default_track_beam_type` / `tas_track_num_max` | 1 / 4(**机扫配 1**) | 跟踪波束类型(原硬编码 1)/ 最大同时跟踪 TAS 目标数(原 `MAX_TAS_NUM`) |
|
||||
| `[singer_model] model_1/2/3_Q_fast/slow` | 0.000001 / 0.01 / 1.0 | 三个 Singer 模型的过程噪声强度(分档见 5.2;原 `RadarPara.Model*_Q_*`) |
|
||||
| `[track_init_prohibite_area]` / `[tas_prohibite_area]` | `*_num = 0` + 空数组 | 起批屏蔽区 / TAS 禁止区(0~30 组,4 个数组长度必须等于 `*_num`;原 `RadarPara.track_prohibite_area_*` / `TAS_prohibite_area_*`) |
|
||||
|
||||
| 参数 | 影响环节 |
|
||||
|---|---|
|
||||
| `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 禁止区 |
|
||||
> 旧 `RadarPara` 中实现未使用的字段(`R_TAS_max/min`、`V_TAS_max/min`、`TAS_Height_1~5`、`cfar_th`、`north_angle`、`Beam_num`)未进入 `RDP_parameter`,TAS 自动开启/退出逻辑仍保持停用。
|
||||
|
||||
> `track_start_threshold / track_asso_threshold / track_asso_threshold_tas` 三个字段**当前未被实现读取**,实际门限来自编译期宏。
|
||||
### 9.2 编译期常量(`parameters.h` 保留)
|
||||
|
||||
| 常量 / 宏 | 值 | 说明 |
|
||||
|---|---|---|
|
||||
| `MAX_TRACK_NUM` | 500 | 输出航迹数组固定容量(宿主接口维度) |
|
||||
| `RDP_TAS_QUEUE_CAPACITY` | 30 | TAS 队列容量上限;运行期上限用 `tas_track_num_max` |
|
||||
| `UNCONF_TARGET` | 0 | 未确认目标类型语义值 |
|
||||
| `PI` / `Round(x)` | 3.1415926f / 宏 | 圆周率 / 四舍五入取整 |
|
||||
| 版本宏 | 1.5.8 + 体制后缀 | `VERSION_STRING` → `V1.5.8.PS` / `V1.5.8.MS` |
|
||||
| `RDP_LOG_*` / `RDP_RECORD_*` | — | 日志落盘开关/目录、数据记录开关与性能参数(见 README 10.4) |
|
||||
|
||||
> 扫描体制(`PHASE_SCANNING` / `MECHANICAL_SCANNING`)仍决定编译期分支(如判圈判据、版本后缀),但**不再决定任何工作参数数值**。
|
||||
|
||||
---
|
||||
|
||||
@@ -598,16 +604,16 @@ flowchart TD
|
||||
**容量与边界**
|
||||
|
||||
- 点迹数上限 150(前处理已限幅);航迹数/航迹号上限 500,全满时新目标不再起批;
|
||||
- 屏蔽区/禁止区的组数按 30 组数组定义,配置数量未做上限校验;
|
||||
- 屏蔽区/禁止区的组数按 30 组数组定义;配置入口已由加载器强制 `*_num ∈ [0,30]` 且 4 个数组长度等于 `*_num`(非法配置在 `parameters_reload` 阶段即失败),内部实现不再二次校验;
|
||||
- 高度平滑缓存只增不减,长时运行下每条航迹的内存占用会持续增长。
|
||||
|
||||
**外推、消亡与输出门控(50e0b5c / fc49707 / 42b598e / e8c56f9 / V1.5.8)**
|
||||
|
||||
- **外推圈数上限**:TWS 只在 `Extrapolate_round <= MAX_EXTRAPOLATE_NUM`(3)时预测并输出;TAS 相反,`Extrapolate_round >= MAX_EXTRAPOLATE_NUM_TAS`(相扫 5 / 机扫 48)时才预测并输出。两处比较方向不一致,此处仅如实记录,**不判定为缺陷**;
|
||||
- **普通 TWS 航迹消亡改按质量(V1.5.8)**:`Quality < TRACK_DIE_QUALITY`(0.3)即消亡,`TRACK_DIE_ROUND`(6)不再被引用;重点航迹仍按 `> TRACK_DIE_ROUND_PRIMARY`(10)、TAS 仍按 `> TRACK_DIE_ROUND_TAS`(相扫 20 / 机扫 96);`manual_delete_flag == 1` 单独判为消亡;
|
||||
- **外推圈数上限**:TWS 只在 `Extrapolate_round <= extrapolate_max`(配置项 3)时预测并输出;TAS 相反,`Extrapolate_round >= extrapolate_max_tas`(配置项,样例相扫 5 / 机扫需配 48)时才预测并输出。两处比较方向不一致,此处仅如实记录,**不判定为缺陷**;
|
||||
- **普通 TWS 航迹消亡改按质量(V1.5.8)**:`Quality < track_die_quality`(配置项 0.3)即消亡,`track_die_round`(配置项 6)不再被引用;重点航迹仍按 `> track_die_round_pramary`(配置项 10)、TAS 仍按 `> track_die_round_tas`(配置项;样例相扫 20 / 机扫需配 96);`manual_delete_flag == 1` 单独判为消亡;
|
||||
- **航迹质量(V1.5.8)**:内部 0~1,临时航迹每延长一点按「距离+SNR」更新、关联实点按「距离+SNR+有效点占比」平滑更新、未关联期间按 `Unupdated_time` 二次衰减;起批需达按长度分级的最低质量;输出 `Track.Quality` 为 0~100(×100)。TAS 通道不维护质量;
|
||||
- **输出门控**:TWS 与 TAS 的输出航迹循环都新增 `Track_Update_Flag == 1` 判定并在输出后清零,只输出**本圈关联到实点或成功外推**的航迹(此前 TWS 每圈把航迹表中全部可靠航迹各输出一行);
|
||||
- **接近/远离判据**(fc49707 + 42b598e):`Flag_Direction` 不再用实测径向速度 `vr_point > 0`(受多普勒速度模糊影响符号可能取反),改为用 IMM 融合速度方向角判定——`Direction_Angle = atan2(X[4], X[1])`(弧度转度后输出到 `Track.Direction_Angle`),`x`、`y` 为经 `Work_Parameter.Sys_delay` 补偿后的位置、`r` 为其模,`acos((-x·cosθ - y·sinθ)/r) < π/2` 判为靠近(1)、否则远离(0),等价于"速度方向与雷达→目标位置方向夹角 > 90°"。中间版本 fc49707 曾写为 `acos((-x·vx - y·vy)/r/Range_V) < π/4`(按合速度归一化、门限 45°),42b598e 改为上式;
|
||||
- **接近/远离判据**(fc49707 + 42b598e):`Flag_Direction` 不再用实测径向速度 `vr_point > 0`(受多普勒速度模糊影响符号可能取反),改为用 IMM 融合速度方向角判定——`Direction_Angle = atan2(X[4], X[1])`(弧度转度后输出到 `Track.Direction_Angle`),`x`、`y` 为经配置项 `radar_system_delay` 补偿后的位置、`r` 为其模,`acos((-x·cosθ - y·sinθ)/r) < π/2` 判为靠近(1)、否则远离(0),等价于"速度方向与雷达→目标位置方向夹角 > 90°"。中间版本 fc49707 曾写为 `acos((-x·vx - y·vy)/r/Range_V) < π/4`(按合速度归一化、门限 45°),42b598e 改为上式;
|
||||
- **归一化幅度 / 速度维 FFT 点数(e8c56f9 新增,输入侧已打通)**:`DataRev` 新增 `amp_asv`(归一化幅度 dB)、`prn_fftn`(速度维 FFT 点数),`Track`/内部 `PointRecv`/`Trust_Track` 也新增同名成对字段;`data_preprocess()` 现已在 TWS 与 TAS 两条分支都把这两个字段搬入 `PointRecv`(并随 `RdpRecPoint` 落库),因此 `track_asso.cpp` 写入航迹并在 TWS 输出行写回的 `amp_asv_point` / `prn_fftn_point` 有有效数据来源;**TAS 的关联与输出路径(`track_asso_tas.cpp`)仍未涉及这两个字段**,TAS 输出行中它们保持宿主原值。
|
||||
- **版本号与记录标识(b227ae1 / c3c7e66 / 5cb67cc)**:`parameters.h` 新增版本宏并拼装为 `VERSION_STRING`(当前 `V1.5.8.PS` / `V1.5.8.MS`),实例创建后可由基类公开成员 `version_info` 读取;`Data_Process` 新增 `point_id` / `frame_id` / `circle_id` 自增计数(`circle_id` 在 TWS 每判满一圈、TAS 每帧末自增),`PointRecv`/`Temp_track`/`Trust_Track` 新增对应的标识字段,记录功能据此把「输入点迹 → 凝聚点迹 → 临时航迹 → 可靠航迹 → 关联信息」串成同一条链路;可靠航迹与关联信息的记录点为 TWS/TAS 双通道(关联信息以 `Asso_Info::tas_flag` 区分来源)。
|
||||
|
||||
@@ -627,7 +633,7 @@ flowchart TD
|
||||
|---|---|---|
|
||||
| 满圈判据 | 当前方位波位号 < 上一帧波位号 | 按体制区分(3e6763c):**相扫**当前波位号 ≤ 上一帧波位号(等价于原判据)、**机扫**方位波位号 == 0(7d5a56f,适应机扫转台反转) |
|
||||
| 点-航迹头判据 | 按距离分段收紧速度门限(半速、1/2~1/3 速) | 该分支注释停用;改为统一速度区间 **+ 两条多普勒-几何一致性约束**(7d5a56f) |
|
||||
| 起始输出历史点 | "临时航迹的全部 L 个点" | 实际输出**前 `track_start_point_num` 个点** |
|
||||
| 起始输出历史点 | "临时航迹的全部 L 个点" | 实际输出**前 `track_init_point_num_min` 个点** |
|
||||
| 起始输入过滤 | "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 节) |
|
||||
|
||||
+81
-78
@@ -49,9 +49,11 @@ flowchart LR
|
||||
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);
|
||||
Trust_Track_Output, Trust_track_num_Output, rdp_parameter);
|
||||
```
|
||||
|
||||
> `rdp_parameter` 为 `Data_Process` 成员(`const RDP_parameter&` 传入),由公开接口 `parameters_reload(配置文件路径)` 从 TOML 配置文件加载(参数来源见 README 第 10 节;改造前为宿主下发 `RadarPara` + `parameters.h` 宏)。
|
||||
|
||||
### 1.2 调用频率 = 扫描圈
|
||||
|
||||
`data_preprocess` 判为一圈完成时返回 1(`data_process.cpp`),宿主据此调用 `track_process(model=1)`。判据按扫描体制区分(3e6763c):**相扫**比较前后两帧方位波位号(当前 ≤ 上一帧即满圈,`last_beam_num` 参与判断,清空后/首帧即满圈),**机扫**判 `Data_Input[0].Beam_index_aiz == 0`(详见 README 7.2 与 `docs/data_process_flow_report.md` 2.2 节)。
|
||||
@@ -69,7 +71,7 @@ track_init.track_init_process_logic(&point_recv, &trust_track, &temp_track,
|
||||
| `tmp_track_die(...)` | private | 临时航迹消亡 |
|
||||
| `alpha_cal_track_init(...)` | private | 三点折角(度) |
|
||||
| `track_init_prohibit(...)` | private | 起批屏蔽区判断 |
|
||||
| `track_index_mangement` | private | 航迹号(1~500)分配器 |
|
||||
| `track_index_mangement` | private | 航迹号(1~`track_index_max`,默认 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`。
|
||||
|
||||
@@ -114,9 +116,9 @@ track_init.track_init_process_logic(&point_recv, &trust_track, &temp_track,
|
||||
| `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`) |
|
||||
| `const RDP_parameter& rdp_parameter` | 输入 | 工作参数(由 TOML 配置加载;改造前为宿主下发的 `RadarPara`) |
|
||||
|
||||
> **输出行数核算**:`track_asso` 先为当圈**有更新**的可靠航迹写 N 行(输出循环判 `Track_Update_Flag==1` 并清零,即只含关联到实点或成功外推的航迹);`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 时不再允许新起批。
|
||||
> **输出行数核算**:`track_asso` 先为当圈**有更新**的可靠航迹写 N 行(输出循环判 `Track_Update_Flag==1` 并清零,即只含关联到实点或成功外推的航迹);`track_die` 随后删除 D 条(已写出的行与计数不变);起始流程每次起批前判 `trust_track->size() < track_index_max`(配置项,默认 500),故最多再追加 `499 − N + D` 行,**合计最多 499 + D 行**。正常情况下 D 很小,远小于 500;但理论上当 D ≥ 2 且当圈起批数量恰好占满配额(需数百条同时起批)时,行号会超过第一维容量 500,存在越界写入风险(极端场景,见 15.14)。第二维 `[j]`(j < L ≤ 10)始终安全。代价是可靠航迹数达 `track_index_max` 时不再允许新起批。
|
||||
|
||||
---
|
||||
|
||||
@@ -157,7 +159,7 @@ for (size_t i=0;i<(*point_recv).size();i++)
|
||||
```
|
||||
|
||||
- 上游 `track_asso` 已把关联上的点从 `point_recv` 中删除,所以这里进来的都是「未关联点」;函数内部**不再**用 `Use_Flag` 过滤输入(`Use_Flag` 只用于本次调用内部两阶段关联的标记)。
|
||||
- 点迹的 `Range` 已在凝聚阶段做过 `[R_MIN=100, R_MAX=100000] m` 量程过滤。
|
||||
- 点迹的 `Range` 已在凝聚阶段做过量程过滤(下限/上限为配置项 `point_cohesion_range_min` / `point_cohesion_range_max`,默认 100 m / 100000 m)。
|
||||
|
||||
### 4.2 buff_round 自增(L40-47)
|
||||
|
||||
@@ -308,13 +310,13 @@ $$
|
||||
|
||||
| 条件 | 含义 |
|
||||
|---|---|
|
||||
| `dis ≤ V_max·ΔT` | 平均速度不超过最大速度(`V_max = Work_Parameter.V_MAX`) |
|
||||
| `dis ≤ V_max·ΔT` | 平均速度不超过最大速度(`V_max = rdp_parameter.track_init_velocity_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` 注释里)。
|
||||
**近/远程分支(L348-356)当前是死分支**:`work_mode==0` 与 `else` 都取 `vmax = rdp_parameter.track_init_velocity_max`(注释"近程模式最大速度减小一点"未生效;原按 `V_MAX/5` 收紧的写法仅保留在未启用的 `track_init_direct_tracking.cpp` 注释里)。
|
||||
|
||||
**ΔT 语义**:判据隐含要求 `ΔT > 0`(若 `ΔT < 0`,则 `dis ≤ 负数` 必假)。`ΔT == 0` 时仅在 `dis == 0`(同时间同位置的点)才可能成立,属退化情形。
|
||||
|
||||
@@ -382,8 +384,8 @@ double range = (*Iter)[L-1].r;
|
||||
double azi = (*Iter)[L-1].azi/PI*180; // 转度,供屏蔽区比较
|
||||
double quality = (*Iter)[L-1].quality; // V1.5.8:末点航迹质量
|
||||
|
||||
if( L <= 10 && L >= Work_Parameter.track_start_point_num
|
||||
&& track_init_prohibit(range, azi, Work_Parameter) == 0
|
||||
if( L <= 10 && L >= rdp_parameter.track_init_point_num_min
|
||||
&& track_init_prohibit(range, azi, rdp_parameter) == 0
|
||||
&& quality >= 按长度取的质量门限 ) // V1.5.8 新增
|
||||
{ 复制到 Track_to_start; 从 temp_track 中 erase; }
|
||||
else
|
||||
@@ -392,18 +394,18 @@ else
|
||||
|
||||
**入选条件**:
|
||||
|
||||
1. **长度**:`track_start_point_num ≤ L ≤ 10`(`track_start_point_num` 由宿主经 `track_process_parameters_initial` 传入,接口层校验范围 **3~9**,`data_process.cpp`);
|
||||
1. **长度**:`track_init_point_num_min ≤ L ≤ 10`(配置项 `[track_init] track_init_point_num_min`,由 TOML 配置文件提供,加载器校验范围 **3~9**;改造前由宿主经 `track_process_parameters_initial` 传入);
|
||||
2. **不在起批屏蔽区**内(用**末点**的距离/方位判断,见 8.5);
|
||||
3. **航迹质量达标(V1.5.8,d9a7bd0)**:按长度取门限 `TRACK_START_QUALITY_L3~L7`——L=3 →0.7、L=4 →0.6、L=5 →0.5、L=6 →0.4、L≥7 →0.3;比较对象是**末点** `quality`,不达标则本圈不起批、留在 `temp_track` 继续积累。
|
||||
3. **航迹质量达标(V1.5.8,d9a7bd0)**:按长度取门限 `track_init_quality_L3~L7`(配置项)——L=3 →0.7、L=4 →0.6、L=5 →0.5、L=6 →0.4、L≥7 →0.3;比较对象是**末点** `quality`,不达标则本圈不起批、留在 `temp_track` 继续积累。
|
||||
|
||||
命中即从 `temp_track` 摘出(先复制后删除,`Track_to_start` 成为候选表)。
|
||||
|
||||
> 结果:**一旦长度达到阈值、不落在屏蔽区且末点质量达标,当圈即起批**(没有"连续 M 圈满足才起批"的确认期)。若因屏蔽区被拦住,航迹会继续在 `temp_track` 中增长,直到离开屏蔽区(此时 `L > track_start_point_num`,见 8.3 的副作用)或达到 `L=10` 被判消亡;若因质量门限被拦住,则继续积累直到质量回升或临时航迹消亡。
|
||||
> 结果:**一旦长度达到阈值、不落在屏蔽区且末点质量达标,当圈即起批**(没有"连续 M 圈满足才起批"的确认期)。若因屏蔽区被拦住,航迹会继续在 `temp_track` 中增长,直到离开屏蔽区(此时 `L > track_init_point_num_min`,见 8.3 的副作用)或达到 `L=10` 被判消亡;若因质量门限被拦住,则继续积累直到质量回升或临时航迹消亡。
|
||||
|
||||
### 8.2 步骤 2:同源去重(L504-550)
|
||||
|
||||
```cpp
|
||||
int L = Work_Parameter.track_start_point_num; // 注意:此 L 是"起批点数",不是航迹长度
|
||||
int L = rdp_parameter.track_init_point_num_min; // 注意:此 L 是"起批点数",不是航迹长度
|
||||
for (i: 0 .. size-2) for (j: i+1 .. size-1) {
|
||||
if (两者都未被标记 asso_flag==2) {
|
||||
if (某条长度 < 3) continue; // 防御性判断(实际不会触发)
|
||||
@@ -417,7 +419,7 @@ for (i: 0 .. size-2) for (j: i+1 .. size-1) {
|
||||
```
|
||||
|
||||
- **同源判定**:两条候选航迹的前三个点中,只要有一个点的 `T` 与 `r` 完全相等(同一点迹被复制而来),就视为同源重复;
|
||||
- **择优准则**:比较 `[L-1].d`(第 `track_start_point_num` 个点的关联统计距离),**保留 d 更小者**;
|
||||
- **择优准则**:比较 `[L-1].d`(第 `track_init_point_num_min` 个点的关联统计距离),**保留 d 更小者**;
|
||||
- **标记位置**:只写 `[0].asso_flag = 2`,最后统一擦除;
|
||||
- 去重仅看**前三点**,且无论航迹实际多长都用固定下标 0/1/2。
|
||||
|
||||
@@ -425,8 +427,8 @@ for (i: 0 .. size-2) for (j: i+1 .. size-1) {
|
||||
|
||||
```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 (trust_track->size() < rdp_parameter.track_index_max) { // 默认 500 条上限
|
||||
int index = track_index_mangement.track_ind_get(trust_track, rdp_parameter);
|
||||
if (index > 0) { ... }
|
||||
}
|
||||
```
|
||||
@@ -436,10 +438,10 @@ if (trust_track->size() < MAX_TRACK_INDEX) { // 500 条上限
|
||||
```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]
|
||||
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1, T2, X, P, rdp_parameter); // 6 维 [x,vx,ax,y,vy,ay]
|
||||
```
|
||||
|
||||
> ⚠ **不一致点**:初始化用「最后三点」,而输出历史点(8.4)只写「前 `L` 个点」。当航迹长度 = `track_start_point_num` 时两者一致;当因屏蔽区滞留使长度 > 阈值时,输出的历史点里**不包含**用于初始化的那三个点。
|
||||
> ⚠ **不一致点**:初始化用「最后三点」,而输出历史点(8.4)只写「前 `L` 个点」。当航迹长度 = `track_init_point_num_min` 时两者一致;当因屏蔽区滞留使长度 > 阈值时,输出的历史点里**不包含**用于初始化的那三个点。
|
||||
|
||||
**Trust_Track 字段构造(L573-608)**:
|
||||
|
||||
@@ -461,25 +463,25 @@ Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1, T2, X, P); // 6 维 [x,vx,ax,y
|
||||
|
||||
> 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(**不起批**,该候选被静默丢弃)。
|
||||
**航迹号分配**(`track_index_mangement.cpp`):空表返回 1;否则从 `lastest_index+1` 向上找第一个未被占用的号(含 `track_index_max`,默认 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 节);
|
||||
- 对 `j = 0 .. L-1`(`L = rdp_parameter.track_init_point_num_min`)写入该航迹的**历史点**:`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])
|
||||
for (i = 0; i < rdp_parameter.track_init_prohibite_area_num; i++)
|
||||
if (r < rdp_parameter.track_init_prohibite_area_range_max[i] && r > rdp_parameter.track_init_prohibite_area_range_min[i] &&
|
||||
azi < rdp_parameter.track_init_prohibite_area_azimuth_max[i] && azi > rdp_parameter.track_init_prohibite_area_azimuth_min[i])
|
||||
return 1; // 位于屏蔽区,禁止起批
|
||||
return 0;
|
||||
```
|
||||
|
||||
- 判据是**距离-方位矩形区域**(`azi` 已转成度),最多 30 个(`RadarPara` 数组长度;数量未做上限校验);
|
||||
- 判据是**距离-方位矩形区域**(`azi` 已转成度),最多 30 个(`RDP_parameter` 定长数组长度;配置入口由加载器校验 `*_num ∈ [0,30]` 且 4 个数组长度等于 `*_num`,非法配置在 `parameters_reload` 阶段即失败);
|
||||
- 采用**严格不等号**:恰好落在边界上的点不算屏蔽;
|
||||
- 判断用**末点**极坐标,不使用高度。
|
||||
|
||||
@@ -492,10 +494,11 @@ L = temp_track_ref.size();
|
||||
d_tmp = exp(-temp_track_ref[L-1].d); // 末点统计距离项(d 已是马氏距离平方)
|
||||
// L == 3:取三个点 SNR 的平均;L > 3:只取末点 SNR
|
||||
snr_tmp = 1 / (1 + exp(-(SNR - 16) / 1.2)); // 以 16 dB 为界的 Sigmoid
|
||||
quality_tmp = (0.5 * d_tmp + 0.3 * snr_tmp) / (0.5 + 0.3); // 只用距离 + SNR 两项(不含长度项)
|
||||
// 权重为配置项 track_quality_d_weight(0.5) / track_quality_snr_weight(0.3)
|
||||
quality_tmp = (w_d * d_tmp + w_snr * snr_tmp) / (w_d + w_snr); // 只用距离 + SNR 两项(不含长度项)
|
||||
|
||||
if (L == 3) 三点全部写 quality_tmp; // 质量起点
|
||||
else 末点 quality = 前一点 quality * 0.8 + quality_tmp * 0.2; // 指数平滑
|
||||
else 末点 quality = 前一点 quality * c + quality_tmp * (1 - c); // 指数平滑,c = track_quality_iter_coeff(0.8)
|
||||
```
|
||||
|
||||
- `Temp_track::quality` 是 **0~1** 的归一化值;起批时 `Trust_Track.Quality = 末点 quality`,`Point_sum = Valid_point_sum = L`;
|
||||
@@ -533,28 +536,28 @@ if ((*Iter)[n-1].buff_round > 2 || n >= 10) erase;
|
||||
|---|---|
|
||||
| **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`;末点不在起批屏蔽区;**末点质量 ≥ 按 L 取的门限(V1.5.8)**;同源去重后保留;`trust_track.size() < 500` 且成功分配航迹号 |
|
||||
| **2. 航迹头 → 2 点**(L325, L368-372) | 航迹头 `size==1` 且 `buff_round ≥ 2`;`dis ≤ track_init_velocity_max·ΔT`;`dis ≥ track_init_velocity_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`,`track_init_threshold=8`);`α < 60°`(`track_init_angle`);`vr_p·vr_末点 > 0` |
|
||||
| **4. 转可靠航迹**(L484, L556) | `track_init_point_num_min ≤ L ≤ 10`;末点不在起批屏蔽区;**末点质量 ≥ 按 L 取的门限(V1.5.8)**;同源去重后保留;`trust_track.size() < track_index_max` 且成功分配航迹号 |
|
||||
| **5. 消亡**(L686) | `末点.buff_round > 2`(连续 2 圈无新点)或 `长度 ≥ 10` |
|
||||
|
||||
### 10.2 门限汇总
|
||||
|
||||
| 判据 | 数值 | 来源 | 可调 |
|
||||
|---|---|---|---|
|
||||
| 最大速度 `V_MAX` | 运行时 | `RadarPara.V_MAX` | ✅ |
|
||||
| 最小速度 `V_MIN` | 运行时 | `RadarPara.V_MIN` | ✅ |
|
||||
| 最大速度 `vmax` | 50 m/s(样例) | 配置项 `track_init.track_init_velocity_max` | ✅(改配置文件) |
|
||||
| 最小速度 `vmin` | 1 m/s(样例) | 配置项 `track_init.track_init_velocity_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` | ✅ |
|
||||
| 起批波门 `d ≤ 8` | `track_init_threshold=8` | 配置项 `track_init.track_init_threshold` | ✅(改配置文件) |
|
||||
| 起批折角 | `track_init_angle=60°` | 配置项 `track_init.track_init_angle` | ✅(改配置文件) |
|
||||
| 起批点数 | 3~9(典型 3~4) | 配置项 `track_init.track_init_point_num_min` | ✅(加载器校验 3~9) |
|
||||
| 临时航迹长度上限 | 10(硬编码) | L484 / L686 | ❌ |
|
||||
| 允许丢圈数 | 1 圈(`buff_round > 2` 消亡) | L686 | ❌ |
|
||||
| 航迹头上限等待 | 2 圈 | L686(1 点航迹同判据) | ❌ |
|
||||
| 起批屏蔽区 | ≤30 个矩形 | `RadarPara.track_prohibite_area_*` | ✅ |
|
||||
| 起批最低质量(V1.5.8) | L3→0.7 / L4→0.6 / L5→0.5 / L6→0.4 / L≥7→0.3 | `TRACK_START_QUALITY_L3~L7` | 编译期 |
|
||||
| 质量权重(V1.5.8) | 距离 0.5 / SNR 0.3 / 长度 0.2 | `TRACK_QUALITY_*_WEIGHT` | 编译期 |
|
||||
| 航迹数上限 | 500 | `MAX_TRACK_INDEX` | 编译期 |
|
||||
| 起批屏蔽区 | ≤30 个矩形 | 配置段 `[track_init_prohibite_area]`(数组长度 = `*_num`) | ✅(加载器校验 0~30) |
|
||||
| 起批最低质量(V1.5.8) | L3→0.7 / L4→0.6 / L5→0.5 / L6→0.4 / L≥7→0.3 | 配置项 `track_init.track_init_quality_L3~L7` | ✅(改配置文件) |
|
||||
| 质量权重(V1.5.8) | 距离 0.5 / SNR 0.3 / 长度 0.2 | 配置段 `[track_quality]` | ✅(改配置文件) |
|
||||
| 航迹数上限 | 500(默认) | 配置项 `track_index.track_index_max` | ✅(加载器校验 1~500) |
|
||||
|
||||
---
|
||||
|
||||
@@ -615,7 +618,7 @@ $$
|
||||
\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°` 表示允许的航向偏折。
|
||||
其中 `D12 = (末点 − 前一点)`、`D23 = (新点 − 末点)`,三点均取 **Temp_track 的直角坐标**(`X[0]` 为 x、`X[2]` 为 y)。`α` 越小表示越接近直线;`track_init_angle`(配置项,样例 60°)表示允许的航向偏折。
|
||||
|
||||
> 无 `acos` 定义域钳位:当两点重合(分母为 0)或浮点误差使比值略超 ±1 时结果为 `NaN`,`NaN < 60` 恒为 false → 表现为「该点无法关联」,不崩溃。
|
||||
|
||||
@@ -625,14 +628,14 @@ $$
|
||||
|
||||
### 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 尚未同步 7d5a56f 的改动;随后已按本节结论更新 README 第 2、7.2、8.1、8.3、9.3、9.6、10.2 等节,「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` 是起批点数,不是航迹长度) |
|
||||
| 点-航迹头判据 | 按距离分段:近程 `r≥1000` 全速 / `r<1000` 半速;中远程 `r≥2000` 全速 / `r<2000` 1/2~1/3 速 | 该分支已**整体注释停用**,改为统一 `track_init_velocity_min·ΔT ≤ dis ≤ track_init_velocity_max·ΔT` **外加两条多普勒一致性条件** `dis ≥ abs(vr_p)·ΔT`、`dis ≥ abs(vr_h)·ΔT`(L361-372) |
|
||||
| 起批触发长度 | 长度满足 `track_init_point_num_min ≤ L ≤ 10` | 一致 |
|
||||
| 初始化用点 | 最近三点 | 不一致:取下标 `track_init_point_num_min-3 ~ -1` 的三个点,航迹长度大于起批点数时**不是**航迹真正的末三点(详见第 15 节第 2 条) |
|
||||
| 输出历史点 | "将临时航迹的全部 L 个点写入" | 实际只写**前 `track_init_point_num_min` 个点**(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`(引导跟踪版,未启用)的差异
|
||||
@@ -640,8 +643,8 @@ $$
|
||||
| 项 | `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`(严格相等) |
|
||||
| 点-航迹头判据 | 统一门限 + 两条多普勒一致性条件 | 仍为分距离段门限(原 `V_MAX/5` 亦在其注释中) |
|
||||
| 起批长度 | `L ≥ track_init_point_num_min`(长度可大于阈值) | `L == track_init_point_num_min`(严格相等) |
|
||||
| 去重 | 有同源去重 | 无 |
|
||||
| 屏蔽区 | 有 | 无 |
|
||||
| 航迹号 | `track_ind_get()` 分配 | 外部传入 `track_ID` |
|
||||
@@ -652,44 +655,44 @@ $$
|
||||
|
||||
## 13. 参数与调参
|
||||
|
||||
### 13.1 编译期宏(`parameters.h`)
|
||||
> **参数配置方式改造后**:与起始流程相关的全部可调参数都在 TOML 配置文件里(样例 `config/config_tmp.toml`),由公开接口 `parameters_reload(配置文件路径)` 加载到 `RDP_parameter`;`parameters.h` 不再有这些宏,宿主也不再下发 `RadarPara`。改参数 = 改配置文件后重新调用 `parameters_reload()`(加载失败时参数保持不变)。完整参数表与严格校验规则见 README 第 10 节。
|
||||
|
||||
| 宏 | 相扫/机扫 | 含义 | 调大效果 |
|
||||
### 13.1 配置项(样例值,均为「配置段.键」)
|
||||
|
||||
| 配置项 | 样例值 | 含义 | 调大效果 |
|
||||
|---|---|---|---|
|
||||
| `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 精度) | — |
|
||||
| `track_init.track_init_threshold` | 8 | 起批波门,实际生效为 `d ≤ 8` | **更容易起批**(虚警增多) |
|
||||
| `track_init.track_init_angle` | 60 | 起批折角(度) | 更容易起批(允许更急的转向) |
|
||||
| `track_associate.sigma_range/azimuth/pitch/velocity` | 10 / 0.02 / 0.2 / 2.0 | EKF 量测噪声 | 影响 `S` 与 `d`,进而影响关联难度 |
|
||||
| `track_index.track_index_max` | 500 | 航迹号/航迹数上限(1~500) | — |
|
||||
| `cohesion.point_cohesion_range_min/max` | 100 / 100000 | 有效量程(凝聚阶段过滤) | — |
|
||||
| `track_init.track_init_point_num_min` | 3 | 起批点数(加载器校验 3~9):调大 → 起批更慢、更抗虚警,但机动目标捕获更迟 | |
|
||||
| `track_init.track_init_velocity_max/min` | 50 / 1 | 头-点关联的速度区间;`min` 调大可滤除静止/慢速点 | |
|
||||
| `radar_params.radar_work_mode` | 0 | 当前对起始流程**无实际影响**(近/远程分支取值相同) | |
|
||||
| `track_init_prohibite_area.*_num` + 4 个数组 | 0 + 空数组 | 起批屏蔽区(距离 m / 方位 度;≤30 组,数组长度 = `*_num`) | |
|
||||
| `track_init.track_init_quality_L3~L7` | 0.7 / 0.6 / 0.5 / 0.4 / 0.3 | 按临时航迹长度的起批最低质量(V1.5.8) | 降低 → 更容易起批 |
|
||||
| `track_quality.*` | 0.5 / 0.3 / 0.2 / 0.8 | 质量权重、平滑系数与衰减系数(V1.5.8) | 影响质量评估与消亡速度 |
|
||||
|
||||
### 13.2 运行期参数(`RadarPara`)
|
||||
> 旧 `RadarPara` 中的 `track_start_threshold` 字段原本就未被实现读取;新结构体只保留实现真正使用/保留的参数(见 README 10.3 对照表)。
|
||||
|
||||
| 字段 | 影响 |
|
||||
|---|---|
|
||||
| `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.2 调参直觉
|
||||
|
||||
### 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`。
|
||||
- **虚警多、起批太容易**:先降 `track_init_threshold`(8 → 5 左右),再降 `track_init_angle`;必要时提高 `track_init_point_num_min`。
|
||||
- **真实目标起批慢**:提高 `track_init_threshold` / `track_init_angle`,或降低 `track_init_point_num_min`(最小 3)。
|
||||
- **静止/低速杂波起批**:提高 `track_init_velocity_min`。
|
||||
- **某方位扇区/距离段禁止起批**(如地物、干扰区):配置起批屏蔽区,比调门限更精准。
|
||||
- **注意**:`config/config_tmp.toml` 是相扫取值;机扫部署时 `extrapolate_max_tas`、`height_smooth_win`、`track_die_round_tas`、`tas_track_num_max` 需按机扫取值配置(见 README 10.1)。
|
||||
|
||||
---
|
||||
|
||||
## 14. 输出字段映射
|
||||
|
||||
对每条新起批航迹的每个历史点 `j = 0 .. track_start_point_num-1`(L614-664):
|
||||
对每条新起批航迹的每个历史点 `j = 0 .. track_init_point_num_min-1`(L614-664):
|
||||
|
||||
| 输出字段 | 来源 | 备注 |
|
||||
|---|---|---|
|
||||
| `Track_Index` | 新分配的航迹号 | 同一行内所有点相同 |
|
||||
| `Point_Sum` | `= track_start_point_num` | 与 `track_asso` 输出的 `Point_Sum=1` 可作区分 |
|
||||
| `Point_Sum` | `= track_init_point_num_min` | 与 `track_asso` 输出的 `Point_Sum=1` 可作区分 |
|
||||
| `Range` / `Azimuth` | 点的 `r` / `azi`(→度) | 极坐标 |
|
||||
| `Elevation` | `asin(h/r)`(度,钳位 ±1) | BUG-27 修复后的定义域钳位 |
|
||||
| `z` | 点的 `height` | |
|
||||
@@ -704,7 +707,7 @@ $$
|
||||
| `Flag_Point` | 恒为 **1**(实点) | |
|
||||
| `Track_Mode` | 恒为 **0**(TWS) | |
|
||||
| `Quality` | **该历史点的临时航迹质量 × 100**(V1.5.8) | 内部 `quality` 为 0~1,输出折算为 0~100;同一条航迹的各个历史点可能取值不同(L=3 时三点相同,之后只有末点更新) |
|
||||
| **未赋值** | `Flag_Direction`、`x_Predict/y_Predict/z_Predict`、`Range_Predict/Azimuth_Predict/Elevation_Predict`、`Target_Type`、`prf_point`、`v_z`、`amp_asv_point`、`prn_fftn_point` | DLL 不清零该数组,宿主读到的是其自身缓存的旧值/未初始化值。注意 `Flag_Direction` 只在关联输出路径赋值(`track_asso.cpp` / `track_asso_tas.cpp`),起批输出不写该字段。该字段的判据已由 f249fdc 的 `vr_point > 0 ? 1 : 0`(实测径向速度,受多普勒速度模糊影响符号可能取反)改为 fc49707 + 42b598e 的**航向-位置夹角法**:`Direction_Angle = atan2(X[4], X[1])`(弧度转度后同时输出到 `Direction_Angle`),`x`、`y` 为经 `Work_Parameter.Sys_delay` 补偿后的位置、`r` 为其模,`acos((-x·cos(Direction_Angle) - y·sin(Direction_Angle))/r) < π/2` 判为靠近(1)、否则远离(0),即滤波速度方向与"雷达→目标"位置方向夹角超 90° 判为接近。另 `amp_asv_point` / `prn_fftn_point` 属 e8c56f9 新增,`data_preprocess()` **现已**把 `DataRev.amp_asv / prn_fftn` 搬入 `PointRecv`(TWS 与 TAS 分支均赋值,并随 `RdpRecPoint` 落库),因此 TWS 关联/输出路径的该字段已有有效来源;TAS 关联与输出路径(`track_asso_tas.cpp`)仍未涉及这两个字段,TAS 输出行中它们保持宿主原值 |
|
||||
| **未赋值** | `Flag_Direction`、`x_Predict/y_Predict/z_Predict`、`Range_Predict/Azimuth_Predict/Elevation_Predict`、`Target_Type`、`prf_point`、`v_z`、`amp_asv_point`、`prn_fftn_point` | DLL 不清零该数组,宿主读到的是其自身缓存的旧值/未初始化值。注意 `Flag_Direction` 只在关联输出路径赋值(`track_asso.cpp` / `track_asso_tas.cpp`),起批输出不写该字段。该字段的判据已由 f249fdc 的 `vr_point > 0 ? 1 : 0`(实测径向速度,受多普勒速度模糊影响符号可能取反)改为 fc49707 + 42b598e 的**航向-位置夹角法**:`Direction_Angle = atan2(X[4], X[1])`(弧度转度后同时输出到 `Direction_Angle`),`x`、`y` 为经配置项 `radar_system_delay` 补偿后的位置、`r` 为其模,`acos((-x·cos(Direction_Angle) - y·sin(Direction_Angle))/r) < π/2` 判为靠近(1)、否则远离(0),即滤波速度方向与"雷达→目标"位置方向夹角超 90° 判为接近。另 `amp_asv_point` / `prn_fftn_point` 属 e8c56f9 新增,`data_preprocess()` **现已**把 `DataRev.amp_asv / prn_fftn` 搬入 `PointRecv`(TWS 与 TAS 分支均赋值,并随 `RdpRecPoint` 落库),因此 TWS 关联/输出路径的该字段已有有效来源;TAS 关联与输出路径(`track_asso_tas.cpp`)仍未涉及这两个字段,TAS 输出行中它们保持宿主原值 |
|
||||
|
||||
---
|
||||
|
||||
@@ -713,21 +716,21 @@ $$
|
||||
> 以下为阅读源码得到的观察,**不代表已确认的缺陷**,部分为有意保留的历史行为;修改前请结合实测数据评估。
|
||||
|
||||
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`,共享前三点但后续分叉的两条航迹会被误判为同源。
|
||||
2. **初始化用点不是航迹真正的末三点**(L505、L563-565 vs L614):起批流程中 `L = track_init_point_num_min`(L505 覆盖了前面循环里的航迹长度变量),Z0/Z1/Z2 取下标 `L-3 / L-2 / L-1`,输出循环也只写到 `j = L-1`。因此当航迹长度 N 大于起批点数时,参与初始化的只是**前 L 个点中的最后三点**,而不是航迹真正的末三点;第 L 个点之后的尾部点既未参与初始化也不出现在输出中(这些点通常来自滞留在起批屏蔽区期间的积累)。
|
||||
3. **去重只比较前三个点**(L517-519 硬编码 0/1/2),且优度指标取 `[track_init_point_num_min-1].d`(长度大于阈值时非末点);若 `track_init_point_num_min > 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` 时务必记得这一层。
|
||||
5. **ΔT ≤ 0 无显式校验**(BUG-33 遗留):头-点关联靠 `dis ≤ track_init_velocity_max·ΔT` 隐式排除负 ΔT;EKF 关联路径 `d_cal_track_init_EKF(ΔT<0)` 会得到无物理意义的结果,只是通常会被 `d ≤ 8` 挡住。
|
||||
6. **判据对马氏距离平方二次平方**(L221):`track_init_threshold=8` 时实际门限为 `d ≤ 8`(3 自由度约 95.4%)。若按字面 `d² ≤ 64` 理解会误判为 `d ≤ 64`(极宽松)。修改 `track_init_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` 摘除 → 起批机会永久丢失。
|
||||
10. **航迹数饱和静默丢弃**:`trust_track.size() ≥ track_index_max`(配置项,默认 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`。
|
||||
14. **输出数组行号可能越界(极端场景)**:容量判据用 `trust_track->size() < track_index_max`,而 `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() < track_index_max`。
|
||||
15. **起批质量门限比较的是「末点」质量而非整条航迹质量(V1.5.8)**:`tmp_track_to_trust_track()` 取 `(*Iter)[L-1].quality` 与按长度取的门限比较,而该质量在 L>3 时只是末点的指数平滑值(L=3 时三点同值),并不代表整条临时航迹的平均质量;长度越大门限越低,实际是「用更宽松的门限校验最新一点」。
|
||||
16. **起批质量来源依赖阶段二**:`quality` 只在「点-临时航迹关联」成功后更新,阶段三产生的 2 点临时航迹不带质量值;长度 3 的临时航迹在第三次延长时写入三点同值,因此其起批质量有定义。若将来把起批点数放宽到 3 以下(当前接口校验 3~9),则会出现质量恒为 0、永远无法通过质量门限的情况。
|
||||
16. **起批质量来源依赖阶段二**:`quality` 只在「点-临时航迹关联」成功后更新,阶段三产生的 2 点临时航迹不带质量值;长度 3 的临时航迹在第三次延长时写入三点同值,因此其起批质量有定义。若将来把起批点数放宽到 3 以下(当前加载器校验 3~9),则会出现质量恒为 0、永远无法通过质量门限的情况。
|
||||
|
||||
---
|
||||
|
||||
@@ -785,4 +788,4 @@ $$
|
||||
| `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]` |
|
||||
| `data_process.cpp` L204-222 | 参数加载 | `parameters_reload()`:TOML 配置 → `rdp_parameter`(原「参数初始化/修改」已删除) |
|
||||
|
||||
+12
-24
@@ -2,34 +2,22 @@
|
||||
|
||||
## 概述
|
||||
|
||||
根据[data_record_perf_report.md](/docs/data_record_perf_report.md)中提出的优化措施,以及本文档的要求,对数据记录功能进行性能优化。
|
||||
本次修改主要是改进雷达数据处理的参数配置方式,由原来的通过宿主程序下发参数结构体(struct RadarPara),改为通过配置文件读取参数。
|
||||
|
||||
## 优化措施实施决策
|
||||
## 说明
|
||||
|
||||
### P0-a 关联信息裁剪/采样
|
||||
* 新的参数结构体已经定义好,在struct.h文件中的(struct RDP_parameter),该结构体包含了所有的工作参数,包括原struct RadarPara结构体的参数以及parameters.h文件中的参数宏定义,已经将不需要和未用到的参数排除。
|
||||
* 配置文件采用toml格式,配置文件中的参数和struct RDP_parameter中的成员一一对应。预定义了一个[配置文件](config\config_tmp.toml)用于测试。读取配置文件使用[tomlplusplus库](https://github.com/marzer/tomlplusplus),该库的头文件已经下载保存到[目录](third_party\toml\toml.hpp)。
|
||||
|
||||
不采用,实际运行时,航迹数和每圈点迹数都在可控范围,数据行数不会过大。
|
||||
## 具体需求
|
||||
|
||||
### P0-b 批量事务
|
||||
### 参数加载函数
|
||||
|
||||
采用
|
||||
* 将data_process.h中的track_process_parameters_initial和track_process_parameters_modify删除;
|
||||
* 新增一个parameters_reload函数,原型为:int parameters_reload(const char* parameter_file_path);输入为配置文件路径,输出为读取是否成功,成功返回0,失败返回-1。该函数将输入的配置文件加载到Data_Process::rdp_parameter中。
|
||||
|
||||
### P0-c 异步写线程+有界队列
|
||||
### 参数替换
|
||||
|
||||
采用
|
||||
|
||||
### P1-a 构造成本迁移
|
||||
|
||||
采用,按方案A执行
|
||||
|
||||
### P1-b:SQLite 配置与健壮性
|
||||
|
||||
采用
|
||||
|
||||
### P2:记录策略与运维
|
||||
|
||||
不采用
|
||||
|
||||
## 其他事项
|
||||
|
||||
其他不明确或有冲突的地方,直接向我提问
|
||||
* 将原有的struct RadarPara Work_Parameter删除,同时将所有引用到struct RadarPara Work_Parameter的参数替换为RDP_parameter rdp_parameter。
|
||||
* 将原来使用parameters.h中的宏定义参数的地方也替换为RDP_parameter rdp_parameter中的参数。
|
||||
* 如果有不确定的参数,直接向我提问,我来决定。
|
||||
Vendored
+17897
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user