Compare commits

...
7 Commits
Author SHA1 Message Date
waiwaylee 7d5a56f3d2 更新:1、修改一周扫描完成的判断条件,原来采用判断波位号变小为一周扫描完成,但机扫模式下,可能存在转台反转的情况,导致波位号变小,但并未扫描一周,因此改为判断波位号为0时,表示扫描一周完成;
2、更新点迹和航迹头关联条件,限制部分切向高速短航迹;

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-16 11:06:01 +08:00
waiwaylee 604f1585f6 更新:1、TAS航迹不关联时间戳更早的TWS点迹,但标记点迹已使用,避免生成TWS航迹;
2、调整目录结构,将过期的文档都放到archived_docs文件夹中留档;

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-12 10:01:15 +08:00
waiwaylee d6cdbc0120 更新:1、更新项目文档;
2、删除一些无用的代码;

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-09 09:35:55 +08:00
waiwaylee 15215926c5 更新:1、增加日志模块,将标准输出信息写入到日志文件;
2、将RadarPara中DATA_RATE_TAS的单位由Hz改为s;
3、修复TAS_Ctrl::tas_beam_output函数中给Tracking_beam->TAS_track_index赋值错误的问题;

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-07 19:16:32 +08:00
waiwaylee 5f820987a9 更新:1、修复全部C4819 / C4018 / C4244警告;
2、更新项目文档;
2026-09-07 14:29:24 +08:00
waiwaylee f438ac53d9 更新:1、优化TAS逻辑
2、优化宏定义

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-07 09:48:58 +08:00
waiwaylee e729c5baee 更新:修改FREQ0宏定义为9.2GHz
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
2026-09-02 11:30:10 +08:00
43 changed files with 742 additions and 516 deletions
+1
View File
@@ -3,4 +3,5 @@ build-data_process_class_dll-Desktop_Qt_5_7_0_MSVC2013_32bit-Release/
debug/
release/
build/
rdp_logs/
.mnemon/
+29 -5
View File
@@ -10,12 +10,36 @@
### Added
- 新增 CHANGELOG.md,采用 Keep a Changelog 格式记录项目变更历史。
- `RadarPara` 结构体新增 `DATA_RATE_TAS`(TAS 模式数据率)字段,由 DLL 调用者通过 `track_process_parameters_initial()` 输入。
- `RadarPara` 结构体新增 `DATA_RATE_TAS`(TAS 模式数据率)字段,由 DLL 调用者通过 `track_process_parameters_initial()` 输入b3e2f52
- `Tracking_Target` 新增 `last_track_time` 字段,记录各 TAS 目标最近一次波束输出时间(f438ac5)。
- 新增 requirements.md,说明 TAS 波束控制逻辑的修改背景与约定(b3e2f52)。
- 更新 README.md:目录结构补充 CHANGELOG.md / requirements.md;版本历史表补充 93d3924、b3e2f52、e729c5b、f438ac5 提交记录。
- 新增日志文件输出功能:`parameters.h` 增加 `RDP_LOG_ENABLE` 开关(默认开启)与 `RDP_LOG_DIR` 目录宏(`.\\rdp_logs\\`);新增 `rdp_log.h/.cpp` 日志模块,首次 `Data_Process_Factory::GetB()` 时按 `RDP_LOG_ENABLE` 决定是否用 `rdbuf()``std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件(`rdp_YYYYMMDD_HHMMSS.log`,目录不存在自动创建),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭文件;开关注释关闭时不重定向、日志仍输出到标准输出;日志文件在 `CMakeLists.txt` 的 SOURCES 中注册。
- 新增 `RDP_LOG` 日志输出宏(`rdp_log.h`):自动在每条日志前附加 `[时间戳][代码文件:行号]` 信息后再输出到标准输出(重定向开启时随之写入日志文件),供后续新增日志统一使用。
- 更新 README.md604f158、d6cdbc0):目录结构补充 `archived_docs/` 归档目录;9.6 节改写 TWS 关联 ΔT≤0 的处理说明;功能特性、模块职责、已知问题与参考文档的文档引用路径同步指向 `archived_docs/`;版本历史表补充 604f158、d6cdbc0 提交记录。
### Changed
- TAS 波束控制增加数据率门控`Beam_Ctrl()`在队首目标航迹时间 `T_track`最新时间戳 `latest_timestamp` 的差值大于 `1 / DATA_RATE_TAS` 秒时输出 TAS 跟踪波束,使 TAS 目标数据率不再随波束时间(Beam_Ctrl 调用频率)变化;队列轮转逻辑保持不变
- 波束门控使用 `Work_Parameter.DATA_RATE_TAS`(运行时参数)而非 `parameters.h` 中的 `DATA_RATE_TAS` 宏;参数 ≤ 0 时不做门控,保持原输出行为
- TAS 波束控制改为按固定数据率调度`Beam_Ctrl()`当目标满足 `1 / DATA_RATE_TAS` 秒的数据率间隔(相对航迹时间 `T_track`上次波束输出 `last_track_time`)时才输出 TAS 跟踪波束,使 TAS 目标数据率不再随波束时间(Beam_Ctrl 调用频率)变化。
- TAS 波束输出目标改为队列内调度:不再固定取队首,而是遍历 TAS 队列,选择满足数据率门控且 CPI 时间最早的目标输出;原每次 Beam_Ctrl 末尾的整队循环移位(队列轮转)已停用(代码注释保留)
- 数据率门控统一使用运行时参数 `Work_Parameter.DATA_RATE_TAS`(由 DLL 调用者经 `track_process_parameters_initial()` 输入,需为 >0 的有效值);`parameters.h``DATA_RATE_TAS` 编译期宏(相扫 0.3 / 机扫 0.0625)已注释停用,引导跟踪外推同步改用运行时参数。
- 编译时以 `#pragma message` 提示当前启用的扫描体制宏(MECHANICAL_SCANNING / PHASE_SCANNING)。
- 频点宏由 `FREQ0~FREQ20`16.8 GHz)精简为单个 `FREQ0`,取值改为 9.2 GHze729c5b)。
- MSVC 编译选项按编译器版本条件追加 `/utf-8`VS2015 及以上);MSVC 2013 不支持该选项,改用源码文件带 BOM 的 UTF-8 编码解决 C4819。
- 存量调试输出收敛为告警级 `RDP_LOG` 日志(1521592):`track_asso.cpp` / `track_asso_tas.cpp``model_filter()` 在 ΔT ≤ 0(乱序/重复时间戳)分支新增告警日志(含航迹/点迹/时间差信息,其中 `track_asso.cpp` 侧后于 604f158 注释停用);`tas_ctrl.cpp` 中 TAS 波束最早目标调度的 `printf` 调试输出移除,`data_process.cpp` 手动删除与 `track_die_tas.cpp` TAS 目标消亡的 `std::cout` 调试输出注释停用。重定向开启时上述日志随 `std::cout` 一并写入日志文件。
- `RadarPara.DATA_RATE_TAS` 单位由 Hz1/s)改为 s(秒)(1521592):`tas_beam_output()` 的 TAS 波束输出门控由 `1 / DATA_RATE_TAS` 秒改为直接按 `DATA_RATE_TAS` 秒比较,`data_process_class_dll.h` 字段注释同步更新;调用者改按「波束输出间隔(秒)」输入该参数(原 0.3 Hz 的含义不再适用,需按新单位换算后传入)。
- requirements.md 内容更新为「雷达数据处理项目修改说明」(1521592):现描述日志文件输出功能需求(rdbuf() 重定向、日志开关宏、日期+时间命名日志、前缀日志宏,附参考示例)。
- TWS 点航关联中 ΔT ≤ 0(点迹 CPI 时间不晚于航迹时间,如 TAS 航迹遇上时间戳更早的 TWS 点迹、或重复时间戳)的候选不再被剔除(604f158):此类候选仍参与最近邻最小 d 的比较与选取,但不执行 IMM/EKF 更新、不刷新航迹时间与新鲜度(`T_track``Extrapolate_round``point_flag``associate_point_number` 均保持原值,该航迹仍按未关联航迹外推),同时把该点迹标记为已使用(`Use_Flag=1`)消费掉,使其无法再参与航迹起始,避免由此生成虚假 TWS 航迹;`track_asso.cpp` 中 ΔT≤0 的 `RDP_LOG` 告警日志随之注释停用(`track_asso_tas.cpp` 中同名告警日志仍启用)。
- 过期文档归档(604f158):`BUG_REPORT.md``BUG_FIX_REPORT.md``requirements.md` 由仓库根目录移入 `archived_docs/` 留档,仓库内对它们的引用路径同步改为 `archived_docs/...`
### Fixed
- 修复 MSVC `/W3` 下全部 C4819 / C4018 / C4244 编译警告:源码文件改为带 BOM 的 UTF-8 编码;signed/unsigned 不匹配处改用 `size_t` 循环变量或显式 `static_cast<int>` 比较;double/float、__int64/double 等窄化转换改为 `static_cast` 显式转换,保持原有计算逻辑不变(涉及 `data_process.cpp``dot_coh.cpp``dot_coh_tas.cpp``tas_ctrl.cpp``track_asso.cpp``track_asso_tas.cpp``track_init.cpp``track_init_direct_tracking.cpp``track_index_mangement.cpp` 等)。
- 修复 `TAS_Ctrl::tas_beam_output()` 把航迹向量下标 `earliest_track_idx` 误赋给 `Tracking_beam->TAS_track_index` 的问题(1521592):现输出实际航迹号 `(*trust_track)[earliest_track_idx].Track_Index`,避免跟踪波束批号与目标不符。
### Removed
- 删除已注释停用的无用代码残留(d6cdbc0):`data_process.cpp` 中 TWS/TAS 分支的 `std::cout` 调试输出注释块,以及 `dot_coh.cpp` 中早期「1 km 以内 SNR < 20 dB 点迹过滤」的注释代码块。仅清理注释,不改变任何运行逻辑。
## [1.5.5] - 2026-08-27
@@ -46,11 +70,11 @@
### Fixed
- 修复若干逻辑 BUG(数组越界/下溢、局部变量未初始化、时间差无效、方位角 ±π 环绕、IMM 概率奇异、输出数组容量越界等),明细见 `BUG_FIX_REPORT.md`
- 修复若干逻辑 BUG(数组越界/下溢、局部变量未初始化、时间差无效、方位角 ±π 环绕、IMM 概率奇异、输出数组容量越界等),明细见 `archived_docs/BUG_FIX_REPORT.md`
- 修复 TAS 外推时间差可能为无效值的问题。
- 修复航迹起始时未输出跟踪类型、可靠航迹手动转跟踪标志未清零的问题。
- 修复量测噪声协方差矩阵 R 赋值错误、数据预处理波位号判断规则、点迹数量上限、雅可比矩阵取值、起批负索引等问题。
### Removed
- 移除 `requirements.md``CLAUDE.md`、Makefile 系列、Qt 工程用户文件、内置 Eigen 3.3.7 冗余文件及「航迹点迹区分区说明」等不必要文件。
- 移除 `requirements.md`93d3924 移除,后于 b3e2f52 重新加入并改为 TAS 波束控制说明)`CLAUDE.md`、Makefile 系列、Qt 工程用户文件、内置 Eigen 3.3.7 冗余文件及「航迹点迹区分区说明」等不必要文件。
+8
View File
@@ -23,6 +23,7 @@ if(NOT CMAKE_GENERATOR MATCHES "Visual Studio")
endif()
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/dot_coh.cpp
@@ -80,6 +81,13 @@ if(MSVC)
/FS
$<$<CONFIG:Release>:/Zc:strictStrings>
)
# 修复 C4819(源文件含当前代码页无法表示的字符)。
# /utf-8 从 VS2015 Update 2 开始支持;本工程使用 MSVC 2013 时无该选项,
# 通过为源码文件添加 UTF-8 BOM 解决 C4819,这里仅对支持 /utf-8 的新版 MSVC 开启。
if(MSVC_VERSION GREATER_EQUAL 1900)
target_compile_options(data_process_class_dll PRIVATE /utf-8)
endif()
endif()
# 输出目录:build/bin 下为 DLL/PDBbuild/lib 下为导入库。
+61 -23
View File
@@ -55,11 +55,12 @@
- **两帧滑窗点迹凝聚**:TWS 点迹先与上一帧缓存合并凝聚,再进入关联,兼顾跨波位(CPI)相邻点迹的分裂问题;近程模式增加速度维约束。
- **逻辑法航迹起始**:航迹头(单点)→ 点-头速度法关联(两点)→ 点-临时航迹 EKF 波门 + 折角约束关联(三点及以上)→ 三点卡尔曼初始化转可靠航迹;带起批屏蔽区与重复航迹去重。
- **3 模型 IMM 滤波**:三个 Singer 模型(低/中/高过程噪声),量测为「距离-方位-径向速度」三维的 EKF,多普勒残差按盲速折叠,似然用 3×3 新息协方差。
- **最近邻数据关联**:TWS 按航迹距离分段收紧模型 3 波门并附加距离门限;TAS 附加按距离分段的高度门限。
- **最近邻数据关联**:TWS 按航迹距离分段收紧模型 3 波门并附加距离门限;TAS 附加按距离分段的高度门限。ΔT ≤ 0(点迹时间戳不晚于航迹时间)的候选不更新航迹状态、仅消费该点迹(标记已使用),避免其继续起批而生成虚假 TWS 航迹。
- **高度维独立平滑**:按距离分段选择滑动平均窗长(5/6/7/9 点),抑制俯仰测量跳变。
- **TAS 波束控制**:手动转 TAS 目标入队(队列长度与容量随扫描体制切换),按最新时间戳外推输出跟踪波束(距离/方位/俯仰/批号)。
- **航迹生命周期管理**:外推轮数计数、超时消亡、手动删除、全清空(含各模块 reset,防重连后幽灵航迹)。
- **防御性加固**:见 BUG_FIX_REPORT.md——越界防护、时间戳正序校验、方位角 ±π 环绕、asin 定义域钳位、IMM 概率奇异保护等已修复项。
- **防御性加固**:见 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` 日志宏在每条日志前附加 `[时间戳][文件:行号]`,开关注释关闭时不重定向、日志仍输出到控制台。
---
@@ -68,8 +69,12 @@
```text
.
├── CMakeLists.txt # CMake 构建脚本(扫描模式切换、导出宏、输出目录)
├── BUG_REPORT.md # 全量逻辑 BUG 分析报告(P0~P3,37 项
├── BUG_FIX_REPORT.md # BUG 修复报告(修复明细 + 编译验证结果
├── CHANGELOG.md # 变更日志(Keep a Changelog 格式,按 git 提交记录维护
├── README.md # 本文件:项目说明(架构、接口、流程、算法、参数
├── archived_docs/ # 过期文档留档(仅作历史参考;接口与流程以本 README 为准)
│ ├── BUG_REPORT.md # 全量逻辑 BUG 分析报告(P0~P3,37 项)
│ ├── BUG_FIX_REPORT.md # BUG 修复报告(修复明细 + 编译验证结果)
│ └── requirements.md # 需求说明:日志文件输出功能修改要求(含 rdbuf() 重定向参考示例)
├── .vscode/ # VS Code 工程配置
│ ├── settings.json # cmake.generator=NMake、MSVC2013 x86 环境变量、compile_commands
│ ├── cmake-variants.yaml # X256_PS / X256_MS × Release / Debug 变体
@@ -83,6 +88,7 @@
│ ├── data_process_class_dll.h # 对外接口:DataRev/Track/TrackingBeam/RadarPara 结构体、抽象基类、工厂类
│ ├── 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
│ ├── data_process.h / .cpp # Data_Process 实现:接口分发、TWS/TAS 主流程、参数管理、清空/删除/转跟踪
│ ├── struct.h # 内部结构体:PointRecv / Trust_Track / Temp_track / Tracking_Target
│ ├── parameters.h # 编译期宏参数(按 MECHANICAL_SCANNING / PHASE_SCANNING 分两组)
@@ -104,6 +110,7 @@
│ ├── X256_PS/ # 相扫构建目录(含 bin/data_process_class_dll.dll、rdp_playback.exe 等)
│ ├── X256_MS/ # 机扫构建目录
│ └── output/ # 早期打包导出(其中 data_process_class_dll.h 为旧版 32 位时间戳接口,勿作为当前 API 依据)
├── rdp_logs/ # 日志输出目录(运行时自动创建于宿主进程当前工作目录下,被 .gitignore 忽略)
└── .gitignore
```
@@ -159,6 +166,12 @@ cmake --build build/X256_PS
仓库 build/X256_PS/bin/ 内还带有 **rdp_playback.exe**(回放测试工具,依赖 Qt5Core.dll、sqlite3.dll,配置样例 RadarConfigParam.ini,可回放 .db 记录的点迹数据验证 DLL 输出),不在本仓库源码范围内。
### 4.5 编译警告处理
- 源码文件统一使用 **带 BOM 的 UTF-8** 编码保存,兼容 MSVC 2013(该版本不支持 `/utf-8`),消除 C4819。
- `CMakeLists.txt` 对 MSVC 19.0 及以上版本自动追加 `/utf-8` 编译选项;使用 MSVC 2013 构建时请保持源码为 UTF-8 with BOM。
- 修复了 `/W3` 下全部 C4018signed/unsigned 不匹配)与 C4244(类型转换可能丢失数据)警告:循环变量按需改为 `size_t`,窄化转换改为 `static_cast` 显式转换,不改变原有计算逻辑。
---
## 5. 总体架构
@@ -234,14 +247,15 @@ classDiagram
| Data_Processdata_process.cpp | 对外接口实现与流程编排:输入缓存、按 model 分发 TWS/TAS 流水线、参数管理、航迹清空/删除、手动转/停 TAS、波束控制转发 |
| Dot_Coh | TWS 点迹凝聚:两帧滑窗合并凝聚(dot_coh_process_buff |
| Dot_Coh_TAS | TAS 点迹凝聚(单帧,距离/方位/速度三维门限) |
| Track_Asso | TWS 点航关联 + IMM 滤波:模型交互 → 门限筛选(NN)→ 各模型 EKF → 概率更新 → 模型输出;未关联航迹外推;高度平滑 |
| Track_Asso | TWS 点航关联 + IMM 滤波:模型交互 → 门限筛选(NN)→ 各模型 EKF → 概率更新 → 模型输出;未关联航迹外推;高度平滑;ΔT≤0(乱序/重复时间戳)的候选取出后不更新滤波、不刷新航迹,仅消费该点迹 |
| Track_Asso_Tas | TAS 航迹关联(只处理指定批号 tas_track_idx,门限与 TWS 略有差异) |
| Track_Init | 逻辑法航迹起始:航迹头维护、点-头关联(两点)、点-临时航迹关联(三点及以上)、起批(三点卡尔曼初始化)、屏蔽区判断、临时航迹消亡 |
| Track_Die / Track_Die_Tas | 可靠航迹消亡:外推轮数超限或手动删除标志置位时输出消亡批号并删除 |
| TAS_Ctrl | TAS 波束控制:手动目标入队(tas_target_add)、失效目标出队(tas_target_del)、跟踪波束预测输出(tas_beam_output,含数据率门控)、队列轮转 |
| TAS_Ctrl | TAS 波束控制:手动目标入队(tas_target_add)、失效目标出队(tas_target_del)、跟踪波束预测输出(tas_beam_output数据率门控 + 队内选择最早 CPI 目标)、调度时刻记录(last_track_time |
| Track_Ind_Mangement | 航迹号 1~500 的顺序分配与回绕复用 |
| kalman | 两点/三点滤波初始化、线性卡尔曼预测/滤波、EKF(3 维量测含多普勒)、马氏统计距离 d、盲速 Bind_speed |
| coor_trans | 极坐标 ↔ 直角坐标转换 |
| rdp_logrdp_log.h/.cpp | 日志输出:按 parameters.h 的 `RDP_LOG_ENABLE` 开关,在 `Init()` 中用 `rdbuf()``std::cout` 重定向到日期+时间命名的日志文件、`Shutdown()` 恢复控制台并关闭;提供 `RDP_LOG` 宏自动附加 `[时间戳][文件:行号]` 前缀 |
| Track_Init_Direct_Tracking / Track_Asso_Direct_Tracking | 引导跟踪(direct tracking)专用起批/关联。**当前入口为空实现、未启用**track_asso_direct_tracking.cpp 未加入 CMake 源列表 |
---
@@ -292,6 +306,8 @@ classDiagram
| tracking_point(azimuth) | 0 | 手动打跟踪波束——**未实现(保留空实现)** |
| direct_tracking_process(...) | 0 | 引导跟踪——**未实现(保留空实现)** |
> 日志输出为 DLL 内部功能(不对外导出,接口见 10.1 的宏开关):首次 `GetB()` 创建实例时自动初始化(`RDP_LOG_ENABLE` 开启则将 `std::cout` 重定向到 `.\rdp_logs\` 下按日期+时间命名的日志文件),`Destroy()` 销毁实例时恢复 `std::cout` 并关闭日志文件。
### 7.2 data_preprocess 返回值的含义
| 返回值 | 含义 | 触发条件 |
@@ -355,7 +371,7 @@ flowchart TD
D --> E["未关联 → 按 latest_timestamp 外推(ΔT≤0 跳过)"]
E --> F["track_die_tas: TAS 超时/手动删除 → 消亡输出"]
F --> G["输出该批号航迹(point_type=1"]
G --> H["Beam_Ctrl: 队列轮转 + 预测波束输出"]
G --> H["Beam_Ctrl: 数据率门控 + 队内最早 CPI 目标 + 预测波束输出"]
```
### 8.3 航迹起始流程(逻辑法)
@@ -646,7 +662,7 @@ $$
$$
v_{bind}=\frac{150000}{f_{GHz}\cdot PRI_{µs}}\quad(\mathrm{m/s}),\qquad
f_{GHz}=16.8+0.02\cdot f_{ind}
f_{GHz}=9.2+0.02\cdot f_{ind}
$$
(PRI ≤ 0 或 f ≤ 0 时返回安全值 1.0,接口约定 PRI 不允许为 0。)
@@ -676,7 +692,17 @@ VT<100\ \text{且}\ R>300\ \Rightarrow 拒绝;\qquad
VT>300\ \text{且}\ R>1200\ \Rightarrow 拒绝
$$
候选对按 d_min = min(d1,d2,d3) 全局排序,**最近邻**原则逐个取点:每条航迹每圈最多关联一个点、每个点只属于一条航迹。关联成功后:三模型分别 EKF 更新 → 模型概率更新 → 更新航迹时间/幅度/SNR/RCS/十字星数据 → 高度平滑。ΔT ≤ 0(乱序/重复时间戳)的候选被剔除且不占用点迹、不刷新航迹状态(BUG-15 修复)。
候选对按 d_min = min(d1,d2,d3) 全局排序,**最近邻**原则逐个取点:每条航迹每圈最多关联一个点、每个点只属于一条航迹。关联成功后:三模型分别 EKF 更新 → 模型概率更新 → 更新航迹时间/幅度/SNR/RCS/十字星数据 → 高度平滑。
**ΔT ≤ 0 的候选(604f158 起的行为)**:ΔT = (点迹 CPI 时间 航迹 T_track)/1000,ΔT ≤ 0 表示该点迹时间戳不晚于航迹时间(典型场景:航迹已按 TAS 点迹更新过,本圈 TWS 点迹的时间戳更早,或存在重复时间戳)。这种候选**不再被剔除**,仍参与最近邻最小 d 的比较与选取,但选中后:
- 不执行 IMM/EKF 更新,不写回 X1/X2/X3、P1/P2/P3 与模型概率;
- 不刷新航迹时间 T_track / GNSS_time 与点迹信息;
- 不置实点标志:`Extrapolate_round``point_flag``associate_point_number` 均保持原值,因此该航迹仍按「未关联航迹」在后续外推环节推进一帧(见 9.9);
- 该候选对连同该航迹本圈的其余候选对被一并移出候选表,因此该航迹本圈不会再关联其他点迹;由于时间戳更早的候选仍参与最小 d 比较,它可能先于有效候选(ΔT>0)被取中;
- 该点迹的 `Use_Flag` 被置 1 消费掉——点迹使用标记是航迹起始的输入过滤条件(track_init 跳过 Use_Flag==1 的点),因此该点迹不会再参与起批,避免生成虚假 TWS 航迹。
> 历史行为:604f158 之前,ΔT ≤ 0 的候选在选取时被直接剔除并 `continue`,点迹保持未被使用,可能被后续用于航迹起始。该分支原有的 `RDP_LOG` 告警日志现已注释停用;TAS 关联(track_asso_tas.cpp,见下)中同类告警日志仍启用。
#### TAStrack_asso_tas.cpp
@@ -708,7 +734,7 @@ $$
### 9.9 航迹外推与消亡
**外推**:未关联到实点的 TWS 航迹(manual_tracking_flag==0)按工作模式数据率 DATA_RATE_SHORT/MIDDLE/FAR 预测一帧;TAS 通道按 latest_timestamp T_track 预测(ΔT ≤ 0 跳过);预测用各子模型 Singer F/Qj 推进,Extrapolate_round+1point_flag=0(虚点/补点),航迹时间同步推进。
**外推**:未关联到实点的 TWS 航迹(manual_tracking_flag==0)按工作模式数据率 DATA_RATE_SHORT/MIDDLE/FAR 预测一帧(ΔT≤0 的候选虽被消费但未刷新航迹状态,其 `point_flag` 仍为 0,故同样在本环节外推,见 9.6)TAS 通道按 latest_timestamp T_track 预测(ΔT ≤ 0 跳过);预测用各子模型 Singer F/Qj 推进,Extrapolate_round+1point_flag=0(虚点/补点),航迹时间同步推进。
**消亡**
@@ -721,16 +747,16 @@ $$
### 9.10 TAS 波束控制(tas_ctrl.cpp
- **入队条件**(仅手动):manual_tracking_flag==1 且 Track_Mode==0 且队列未满(MAX_TAS_NUM,相扫 4 / 机扫 1)且不在 TAS 禁止区内。入队即置 Track_Mode=1 并输出一次航迹更新。
- **入队条件**(仅手动):manual_tracking_flag==1 且 Track_Mode==0 且队列未满(MAX_TAS_NUM,相扫 4 / 机扫 1)且不在 TAS 禁止区内。入队即置 Track_Mode=1、以航迹时间初始化 last_track_time并输出一次航迹更新。
- **出队**:队列中目标在航迹表中不存在、或不再处于手动 TAS 状态时清除。(自动转 TAS / 自动退出的 tas_auto_start/tas_auto_end 逻辑全部注释停用——只支持手动。)
- **波束输出**取队首目标,先用**数据率门控**航迹时间 T_track 与当前最新时间戳 latest_timestamp 的差值须大于 `1 / DATA_RATE_TAS` 秒才输出(保证 TAS 目标数据率不随波束时间变化;DATA_RATE_TAS ≤ 0 时不做门控,按原逻辑输出)。门控通过后,用其状态外推到 latest_timestamp
- **波束输出**在 TAS 队列内遍历,选择满足**数据率门控**的目标——目标距上次波束输出(`last_track_time`)的时间、以及航迹时间 T_track 与最新时间戳 latest_timestamp 的差值,均须大于运行时参数 `Work_Parameter.DATA_RATE_TAS` 秒(单位 s1521592 起由 Hz 改为 s,需为 >0 的有效值),保证 TAS 目标输出间隔不随波束时间(Beam_Ctrl 调用频率)变化;多个候选中取 CPI 时间最早者,用其状态外推到 latest_timestamp
$$
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。每次 Beam_Ctrl 末尾做队列轮转(实现 TAS_QUEUE_LENGTH 深度的循环跟踪)
输出 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 末尾的整队循环移位(队列轮转)已停用,改为按数据率门控在队列内轮换目标
- tas_ctrl_process 入口强制 *Trust_track_num_Output=0 并在写入前检查容量,防止输出数组越界(BUG-09 修复)。
---
@@ -741,7 +767,9 @@ $$
| 宏 | 相扫 PHASE_SCANNING | 机扫 MECHANICAL_SCANNING | 含义 |
|---|---|---|---|
| DATA_RATE_TAS | 0.3 | 0.0625 | TAS 数据率(1/s)。**TAS 波束输出门控已改用运行时参数 RadarPara.DATA_RATE_TAS(见 10.2),不再使用该宏**;宏目前仅被未加入 CMake 的 track_asso_direct_tracking.cpp 引用 |
| RDP_LOG_ENABLE | 定义(默认开启) | 同左 | 日志文件输出开关:定义时将 `std::cout` 重定向到日志文件;注释掉则不重定向(日志仍输出到标准输出/控制台) |
| RDP_LOG_DIR | `.\rdp_logs\` | 同左 | 日志文件输出目录(相对宿主进程当前工作目录,目录不存在时自动创建) |
| DATA_RATE_TAS | 已停用(原 0.3 | 已停用(原 0.0625) | 编译期宏已注释停用;TAS 数据率统一由运行时参数 RadarPara.DATA_RATE_TAS 提供(见 10.2 |
| SIGMA_R / SIGMA_A / SIGMA_E / SIGMA_V | 10.0 / 0.02 / 0.2 / 2.0(两体制相同) | 同左 | 量测误差:距离 m / 方位 rad / 俯仰 rad / 速度 m/s |
| DOT_COH_RANGE / DOT_COH_V / DOT_COH_AZI | 80 / 2 / 6 | 同左 | 凝聚门限:距离 m / 速度 m/s / 方位 ° |
| MAX_BEAM_NUM | 100 | 同左 | 最大波位数 |
@@ -754,7 +782,7 @@ $$
| ASSO_THORD | 3 | 同左 | 关联波门 |
| H_F_WIN_LEN | 3 | 同左 | 高度平滑窗长基数(实际 +2/+3/+4/+6 |
| TAS_QUEUE_LENGTH / MAX_TAS_NUM | 4 / 4 | 1 / 1 | TAS 队列长度 / 最大 TAS 目标数 |
| FREQ0~FREQ20 | 16.8 GHz | 同左 | 各频点频率 |
| FREQ0 | 9.2 GHz | 同左 | 载波频点(原 FREQ0~FREQ20 一组宏已精简为单个 FREQ0e729c5b 起由 16.8 GHz 改为 9.2 GHz |
### 10.2 运行时参数(RadarPara
@@ -771,7 +799,7 @@ $$
| north_angle | 北偏角 | 保留 |
| V_MAX / V_MIN | 目标速度上下限(m/s) | 航迹头关联速度区间门限 |
| DATA_RATE_SHORT / MIDDLE / FAR | 三模式数据率(s | TWS 外推步长 |
| DATA_RATE_TAS | TAS 模式数据率(1/s | 由调用者经 track_process_parameters_initial 输入TAS 波束输出门控阈值 1/DATA_RATE_TAS 秒,≤0 时不做门控 |
| 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 |
@@ -780,15 +808,15 @@ $$
## 11. 已知问题与限制
详见 BUG_REPORT.md37 项分析)与 BUG_FIX_REPORT.md(修复明细)。要点:
详见 archived_docs/BUG_REPORT.md37 项分析)与 archived_docs/BUG_FIX_REPORT.md(修复明细)。要点:
### 11.1 已修复(BUG_FIX_REPORT.md
### 11.1 已修复(archived_docs/BUG_FIX_REPORT.md
- **越界/防御**TAS Point_Sum 限幅 150BUG-04);size()-1 下溢(BUG-05);起批点数校验(BUG-06);航迹号下标校验(BUG-10);Beam_Ctrl 输出计数清零+容量检查(BUG-09);消亡空向量保护(BUG-34);航迹号 500 可复用(BUG-35);局部结构体值初始化(BUG-37)。
- **时间戳/时序**TWS/TAS 关联 ΔT≤0 拒绝(BUG-15/16);TAS 外推负时间差跳过(BUG-17);Bind_speed 除零保护(BUG-19)。
- **时间戳/时序**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);航向角 atan2BUG-26);asin 定义域钳位(BUG-27);近程模型 3 门限浮点除法(BUG-21);track_clear_all 全模块 resetBUG-28);未初始化变量修复(BUG-11/12/14)。
### 11.2 有意保留 / 未修复(标注于 BUG_REPORT
### 11.2 有意保留 / 未修复(标注于 archived_docs/BUG_REPORT.md
- **BUG-01**:基类无虚析构,Destroy() 按基类指针 delete。
- **BUG-02**Hight_smooth 只增不减,长时运行内存持续增长。
@@ -819,8 +847,16 @@ $$
| 提交 | 内容 |
|---|---|
| 604f158 | TWS 关联中 ΔT≤0(点迹时间戳不晚于航迹时间)的候选不再剔除:仍参与最近邻选取但不更新 IMM/EKF、不刷新航迹时间与新鲜度,并标记该点迹已使用(`Use_Flag=1`)消费掉,避免其继续参与航迹起始生成虚假 TWS 航迹;`track_asso.cpp` 的 ΔT≤0 告警日志注释停用;过期文档(BUG_REPORT.md、BUG_FIX_REPORT.md、requirements.md)移入 archived_docs/ 留档 |
| d6cdbc0 | 删除 `data_process.cpp``dot_coh.cpp` 中已注释停用的调试输出与早期 1 km SNR 过滤代码残留(不改变运行逻辑);同步更新 README/CHANGELOG |
| 1521592 | 新增日志模块(`rdp_log.h/.cpp`CMake SOURCES 注册、.gitignore 忽略 `rdp_logs/`):`RDP_LOG_ENABLE` / `RDP_LOG_DIR` 宏(parameters.h)、std::cout 经 `rdbuf()` 重定向到 `.\rdp_logs\` 下日期+时间命名日志、`RDP_LOG` 前缀宏、ΔT≤0 告警日志;`RadarPara.DATA_RATE_TAS` 单位由 Hz 改为 s;修复 `tas_beam_output()` 将向量下标误赋给 `Tracking_beam->TAS_track_index` 的问题;更新 README/CHANGELOG |
| 5f82098 | 修复 MSVC `/W3` 下全部 C4819 / C4018 / C4244 警告(UTF-8 with BOM、size_t / static_cast 等);更新项目文档 |
| f438ac5 | 优化 TAS 调度:波束输出改为在队列内选择满足数据率门控且 CPI 时间最早的目标,停用整队循环移位;DATA_RATE_TAS 编译期宏停用、统一使用运行时参数;#pragma message 提示扫描体制 |
| e729c5b | 频点宏精简为 FREQ0 = 9.2 GHz(原 FREQ0~FREQ20 均为 16.8 GHz |
| b3e2f52 | TAS 波束控制改为按固定数据率调度,RadarPara 新增 DATA_RATE_TAS 运行时参数;新增 CHANGELOG.md、requirements.mdREADME 补充相应说明 |
| 93d3924 | 新增 README.md(本文档);移除 requirements.md;调整 .gitignore |
| 3500652 | 去掉 1 km 以内航迹起批的 20 dB 信噪比门限 |
| 01d28e0 | 改用 VS Code + CMake 重新编译(编译器不变);修复若干逻辑 BUG(见 BUG_FIX_REPORT.md |
| 01d28e0 | 改用 VS Code + CMake 重新编译(编译器不变);修复若干逻辑 BUG(见 archived_docs/BUG_FIX_REPORT.md |
| 0cb31b4 | 删除不必要文件;输入点迹/输出航迹的 CPI 与 GNSS 时间改为 64 位整型;修复 TAS 外推时间差可能无效的问题 |
| d101fe9 | 增加航迹清空功能(解决重连乱窜);TAS 外推时间间隔按波束时间计算;增加 1 km 内 SNR<20 dB 不起批限制(后于 3500652 移除) |
| 8636179 | 机扫/相扫程序合并,宏定义配置切换 |
@@ -841,8 +877,10 @@ $$
| 文档 | 内容 |
|---|---|
| BUG_REPORT.md | 全量逻辑 BUG 分析(P0~P3 共 37 项,含位置、说明、建议、处置结论 |
| BUG_FIX_REPORT.md | 本次修复明细、修复状态汇总表与 MSVC 2013 x86 编译验证记录 |
| CHANGELOG.md | 变更日志(Keep a Changelog 格式,按 git 提交记录维护 |
| archived_docs/BUG_REPORT.md | 全量逻辑 BUG 分析(P0~P3 共 37 项,含位置、说明、建议、处置结论)——过期留档,仅作历史参考 |
| archived_docs/BUG_FIX_REPORT.md | 修复明细、修复状态汇总表与 MSVC 2013 x86 编译验证记录——过期留档(对应 01d28e0 时期) |
| archived_docs/requirements.md | 修改需求说明:日志文件输出功能(rdbuf() 重定向、parameters.h 日志开关宏、日期+时间命名日志、前缀日志宏,附参考示例)——已实现,留档 |
| backup/data_process_class_dll/data_process_class_dll.pro | 迁移前的 Qt/qmake 工程文件(源文件清单参考) |
| .vscode/tasks.json、.vscode/cmake-variants.yaml | VS Code 构建任务与变体定义 |
| build/X256_PS/bin/RadarConfigParam.ini | 回放测试工具(rdp_playback.exe)配置样例 |
+42
View File
@@ -0,0 +1,42 @@
# 雷达数据处理项目修改说明
## 概述
增加日志文件输出功能,具体需求如下:
- 将标准输出std::cout重定向到日志文件,考虑使用 rdbuf() 方法,见[参考1](#参考1)
- 在parameters.h中添加一个日志输出开关的宏定义,开关关闭时,不重定向输出到文件
- 日志文件输出路径为“.\\rdp_logs\\”,日志文件名按当前日期+时间命名
- 增加宏定义,在输出日志时添加时间戳、代码文件、代码行数等关键信息再输出到标准输出
## 参考
### 参考1
使用 rdbuf() 方法重定向标准输出,更符合C++的流式风格,通过替换 std::cout 的底层流缓冲区(streambuf),将其指向一个文件输出流(std::ofstream)。
```cpp
#include <iostream>
#include <fstream>
int main() {
std::ofstream file("output.txt");
if (!file.is_open()) {
return 1;
}
// 保存原始的 cout 缓冲区,以便之后恢复
auto original_buf = std::cout.rdbuf();
// 将 cout 的缓冲区替换为 file 的缓冲区
std::cout.rdbuf(file.rdbuf());
// 此后的输出都会写入文件
std::cout << "这条信息会写入文件。" << std::endl;
// 恢复 cout 的原始缓冲区,让输出回到控制台
std::cout.rdbuf(original_buf);
// file 对象会在析构时自动关闭
std::cout << "这条信息会回到控制台。" << std::endl;
return 0;
}
```
+1 -1
View File
@@ -1,4 +1,4 @@
#include "coor_trans.h"
#include "coor_trans.h"
#include <cmath>
#include <cstring>
#include <vector>
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef COOR_TRANS_H
#ifndef COOR_TRANS_H
#define COOR_TRANS_H
#include "parameters.h"
+20 -18
View File
@@ -1,4 +1,5 @@
#include "data_process.h"
#include "data_process.h"
#include "rdp_log.h"
#include <cstring>
#include <vector>
#include <cmath>
@@ -45,7 +46,9 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
Data_buffer.push_back(Data_buffer_temp);
}
if(Data_Input[0].Beam_index_aiz < last_beam_num) //TWS
// 修改一周扫描完成的判断条件,原来采用判断波位号变小为一周扫描完成,但机扫模式下,可能存在转台反转的情况,导致波位号变小,但并未扫描一周,因此改为判断波位号为0时,表示扫描一周完成
if(Data_Input[0].Beam_index_aiz == 0) //TWS
{
last_beam_num = Data_Input[0].Beam_index_aiz;
@@ -56,6 +59,17 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
last_beam_num = Data_Input[0].Beam_index_aiz;
return 2;
}
// if(Data_Input[0].Beam_index_aiz < last_beam_num) //TWS
// {
// last_beam_num = Data_Input[0].Beam_index_aiz;
// return 1;
// }
// else
// {
// last_beam_num = Data_Input[0].Beam_index_aiz;
// return 2;
// }
}
else if(Data_Input[0].point_type == 1) //tas数据
@@ -65,7 +79,6 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
data_num = min(max(Data_Input[0].Point_Sum, 0), 150);
TAS_track_idx = Data_Input[0].TAS_track_index;
latest_timestamp = Data_Input[0].CPI_time;
for (int i=0; i<data_num;i++)
{
PointRecv Data_buffer_temp;
@@ -144,11 +157,6 @@ int model)
Trust_Track_Output,
Trust_track_num_Output,
Work_Parameter);
// std::cout << "point_recv: " <<point_recv.size();
// std::cout << "trust_track: " <<trust_track.size();
// std::cout << "temp_track: " <<temp_track.size();
// std::cout << "Trust_track_num_Output: " <<*Trust_track_num_Output;
// std::cout << "Track_die_num_Output: " <<*Track_die_num_Output;
}
@@ -160,12 +168,6 @@ int model)
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
// if(point_recv_tas.size()>0)
// {
// std::cout << "point_recv_tas :" <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
// }
//std::cout << "TAS process :" << TAS_track_idx << "p a r:"<<point_recv_tas[0].Point_Sum<<" " <<point_recv_tas[0].Azimuth/PI*180<<" "<<point_recv_tas[0].Range;
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件
@@ -247,13 +249,13 @@ int Data_Process:: track_delete(int delete_track_num, //手动
{
for (int i=0;i<delete_track_num ;i++)
{
for (int j=0;j<trust_track.size();j++)
for (size_t j=0;j<trust_track.size();j++)
{
if(trust_track[j].Track_Index == delete_track_index[i])
{
trust_track[j].manual_delete_flag=1;
std::cout << "Delete :" << trust_track[j].Track_Index << std::endl;
// RDP_LOG << "Delete :" << trust_track[j].Track_Index << std::endl;
}
}
@@ -266,7 +268,7 @@ int Data_Process:: track_delete(int delete_track_num, //手动
int Data_Process:: tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号
{
for ( int i=0; i<trust_track.size();i++)
for ( size_t i=0; i<trust_track.size();i++)
if(trust_track[i].Track_Index==track_index)
{
trust_track[i].manual_tracking_flag = 1;
@@ -279,7 +281,7 @@ int Data_Process:: tracking_start(int track_index) //需要手动
int Data_Process:: tracking_stop(int track_index) //需要手动转入TAS跟踪的航迹号
{
for ( int i=0; i<trust_track.size();i++){
for ( size_t i=0; i<trust_track.size();i++){
if(trust_track[i].Track_Index==track_index)
{
trust_track[i].manual_tracking_flag = 0;
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef DATA_PROCESS_H
#ifndef DATA_PROCESS_H
#define DATA_PROCESS_H
#pragma once
#include "data_process_class_dll.h"
@@ -1,8 +1,9 @@
#ifndef DATA_PROCESS_CLASS_DLL_LIBRARY
#ifndef DATA_PROCESS_CLASS_DLL_LIBRARY
#define DATA_PROCESS_CLASS_DLL_LIBRARY
#endif
#include "data_process_class_dll.h"
#include "data_process.h"
#include "rdp_log.h"
/*
Data_process_class_dll::Data_process_class_dll()
@@ -16,7 +17,9 @@ Data_process_class_dll *Data_Process_Factory::GetB()
{
if(!p)
{
p=new Data_Process();
//初始化日志输出:RDP_LOG_ENABLE 开启时重定向 std::cout 到日志文件
//rdp_log::Init();
p=new Data_Process();
}
return p;
@@ -30,5 +33,8 @@ void Data_Process_Factory::Destroy()
if (p) {
delete p;
p = nullptr;
//恢复 std::cout 到控制台并关闭日志文件
//rdp_log::Shutdown();
}
}
@@ -1,4 +1,4 @@
#pragma once
#pragma once
#ifndef DATA_PROCESS_CLASS_DLL_H
#define DATA_PROCESS_CLASS_DLL_H
@@ -147,7 +147,7 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara
float DATA_RATE_MIDDLE;
float DATA_RATE_FAR;
float DATA_RATE_TAS; //TAS模式数据率,由dll调用者输入;TAS波束输出间隔阈值为 1/DATA_RATE_TAS 秒
float DATA_RATE_TAS; //TAS模式数据率,由dll调用者输入;TAS波束输出间隔阈值为 DATA_RATE_TAS 秒
//数据关联参数
int track_start_point_num; //起批点数(典型值 3或4)
@@ -1,4 +1,4 @@
#ifndef DATA_PROCESS_CLASS_DLL_GLOBAL_H
#ifndef DATA_PROCESS_CLASS_DLL_GLOBAL_H
#define DATA_PROCESS_CLASS_DLL_GLOBAL_H
#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY)
+39 -47
View File
@@ -1,4 +1,4 @@
#include "dot_coh.h"
#include "dot_coh.h"
#include <cmath>
#include <cstring>
#include <vector>
@@ -16,18 +16,18 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
{
if( (*data_input)[loop_of_point].Use_Flag!=1)
{
float point_0_R=(*data_input)[loop_of_point].Range;
float point_0_V=(*data_input)[loop_of_point].Velocity;
float point_0_F=(*data_input)[loop_of_point].Azimuth;
float point_0_A=(*data_input)[loop_of_point].Amplitude;
float point_0_R=static_cast<float>((*data_input)[loop_of_point].Range);
float point_0_V=static_cast<float>((*data_input)[loop_of_point].Velocity);
float point_0_F=static_cast<float>((*data_input)[loop_of_point].Azimuth);
float point_0_A=static_cast<float>((*data_input)[loop_of_point].Amplitude);
for (unsigned int i=loop_of_point+1;i<data_input->size();i++)
{
if((*data_input)[loop_of_point].Use_Flag!=1 && (*data_input)[i].Use_Flag!=1)
{
float point_1_R=(*data_input)[i].Range;
float point_1_V=(*data_input)[i].Velocity;
float point_1_F=(*data_input)[i].Azimuth;
float point_1_A=(*data_input)[i].Amplitude;
float point_1_R=static_cast<float>((*data_input)[i].Range);
float point_1_V=static_cast<float>((*data_input)[i].Velocity);
float point_1_F=static_cast<float>((*data_input)[i].Azimuth);
float point_1_A=static_cast<float>((*data_input)[i].Amplitude);
//凝聚条件: 距离、方位接近
if(Work_Parameter.work_mode == 0)
@@ -36,10 +36,10 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
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)//
&& point_0_A <point_1_A )//
{
point_0_R=(*data_input)[i].Range;
point_0_V=(*data_input)[i].Velocity;
point_0_A=(*data_input)[i].Amplitude;
point_0_F=(*data_input)[i].Azimuth;
point_0_R=static_cast<float>((*data_input)[i].Range);
point_0_V=static_cast<float>((*data_input)[i].Velocity);
point_0_A=static_cast<float>((*data_input)[i].Amplitude);
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)//
@@ -54,10 +54,10 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
&& point_0_A <point_1_A )//
{
point_0_R=(*data_input)[i].Range;
point_0_V=(*data_input)[i].Velocity;
point_0_A=(*data_input)[i].Amplitude;
point_0_F=(*data_input)[i].Azimuth;
point_0_R=static_cast<float>((*data_input)[i].Range);
point_0_V=static_cast<float>((*data_input)[i].Velocity);
point_0_A=static_cast<float>((*data_input)[i].Amplitude);
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 )//
@@ -88,7 +88,7 @@ int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
}
}
for (int i=0;i<data_input->size();i++) //data_buffer_2 ---> point_recv
for (size_t i=0;i<data_input->size();i++) //data_buffer_2 ---> point_recv
{
(*point_recv).push_back((*data_input)[i]);
}
@@ -106,26 +106,18 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
//1.1 data_input、data_input_buff中的数据放在一起
std::vector <PointRecv> data_tmp;
for (int i=0;i<data_input_buff.size();i++)
for (size_t i=0;i<data_input_buff.size();i++)
{
data_tmp.push_back(data_input_buff[i]);
data_tmp[data_tmp.size()-1].point_section_asso = 1;
}
for (int i=0;i<data_input->size();i++)
for (size_t i=0;i<data_input->size();i++)
{
data_tmp.push_back((*data_input)[i]);
data_tmp[data_tmp.size()-1].point_section_asso = 2;
}
// //去除点迹中,1km以内,SNR小于20dB的点
// for (int i=0;i<data_input->size();i++)
// {
// if((*data_input)[i].Range > 1000 || (*data_input)[i].snr >= 20){
// data_tmp.push_back((*data_input)[i]);
// data_tmp[data_tmp.size()-1].point_section_asso = 2;
// }
// }
//1.2 把data_input、data_input_buff清空
@@ -140,18 +132,18 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
{
if( data_tmp[loop_of_point].Use_Flag!=1)
{
float point_0_R=data_tmp[loop_of_point].Range;
float point_0_V=data_tmp[loop_of_point].Velocity;
float point_0_F=data_tmp[loop_of_point].Azimuth;
float point_0_A=data_tmp[loop_of_point].Amplitude;
float point_0_R=static_cast<float>(data_tmp[loop_of_point].Range);
float point_0_V=static_cast<float>(data_tmp[loop_of_point].Velocity);
float point_0_F=static_cast<float>(data_tmp[loop_of_point].Azimuth);
float point_0_A=static_cast<float>(data_tmp[loop_of_point].Amplitude);
for (unsigned int i=loop_of_point+1;i<data_tmp.size();i++)
{
if(data_tmp[loop_of_point].Use_Flag!=1 && data_tmp[i].Use_Flag!=1)
{
float point_1_R=data_tmp[i].Range;
float point_1_V=data_tmp[i].Velocity;
float point_1_F=data_tmp[i].Azimuth;
float point_1_A=data_tmp[i].Amplitude;
float point_1_R=static_cast<float>(data_tmp[i].Range);
float point_1_V=static_cast<float>(data_tmp[i].Velocity);
float point_1_F=static_cast<float>(data_tmp[i].Azimuth);
float point_1_A=static_cast<float>(data_tmp[i].Amplitude);
//凝聚条件: 距离、方位接近
if(Work_Parameter.work_mode == 0)
@@ -160,10 +152,10 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
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)//
&& point_0_A <point_1_A )//
{
point_0_R=data_tmp[i].Range;
point_0_V=data_tmp[i].Velocity;
point_0_A=data_tmp[i].Amplitude;
point_0_F=data_tmp[i].Azimuth;
point_0_R=static_cast<float>(data_tmp[i].Range);
point_0_V=static_cast<float>(data_tmp[i].Velocity);
point_0_A=static_cast<float>(data_tmp[i].Amplitude);
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)//
@@ -178,10 +170,10 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
&& point_0_A <point_1_A )//
{
point_0_R=data_tmp[i].Range;
point_0_V=data_tmp[i].Velocity;
point_0_A=data_tmp[i].Amplitude;
point_0_F=data_tmp[i].Azimuth;
point_0_R=static_cast<float>(data_tmp[i].Range);
point_0_V=static_cast<float>(data_tmp[i].Velocity);
point_0_A=static_cast<float>(data_tmp[i].Amplitude);
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 )//
@@ -213,7 +205,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
}
//1.5 将data_tmp中凝聚后的点再分到 data_input_buff和data_input中
for (int i=0;i<data_tmp.size();i++)
for (size_t i=0;i<data_tmp.size();i++)
{
if(data_tmp[i].point_section_asso==1)
{
@@ -229,7 +221,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
//2.data_input_buff数据输出给point_recv
for (int i=0;i<data_input_buff.size();i++)
for (size_t i=0;i<data_input_buff.size();i++)
{
(*point_recv).push_back(data_input_buff[i]);
}
@@ -237,7 +229,7 @@ int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input,
std::vector<PointRecv>().swap(data_input_buff);
//3.data_input数据输出给data_input_buff
for (int i=0;i<data_input->size();i++)
for (size_t i=0;i<data_input->size();i++)
{
data_input_buff.push_back((*data_input)[i]);
}
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef DOT_COH_H
#ifndef DOT_COH_H
#define DOT_COH_H
#include "data_process_class_dll.h"
#include "parameters.h"
+16 -16
View File
@@ -1,4 +1,4 @@
#include "dot_coh_tas.h"
#include "dot_coh_tas.h"
#include <cmath>
#include <cstring>
#include <vector>
@@ -12,32 +12,32 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
return 1;
}
for (int loop_of_point=0; loop_of_point<data_input->size()-1;loop_of_point++ )
for (size_t loop_of_point=0; loop_of_point<data_input->size()-1;loop_of_point++ )
{
if( (*data_input)[loop_of_point].Use_Flag!=1)
{
float point_0_R=(*data_input)[loop_of_point].Range;
float point_0_V=(*data_input)[loop_of_point].Velocity;
float point_0_F=(*data_input)[loop_of_point].Azimuth;
float point_0_A=(*data_input)[loop_of_point].Amplitude;
for ( int i=loop_of_point+1;i<(*data_input).size();i++)
float point_0_R=static_cast<float>((*data_input)[loop_of_point].Range);
float point_0_V=static_cast<float>((*data_input)[loop_of_point].Velocity);
float point_0_F=static_cast<float>((*data_input)[loop_of_point].Azimuth);
float point_0_A=static_cast<float>((*data_input)[loop_of_point].Amplitude);
for ( size_t i=loop_of_point+1;i<(*data_input).size();i++)
{
if((*data_input)[loop_of_point].Use_Flag!=1 && (*data_input)[i].Use_Flag!=1)
{
float point_1_R=(*data_input)[i].Range;
float point_1_V=(*data_input)[i].Velocity;
float point_1_F=(*data_input)[i].Azimuth;
float point_1_A=(*data_input)[i].Amplitude;
float point_1_R=static_cast<float>((*data_input)[i].Range);
float point_1_V=static_cast<float>((*data_input)[i].Velocity);
float point_1_F=static_cast<float>((*data_input)[i].Azimuth);
float point_1_A=static_cast<float>((*data_input)[i].Amplitude);
//凝聚条件: 距离、方位接近
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)
&& point_0_A <point_1_A )
{
point_0_R=(*data_input)[i].Range;
point_0_V=(*data_input)[i].Velocity;
point_0_A=(*data_input)[i].Amplitude;
point_0_F=(*data_input)[i].Azimuth;
point_0_R=static_cast<float>((*data_input)[i].Range);
point_0_V=static_cast<float>((*data_input)[i].Velocity);
point_0_A=static_cast<float>((*data_input)[i].Amplitude);
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)
@@ -65,7 +65,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(std::vector <PointRecv> *dat
}
}
for (int i=0;i<data_input->size();i++)
for (size_t i=0;i<data_input->size();i++)
{
(*point_recv_tas).push_back((*data_input)[i]);
}
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef DOT_COH_TAS_H
#ifndef DOT_COH_TAS_H
#define DOT_COH_TAS_H
#include "parameters.h"
#include "struct.h"
+1 -1
View File
@@ -1,4 +1,4 @@
#include "kalman.h"
#include "kalman.h"
#include "coor_trans.h"
#include "parameters.h"
#include <cmath>
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef KALMAN_H
#ifndef KALMAN_H
#define KALMAN_H
#include "parameters.h"
#include "struct.h"
+19 -24
View File
@@ -1,38 +1,29 @@
#ifndef PARAMETERS_H
#ifndef PARAMETERS_H
#define PARAMETERS_H
#pragma once
/**********************************雷达参数*************************************************/
#define PI 3.1415926f
//频点(GHz)
#define FREQ0 16.8
#define FREQ1 16.8
#define FREQ2 16.8
#define FREQ3 16.8
#define FREQ4 16.8
#define FREQ5 16.8
#define FREQ6 16.8
#define FREQ7 16.8
#define FREQ8 16.8
#define FREQ9 16.8
#define FREQ10 16.8
#define FREQ11 16.8
#define FREQ12 16.8
#define FREQ13 16.8
#define FREQ14 16.8
#define FREQ15 16.8
#define FREQ16 16.8
#define FREQ17 16.8
#define FREQ18 16.8
#define FREQ19 16.8
#define FREQ20 16.8
#define FREQ0 9.2
/**********************************日志输出开关**********************************************/
// 日志文件输出功能开关:
// 1) 定义(默认)RDP_LOG_ENABLE:启用日志文件输出 —— 调用 Data_Process_Factory::GetB()
// 时自动将 std::cout 重定向到日志文件,日志文件存放于 RDP_LOG_DIR 目录,
// 文件名按当前日期+时间自动命名(如 rdp_20240826_153000.log);
// 2) 注释掉 RDP_LOG_ENABLE:关闭 —— 不做重定向,日志仍输出到标准输出(控制台)。
//#define RDP_LOG_ENABLE
//日志文件输出目录(相对当前工作目录的路径,目录不存在时自动创建)
#define RDP_LOG_DIR ".\\rdp_logs\\"
//#define MECHANICAL_SCANNING
//#define PHASE_SCANNING
#ifdef MECHANICAL_SCANNING
/******************************数据处理参数**************************************/
#define DATA_RATE_TAS 0.0625
// #define DATA_RATE_TAS 0.0625
#define SIGMA_R 10.0 //测量误差
#define SIGMA_A 0.02
@@ -72,12 +63,14 @@
#define DIRECT_TRACKING_QUEUE_LENGTH 10
#pragma message("MECHANICAL_SCANNING")
#endif
#ifdef PHASE_SCANNING
/******************************数据处理参数**************************************/
#define DATA_RATE_TAS 0.3
// #define DATA_RATE_TAS 0.3
#define SIGMA_R 10.0 //测量误差
#define SIGMA_A 0.02
@@ -119,6 +112,8 @@
#define DIRECT_TRACKING_QUEUE_LENGTH 10
#pragma message("PHASE_SCANNING")
#endif
#define Round(x) (((x) > 0) \
+154
View File
@@ -0,0 +1,154 @@
#include "rdp_log.h"
#include "parameters.h"
#include <fstream>
#include <sstream>
#include <time.h>
#include <direct.h>
namespace
{
//日志模块内部状态:日志文件流 + 重定向前 std::cout 的原始缓冲区
struct RdpLogState
{
std::ofstream file; //日志文件输出流
std::streambuf *original_buf; //重定向前 std::cout 的原始缓冲区
bool enabled; //是否已成功重定向到日志文件
RdpLogState() : original_buf(0), enabled(false) {}
~RdpLogState()
{
//进程退出兜底:若宿主进程未调用 Data_Process_Factory::Destroy()
//此处恢复 std::cout,避免其仍指向即将析构的日志文件流缓冲区。
if (enabled && original_buf)
{
std::cout.rdbuf(original_buf);
enabled = false;
}
}
};
RdpLogState &State()
{
static RdpLogState s;
return s;
}
//当前本地时间字符串,格式 "YYYY-MM-DD HH:MM:SS"
std::string CurrentTime()
{
char buf[32] = {0};
time_t now = time(NULL);
struct tm local;
if (localtime_s(&local, &now) != 0)
{
return std::string();
}
strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", &local);
return std::string(buf);
}
//提取纯文件名(去掉路径前缀),日志前缀中只显示 文件名:行号
const char *FileName(const char *path)
{
if (path == 0)
{
return "";
}
const char *base = path;
for (const char *p = path; *p != '\0'; ++p)
{
if (*p == '\\' || *p == '/')
{
base = p + 1;
}
}
return base;
}
}
namespace rdp_log
{
bool Init()
{
RdpLogState &st = State();
if (st.enabled)
{
return true; //已经重定向,避免重复初始化
}
#ifdef RDP_LOG_ENABLE
//创建日志目录(目录已存在时 _mkdir 返回 EEXIST,忽略)
std::string log_dir = RDP_LOG_DIR;
while (log_dir.size() > 0 &&
(log_dir[log_dir.size() - 1] == '\\' || log_dir[log_dir.size() - 1] == '/'))
{
log_dir.erase(log_dir.size() - 1);
}
if (log_dir.size() > 0)
{
_mkdir(log_dir.c_str());
}
//按当前日期+时间生成日志文件名:rdp_YYYYMMDD_HHMMSS.log
char time_buf[32] = {0};
time_t now = time(NULL);
struct tm local;
if (localtime_s(&local, &now) != 0)
{
return false;
}
strftime(time_buf, sizeof(time_buf), "%Y%m%d_%H%M%S", &local);
std::string log_path = RDP_LOG_DIR;
log_path += "rdp_";
log_path += time_buf;
log_path += ".log";
st.file.open(log_path.c_str(), std::ios::out | std::ios::trunc);
if (!st.file.is_open())
{
//日志文件打开失败(目录不可写等):保持控制台输出,不做重定向
std::cout << "[rdp_log] cannot open log file: " << log_path << std::endl;
return false;
}
//用 rdbuf() 将 std::cout 的流缓冲区替换为日志文件输出流
st.original_buf = std::cout.rdbuf();
std::cout.rdbuf(st.file.rdbuf());
st.enabled = true;
//重定向成功后写入起始标记(该行已写入日志文件)
std::cout << prefix(__FILE__, __LINE__)
<< "rdp_log enabled, log file: " << log_path << std::endl;
return true;
#else
//日志开关关闭:不重定向,日志仍输出到标准输出
return false;
#endif
}
void Shutdown()
{
RdpLogState &st = State();
if (!st.enabled)
{
return;
}
//写入结束标记后刷新、恢复 std::cout 并关闭日志文件
std::cout << prefix(__FILE__, __LINE__) << "rdp_log shutdown" << std::endl;
st.file.flush();
std::cout.rdbuf(st.original_buf);
st.enabled = false;
st.file.close();
}
std::string prefix(const char *file, int line)
{
std::ostringstream oss;
oss << "[" << CurrentTime() << "][" << FileName(file) << ":" << line << "] ";
return oss.str();
}
}
+27
View File
@@ -0,0 +1,27 @@
#ifndef RDP_LOG_H
#define RDP_LOG_H
#pragma once
/*************************************日志输出模块*******************************************/
// 功能:
// 1) rdp_log::Init():按 parameters.h 中 RDP_LOG_ENABLE 宏开关决定是否将 std::cout
// 重定向到日志文件(RDP_LOG_DIR 目录,文件名按当前日期+时间自动生成)。
// 开关关闭(注释掉 RDP_LOG_ENABLE)时不做重定向,日志仍输出到标准输出。
// 2) rdp_log::Shutdown():恢复 std::cout 原始缓冲区(回到控制台)并关闭日志文件。
// 3) RDP_LOG 宏:日志输出宏,在每条日志前自动附加 [时间戳][代码文件:行号] 信息,
// 随后输出到 std::cout(若开关开启则随 cout 一并写入日志文件)。
// 用法示例:
// RDP_LOG << "目标批号:" << track_index << std::endl;
#include <iostream>
#include <string>
namespace rdp_log
{
bool Init(); //初始化日志输出(由 Data_Process_Factory::GetB() 调用)
void Shutdown(); //恢复 std::cout 并关闭日志文件(由 Data_Process_Factory::Destroy() 调用)
std::string prefix(const char *file, int line); //生成 "[时间戳][文件名:行号] " 前缀字符串
}
//日志输出宏:自动附加 时间戳、代码文件、代码行号 后输出到标准输出(std::cout)
#define RDP_LOG std::cout << rdp_log::prefix(__FILE__, __LINE__)
#endif // RDP_LOG_H
+2 -2
View File
@@ -1,4 +1,4 @@
#ifndef STRUCT_H
#ifndef STRUCT_H
#define STRUCT_H
#pragma once
@@ -125,7 +125,7 @@ struct Tracking_Target
{
int Index; //目标批号
int empty_flag; //是否为空标志位 1非空 0空
long long last_track_time; //最新跟踪时间
};
//引导跟踪目标结构体
+95 -85
View File
@@ -1,8 +1,11 @@
#include "tas_ctrl.h"
#include "tas_ctrl.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <cmath>
#include <cstring>
#include <vector>
#include <iomanip>
#include <sstream>
using namespace std;
@@ -39,14 +42,14 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track
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);
//跟踪队列移位
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_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
}
memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
// //跟踪队列移位
// 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_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
// }
// memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
};
void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
@@ -55,7 +58,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
struct RadarPara Work_Parameter)
{
for (int i=0;i<trust_track->size();i++)
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));
@@ -81,6 +84,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
//插入跟踪队列
tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
tas_target_queue[j].empty_flag=1;
tas_target_queue[j].last_track_time=(*trust_track)[i].T_track;
//跟踪目标数目加一
tas_target_num=tas_target_num+1;
@@ -94,18 +98,18 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
coor_trans Coor_trans;
Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r_output);
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azmi_output/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index; //航迹号
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=static_cast<float>((*trust_track)[i].Amplitude);
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=asin((*trust_track)[i].Height/r_output)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
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].Target_Type=(*trust_track)[i].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
break;
}
@@ -163,7 +167,7 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
{
//查找TAS队列里的目标是否存在
int flag=0;
for (int j=0;j<trust_track->size();j++){
for (size_t j=0;j<trust_track->size();j++){
{
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1 && (*trust_track)[j].manual_tracking_flag == 1)
{
@@ -212,17 +216,17 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// coor_trans Coor_trans;
// Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r_output;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azmi_output/PI*180;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r_output);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azmi_output/PI*180);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index; //航迹号
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=(*trust_track)[i].Amplitude;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
// Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=static_cast<float>((*trust_track)[i].Amplitude);
// 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=asin((*trust_track)[i].Height/r_output)/PI*180;
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height;
// 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].Target_Type=(*trust_track)[i].Target_Type;
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
// }
// }
// }
@@ -255,61 +259,74 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
struct RadarPara Work_Parameter,
long long latest_timestamp)
{
if(tas_target_queue[0].empty_flag==1)
long long CPI_time = 0;
long long earliest_CPI_time = LLONG_MAX;
float delta_T = 0;
double H_track = 0;
int earliest_track_idx=-1;
int earliest_tas_queue_idx=-1;
double X_now[6] = {0};
//查找TAS队列中CPI时间最早的目标且满足TAS数据率的目标
for(int tas_idx=0;tas_idx<TAS_QUEUE_LENGTH;tas_idx++)
{
double H_track = 0;
double X_now[6] = {0};
long long CPI_time = 0;
bool found_track = false;
for (int i=0;i<trust_track->size();i++ )
if(tas_target_queue[tas_idx].empty_flag==0)
{
if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
continue;
}
for (size_t track_idx=0;track_idx<trust_track->size();track_idx++ )
{
if((*trust_track)[track_idx].Track_Index == tas_target_queue[tas_idx].Index)
{
H_track = (*trust_track)[i].Height;
CPI_time = (*trust_track)[i].T_track;
for(int ii=0;ii<6;ii++)
X_now[ii]=(*trust_track)[i].X[ii];
found_track = true;
CPI_time = (*trust_track)[track_idx].T_track;
delta_T = (latest_timestamp - CPI_time) / 1000.0f;
//查找最早的CPI时间, 航迹时间与当前最新时间戳的差值大于 DATA_RATE_TAS 时才输出TAS跟踪波束
if(CPI_time < earliest_CPI_time && (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f > Work_Parameter.DATA_RATE_TAS && delta_T > Work_Parameter.DATA_RATE_TAS)
{
earliest_CPI_time = CPI_time;
H_track = (*trust_track)[track_idx].Height;
earliest_track_idx = track_idx;
earliest_tas_queue_idx = tas_idx;
for(int ii=0;ii<6;ii++){
X_now[ii]=(*trust_track)[track_idx].X[ii];
}
}
break;
}
}
}
if (!found_track)
{
Tracking_beam->open_flag = 0;
return;
}
//没有满足条件的TAS目标,关闭跟踪波束
if(earliest_track_idx == -1)
{
Tracking_beam->open_flag = 0;
return;
}
//数据率门控:航迹时间与当前最新时间戳的差值大于 1/DATA_RATE_TAS 时才输出TAS跟踪波束,
//保证TAS目标数据率不随波束时间变化(使用Work_Parameter中的DATA_RATE_TAS参数值)。
//DATA_RATE_TAS<=0 时不做门控,按原逻辑每次输出(避免除零,兼容未配置该参数的调用者)。
if (Work_Parameter.DATA_RATE_TAS > 0)
{
double delta_T_track = (double)(latest_timestamp - CPI_time) / 1000.0;
if (delta_T_track <= 1.0 / Work_Parameter.DATA_RATE_TAS)
{
//更新间隔未到 不输出跟踪波束
Tracking_beam->open_flag = 0;
return;
}
}
//更新目标最后跟踪时间
tas_target_queue[earliest_tas_queue_idx].last_track_time = latest_timestamp;
float delta_T = (latest_timestamp - CPI_time) / 1000.0f;
//预测目标位置 计算跟踪波束波位号 俯仰角
double x_track=X_now[0]+X_now[1]*delta_T;
double y_track=X_now[3]+X_now[4]*delta_T;
double amzi,range;
coor_trans Coor_trans;
Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
//预测目标位置 计算跟踪波束波位号 俯仰角
double x_track=X_now[0]+X_now[1]*delta_T;
double y_track=X_now[3]+X_now[4]*delta_T;
double amzi,range;
coor_trans Coor_trans;
Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
//目标距离
Tracking_beam->Range=range;
//目标方位
Tracking_beam->Azi=amzi/PI*180;
//目标距离
Tracking_beam->Range=(float)range;
//目标方位
Tracking_beam->Azi=(float)amzi/PI*180;
// Tracking_beam->Azi+=6;
//目标俯仰角
double elev=asin(H_track/range)/PI*180;
//目标俯仰角
double elev=asin(H_track/range)/PI*180;
// if(elev<=0)
// elev=0;
@@ -318,23 +335,16 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
// else
// elev=elev;
Tracking_beam->Elev=elev;
Tracking_beam->Elev=(float)elev;
//跟踪波束类型
Tracking_beam->type=1;
//跟踪波束类型
Tracking_beam->type=1;
//跟踪目标批号
Tracking_beam->TAS_track_index = tas_target_queue[0].Index;
//跟踪目标批号
Tracking_beam->TAS_track_index = (*trust_track)[earliest_track_idx].Track_Index;
//跟踪波束开关开启
Tracking_beam->open_flag=1;
//std::cout << "TAS: range: " <<Tracking_beam->Range << "azi: " <<Tracking_beam->Azi <<"pit: " <<Tracking_beam->Elev;
}
else
{
//跟踪波束开关关闭
Tracking_beam->open_flag=0;
}
//跟踪波束开关开启
Tracking_beam->open_flag=1;
//RDP_LOG << "TAS <" << (*trust_track)[earliest_track_idx].Track_Index << "> output: range: " <<Tracking_beam->Range << "azi: " <<Tracking_beam->Azi <<"pit: " <<Tracking_beam->Elev << std::endl;
}
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TAS_CTRL_H
#ifndef TAS_CTRL_H
#define TAS_CTRL_H
#include "data_process_class_dll.h"
#include "parameters.h"
+51 -75
View File
@@ -1,6 +1,7 @@
#include "track_asso.h"
#include "track_asso.h"
#include "kalman.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <cmath>
#include <cstring>
#include <vector>
@@ -20,7 +21,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
{
//取出对应点迹区的点
for (int i=0;i< point_recv->size();i++)
for (size_t i=0;i< point_recv->size();i++)
{
point_process.push_back((*point_recv)[i]);
int n=point_process.size();
@@ -51,7 +52,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
//剩余点重新存入点迹
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
for (size_t i=0;i<point_process.size();i++)
{
(*point_recv).push_back(point_process[i]);
@@ -62,7 +63,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (int i=0;i<trust_track->size();i++)
for (size_t i=0;i<trust_track->size();i++)
{ // 输出更新航迹条件:
// if((*trust_track)[i].manual_tracking_flag==0)
// {
@@ -76,53 +77,53 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r);
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azi/PI*180);
if((*trust_track)[i].Height<r)
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin((*trust_track)[i].Height/r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>(asin((*trust_track)[i].Height/r)/PI*180);
else
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = (*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i].Amplitude;
static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = static_cast<float>((*trust_track)[i].Amplitude);
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].track_time=(*trust_track)[i].T_track/1000.0;
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=(*trust_track)[i].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=static_cast<float>((*trust_track)[i].vr_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = static_cast<float>((*trust_track)[i].prf_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
//直接输出点迹高度-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 = (*trust_track)[i].Height+Work_Parameter.Height;
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].Elevation = (*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];
Trust_Track_Output[*Trust_track_num_Output-1][0].x=static_cast<float>((*trust_track)[i].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][0].y=static_cast<float>((*trust_track)[i].X[3]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=static_cast<float>((*trust_track)[i].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
@@ -137,7 +138,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
void Track_Asso:: model_interaction(std::vector <Trust_Track> *trust_track)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
for (size_t loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag == 0)
@@ -413,13 +414,13 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
std::vector <asso_info> associated_info;
//遍历所有点迹航迹 计算点迹和航迹的统计距离
for (int loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
// {
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//时间差
@@ -554,8 +555,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
associated_info_tmp.d1=d1;
associated_info_tmp.d2=d2;
associated_info_tmp.d3=d3;
associated_info_tmp.point_index=loop_of_point+1;
associated_info_tmp.track_index=loop_of_track+1;
associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+1;
if(d1<=d2&&d1<=d3)
associated_info_tmp.d_min=d1;
else if(d2<=d1&& d2<=d3)
@@ -571,7 +572,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//最近邻法关联
int associated_num=associated_info.size();
for (int i=0;i<trust_track->size();i++)
for (size_t i=0;i<trust_track->size();i++)
{
//寻找最小d
if(associated_num>0)
@@ -611,34 +612,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
if (delta_T <= 0)
{
// RDP_LOG<<"delta_T <= 0, track_index: "<<track_index<<", point_index: "<<point_index<<", delta_T: "<<delta_T<<std::endl;
// 乱序/重复时间戳:不作为有效关联,不标记点迹已使用、不刷新航迹新鲜度
for (int ii=0;ii<associated_num;)
{
if(associated_info[ii].track_index==track_index)
{
associated_info.erase(associated_info.begin()+ii);
associated_num=associated_info.size();
}
else
ii++;
}
associated_num=associated_info.size();
for (int ii=0;ii<associated_num;)
{
if(associated_info[ii].point_index==point_index)
{
associated_info.erase(associated_info.begin()+ii);
associated_num=associated_info.size();
}
else
ii++;
}
associated_num=associated_info.size();
continue;
}
if(delta_T>0)
{
}else{
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;
@@ -650,9 +626,9 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
{
S1_out(ii,jj)=S1[ii][jj];
S2_out(ii,jj)=S2[ii][jj];
S3_out(ii,jj)=S3[ii][jj];
S1_out(ii,jj)=static_cast<float>(S1[ii][jj]);
S2_out(ii,jj)=static_cast<float>(S2[ii][jj]);
S3_out(ii,jj)=static_cast<float>(S3[ii][jj]);
}
double det_S1=S1_out.determinant();
double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
@@ -719,12 +695,12 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
//更新高度
track_hight_update(track_index,point_index,trust_track);
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
(*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1
}
(*trust_track)[track_index-1].Extrapolate_round = 0; //连续未用实点更新时间
(*trust_track)[track_index-1].point_flag=1; //实点
(*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1
//删除associated_info中关联上的航迹 点迹信息
for (int ii=0;ii<associated_num;)
{
@@ -755,7 +731,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
}
//未关联上的航迹 进行外推 tws
for (int i =0; i<(*trust_track).size();i++ )
for (size_t i =0; i<(*trust_track).size();i++ )
{
if((*trust_track)[i].point_flag == 0 && (*trust_track)[i].manual_tracking_flag == 0)
{
@@ -804,8 +780,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));
(*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].GNSS_time = (*trust_track)[i].GNSS_time+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].T_track = static_cast<long long>((*trust_track)[i].T_track+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].GNSS_time = static_cast<long long>((*trust_track)[i].GNSS_time+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
(*trust_track)[i].point_flag=0; //虚点
}
@@ -815,7 +791,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)
{
for(int loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
for(size_t loop_of_track=0; loop_of_track<trust_track->size(); loop_of_track++)
{
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
@@ -919,7 +895,7 @@ void Track_Asso:: track_hight_update(int updata_
)
{
if(updata_track_index<=trust_track->size() && asso_point_index<=point_process.size())
if(updata_track_index<=static_cast<int>(trust_track->size()) && asso_point_index<=static_cast<int>(point_process.size()))
{
(*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
@@ -944,10 +920,10 @@ void Track_Asso:: track_hight_update(int updata_
}
double sum=0;
if((*trust_track)[updata_track_index-1].Hight_smooth.size()<height_win_length)
if((*trust_track)[updata_track_index-1].Hight_smooth.size()<static_cast<size_t>(height_win_length))
{
for (int i=0;i<(*trust_track)[updata_track_index-1].Hight_smooth.size();i++)
for (size_t i=0;i<(*trust_track)[updata_track_index-1].Hight_smooth.size();i++)
{
sum=sum+(*trust_track)[updata_track_index-1].Hight_smooth[i];
@@ -956,7 +932,7 @@ void Track_Asso:: track_hight_update(int updata_
(*trust_track)[updata_track_index-1].Height=sum/(*trust_track)[updata_track_index-1].Hight_smooth.size();
}
else if((*trust_track)[updata_track_index-1].Hight_smooth.size()>=height_win_length)
else if((*trust_track)[updata_track_index-1].Hight_smooth.size()>=static_cast<size_t>(height_win_length))
{
int N=(*trust_track)[updata_track_index-1].Hight_smooth.size();
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TRACK_ASSO_H
#ifndef TRACK_ASSO_H
#define TRACK_ASSO_H
#include "data_process_class_dll.h"
@@ -1,4 +1,4 @@
#include "track_asso_direct_tracking.h"
#include "track_asso_direct_tracking.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
@@ -424,7 +424,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track,
//未关联上,航迹外推
else
{
double delta_T = DATA_RATE_TAS;
double delta_T = Work_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);
@@ -1,4 +1,4 @@
#ifndef TRACK_ASSO_DIRECT_TRACKING_H
#ifndef TRACK_ASSO_DIRECT_TRACKING_H
#define TRACK_ASSO_DIRECT_TRACKING_H
#include "data_process_class_dll.h"
#include "parameters.h"
+48 -49
View File
@@ -1,6 +1,7 @@
#include "track_asso_tas.h"
#include "track_asso_tas.h"
#include "kalman.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <cmath>
#include <cstring>
#include <vector>
@@ -20,7 +21,7 @@ long long latest_timestamp //最新时间戳
{
//取出点迹
for (int i=0;i< point_recv_tas->size();i++)
for (size_t i=0;i< point_recv_tas->size();i++)
{
point_process.push_back((*point_recv_tas)[i]);
}
@@ -36,7 +37,7 @@ long long latest_timestamp //最新时间戳
std::vector<PointRecv>().swap(point_process);
//输出航迹
for (int i=0;i<trust_track->size();i++)
for (size_t i=0;i<trust_track->size();i++)
{ // 输出更新航迹条件:
if((*trust_track)[i].Track_Index == tas_track_idx)
{
@@ -49,42 +50,42 @@ long long latest_timestamp //最新时间戳
coor_trans Coor_trans;
double r, azi;
Coor_trans.cart2polar(x, y, &r, &azi);
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=r;
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r);
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azi/PI*180);
{
double elev_ratio = (r > 0.0) ? (*trust_track)[i].Height / r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = asin(elev_ratio)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>(asin(elev_ratio)/PI*180);
}
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V =
sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = (*trust_track)[i].Amplitude;
static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = static_cast<float>((*trust_track)[i].Amplitude);
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].track_time=(*trust_track)[i].T_track/1000.0;
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=(*trust_track)[i].Height+Work_Parameter.Height;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
//关联点信息
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=(*trust_track)[i].range_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=(*trust_track)[i].azi_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=(*trust_track)[i].elev_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=(*trust_track)[i].vr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].elev_point=static_cast<float>((*trust_track)[i].elev_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=static_cast<float>((*trust_track)[i].vr_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=1;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = (*trust_track)[i].prf_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = (*trust_track)[i].snr_point;
Trust_Track_Output[*Trust_track_num_Output-1][0].x=(*trust_track)[i].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][0].y=(*trust_track)[i].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=(*trust_track)[i].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=(*trust_track)[i].X[4];
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = (*trust_track)[i].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = static_cast<float>((*trust_track)[i].prf_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
Trust_Track_Output[*Trust_track_num_Output-1][0].x=static_cast<float>((*trust_track)[i].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][0].y=static_cast<float>((*trust_track)[i].X[3]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=static_cast<float>((*trust_track)[i].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
@@ -101,7 +102,7 @@ long long latest_timestamp //最新时间戳
void Track_Asso_Tas:: model_interaction( std::vector<Trust_Track> *trust_track, int tas_track_idx)
{
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
for (size_t loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
@@ -238,17 +239,12 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
double r_track = 0;
double h_track = 0;
bool found_tas_track = false;
for (int loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
for (size_t loop_of_track=0;loop_of_track<trust_track->size();loop_of_track++)
{
if((*trust_track)[loop_of_track].Track_Index == tas_track_idx)
{
found_tas_track = true;
// if(point_process.size()>0)
// {
// std::cout << "model_filter point_process :" <<point_process[0].Azimuth/PI*180<<" "<<point_process[0].Range;
// }
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
@@ -257,7 +253,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
T_track = (*trust_track)[loop_of_track].T_track;
T_track = static_cast<double>((*trust_track)[loop_of_track].T_track);
v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
h_track = (*trust_track)[loop_of_track].Height;
@@ -269,7 +265,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
return;
// 计算量测和航迹统计距离
for (int loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
{
//量测信息
// double Z[2]={point_process[loop_of_point].Range*cos(point_process[loop_of_point].Azimuth),
@@ -281,8 +277,11 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
double prt = point_process[loop_of_point].PRF_index;
double freq_ind = point_process[loop_of_point].Freq_index;
double delta_T = (point_process[loop_of_point].CPI_Time - T_track)/1000.0;
if (delta_T <= 0)
if (delta_T <= 0){
RDP_LOG << "Warning: delta_T <= 0, skipping point association for point index " << point_process[loop_of_point].CPI_Time << " " << T_track << std::endl;
continue;
}
double h_point = point_process[loop_of_point].Height;
//计算统计距离d
@@ -355,7 +354,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
associated_info_tmp.d1=d1;
associated_info_tmp.d2=d2;
associated_info_tmp.d3=d3;
associated_info_tmp.point_index=loop_of_point+1;
associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
if(d1<=d2&&d1<=d3)
associated_info_tmp.d_min=d1;
else if(d2<=d1&& d2<=d3)
@@ -370,18 +369,18 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
//有关联 滤波
if (associated_info.size()>0)
{
//RDP_LOG << "associated point for track index " << tas_track_idx << std::endl;
// std::cout << "TAS asoooooo222" ;
//找最近点
int min_index=1;
double min_d=associated_info[0].d_min;
for (int ii=0;ii<associated_info.size();ii++)
for (size_t ii=0;ii<associated_info.size();ii++)
{
if(associated_info[ii].d_min<min_d)
{
min_d=associated_info[ii].d_min;
min_index=ii+1;
min_index=static_cast<int>(ii)+1;
}
}
int point_index=associated_info[min_index-1].point_index;
@@ -413,9 +412,9 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
for (int ii=0;ii<3;ii++)
for (int jj=0;jj<3;jj++)
{
S1_out(ii,jj)=S1[ii][jj];
S2_out(ii,jj)=S2[ii][jj];
S3_out(ii,jj)=S3[ii][jj];
S1_out(ii,jj)=static_cast<float>(S1[ii][jj]);
S2_out(ii,jj)=static_cast<float>(S2[ii][jj]);
S3_out(ii,jj)=static_cast<float>(S3[ii][jj]);
}
double det_S1=S1_out.determinant();
double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0;
@@ -425,7 +424,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
//更新航迹
for(int i=0;i<trust_track->size();i++)
for(size_t i=0;i<trust_track->size();i++)
if((*trust_track)[i].Track_Index == tas_track_idx)
{
@@ -495,8 +494,8 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
//未关联上,航迹外推
else
{
for(int i=0;i<trust_track->size();i++)
//RDP_LOG << "unassociated point for track index " << tas_track_idx << std::endl;
for(size_t i=0;i<trust_track->size();i++)
if((*trust_track)[i].Track_Index == tas_track_idx)
{
double delta_T = (latest_timestamp - T_track)/1000.0;
@@ -517,8 +516,8 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));
(*trust_track)[i].T_track = (*trust_track)[i].T_track+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].GNSS_time = (*trust_track)[i].GNSS_time+delta_T*1000.0; //更新航迹时间
(*trust_track)[i].T_track = static_cast<long long>((*trust_track)[i].T_track+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].GNSS_time = static_cast<long long>((*trust_track)[i].GNSS_time+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].point_flag=0; //虚点
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1;
}
@@ -529,7 +528,7 @@ void Track_Asso_Tas::model_filter( std::vector<Trust_Track> *t
void Track_Asso_Tas::model_output(std::vector<Trust_Track> *trust_track, int tas_track_idx )
{
for(int i=0; i<trust_track->size(); i++)
for(size_t i=0; i<trust_track->size(); i++)
if((*trust_track)[i].Track_Index==tas_track_idx)
{
@@ -737,7 +736,7 @@ void Track_Asso_Tas::track_hight_update(int tas_
std::vector<Trust_Track> *trust_track //航迹
)
{
for(int i=0; i<trust_track->size(); i++)
for(size_t i=0; i<trust_track->size(); i++)
if((*trust_track)[i].Track_Index==tas_track_idx)
{
(*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height);
@@ -761,10 +760,10 @@ void Track_Asso_Tas::track_hight_update(int tas_
}
double sum=0;
if((*trust_track)[i].Hight_smooth.size()<height_win_length)
if((*trust_track)[i].Hight_smooth.size()<static_cast<size_t>(height_win_length))
{
for (int ii=0;ii<(*trust_track)[i].Hight_smooth.size();ii++)
for (size_t ii=0;ii<(*trust_track)[i].Hight_smooth.size();ii++)
{
sum=sum+(*trust_track)[i].Hight_smooth[ii];
@@ -773,7 +772,7 @@ void Track_Asso_Tas::track_hight_update(int tas_
(*trust_track)[i].Height=sum/(*trust_track)[i].Hight_smooth.size();
}
else if((*trust_track)[i].Hight_smooth.size()>=height_win_length)
else if((*trust_track)[i].Hight_smooth.size()>=static_cast<size_t>(height_win_length))
{
int N=(*trust_track)[i].Hight_smooth.size();
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TRACK_ASSO_TAS_H
#ifndef TRACK_ASSO_TAS_H
#define TRACK_ASSO_TAS_H
#include "data_process_class_dll.h"
#include "parameters.h"
+1 -1
View File
@@ -1,4 +1,4 @@
#include "track_die.h"
#include "track_die.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TRACK_DIE_H
#ifndef TRACK_DIE_H
#define TRACK_DIE_H
#include "data_process_class_dll.h"
#include "parameters.h"
+3 -2
View File
@@ -1,6 +1,7 @@
#include "track_die_tas.h"
#include "track_die_tas.h"
#include "kalman.h"
#include "coor_trans.h"
#include "rdp_log.h"
#include <iostream>
#include <cmath>
#include <cstring>
@@ -20,7 +21,7 @@ void Track_Die_Tas::track_die_process_tas( std::vector <Trust_Track> *trust_trac
||(*Iter).manual_delete_flag == 1)
{
std::cout << "remove tas target :" << (*Iter).Track_Index << std::endl;
// RDP_LOG << "remove tas target :" << (*Iter).Track_Index << std::endl;
//输出消亡信息
*Track_die_num_Output=*Track_die_num_Output+1;
Track_die_Index_Output[*Track_die_num_Output-1]=(*Iter).Track_Index;
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TRACK_DIE_TAS_H
#ifndef TRACK_DIE_TAS_H
#define TRACK_DIE_TAS_H
#include "data_process_class_dll.h"
#include "parameters.h"
@@ -1,4 +1,4 @@
#include "track_index_mangement.h"
#include "track_index_mangement.h"
#include <cmath>
#include <cstring>
#include <vector>
@@ -30,7 +30,7 @@ int Track_Ind_Mangement ::track_ind_get( std::vector<Trust_Track> *trust_track
{
//建立航迹号列表
int List[MAX_TRACK_INDEX]={0};
for (int i=0;i<trust_track->size();i++)
for (size_t i=0;i<trust_track->size();i++)
{
int ti = (*trust_track)[i].Track_Index;
if (ti >= 1 && ti <= MAX_TRACK_INDEX)
@@ -1,4 +1,4 @@
#ifndef TRACK_INDEX_MANGEMENT_H
#ifndef TRACK_INDEX_MANGEMENT_H
#define TRACK_INDEX_MANGEMENT_H
#include "data_process_class_dll.h"
#include "parameters.h"
+62 -92
View File
@@ -1,4 +1,4 @@
#include "track_init.h"
#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
@@ -34,13 +34,13 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
// }
//取出待起航的点迹数据
for (int i=0;i<(*point_recv).size();i++)
for (size_t i=0;i<(*point_recv).size();i++)
point_process.push_back((*point_recv)[i]);
//临时航迹的buff_round+1
for (int i=0;i<temp_track->size();i++)
for (size_t i=0;i<temp_track->size();i++)
{
for (int j=0;j<(*temp_track)[i].size();j++)
for (size_t j=0;j<(*temp_track)[i].size();j++)
{
(*temp_track)[i][j].buff_round = (*temp_track)[i][j].buff_round+1;
}
@@ -71,7 +71,7 @@ int Track_Init::track_init_process_logic( std::vector <PointRecv>
//剩余点重新存入点迹
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
for (size_t i=0;i<point_process.size();i++)
(*point_recv).push_back(point_process[i]);
//剩余点转为航迹头
@@ -155,9 +155,9 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
};
std::vector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
for ( size_t i=0;i<point_process.size();i++)
{
for ( int j=0;j<(*temp_track).size();j++)
for ( size_t j=0;j<(*temp_track).size();j++)
{
int L = (*temp_track)[j].size();
@@ -174,7 +174,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double prt = point_process[i].PRF_index;
double freq_ind = point_process[i].Freq_index;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
double T_point = static_cast<double>(point_process[i].CPI_Time);
//航迹信息
double X[4];
@@ -184,7 +184,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
double v_temp_track=(*temp_track)[j][L-1].vr;
double r_temp_track=(*temp_track)[j][L-1].r;
double h_temp_track=(*temp_track)[j][L-1].height;
double T_track_head = (*temp_track)[j][L-1].T;
double T_track_head = static_cast<double>((*temp_track)[j][L-1].T);
double delta_T = (T_point - T_track_head)/1000.0;
@@ -221,8 +221,8 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
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
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
asso_info_tmp.track_idx = static_cast<int>(j)+1;
asso_info_tmp.d = d;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][L-1].asso_flag = 1;
@@ -235,7 +235,7 @@ void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>>
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
for (size_t i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(std::vector <Temp_track> ());
@@ -316,10 +316,10 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
std::vector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
for ( size_t i=0;i<point_process.size();i++)
{
for ( int j=0;j<temp_track->size();j++)
for ( size_t j=0;j<temp_track->size();j++)
{
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
@@ -332,7 +332,7 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
vr_point=point_process[i].Velocity;
double r_point=point_process[i].Range;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
double T_point = static_cast<double>(point_process[i].CPI_Time);
//航迹信息
double x_track_head, y_track_head;
@@ -340,82 +340,52 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
y_track_head=(*temp_track)[j][0].X[2];
double v_track_head=(*temp_track)[j][0].vr;
double h_track_head = (*temp_track)[j][0].height;
double T_track_head = (*temp_track)[j][0].T;
double T_track_head = static_cast<double>((*temp_track)[j][0].T);
// if(T_track_head == 30675948)
// {
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
// if(T_point == 30682320)
// {
// std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
{
vmax = Work_Parameter.V_MAX;
}
else //中远程模式 最大速度正常用
{
vmax = Work_Parameter.V_MAX;
}
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
double delta_T = (T_point - T_track_head)/1000.0;
double vmax;
if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点
{
vmax = Work_Parameter.V_MAX;
}
else //中远程模式 最大速度正常用
{
vmax = Work_Parameter.V_MAX;
}
double delta_T = (T_point - T_track_head)/1000.0;
//满足关联条件的点航
// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=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(
( (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) ))
)
&& vr_point*v_track_head>0 )
{
// if(T_track_head == 30675948)
// {
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
// << "y_track_head=" << y_track_head
// << "v_track_head=" << v_track_head
// << "T_track_head=" << T_track_head;
// }
// 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) ))
// )
// && vr_point*v_track_head>0 )
//点航关联条件
if( dis <= vmax* delta_T && dis >= Work_Parameter.V_MIN * delta_T && \
vr_point*v_track_head>0 && \
dis >= abs(vr_point) * delta_T && \
dis >= abs(v_track_head) * delta_T
)
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
asso_info_tmp.track_idx = static_cast<int>(j)+1;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][0].asso_flag = 1;
point_process[i].Use_Flag = 1;
//std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
}
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
for (size_t i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(std::vector <Temp_track> ());
@@ -646,19 +616,19 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=index;
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=static_cast<float>(Track_to_start[i][j].r);
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=static_cast<float>(Track_to_start[i][j].azi/PI*180);
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(elev_ratio)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=static_cast<float>(asin(elev_ratio)/PI*180);
}
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=Track_to_start[i][j].height;
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2)));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>(Track_to_start[i][j].height);
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>(Track_to_start[i][j].Amp);
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].pitch_num = Track_to_start[i][j].pitch_num;
@@ -668,25 +638,25 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
Direction_Angle=atan2(Track_to_start[i][j].X[3],Track_to_start[i][j].X[1]);
if(Direction_Angle<0)
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>((Track_to_start[i][j].T)/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time;
Trust_Track_Output[*Trust_track_num_Output-1][j].x=Track_to_start[i][j].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=Track_to_start[i][j].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][j].y=Track_to_start[i][j].X[2];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=Track_to_start[i][j].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>(Track_to_start[i][j].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast<float>(Track_to_start[i][j].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast<float>(Track_to_start[i][j].X[2]);
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=static_cast<float>(Track_to_start[i][j].X[3]);
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=static_cast<float>(Track_to_start[i][j].r);
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=static_cast<float>(Track_to_start[i][j].azi/PI*180);
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(elev_ratio)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=static_cast<float>(asin(elev_ratio)/PI*180);
}
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=Track_to_start[i][j].vr;
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=static_cast<float>(Track_to_start[i][j].vr);
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=0;//跟踪模式 TWS 0
+1 -1
View File
@@ -1,4 +1,4 @@
#ifndef TRACK_INIT_H
#ifndef TRACK_INIT_H
#define TRACK_INIT_H
#include "data_process_class_dll.h"
#include "track_index_mangement.h"
@@ -1,4 +1,4 @@
#include "track_init_direct_tracking.h"
#include "track_init_direct_tracking.h"
#include "kalman.h"
#include "coor_trans.h"
#include <cmath>
@@ -33,9 +33,9 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
};
std::vector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
for ( size_t i=0;i<point_process.size();i++)
{
for ( int j=0;j<(*temp_track).size();j++)
for ( size_t j=0;j<(*temp_track).size();j++)
{
int L = (*temp_track)[j].size();
@@ -52,7 +52,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
double prt = point_process[i].PRF_index;
double freq_ind = point_process[i].Freq_index;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
double T_point = static_cast<double>(point_process[i].CPI_Time);
//航迹信息
double X[4];
@@ -62,7 +62,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
double v_temp_track=(*temp_track)[j][L-1].vr;
double r_temp_track=(*temp_track)[j][L-1].r;
double h_temp_track=(*temp_track)[j][L-1].height;
double T_track_head = (*temp_track)[j][L-1].T;
double T_track_head = static_cast<double>((*temp_track)[j][L-1].T);
double delta_T = (T_point - T_track_head)/1000.0;
@@ -85,8 +85,8 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
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
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
asso_info_tmp.track_idx = static_cast<int>(j)+1;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][L-1].asso_flag = 1;
point_process[i].Use_Flag = 1;
@@ -97,7 +97,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector <std::vector<
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
for (size_t i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(std::vector <Temp_track> ());
@@ -175,10 +175,10 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
std::vector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
for ( size_t i=0;i<point_process.size();i++)
{
for ( int j=0;j<temp_track->size();j++)
for ( size_t j=0;j<temp_track->size();j++)
{
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
@@ -191,7 +191,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
vr_point=point_process[i].Velocity;
double r_point=point_process[i].Range;
double h_point = point_process[i].Height;
double T_point = point_process[i].CPI_Time;
double T_point = static_cast<double>(point_process[i].CPI_Time);
//航迹信息
double x_track_head, y_track_head;
@@ -199,7 +199,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
y_track_head=(*temp_track)[j][0].X[2];
double v_track_head=(*temp_track)[j][0].vr;
double h_track_head = (*temp_track)[j][0].height;
double T_track_head = (*temp_track)[j][0].T;
double T_track_head = static_cast<double>((*temp_track)[j][0].T);
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
@@ -225,8 +225,8 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
&& 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
{
struct Asso_info asso_info_tmp;
asso_info_tmp.point_idx = i+1;
asso_info_tmp.track_idx = j+1;
asso_info_tmp.point_idx = static_cast<int>(i)+1;
asso_info_tmp.track_idx = static_cast<int>(j)+1;
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][0].asso_flag = 1;
point_process[i].Use_Flag = 1;
@@ -237,7 +237,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( std::vector <std::vector
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
for (size_t i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(std::vector <Temp_track> ());
@@ -370,29 +370,29 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector<Trust_Tr
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Index=track_ID;
Trust_Track_Output[*Trust_track_num_Output-1][j].Point_Sum=L;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=(*Iter)[j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=(*Iter)[j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=(*Iter)[j].height;
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=(*Iter)[j].Amp;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = (*Iter)[j].snr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = (*Iter)[j].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=static_cast<float>((*Iter)[j].r);
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=static_cast<float>((*Iter)[j].azi/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=static_cast<float>(asin((*Iter)[j].height/(*Iter)[j].r)/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=static_cast<float>(sqrt(pow((*Iter)[j].X[1],2)+pow((*Iter)[j].X[3],2)));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=static_cast<float>((*Iter)[j].height);
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=static_cast<float>((*Iter)[j].Amp);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = static_cast<float>((*Iter)[j].snr);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = static_cast<float>((*Iter)[j].RCS);
double Direction_Angle;
Direction_Angle=atan2((*Iter)[j].X[3],(*Iter)[j].X[1]);
if(Direction_Angle<0)
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=((*Iter)[j].T)/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].x=(*Iter)[j].X[0];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=(*Iter)[j].X[1];
Trust_Track_Output[*Trust_track_num_Output-1][j].y=(*Iter)[j].X[2];
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=(*Iter)[j].X[3];
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=(*Iter)[j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=(*Iter)[j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin((*Iter)[j].height/(*Iter)[j].r)/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=(*Iter)[j].vr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=static_cast<float>(((*Iter)[j].T)/1000.0);
Trust_Track_Output[*Trust_track_num_Output-1][j].x=static_cast<float>((*Iter)[j].X[0]);
Trust_Track_Output[*Trust_track_num_Output-1][j].v_x=static_cast<float>((*Iter)[j].X[1]);
Trust_Track_Output[*Trust_track_num_Output-1][j].y=static_cast<float>((*Iter)[j].X[2]);
Trust_Track_Output[*Trust_track_num_Output-1][j].v_y=static_cast<float>((*Iter)[j].X[3]);
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=static_cast<float>((*Iter)[j].r);
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=static_cast<float>((*Iter)[j].azi/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=static_cast<float>(asin((*Iter)[j].height/(*Iter)[j].r)/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=static_cast<float>((*Iter)[j].vr);
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
Trust_Track_Output[*Trust_track_num_Output-1][j].Track_Mode=1;//跟踪模式 TWS 0
@@ -1,4 +1,4 @@
#ifndef TRACK_INIT_DIRECT_TRACKING_H
#ifndef TRACK_INIT_DIRECT_TRACKING_H
#define TRACK_INIT_DIRECT_TRACKING_H
#include "data_process_class_dll.h"
#include "parameters.h"
-19
View File
@@ -1,19 +0,0 @@
# 雷达数据处理项目修改说明
## 概述
本次主要修改TAS波束控制逻辑,
原有逻辑为:
- 维护一个tas_target_queue队列,队列中每个元素可以保存一个TAS跟踪目标
- 每次调用Beam_Ctrl()函数时,判断队列首个元素是否为空(empty_flag),若不为空,则输出TAS跟踪波束信息,否则不输出
- 将队列进行循环移位,队列第一个目标移动至队尾,下一次Beam_Ctrl()调用将会跟踪下一个目标
原有逻辑的问题:
- 原有逻辑下,TAS跟踪目标的数据率(目标信息更新频率)与波束时间相关,波束时间越短,则调用Beam_Ctrl()的频率越高,TAS目标的数据率也越高
新的逻辑为保证TAS目标的数据率不变:
- 维护一个tas_target_queue队列,队列中每个元素可以保存一个TAS跟踪目标
- 每次调用Beam_Ctrl()函数时,判断队列首个元素是否为空(empty_flag),若不为空,则再判断目标的航迹时间(T_track)和当前最新时间戳(latest_timestamp)的差值,若差值大于1 / DATA_RATE_TAS,则输出TAS跟踪波束信息,否则不输出
- 将队列进行循环移位,队列第一个目标移动至队尾,下一次Beam_Ctrl()调用将会跟踪下一个目标
注意:在RadarPara结构体中新增一项:DATA_RATE_TAS(TAS模式数据率),由dll调用者输入该参数的值(track_process_parameters_initial()),上述逻辑中DATA_RATE_TAS使用Work_Parameter中的DATA_RATE_TAS参数值,而不是使用parameters.h中的DATA_RATE_TAS宏定义值。