From 01d28e0d28daf3dc5ea17a19c3be403878a0b144 Mon Sep 17 00:00:00 2001 From: waiwaylee Date: Tue, 18 Aug 2026 11:26:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81=E4=BD=BF?= =?UTF-8?q?=E7=94=A8VS=20code+cmake=E9=87=8D=E6=96=B0=E7=BC=96=E8=AF=91?= =?UTF-8?q?=EF=BC=8C=E7=BC=96=E8=AF=91=E5=99=A8=E4=BF=9D=E6=8C=81=E4=B8=8D?= =?UTF-8?q?=E5=8F=98=EF=BC=9B=202=E3=80=81=E4=BF=AE=E5=A4=8D=E8=8B=A5?= =?UTF-8?q?=E5=B9=B2=E9=80=BB=E8=BE=91bug=EF=BC=8C=E5=85=B7=E4=BD=93?= =?UTF-8?q?=E5=8F=82=E8=80=83BUG=5FFIX=5FREPORT.md?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: waiwaylee --- .vscode/c_cpp_properties.json | 49 ++++ .vscode/cmake-variants.yaml | 25 ++ .vscode/extensions.json | 7 + .vscode/launch.json | 24 ++ .vscode/scripts/build.bat | 11 + .vscode/scripts/clean.bat | 11 + .vscode/scripts/configure.bat | 21 ++ .vscode/settings.json | 12 + .vscode/tasks.json | 170 +++++++++++ BUG_FIX_REPORT.md | 186 ++++++++++++ BUG_REPORT.md | 276 ++++++++++++++++++ CLAUDE.md | 134 --------- CMakeLists.txt | 100 +++++++ README.md | 33 --- .../data_process_class_dll.pro | 0 .../data_process_class_dll.pro.user | 0 data_process_class_dll/coor_trans.cpp | 6 +- data_process_class_dll/coor_trans.h | 1 - data_process_class_dll/data_process.cpp | 73 +++-- data_process_class_dll/data_process.h | 25 +- .../data_process_class_dll.cpp | 3 +- .../data_process_class_dll.h | 34 +-- .../data_process_class_dll_global.h | 18 +- data_process_class_dll/dot_coh.cpp | 66 ++--- data_process_class_dll/dot_coh.h | 21 +- data_process_class_dll/dot_coh_tas.cpp | 27 +- data_process_class_dll/dot_coh_tas.h | 9 +- data_process_class_dll/kalman.cpp | 70 ++--- data_process_class_dll/kalman.h | 6 +- data_process_class_dll/struct.h | 28 +- data_process_class_dll/tas_ctrl.cpp | 70 ++--- data_process_class_dll/tas_ctrl.h | 19 +- data_process_class_dll/track_asso.cpp | 141 ++++----- data_process_class_dll/track_asso.h | 27 +- .../track_asso_direct_tracking.cpp | 90 +++--- .../track_asso_direct_tracking.h | 12 +- data_process_class_dll/track_asso_tas.cpp | 148 ++++------ data_process_class_dll/track_asso_tas.h | 30 +- data_process_class_dll/track_die.cpp | 12 +- data_process_class_dll/track_die.h | 9 +- data_process_class_dll/track_die_tas.cpp | 17 +- data_process_class_dll/track_die_tas.h | 9 +- .../track_index_mangement.cpp | 24 +- .../track_index_mangement.h | 10 +- data_process_class_dll/track_init.cpp | 187 +++++------- data_process_class_dll/track_init.h | 29 +- .../track_init_direct_tracking.cpp | 85 +++--- .../track_init_direct_tracking.h | 28 +- requirements.md | 16 + 49 files changed, 1452 insertions(+), 957 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/cmake-variants.yaml create mode 100644 .vscode/extensions.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/scripts/build.bat create mode 100644 .vscode/scripts/clean.bat create mode 100644 .vscode/scripts/configure.bat create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json create mode 100644 BUG_FIX_REPORT.md create mode 100644 BUG_REPORT.md delete mode 100644 CLAUDE.md create mode 100644 CMakeLists.txt delete mode 100644 README.md rename {data_process_class_dll => backup/data_process_class_dll}/data_process_class_dll.pro (100%) rename {data_process_class_dll => backup/data_process_class_dll}/data_process_class_dll.pro.user (100%) create mode 100644 requirements.md diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..6d40f6d --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,49 @@ +{ + "version": 4, + "configurations": [ + { + "compilerPath": "D:\\APP\\VisualStudio2013\\VC\\bin\\cl.exe", + "cStandard": "c11", + "cppStandard": "c++11", + "intelliSenseMode": "windows-msvc-x86", + "defines": [ + "WIN32", + "_WINDOWS", + "UNICODE", + "_UNICODE", + "NOMINMAX", + "DATA_PROCESS_CLASS_DLL_LIBRARY", + "PHASE_SCANNING" + ], + "includePath": [ + "${workspaceFolder}/data_process_class_dll", + "${workspaceFolder}/data_process_class_dll/Eigen", + "${workspaceFolder}/**" + ], + "name": "X256_PS (MSVC2013 x86)", + "compileCommands": "${workspaceFolder}/build/X256_PS/compile_commands.json" + }, + { + "compilerPath": "D:\\APP\\VisualStudio2013\\VC\\bin\\cl.exe", + "cStandard": "c11", + "cppStandard": "c++11", + "intelliSenseMode": "windows-msvc-x86", + "defines": [ + "WIN32", + "_WINDOWS", + "UNICODE", + "_UNICODE", + "NOMINMAX", + "DATA_PROCESS_CLASS_DLL_LIBRARY", + "MECHANICAL_SCANNING" + ], + "includePath": [ + "${workspaceFolder}/data_process_class_dll", + "${workspaceFolder}/data_process_class_dll/Eigen", + "${workspaceFolder}/**" + ], + "name": "X256_MS (MSVC2013 x86)", + "compileCommands": "${workspaceFolder}/build/X256_MS/compile_commands.json" + } + ] +} diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml new file mode 100644 index 0000000..6422e08 --- /dev/null +++ b/.vscode/cmake-variants.yaml @@ -0,0 +1,25 @@ +buildType: + default: release + choices: + release: + short: Release + long: Release (MSVC2013 x86) + buildType: Release + debug: + short: Debug + long: Debug (MSVC2013 x86) + buildType: Debug + +scanMode: + default: ps + choices: + ps: + short: X256_PS + long: X256_PS - PHASE_SCANNING + settings: + RADAR_SCAN_MODE: PHASE_SCANNING + ms: + short: X256_MS + long: X256_MS - MECHANICAL_SCANNING + settings: + RADAR_SCAN_MODE: MECHANICAL_SCANNING diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..07687e2 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + "recommendations": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "twxs.cmake" + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..74e6918 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,24 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "调试 DLL", + "type": "cppvsdbg", + "request": "launch", + "program": "${workspaceFolder}/build/X256_PS/bin/rdp_playback.exe", + "args": ["-d", "E:/Demo/QT/simianzhen256-v1.5.5-ms-vscode/build/X256_PS/bin/2026-07-18/15-34-32", "-i", "E:/Demo/QT/simianzhen256-v1.5.5-ms-vscode/build/X256_PS/bin/RadarConfigParam.ini", "-c", "E:/Demo/QT/simianzhen256-v1.5.5-ms-vscode/build/X256_PS/bin/2026-07-18/15-34-32/track.csv"], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "preLaunchTask": "CMake: 配置并构建 X256_PS (Debug)", + "symbolSearchPath": "${workspaceFolder}/build/X256_PS/bin" + }, + { + "name": "附加到宿主进程（调试 DLL）", + "type": "cppvsdbg", + "request": "attach", + "processId": "${command:pickProcess}" + } + ] +} diff --git a/.vscode/scripts/build.bat b/.vscode/scripts/build.bat new file mode 100644 index 0000000..8784cb9 --- /dev/null +++ b/.vscode/scripts/build.bat @@ -0,0 +1,11 @@ +@echo off +setlocal +set "BUILD_DIR=%~1" +if "%BUILD_DIR%"=="" ( + echo Usage: build.bat ^ + exit /b 1 +) +call D:\APP\VisualStudio2013\VC\vcvarsall.bat x86 >nul +if errorlevel 1 exit /b %errorlevel% +cmake --build "%BUILD_DIR%" +exit /b %errorlevel% diff --git a/.vscode/scripts/clean.bat b/.vscode/scripts/clean.bat new file mode 100644 index 0000000..2c0fd39 --- /dev/null +++ b/.vscode/scripts/clean.bat @@ -0,0 +1,11 @@ +@echo off +setlocal +set "BUILD_DIR=%~1" +if "%BUILD_DIR%"=="" ( + echo Usage: clean.bat ^ + exit /b 1 +) +call D:\APP\VisualStudio2013\VC\vcvarsall.bat x86 >nul +if errorlevel 1 exit /b %errorlevel% +cmake --build "%BUILD_DIR%" --target clean +exit /b %errorlevel% diff --git a/.vscode/scripts/configure.bat b/.vscode/scripts/configure.bat new file mode 100644 index 0000000..8b2659d --- /dev/null +++ b/.vscode/scripts/configure.bat @@ -0,0 +1,21 @@ +@echo off +setlocal +set "BUILD_DIR=%~1" +set "SCAN_MODE=%~2" +set "BUILD_TYPE=%~3" +if "%BUILD_DIR%"=="" ( + echo Usage: configure.bat ^ ^ [Debug^|Release] + exit /b 1 +) +if "%SCAN_MODE%"=="" ( + echo Usage: configure.bat ^ ^ [Debug^|Release] + exit /b 1 +) +if "%BUILD_TYPE%"=="" set "BUILD_TYPE=Release" +call D:\APP\VisualStudio2013\VC\vcvarsall.bat x86 >nul +if errorlevel 1 exit /b %errorlevel% +pushd "%~dp0..\.." +cmake -S . -B "%BUILD_DIR%" -G "NMake Makefiles" -DCMAKE_BUILD_TYPE=%BUILD_TYPE% -DRADAR_SCAN_MODE=%SCAN_MODE% +set "CMAKE_EXIT=%errorlevel%" +popd +exit /b %CMAKE_EXIT% diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..72b63a1 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "cmake.configureOnOpen": false, + "cmake.generator": "NMake Makefiles", + "cmake.buildDirectory": "${workspaceFolder}/build/X256_PS", + "cmake.environment": { + "PATH": "D:\\APP\\VisualStudio2013\\VC\\BIN;D:\\APP\\VisualStudio2013\\Common7\\IDE;C:\\Program Files (x86)\\Windows Kits\\8.1\\bin\\x86;${env:PATH}", + "INCLUDE": "D:\\APP\\VisualStudio2013\\VC\\INCLUDE;D:\\APP\\VisualStudio2013\\VC\\ATLMFC\\INCLUDE;C:\\Program Files (x86)\\Windows Kits\\8.1\\include\\shared;C:\\Program Files (x86)\\Windows Kits\\8.1\\include\\um;C:\\Program Files (x86)\\Windows Kits\\8.1\\include\\winrt;${env:INCLUDE}", + "LIB": "D:\\APP\\VisualStudio2013\\VC\\LIB;D:\\APP\\VisualStudio2013\\VC\\ATLMFC\\LIB;C:\\Program Files (x86)\\Windows Kits\\8.1\\lib\\winv6.3\\um\\x86;${env:LIB}" + }, + "C_Cpp.default.cppStandard": "c++11", + "C_Cpp.default.compileCommands": "${workspaceFolder}/build/X256_PS/compile_commands.json" +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..8d8f76c --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,170 @@ +{ + "version": "2.0.0", + "options": { + "cwd": "${workspaceFolder}" + }, + "tasks": [ + { + "label": "CMake: 配置 X256_PS (Release, MSVC2013 x86)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\configure.bat", + "args": [ + "${workspaceFolder}/build/X256_PS", + "PHASE_SCANNING", + "Release" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 配置 X256_PS (Debug, MSVC2013 x86)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\configure.bat", + "args": [ + "${workspaceFolder}/build/X256_PS", + "PHASE_SCANNING", + "Debug" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 构建 X256_PS (Release)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\build.bat", + "args": [ + "${workspaceFolder}/build/X256_PS" + ], + "group": "build", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "CMake: 构建 X256_PS (Debug)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\build.bat", + "args": [ + "${workspaceFolder}/build/X256_PS" + ], + "group": "build", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "CMake: 清理 X256_PS", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\clean.bat", + "args": [ + "${workspaceFolder}/build/X256_PS" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 配置 X256_MS (Release, MSVC2013 x86)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\configure.bat", + "args": [ + "${workspaceFolder}/build/X256_MS", + "MECHANICAL_SCANNING", + "Release" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 配置 X256_MS (Debug, MSVC2013 x86)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\configure.bat", + "args": [ + "${workspaceFolder}/build/X256_MS", + "MECHANICAL_SCANNING", + "Debug" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 构建 X256_MS (Release)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\build.bat", + "args": [ + "${workspaceFolder}/build/X256_MS" + ], + "group": "build", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "CMake: 构建 X256_MS (Debug)", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\build.bat", + "args": [ + "${workspaceFolder}/build/X256_MS" + ], + "group": "build", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "CMake: 清理 X256_MS", + "type": "shell", + "command": "${workspaceFolder}\\.vscode\\scripts\\clean.bat", + "args": [ + "${workspaceFolder}/build/X256_MS" + ], + "problemMatcher": [] + }, + { + "label": "CMake: 配置并构建 X256_PS (Release)", + "dependsOrder": "sequence", + "dependsOn": [ + "CMake: 配置 X256_PS (Release, MSVC2013 x86)", + "CMake: 构建 X256_PS (Release)" + ], + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [] + }, + { + "label": "CMake: 配置并构建 X256_PS (Debug)", + "dependsOrder": "sequence", + "dependsOn": [ + "CMake: 配置 X256_PS (Debug, MSVC2013 x86)", + "CMake: 构建 X256_PS (Debug)" + ], + "group": "build", + "problemMatcher": [] + }, + { + "label": "CMake: 配置并构建 X256_MS (Release)", + "dependsOrder": "sequence", + "dependsOn": [ + "CMake: 配置 X256_MS (Release, MSVC2013 x86)", + "CMake: 构建 X256_MS (Release)" + ], + "group": "build", + "problemMatcher": [] + }, + { + "label": "CMake: 配置并构建 X256_MS (Debug)", + "dependsOrder": "sequence", + "dependsOn": [ + "CMake: 配置 X256_MS (Debug, MSVC2013 x86)", + "CMake: 构建 X256_MS (Debug)" + ], + "group": "build", + "problemMatcher": [] + }, + { + "label": "CMake: 配置并构建全部 (X256_PS + X256_MS, Release)", + "dependsOrder": "sequence", + "dependsOn": [ + "CMake: 配置并构建 X256_PS (Release)", + "CMake: 配置并构建 X256_MS (Release)" + ], + "problemMatcher": [] + } + ] +} diff --git a/BUG_FIX_REPORT.md b/BUG_FIX_REPORT.md new file mode 100644 index 0000000..c990ac8 --- /dev/null +++ b/BUG_FIX_REPORT.md @@ -0,0 +1,186 @@ +# Bug 修复报告 + +根据 `BUG_REPORT.md` 中的“是否修复”指示，本次完成了所有标记为“按建议修改/按建议修复”的项；标记为“暂不修复/暂不修改/暂不处理”的项保持原样未改动。 + +--- + +## 修复状态汇总 + +| 编号 | 标题 | 指示 | 状态 | +|---|---|---|---| +| BUG-01 | 基类无虚析构函数 | 暂不修复 | 未修改 | +| BUG-02 | Hight_smooth 无限增长 | 暂不修复 | 未修改 | +| BUG-03 | 工厂单例非线程安全 | 暂不修复 | 未修改 | +| BUG-04 | TAS Point_Sum 未限幅 | 按建议修复 | ✅ 已修复 | +| BUG-05 | Track_to_start.size()-1 下溢 | 按建议修复 | ✅ 已修复 | +| BUG-06 | track_start_point_num 未校验 | 按建议修复 | ✅ 已修复 | +| BUG-07 | 禁止区域个数未限制 | 暂不修复 | 未修改 | +| BUG-08 | 消亡航迹输出无容量检查 | 暂不修改 | 未修改 | +| BUG-09 | Beam_Ctrl 输出计数越界风险 | 按建议修改 | ✅ 已修复 | +| BUG-10 | 航迹号数组下标未校验 | 按建议修改 | ✅ 已修复 | +| BUG-11 | TAS model_filter 未初始化变量 | 按建议修改 | ✅ 已修复 | +| BUG-12 | tas_beam_output 未初始化变量 | 按建议修改 | ✅ 已修复 | +| BUG-13 | Work_Parameter 未初始化 | 暂不修复 | 未修改 | +| BUG-14 | lastest_index 未初始化 | 按建议修改 | ✅ 已修复 | +| BUG-15 | TWS delta_T<=0 仍关联 | 按建议修改 | ✅ 已修复 | +| BUG-16 | TAS 无 delta_T>0 检查 | 按建议修改 | ✅ 已修复 | +| BUG-17 | TAS 外推负时间差 | 按建议修改 | ✅ 已修复 | +| BUG-18 | 卡尔曼时间差除零 | 暂不修改 | 未修改 | +| BUG-19 | Bind_speed prt=0 除零 | 按建议修改 | ✅ 已修复 | +| BUG-20 | 门限重复平方 | 暂不修改 | 未修改 | +| BUG-21 | 近程模型 3 门限整数除法 | 按建议修改 | ✅ 已修复 | +| BUG-22 | EKF/凝聚方位角未环绕 | 按建议修改 | ✅ 已修复 | +| BUG-23 | IMM 概率零分母/奇异矩阵 | 按建议修改 | ✅ 已修复 | +| BUG-24 | EKF 似然只取 2x2 S | 按建议修改 | ✅ 已修复 | +| BUG-25 | TWS 处理 TAS 航迹 | 暂不处理 | 未修改 | +| BUG-26 | 输出航向角使用 atan | 按建议修改 | ✅ 已修复 | +| BUG-27 | asin(height/range) 未保护 | 按建议修改 | ✅ 已修复 | +| BUG-28 | track_clear_all 清理不彻底 | 按建议修改 | ✅ 已修复 | +| BUG-29 | tracking_stop 队列延迟 | 暂不修改 | 未修改 | +| BUG-30 | work_mode 注释不一致 | 注释有误 | ✅ 已修复注释 | +| BUG-31 | tracking_point/引导跟踪空实现 | 注释明确 | ✅ 已加注释 | +| BUG-32 | 凝聚内层未跳过已用点 | 按建议修改 | ✅ 已修复 | +| BUG-33 | 起批 delta_T 未校验 | 暂不修改 | 未修改 | +| BUG-34 | tmp_track_die 空向量越界 | 按建议修改 | ✅ 已修复 | +| BUG-35 | 航迹号 500 不复用 | 按建议修改 | ✅ 已修复 | +| BUG-36 | TAS 波束关闭字段未清 | 暂不修改 | 未修改 | +| BUG-37 | 局部结构体未完整初始化 | 按建议修改 | ✅ 已修复 | + +--- + +## 已修复项明细 + +### BUG-04 +- 文件：`data_process.cpp` +- 修改：TAS 分支 `data_num = min(max(Point_Sum,0), 150)`，避免 `Point_Sum > 150` 时越界读取输入数组。 + +### BUG-05 +- 文件：`track_init.cpp` +- 修改：重复航迹去重循环改为 `for (size_t i = 0; i + 1 < Track_to_start.size(); ++i)`，避免空容器时 `size()-1` 下溢。 + +### BUG-06 +- 文件：`data_process.cpp`、`track_init.cpp`、`track_init_direct_tracking.cpp` +- 修改： + - `track_process_parameters_initial/modify` 校验 `3 <= track_start_point_num <= 9`，非法参数返回 `-1`。 + - 起批转可靠航迹前增加 `Track_to_start.empty()` 和每条航迹 `size() >= 3` 防御。 + - 直接跟踪的 `tmp_track_to_trust_track` 也增加 `L >= 3` 保护。 + +### BUG-09 +- 文件：`tas_ctrl.cpp` +- 修改： + - `tas_ctrl_process` 开始时校验 `Trust_track_num_Output` 非空并清零。 + - `tas_target_add` 写入输出数组前检查 `*Trust_track_num_Output < MAX_TRACK_NUM`。 + +### BUG-10 +- 文件：`track_index_mangement.cpp` +- 修改：写入 `List[]` 前校验 `Track_Index` 在 `[1, MAX_TRACK_INDEX]`。 + +### BUG-11 +- 文件：`track_asso_tas.cpp` +- 修改：`model_filter` 增加 `found_tas_track` 标记；找不到目标时直接返回，局部变量均初始化。 + +### BUG-12 +- 文件：`tas_ctrl.cpp` +- 修改：`tas_beam_output` 初始化 `H_track/X_now/CPI_time`，找不到目标时 `open_flag=0` 并返回。 + +### BUG-14 +- 文件：`track_index_mangement.h` +- 修改：`lastest_index` 声明时初始化为 `0`。 + +### BUG-15 +- 文件：`track_asso.cpp` +- 修改：TWS 关联中 `delta_T <= 0` 时从候选列表移除该无效配对并 `continue`，不再标记点迹已使用、不刷新航迹状态。 + +### BUG-16 +- 文件：`track_asso_tas.cpp` +- 修改：TAS 关联点循环中 `delta_T <= 0` 时跳过该点。 + +### BUG-17 +- 文件：`track_asso_tas.cpp` +- 修改：TAS 外推时 `delta_T <= 0` 直接跳过，避免航迹时间回退和负时间预测。 + +### BUG-19 +- 文件：`kalman.cpp`、`data_process_class_dll.h` +- 修改：`Bind_speed` 对 `prt <= 0` 或 `freq <= 0` 返回安全非零值；接口注释改为“PRI 不允许为 0”。 + +### BUG-21 +- 文件：`track_asso.cpp` +- 修改：近程门限中 `/1000`、`/100` 改为浮点除法，避免整数除零恒不成立；保留 `d*d` 比较方式（未修改 BUG-20）。 + +### BUG-22 +- 文件：`kalman.cpp`、`dot_coh.cpp`、`dot_coh_tas.cpp` +- 修改： + - 新增 `wrapAnglePi()`，EKF 各残差计算后对方位角分量做 `[-π,π]` 环绕。 + - 点迹凝聚 `work_mode==0` 分支和 TAS 凝聚统一使用最小角度差 `delta_F`。 + +### BUG-23 +- 文件：`track_asso.cpp`、`track_asso_tas.cpp`、`track_asso_direct_tracking.cpp` +- 修改： + - IMM 交互中 `c[0..2]` 为 0/负时钳位到 `1e-12`。 + - 模型概率更新增加分母保护，分母异常时保持上一拍概率。 + - 似然计算对 `det_S <= 0` 返回 0，避免 NaN。 + +### BUG-24 +- 文件：`kalman.h`、`kalman.cpp`、`track_asso.cpp`、`track_asso_tas.cpp`、`track_asso_direct_tracking.cpp` +- 修改： + - `kalman_filter_EKF` 的 `S_filter` 由 `[2][2]` 改为 `[3][3]`。 + - 调用侧使用 `Matrix3f` 和 3x3 行列式，似然归一化改为 `1/sqrt(pow(2*PI,3)*detS)`。 + +### BUG-26 +- 文件：`track_asso.cpp`、`track_asso_tas.cpp`、`track_asso_direct_tracking.cpp` +- 修改：航向角改用 `atan2(X[4], X[1])` 并归一化到 `[0,2π)`。 + +### BUG-27 +- 文件：`track_asso_tas.cpp`、`track_asso_direct_tracking.cpp`、`track_init.cpp` +- 修改：计算 `asin` 前对 `height/range` 做定义域钳位，`range <= 0` 时按 0 处理。 + +### BUG-28 +- 文件：`data_process.cpp`、`dot_coh.h/cpp`、`track_asso.h/cpp`、`track_asso_tas.h/cpp`、`track_init.h/cpp`、`tas_ctrl.h/cpp`、`track_index_mangement.h/cpp` +- 修改： + - 为 `Dot_Coh`、`Track_Asso`、`Track_Asso_Tas`、`Track_Init`、`TAS_Ctrl`、`Track_Ind_Mangement` 增加 `reset()`。 + - `track_clear_all()` 调用上述 reset，并恢复 `last_beam_num = INT_MAX`。 + - 直接跟踪相关类也补充了 `reset()`。 + +### BUG-30 +- 文件：`data_process_class_dll.h` +- 修改：`work_mode` 注释由“0进程 1中程 3远程”改为“0进程 1中程 2远程”。 + +### BUG-31 +- 文件：`data_process_class_dll.h`、`data_process.cpp`、`track_init_direct_tracking.cpp` +- 修改：在 `direct_tracking_process`、`tracking_point`、直接跟踪入口处注释明确“当前接口未启用，保留空实现”。 + +### BUG-32 +- 文件：`dot_coh.cpp`、`dot_coh_tas.cpp` +- 修改：内层凝聚比较增加对 `i` 点 `Use_Flag` 的检查，已使用点不再参与凝聚。 + +### BUG-34 +- 文件：`track_init.cpp`、`track_init_direct_tracking.cpp` +- 修改：`tmp_track_die` 先判断 `n <= 0`，空向量直接删除。 + +### BUG-35 +- 文件：`track_index_mangement.cpp` +- 修改：两处回绕循环由 `i < MAX_TRACK_INDEX` 改为 `i <= MAX_TRACK_INDEX`，使 500 号可复用。 + +### BUG-37 +- 文件：`track_init.cpp`、`track_init_direct_tracking.cpp` +- 修改：`Temp_track`、`Trust_Track` 局部变量改为 `= {}` 值初始化，避免未初始化字段。 + +--- + +## 编译与运行验证 + +使用 **MSVC 2013 (Visual Studio 12) x86** 实际编译： + +| 配置 | 结果 | +|---|---| +| X256_PS Release | ✅ 编译链接通过 | +| X256_MS Release | ✅ 编译链接通过 | +| X256_PS Debug | ✅ 编译链接通过 | +| MSVC `/W4` Release | ✅ 编译通过，未再出现 C4700/C4701 未初始化变量告警 | + +另编写临时宿主程序验证 DLL： +- 初始化 `RadarPara`，调用 `track_process_parameters_initial` 成功。 +- 调用 `track_process`，返回 `1`，输出计数正常。 +- 工厂创建/销毁流程正常。 + +> 说明：当前构建仍会输出原有代码的 C4244 转换告警、C4018 有符号/无符号比较告警、C4819 编码告警，均非本次修复引入，不影响链接和运行。 diff --git a/BUG_REPORT.md b/BUG_REPORT.md new file mode 100644 index 0000000..55f25da --- /dev/null +++ b/BUG_REPORT.md @@ -0,0 +1,276 @@ +# 雷达数据处理 DLL 逻辑 BUG 分析报告 + +分析范围：`data_process_class_dll/` 下全部源码（不含 `Eigen/`），以迁移后的当前代码行号为准。 +严重级别定义： + +- **P0**：可能导致崩溃、越界读写、内存破坏或长时间运行内存泄漏，建议优先修复。 +- **P1**：在常见异常时序/边界输入下会出错，或属于明显算法逻辑错误。 +- **P2**：健壮性、数值稳定性、状态一致性、功能缺失等问题。 +- **P3**：代码质量/可维护性问题。 + +--- + +## 1. 内存与生命周期 + +### BUG-01 [P0] 基类没有虚析构函数，工厂按基类指针 delete 派生对象 +- 位置：`data_process_class_dll.h:168`、`data_process_class_dll.cpp:25-32` +- 说明：`Data_process_class_dll` 没有声明虚析构函数，而 `Data_Process` 内部包含多个 `std::vector` 和若干成员对象。`Data_Process_Factory::Destroy()` 执行 `delete p` 时静态类型是基类指针，只会调用基类析构函数，派生类成员（`Data_buffer`、`trust_track`、`temp_track`、Eigen 相关对象等）的析构不会执行，属于未定义行为并造成内存泄漏。 +- 建议：在基类中增加 `virtual ~Data_process_class_dll() {}`；同时让工厂支持重复销毁、销毁后返回 nullptr。 +- 暂不修复 + +### BUG-02 [P1] `Hight_smooth` 高度平滑缓存只增不减，长时间运行内存持续增长 +- 位置：`track_asso.cpp:890`、`track_asso_tas.cpp:722`、`track_asso_direct_tracking.cpp:639` +- 说明：每次高度更新都执行 `Hight_smooth.push_back(...)`，从未裁剪或清空。航迹存活时间越长，该 vector 越大；500 条航迹长时间运行时内存会持续增长。 +- 建议：仅保留最近 `height_win_length` 个高度值（如 `resize`/`erase(begin)` 后再 push），或改用固定长度 `std::deque`；航迹消亡时随结构体释放。 +- 暂不修复 + +### BUG-03 [P2] 工厂单例创建/销毁非线程安全 +- 位置：`data_process_class_dll.cpp:15-32` +- 说明：`GetB()` 和 `Destroy()` 对静态指针 `p` 无任何同步。若宿主在多线程环境调用，可能创建两个实例（泄漏一个）或对同一对象重复销毁。 +- 建议：用 C++11 `static Data_Process instance;` 返回地址，或对工厂方法加锁；明确 DLL 接口的线程模型。 +- 暂不修复 + +--- + +## 2. 数组/向量越界 + +### BUG-04 [P0] TAS 输入点迹未限制 `Point_Sum <= 150` +- 位置：`data_process.cpp:66` +- 说明：TWS 分支使用 `min(Point_Sum, 150)`，但 TAS 分支直接 `data_num = Data_Input[0].Point_Sum` 并循环读取 `Data_Input[i]`。当协议传入 `Point_Sum > 150` 时，会越界读取 `Data_Input[150]`。 +- 建议：与 TWS 分支一致，使用 `data_num = min(max(Point_Sum,0), 150)`，并校验 `Point_Sum >= 0`。 +- 按建议修复 + +### BUG-05 [P0] `Track_to_start.size()-1` 在空容器时下溢导致越界 +- 位置：`track_init.cpp:534` +- 说明：与已发现示例一致。`size()` 返回 `size_t`，空容器减 1 得到极大值，随后 `Track_to_start[i]` 越界。 +- 建议：`if (Track_to_start.empty()) return;`，或改为 `for (size_t i = 0; i + 1 < Track_to_start.size(); ++i)`。 +- 按建议修复，改为 `for (size_t i = 0; i + 1 < Track_to_start.size(); ++i)` + +### BUG-06 [P0] `track_start_point_num` 未校验，取值 0/1/2 或大于 9 时多处越界 +- 位置：`track_init.cpp:533,544-545,588-594`、`track_init_direct_tracking.cpp:316-322` +- 说明：重复航迹比较固定访问 `Track_to_start[i][1]`、`[2]`，三点初始化访问 `[L-3]/[L-2]/[L-1]`。该参数由外部 `RadarPara` 传入，若未初始化或配置为 0/1/2，会出现负下标或越界；若大于 9，`tmp_track_die` 会在 `n>=10` 时删除临时航迹，逻辑也无法起批。 +- 建议：在 `track_process_parameters_initial/modify` 中校验 `3 <= track_start_point_num <= 9`；使用 `Track_to_start[i].size()` 作为实际长度并在访问前检查。 +- 按建议修复 + +### BUG-07 [P0] 禁止区域个数未限制在 30 以内，外部配置过大时数组越界 +- 位置：`track_init.cpp:711`（`track_prohibite_area_num`）、`tas_ctrl.cpp:123`（`TAS_prohibite_area_num`） +- 说明：循环上界直接使用外部传入的计数，而对应数组固定为 `[30]`。配置大于 30 时越界读。 +- 建议：循环上界改为 `min(count, 30)`，并在参数初始化/修改时拒绝非法计数或截断。 +- 暂不修复 + +### BUG-08 [P1] 消亡航迹号输出无数组容量检查 +- 位置：`track_die.cpp:24`、`track_die_tas.cpp:26` +- 说明：`Track_die_Index_Output[*Track_die_num_Output-1] = ...` 不检查数组容量。正常流量下最多 500 条航迹，与 `MAX_TRACK_NUM` 一致，但接口没有把容量传入，一旦宿主传入较小数组、或计数被异常修改，就会越界写。 +- 建议：接口增加 `die_array_capacity` 参数，或内部保证 `*Track_die_num_Output < MAX_TRACK_NUM` 后再写。 +- 暂不修改 + +### BUG-09 [P1] `Beam_Ctrl` 路径中输出计数未初始化/无上界，可能越界写航迹输出数组 +- 位置：`data_process.cpp:193-198`、`tas_ctrl.cpp:74-78` +- 说明：`Beam_Ctrl()` 直接使用宿主传入的 `Trust_track_num_Output`，而 `tas_ctrl_process` 不会先清零，`tas_target_add` 会基于旧值自增并写 `Trust_Track_Output[*Trust_track_num_Output-1]`。若调用方未清零或旧值接近 `MAX_TRACK_NUM`，会越界写。 +- 建议：`tas_ctrl_process` 内部保存 `*count = 0` 或在每次写入前检查 `*count < MAX_TRACK_NUM`；同时校验指针非空。 +- 按建议修改 + +### BUG-10 [P1] 航迹号直接作为数组下标，未校验范围 +- 位置：`track_index_mangement.cpp:29` +- 说明：`List[(*trust_track)[i].Track_Index-1] = 1`，若航迹号不在 `[1, MAX_TRACK_INDEX]` 内（异常数据、内存损坏、外部修改），立即越界。 +- 建议：写前检查 `Track_Index >= 1 && Track_Index <= MAX_TRACK_INDEX`，异常航迹号返回错误或跳过。 +- 按建议修改 + +--- + +## 3. 未初始化变量 + +### BUG-11 [P1] `Track_Asso_Tas::model_filter` 在找不到 TAS 目标时使用未初始化变量 +- 位置：`track_asso_tas.cpp:228-232`，随后在 `271/276/280/282` 等使用 +- 说明：`X1/X2/X3/P1/P2/P3/T_track/v_track/r_track/h_track` 只在 `Track_Index == tas_track_idx` 时赋值。若 `tas_track_idx` 不存在（例如目标已被消亡、主程序传入失效批号），后续仍用这些未初始化值计算 IMM、距离门限和外推，结果不可预测。MSVC `/W4` 已报 C4701。 +- 建议：函数开头初始化这些变量，并在找不到目标时直接 return；上层也应处理“TAS 目标不存在”的返回值。 +- 按建议修改 + +### BUG-12 [P1] `tas_beam_output` 在队列目标不在航迹表中时使用未初始化变量 +- 位置：`tas_ctrl.cpp:242-271` +- 说明：`H_track`、`X_now[]` 只在找到匹配航迹时赋值；若 `track_clear_all()` 后队列未清空、或目标已被删除而队列未同步，循环找不到目标，随后 `asin(H_track/range)`、`X_now[...]` 使用未初始化数据。Cppcheck 和 MSVC `/W4` 均报 C4701。 +- 建议：找不到目标时立即 `open_flag=0` 并 return；变量声明时初始化。 +- 按建议修改 + +### BUG-13 [P2] `Data_Process::Work_Parameter` 及若干成员在构造后未初始化 +- 位置：`data_process.h:28-31,78-95` +- 说明：构造函数为空，`Work_Parameter`、`Beam_num`、`data_num`、`TAS_track_idx` 等未初始化。若宿主在调用 `track_process_parameters_initial` 前就调用 `data_preprocess/track_process/Beam_Ctrl`，`Work_Parameter.track_start_point_num`、`Sys_delay`、`V_MIN/V_MAX` 等是垃圾值，可能导致起批越界、除零或异常门限。 +- 建议：构造函数中对 `Work_Parameter` 进行 `memset`/值初始化并设置安全默认参数；在处理函数入口检查“参数是否已初始化”。 +- 暂不修复 + +### BUG-14 [P3] `Track_Ind_Mangement::lastest_index` 未初始化 +- 位置：`track_index_mangement.h:17`、`track_index_mangement.cpp:33` +- 说明：只有空航迹表分支会赋值为 1；如果首次调用时航迹表非空，就会读取未初始化的 `lastest_index`。 +- 建议：声明为 `int lastest_index = 0;` 或增加构造函数初始化。 +- 按建议修改 + +--- + +## 4. 时间戳/时序处理 + +### BUG-15 [P1] TWS 关联在 `delta_T <= 0` 时仍把点迹标记为已关联并刷新航迹状态 +- 位置：`track_asso.cpp:610-691` +- 说明：滤波和航迹信息更新在 `if (delta_T > 0)` 内，但 `Extrapolate_round=0`、`point_flag=1`、`associate_point_number++`、点迹 `Use_Flag=1` 都在 if 之外。重复/乱序时间戳的点仍会“占用”点迹、让航迹看起来已更新，实际状态未更新。 +- 建议：`delta_T <= 0` 时直接跳过该关联候选（或作为无效量测处理），不要标记点迹已使用、不要刷新航迹新鲜度。 +- 按建议修改 + +### BUG-16 [P1] TAS 关联完全没有 `delta_T > 0` 检查 +- 位置：`track_asso_tas.cpp:271-335,385-452` +- 说明：TAS 路径计算 `delta_T` 后直接进入统计距离和滤波，即使 `delta_T <= 0` 也会生成 F/Q 并更新航迹，可能把航迹时间更新到过去。 +- 建议：与 TWS 一致，只有 `delta_T > 0` 才允许关联；否则跳过该点。 +- 按建议修改 + +### BUG-17 [P1] TAS 外推使用未校验的 `latest_timestamp - T_track`，负时间差导致反向预测 +- 位置：`track_asso_tas.cpp:483-506` +- 说明：若 `latest_timestamp < T_track`（乱序/重复时间戳），`delta_T` 为负，`T_track += delta_T*1000` 会回退航迹时间，IMM F/Q 也按负时间生成。 +- 建议：`delta_T = max(0, latest_timestamp - T_track)/1000.0`；若为 0 则直接返回或保持原状态。 +- 按建议修改 + +### BUG-18 [P1] 卡尔曼初始化及统计距离函数对零/负时间差没有保护 +- 位置：`kalman.cpp:13-46,126-130,158-168`；调用点 `track_init.cpp:275,450,593-594`、`track_init_direct_tracking.cpp:135,271,321-322` +- 说明：两点/三点初始化直接除以 `T/T1/T2`。同一 CPI 重复点、时间戳相等或乱序会产生除零、inf/NaN，随后污染航迹协方差和模型概率。 +- 建议：调用前统一校验时间差大于最小阈值（如 >0 或 >1 ms）；`kalman_filter_init_2dots/3dots` 内部对非法时间返回错误。 +- 暂不修改 + +### BUG-19 [P1] `Bind_speed` 对 `prt == 0` 无保护，而接口说明 PRI 可给 0 +- 位置：`kalman.cpp:766-770`、`data_process_class_dll.h:29` +- 说明：`return 150000.0/(freq*prt)`，当 `PRI=0` 时除零；结果传给 `Round()` 会把 inf/NaN 转换为未定义整型。 +- 建议：`prt <= 0` 时返回固定安全值或直接返回无效距离；统一约定 PRI 单位与默认值。 +- 按建议修改，同时修改接口说明：PRI不可为0 + +--- + +## 5. 关联门限与滤波算法逻辑 + +### BUG-20 [P1] 统计距离 d 已经是平方形式，门限中又平方了一次 +- 位置：`track_asso.cpp:544-546`、`track_asso_tas.cpp:335`、`track_asso_direct_tracking.cpp:286`、`track_init.cpp:221` +- 说明：`d_cal_EKF` 返回的是 `delta_z^T S^{-1} delta_z`（马氏距离平方），后续门限却写成 `d*d < THRESHOLD*THRESHOLD`。例如阈值 3 时本意是 `d < 9`，实际变成 `d < 3`，波门明显偏小。 +- 建议：统一改为 `d < TRACK_START_THRESHOLD*TRACK_START_THRESHOLD` / `d < ASSO_THORD*ASSO_THORD`。 +- 暂不修改 + +### BUG-21 [P1] 近程模型 3 门限因整数除法恒为 0 +- 位置：`track_asso.cpp:544-545` +- 说明：`ASSO_THORD` 是 int 宏，`ASSO_THORD*ASSO_THORD/1000` 和 `/100` 按整数计算，结果均为 0，导致 `d3*d3 < 0` 永远不成立；近距离下模型 3 实际被禁用。 +- 建议：写为 `ASSO_THORD*ASSO_THORD/1000.0`、`/100.0`，同时按 BUG-20 修正平方关系。 +- 按建议修改，但不修改BUG-20 + +### BUG-22 [P1] EKF 方位角残差未按 0/2π 环绕处理 +- 位置：`kalman.cpp:265-298,457-492,642-680`；凝聚 `dot_coh.cpp:35,44`（`work_mode==0` 分支）、`dot_coh_tas.cpp:33,42` +- 说明：预测方位被归一化到 `[0,2π)`，但量测方位未归一化，`delta_z = Z_mea - Z_pred` 未做 ±π 环绕。目标跨正北时残差会接近 2π，导致错误拒绝/错误滤波。凝聚中 `work_mode==0` 和 TAS 凝聚也直接 `fabs(angle)`，未处理 360° 环绕。 +- 建议：方位差统一按 `wrapToPi()` 处理；`work_mode==0` 与 TAS 凝聚复用同一环绕角差函数。 +- 按建议修改 + +### BUG-23 [P1] IMM 模型概率计算缺少零分母/奇异矩阵保护 +- 位置：`track_asso.cpp:157-165,627-647`、`track_asso_tas.cpp:122-130,406-431`、`track_asso_direct_tracking.cpp:95-103,352-374` +- 说明：`c[0..2]`、`Possibility1*c[0]+...`、`det_S` 都可能为 0 或负；`1/sqrt(2*PI*det_S)` 对负/零行列式产生 NaN。一旦概率变 NaN，后续 `model_output` 会把整条航迹状态污染。 +- 建议：计算前检查 `det_S > eps`、分母 > eps；异常时保持上一拍模型概率或回退为等概率 `{1/3,1/3,1/3}`。 +- 按建议修改 + +### BUG-24 [P2] EKF 似然只取 3x3 新息协方差左上角 2x2 的行列式 +- 位置：`kalman.cpp:515-519`（S 为 3x3，但输出只保存 2x2）；调用处 `track_asso.cpp:627-633`、`track_asso_tas.cpp:406-412`、`track_asso_direct_tracking.cpp:352-358` +- 说明：EKF 量测为距离/方位/径向速度三维，新息协方差 S 是 3x3，但接口 `S_filter[2][2]` 和似然计算只使用二维子块，模型似然不完整。 +- 建议：将接口改为 3x3，并采用完整 3 维高斯归一化因子 `1/sqrt(pow(2*PI,3)*detS)`（若只比较相对大小，也至少应保持三个模型使用相同维数）。 +- 按建议修改 + +### BUG-25 [P1] TWS 航迹关联/输出没有排除 TAS 航迹 +- 位置：`track_asso.cpp:65-133`（输出）、`141-228`（交互 +）、`414-570`（滤波） +- 说明：原代码中有 `// if manual_tracking_flag==0` 的过滤逻辑但被注释掉。当前 TWS 处理会遍历并更新所有航迹，包括 `Track_Mode==1` 的 TAS 航迹；TWS 输出循环也会把 TAS 航迹当作 `point_type=0` 输出，造成 TAS 航迹被 TWS 点迹错误更新和重复输出。 +- 建议：TWS 的交互、关联、输出统一跳过 `Track_Mode == 1`（或 `manual_tracking_flag == 1`）的航迹；TAS 目标只在 TAS 流程中处理。 +- 暂不处理 + +### BUG-26 [P2] 输出航向角使用 `atan(y/x)` 而不是 `atan2` +- 位置：`track_asso.cpp:100`、`track_asso_tas.cpp:66`、`track_asso_direct_tracking.cpp:59` +- 说明：当 `vx == 0` 时除法结果接近 ±inf，现有象限修正逻辑不完整，某些象限会输出负角度或 90° 偏差。 +- 建议：统一使用 `atan2(X[4], X[1])` 并归一化到 `[0,2π)`。 +- 按建议修改 + +### BUG-27 [P2] `asin(height/range)` 缺少定义域和零距离保护 +- 位置：`track_asso_tas.cpp:54`、`track_asso_direct_tracking.cpp:48`、`track_init.cpp:646,673` +- 说明：`range` 为 0 或 `height > range` 时，`asin` 参数超出 `[-1,1]` 产生 NaN，并输出到 `Track.Elevation`。 +- 建议：计算前钳位 `h/r` 到 `[-1,1]`，并对 `r <= eps` 特殊处理。 +- 按建议修改 + +--- + +## 6. 状态清理与接口一致性 + +### BUG-28 [P1] `track_clear_all` 清理不彻底，重连后可能输出幽灵航迹/幽灵波束 +- 位置：`data_process.cpp:219-229` +- 说明：只清空 6 个顶层 vector，未清理： + - `Dot_Coh::data_input_buff`（滑窗凝聚缓存）； + - `TAS_Ctrl::tas_target_queue` 和 `tas_target_num`； + - `last_beam_num`（扫描圈判断）； + - `Track_Ind_Mangement::lastest_index`； + - 各关联类的 `point_process` 成员。 + 清空后若立即调用 `Beam_Ctrl`，TAS 队列仍认为有目标，但 `trust_track` 已空，会触发 BUG-12 的未初始化路径并输出错误波束；下一次 TWS 处理也可能输出清空前的缓存点迹。 +- 建议：为 `Dot_Coh`、`TAS_Ctrl`、`Track_Ind_Mangement` 增加 `reset()`，`track_clear_all` 调用所有 reset，并恢复 `last_beam_num=INT_MAX`。 +- 按建议修改 + +### BUG-29 [P2] `tracking_stop` 后 TAS 队列要到下一次 `Beam_Ctrl` 才移除 +- 位置：`data_process.cpp:260-270`、`tas_ctrl.cpp:138-162` +- 说明：`tracking_stop` 只清标志位和 `Track_Mode`，不会立即清 `tas_target_queue`。在调用 `Beam_Ctrl` 前，队列仍会输出该目标的跟踪波束。 +- 建议：`tracking_stop` 内同步删除 TAS 队列项并更新 `tas_target_num`，或明确接口时序并文档化。 +- 暂不修改 + +### BUG-30 [P2] 工作模式枚举与代码分支不一致 +- 位置：`data_process_class_dll.h:140`（注释：0 进程、1 中程、3 远程）、`track_asso.cpp:729-744`（按 0/1/2/else 分支） +- 说明：注释定义远程模式为 3，但代码按 `work_mode==2` 使用远距数据率；当外部按注释传 3 时，会落入 else 使用近程数据率 `DATA_RATE_SHORT`。 +- 建议：统一枚举定义，或代码改为 `work_mode==3` 使用 `DATA_RATE_FAR`。 +- 注释有误，修改注释为：0 进程、1 中程、2 远程 + +### BUG-31 [P2] `tracking_point` 和引导跟踪处理是空实现 +- 位置：`data_process.cpp:282-286`、`data_process_class_dll.h:200-213`、`track_init_direct_tracking.cpp:11-21` +- 说明：`tracking_point` 直接返回 0；`Data_Process` 没有重写 `direct_tracking_process`，基类默认返回 0；`Track_Init_Direct_Tracking::track_init_process_logic` 也是空函数。若接口已被主程序调用，则相关功能实际未生效。 +- 建议：确认这两个接口是否已废弃；若仍需要，补齐实现或至少在文档中明确为未实现并返回错误码。 +- 在注释中明确接口未启用，保留空实现 + +--- + +## 7. 其他逻辑与健壮性 + +### BUG-32 [P2] 凝聚内层循环没有跳过已标记使用的点 +- 位置：`dot_coh.cpp:25,148`、`dot_coh_tas.cpp:25` +- 说明：内层只检查当前基准点 `loop_of_point` 的 `Use_Flag`，没有检查被比较点 `i` 的 `Use_Flag`。已被凝聚掉的点仍可参与后续比较，甚至反过来把高幅度基准点标记掉。 +- 建议：内层同样判断 `(*data_input)[i].Use_Flag != 1`（以及 `data_tmp[i]`）。 +- 按建议修改 + +### BUG-33 [P2] 起批/临时航迹关联未校验 `delta_T > 0`，重复时间戳可能产生非法卡尔曼初始化 +- 位置：`track_init.cpp:189,381,450`、`track_init_direct_tracking.cpp:67,221,271` +- 说明：点迹与临时航迹、点迹与航迹头的关联均使用 `T_point - T_track_head`，未要求正时间差。`kalman_filter_init_2dots` 遇到 0/负 T 会产生除零。 +- 建议：在关联条件中显式要求 `T_point > T_track_head`，且时间差需大于最小步长。 +- 暂不修改 + +### BUG-34 [P2] 临时航迹消亡函数在空向量时直接取 `[n-1]` +- 位置：`track_init.cpp:694-695`、`track_init_direct_tracking.cpp:417-418` +- 说明：`int n = (*Iter).size();` 后立即 `(*Iter)[n-1]`。正常流程每条临时航迹至少 1 个点，但没有防御；一旦出现空内层向量，`n-1` 为负并越界。 +- 建议：先判断 `n > 0`，空向量直接删除。 +- 按建议修改 + +### BUG-35 [P3] 航迹号复用逻辑漏掉 `MAX_TRACK_INDEX` +- 位置：`track_index_mangement.cpp:44,56` +- 说明：回绕查找循环写作 `i < MAX_TRACK_INDEX`，因此 500 号航迹永远不会被复用；与第一段 `i <= MAX_TRACK_INDEX` 不一致。 +- 建议：两处回绕循环改为 `i <= MAX_TRACK_INDEX`。 +- 按建议修改 + +### BUG-36 [P3] TAS 波束关闭时只清 `open_flag`，其他字段保留旧值 +- 位置：`tas_ctrl.cpp:277-284` +- 说明：当 `tas_target_queue[0].empty_flag==0` 时，仅设置 `Tracking_beam->open_flag=0`，`Range/Azi/Elev/type/TAS_track_index` 保留上一拍内容。主程序若只按 `open_flag` 判断则无问题，但字段语义不清晰。 +- 建议：关闭时同时清零 `type/Range/Azi/Elev/TAS_track_index`。 +- 暂不修改 + +### BUG-37 [P3] 部分结构体局部变量未完整初始化 +- 位置：`track_init.cpp:80,598`、`track_init_direct_tracking.cpp:325` +- 说明：`Temp_track temp_track_tmp`、`Trust_Track trust_track_tmp` 的 `P` 矩阵等字段未显式初始化。当前流程部分字段未被读取，但依赖调用顺序；后续维护容易读到垃圾值。 +- 建议：使用值初始化 `Temp_track tmp = {};` 或为结构体提供构造函数/`init()` 统一初始化所有字段。 +- 按建议修改 + +--- + +## 建议修复顺序 + +1. 先修 P0：BUG-01、BUG-04、BUG-05、BUG-06、BUG-07、BUG-11、BUG-12。 +2. 再修 P1 时间与关联逻辑：BUG-15~BUG-23、BUG-25、BUG-28。 +3. 最后处理数值稳定性、状态一致性和未启用功能：BUG-24、BUG-26~BUG-37。 + +> 说明：`track_asso_direct_tracking.cpp` 和 `track_init_direct_tracking.cpp` 当前未加入 CMake 构建（与旧 .pro 一致），其中问题与主流程同类问题重复，修复主流程时可同步修改或暂缓。 diff --git a/CLAUDE.md b/CLAUDE.md deleted file mode 100644 index e52aa39..0000000 --- a/CLAUDE.md +++ /dev/null @@ -1,134 +0,0 @@ -# CLAUDE.md - -This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. - -## 项目概述 - -2003 雷达数据处理动态链接库。实现航迹跟踪处理、TAS/TWS 跟踪控制。将原始雷达点迹数据转换为稳定的目标航迹输出。 - -- **开发环境**: Qt 5.7 + MSVC 2013, C++ -- **编译产物**: `data_process_class_dll.dll`（动态链接库） -- **线性代数**: 使用 Eigen 3.3.7 (`Eigen/Dense`) - -## 构建 - -项目使用 qmake 生成 Makefile，然后通过 nmake 编译。 - -```bash -# 生成 Makefile（在项目根目录执行） -qmake.exe -spec win32-msvc2013 -o Makefile data_process_class_dll\data_process_class_dll.pro - -# 编译 Release 版本 -nmake release - -# 编译 Debug 版本 -nmake debug - -# 清理 -nmake clean -``` - -已生成的 Makefile 位于根目录：`Makefile`、`Makefile.Debug`、`Makefile.Release`。构建输出在 `release/` 和 `debug/` 目录。 - -## 架构 - -### 设计模式 - -- **接口/工厂模式**：`Data_process_class_dll` 是抽象基类（定义在 `data_process_class_dll.h`），`Data_Process` 是具体实现（`data_process.h/cpp`）。外部通过 `Data_Process_Factory::GetB()` 获取实例，`Destroy()` 销毁。 -- 所有外部接口都是虚函数，主程序只需要链接 DLL 并调用这些接口。 - -### 数据处理三模式 - -`data_preprocess()` 根据输入点迹类型返回处理模式： - -| 返回值 | 模式 | 条件 | -|--------|------|------| -| `0` | 无数据 | 既非 TWS 也非 TAS | -| `1` | TWS 航迹处理 | `point_type == 0` 且波位号 == 16 | -| `2` | TWS 蓄数据 | `point_type == 0` 但波位号 != 16（继续缓存） | -| `3` | TAS 处理 | `point_type == 1` | - -### TWS 处理流水线（model=1） - -``` -data_preprocess() → 缓存点迹到 Data_buffer - → dot_coh.dot_coh_process_buff() 点迹凝聚（滑窗聚合） - → track_asso.track_asso_process() 航迹关联（IMM 卡尔曼滤波更新已有航迹） - → track_die.track_die_process() 航迹消亡（清除超时未更新的航迹） - → track_init.track_init_process_logic() 航迹起始（从未关联点迹创建新航迹，逻辑法） -``` - -### TAS 处理流水线（model=3） - -``` -data_preprocess() → 缓存点迹到 Data_buffer_tas - → dot_coh_tas.dot_coh_tas_process() TAS 点迹凝聚 - → track_asso_tas.track_asso_process_tas() TAS 航迹关联 - → track_die_tas.track_die_process_tas() TAS 航迹消亡 -``` - -### 核心算法模块 - -| 文件 | 职责 | -|------|------| -| `kalman.cpp/h` | 卡尔曼滤波（两点/三点初始化）、EKF（含多普勒）、IMM 统计距离计算 | -| `track_asso.cpp/h` | TWS 航迹关联：IMM 模型交互→三个子模型滤波→模型输出。3 个 Singer 模型（不同 Q 值应对不同机动性） | -| `track_asso_tas.cpp/h` | TAS 航迹关联（独立实现，因 TAS 数据率不同） | -| `track_init.cpp/h` | 逻辑法航迹起始：点迹→航迹头关联→临时航迹转可靠航迹（需满足角度/波门条件） | -| `track_init_direct_tracking.cpp/h` | 引导跟踪航迹起始 | -| `dot_coh.cpp/h` | TWS 点迹凝聚：滑窗方式在距离-方位-速度维聚合相近点迹 | -| `dot_coh_tas.cpp/h` | TAS 点迹凝聚 | -| `track_die.cpp/h` | TWS 航迹消亡：持续外推超过阈值（4 圈）则消亡 | -| `track_die_tas.cpp/h` | TAS 航迹消亡（5 圈阈值） | -| `tas_ctrl.cpp/h` | TAS 波束调度：管理 TAS 目标队列（最多 4 个），自动/手动开启和退出 | -| `track_index_mangement.cpp/h` | 航迹号分配管理 | -| `coor_trans.cpp/h` | 坐标转换（雷达球坐标系 ↔ 直角坐标系） | - -### 关键数据结构 - -- **`DataRev`**（`data_process_class_dll.h`）— 输入：雷达原始点迹，含距离/方位/俯仰/速度/幅度/SNR/RCS/波位等 -- **`PointRecv`**（`struct.h`）— 内部：预处理后的点迹，增加高度（由距离×sin(俯仰)计算）、使用标志 -- **`Trust_Track`**（`struct.h`）— 内部：可靠航迹。含 IMM 全状态：X[6]/P[6][6] 总估计 + 三个子模型的 X1/P1、X2/P2、X3/P3 + 模型概率 u[3] -- **`Temp_track`**（`struct.h`）— 内部：临时航迹。简化卡尔曼 X[4]/P[4][4]（无加速度分量） -- **`Track`**（`data_process_class_dll.h`）— 输出：航迹信息（位置/速度/预测值/航迹号/跟踪模式等），二维数组 `[MAX_TRACK_NUM][10]` 每条航迹最多 10 个历史点 -- **`RadarPara`**（`data_process_class_dll.h`）— 外部配置：雷达参数、跟踪门限、IMM 机动参数、禁止区域 -- **`TrackingBeam`**（`data_process_class_dll.h`）— 输出：TAS 波束调度指令 - -### IMM 模型说明 - -TWS 航迹关联使用 3 模型 IMM（Interacting Multiple Model）处理目标机动： - -- **Model 1**: Singer 模型，低过程噪声 Q（`Model1_Q_fast/slow`），适合匀速目标 -- **Model 2**: Singer 模型，中过程噪声 Q（`Model2_Q_fast/slow`），适合中等机动 -- **Model 3**: Singer 模型，高过程噪声 Q（`Model3_Q_fast/slow`），适合强机动 - -模型转移概率矩阵 `Pt[3][3]` 定义在 `Track_Asso` 类中。每个 CPI 周期执行：模型交互 → 各子模型卡尔曼预测+更新 → 模型概率更新 → 加权融合输出。 - -### 航迹区概念 - -按方位角将 360° 划分为 9 个航迹区（每个 40°），航迹按区组织以减少计算量。点迹区类似划分（每个区含 6 个波位）。相关参数在 `parameters.h` 中（当前版本大部分被注释，实际通过 `RadarPara` 动态配置）。 - -## 外部接口调用顺序 - -``` -1. track_process_parameters_initial() — 初始化雷达参数 -2. 每帧循环： - a. data_preprocess() — 输入原始点迹 - b. 根据返回值调用 track_process() — 执行 TWS 或 TAS 处理 - c. Beam_Ctrl() — TAS 波束调度 -3. tracking_start() / track_delete() — 手动控制（按需） -4. track_clear_all() — 清空所有航迹 -5. Data_Process_Factory::Destroy() — 销毁实例 -``` - -## 参数配置 - -关键可调参数（通过 `RadarPara` 传入或 `parameters.h` 定义）： - -- `track_start_point_num` / `track_start_threshold` — 起航所需点数和波门大小 -- `track_asso_threshold` — TWS 关联波门（统计距离阈值） -- `track_asso_threshold_tas` — TAS 关联波门 -- `TRACK_DIE_ROUND` (4) / `TRACK_DIE_ROUND_TAS` (5) — 航迹消亡圈数 -- `DOT_COH_RANGE` / `DOT_COH_V` / `DOT_COH_AZI` — 凝聚门限（距离/速度/方位） -- `TAS_QUEUE_LENGTH` (4) / `MAX_TAS_NUM` (4) — TAS 队列长度和最大目标数 -- `Model1/2/3_Q_fast/slow` — IMM Singer 模型过程噪声 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..09f1bf9 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.10) + +# 雷达数据处理动态链接库 +# 开发环境已由 Qt/qmake 迁移到 VS Code + CMake。 +# 编译器保持为 Microsoft Visual C++ Compiler 12.0 (x86)。 +project(data_process_class_dll LANGUAGES CXX) + +# 扫描模式： +# PHASE_SCANNING -> 相扫雷达（X256_PS，原 Qt 工程默认激活配置） +# MECHANICAL_SCANNING -> 机扫雷达（X256_MS） +# 用法：cmake -S . -B build -DRADAR_SCAN_MODE=PHASE_SCANNING +set(RADAR_SCAN_MODE "PHASE_SCANNING" CACHE STRING "Radar scan mode (PHASE_SCANNING or MECHANICAL_SCANNING)") +set_property(CACHE RADAR_SCAN_MODE PROPERTY STRINGS PHASE_SCANNING MECHANICAL_SCANNING) + +if(NOT RADAR_SCAN_MODE STREQUAL "PHASE_SCANNING" AND NOT RADAR_SCAN_MODE STREQUAL "MECHANICAL_SCANNING") + message(FATAL_ERROR "RADAR_SCAN_MODE 必须为 PHASE_SCANNING 或 MECHANICAL_SCANNING，当前值：${RADAR_SCAN_MODE}") +endif() + +# 导出编译命令，便于 VS Code C/C++ 插件 IntelliSense 使用。 +# Visual Studio 生成器不支持该选项，因此仅对 Makefile/Ninja 生成器开启。 +if(NOT CMAKE_GENERATOR MATCHES "Visual Studio") + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +endif() + +set(SOURCES + data_process_class_dll/data_process_class_dll.cpp + data_process_class_dll/data_process.cpp + data_process_class_dll/dot_coh.cpp + data_process_class_dll/kalman.cpp + data_process_class_dll/coor_trans.cpp + data_process_class_dll/track_asso.cpp + data_process_class_dll/track_init.cpp + data_process_class_dll/track_index_mangement.cpp + data_process_class_dll/tas_ctrl.cpp + data_process_class_dll/track_die.cpp + data_process_class_dll/dot_coh_tas.cpp + data_process_class_dll/track_asso_tas.cpp + data_process_class_dll/track_init_direct_tracking.cpp + data_process_class_dll/track_die_tas.cpp +) + +set(PUBLIC_HEADERS + data_process_class_dll/data_process_class_dll.h + data_process_class_dll/data_process_class_dll_global.h +) + +add_library(data_process_class_dll SHARED ${SOURCES}) + +# 编译 DLL 时定义导出宏；引用 DLL 的工程不应定义该宏。 +target_compile_definitions(data_process_class_dll PRIVATE + DATA_PROCESS_CLASS_DLL_LIBRARY + ${RADAR_SCAN_MODE} +) + +# 与 qmake 工程保持一致的 Windows 编译定义 +if(WIN32) + target_compile_definitions(data_process_class_dll PRIVATE + WIN32 + _WINDOWS + _UNICODE + UNICODE + NOMINMAX + ) +endif() + +# Eigen 头文件位于源码目录下（）。 +target_include_directories(data_process_class_dll PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/data_process_class_dll +) + +# 原工程使用 MSVC 2013（Visual Studio 12）编译器；保持兼容，不强制更高 C++ 标准。 +# 源码使用 C++11 特性（nullptr、类内成员初始化等），MSVC 2013 默认即可支持。 + +if(MSVC) + # 与原 qmake 生成的编译选项保持一致。 + # 注意：MSVC 2013 的 Debug 运行库与 /Zc:strictStrings 同时使用会触发 STL 编译错误， + # 原 qmake 工程也仅在 Release 配置中使用该选项。 + target_compile_options(data_process_class_dll PRIVATE + /Zc:wchar_t + /FS + $<$:/Zc:strictStrings> + ) +endif() + +# 输出目录：build/bin 下为 DLL/PDB，build/lib 下为导入库。 +set_target_properties(data_process_class_dll PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" + ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" + PDB_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" + PUBLIC_HEADER "${PUBLIC_HEADERS}" +) + +# 安装规则：DLL/导入库 -> bin/lib，公开头文件 -> include。 +include(GNUInstallDirs) +install(TARGETS data_process_class_dll + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) diff --git a/README.md b/README.md deleted file mode 100644 index 69448ec..0000000 --- a/README.md +++ /dev/null @@ -1,33 +0,0 @@ -2003雷达数据处理动态链接库程序 - -功能：航迹跟踪处理、TAS/TWS跟踪控制 - -开发环境： QT5.7 MSVC2013 c++ - -使用方法：构建项目，添加data_process_class_dll.dll文件至界面 - - - - -改进的情况： - -1. 目标凝聚中考虑360°两侧点的凝聚；（已经改20260213） - - 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); - -2. 起航的门限增大8（已经改20260213） - -3. 改了点迹凝聚的逻辑，每一包数据输入的时候滑窗凝聚（ 改了一下数据预处理函数的返回值程序才能执行，20260214） - -4. 起航的时候要删除有重叠航迹（20260215 修改了，验证中） - - - -20260228 -v1.5.1 雷达通信协议修改后的新程序、加了TAS逻辑，待测试 - - - - - - diff --git a/data_process_class_dll/data_process_class_dll.pro b/backup/data_process_class_dll/data_process_class_dll.pro similarity index 100% rename from data_process_class_dll/data_process_class_dll.pro rename to backup/data_process_class_dll/data_process_class_dll.pro diff --git a/data_process_class_dll/data_process_class_dll.pro.user b/backup/data_process_class_dll/data_process_class_dll.pro.user similarity index 100% rename from data_process_class_dll/data_process_class_dll.pro.user rename to backup/data_process_class_dll/data_process_class_dll.pro.user diff --git a/data_process_class_dll/coor_trans.cpp b/data_process_class_dll/coor_trans.cpp index 33187c9..bbe4def 100644 --- a/data_process_class_dll/coor_trans.cpp +++ b/data_process_class_dll/coor_trans.cpp @@ -1,7 +1,7 @@ #include "coor_trans.h" -#include -#include"memory.h" -#include +#include +#include +#include using namespace std; //极坐标转直角坐标 diff --git a/data_process_class_dll/coor_trans.h b/data_process_class_dll/coor_trans.h index 6aac2f1..9280888 100644 --- a/data_process_class_dll/coor_trans.h +++ b/data_process_class_dll/coor_trans.h @@ -10,6 +10,5 @@ public: void cart2polar(double x, double y, double *Range, double *Azimuth); - }; #endif // COOR_TRANS_H diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index b996ef0..978bc2c 100644 --- a/data_process_class_dll/data_process.cpp +++ b/data_process_class_dll/data_process.cpp @@ -1,7 +1,7 @@ #include "data_process.h" -#include "memory.h" -#include -#include +#include +#include +#include #include using namespace std; @@ -11,7 +11,6 @@ using namespace std; /*******************************************************************************/ /*******************************************************************************/ - //数据预处理 int Data_Process::data_preprocess(struct DataRev Data_Input[150]) { @@ -22,7 +21,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) if(Data_Input[0].point_type == 0) //tws数据 { data_num=min(Data_Input[0].Point_Sum, 150); - //qDebug() << "TARGET azi beam idx:" <().swap(Data_buffer); + //std::cout << "point_recv :" <().swap(Data_buffer); } - - if(model==1) { *Trust_track_num_Output=0; //航迹更新数 @@ -149,11 +144,11 @@ int model) Trust_Track_Output, Trust_track_num_Output, Work_Parameter); - // qDebug() << "point_recv: " <0) // { - // qDebug() << "point_recv_tas :" <().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer + std::vector().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件 &trust_track, //航迹文件 @@ -182,22 +177,18 @@ int model) latest_timestamp //最新时间戳 ); - - QVector().swap(point_recv_tas); //清除 point_recv_tas + std::vector().swap(point_recv_tas); //清除 point_recv_tas track_die_tas.track_die_process_tas( &trust_track, Track_die_Index_Output, Track_die_num_Output); - - //qDebug() << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round; + //std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round; } return 1; } - - //波束控制 void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam, struct Track Trust_Track_Output[][10], @@ -212,6 +203,8 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame { //工作参数设置 + if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9) + return -1; memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); return 0; @@ -220,11 +213,12 @@ int Data_Process::track_process_parameters_initial(struct RadarPara Radar_Parame //数据处理参数修改 int Data_Process::track_process_parameters_modify(struct RadarPara Radar_Parameter) { + if (Radar_Parameter.track_start_point_num < 3 || Radar_Parameter.track_start_point_num > 9) + return -1; memcpy(&Work_Parameter, &Radar_Parameter, sizeof(RadarPara)); return 0; } - //航迹清空函数 int Data_Process::track_clear_all(void) { @@ -236,10 +230,17 @@ int Data_Process::track_clear_all(void) trust_track.clear(); temp_track.clear(); + // 同步清理内部缓存和状态，避免重连后输出幽灵航迹/幽灵波束 + dot_coh.reset(); + track_asso.reset(); + track_asso_tas.reset(); + track_init.reset(); + tas_ctrl.reset(); + last_beam_num = INT_MAX; + return 0; } - //手动航迹删除函数 int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目 int delete_track_index[]) //手动删除的航迹号 @@ -252,13 +253,12 @@ int Data_Process:: track_delete(int delete_track_num, //手动 { trust_track[j].manual_delete_flag=1; - qDebug() << "Delete :" << trust_track[j].Track_Index; + std::cout << "Delete :" << trust_track[j].Track_Index << std::endl; } } } - return 0; } @@ -272,7 +272,6 @@ int Data_Process:: tracking_start(int track_index) //需要手动 trust_track[i].manual_tracking_flag = 1; } - return 0; } @@ -292,12 +291,8 @@ int Data_Process:: tracking_stop(int track_index) //需要手动转 } //手动打跟踪波束 -int Data_Process::tracking_point(float Azimuth)//方位角 +int Data_Process::tracking_point(float Azimuth)//方位角（当前接口未启用，保留空实现） { return 0; } - - - - diff --git a/data_process_class_dll/data_process.h b/data_process_class_dll/data_process.h index cd40c76..7d841da 100644 --- a/data_process_class_dll/data_process.h +++ b/data_process_class_dll/data_process.h @@ -12,9 +12,10 @@ #include "track_die.h" #include "track_die_tas.h" #include "tas_ctrl.h" -#include +#include +#include #include -#include + using namespace Eigen; using namespace std; @@ -29,7 +30,6 @@ public: } - //数据预处理函数 int data_preprocess(struct DataRev Data_Input[150]); @@ -49,7 +49,6 @@ public: //航迹清空函数 int track_clear_all(void); - //手动航迹删除函数 int track_delete(int delete_track_num, //手动删除的航迹数目 int delete_track_index[]); //手动删除的航迹号 @@ -69,17 +68,15 @@ public: //参数设置修改函数 int track_process_parameters_modify(struct RadarPara Radar_Parameter); - private: - QVector Data_buffer; //接收点迹 TWS 输入凝聚 - QVector Data_buffer_tas; //接收点迹 TAS 输入凝聚 - - QVector point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始 - QVector point_recv_tas; //凝聚后输出的点迹 TAS - QVector trust_track; //可靠航迹 按航迹区存入 - QVector > temp_track; //临时航迹 按临时航迹区存入 + std::vector Data_buffer; //接收点迹 TWS 输入凝聚 + std::vector Data_buffer_tas; //接收点迹 TAS 输入凝聚 + std::vector point_recv; //凝聚后输出的点迹 按点迹区存入 输入航迹关联、起始 + std::vector point_recv_tas; //凝聚后输出的点迹 TAS + std::vector trust_track; //可靠航迹 按航迹区存入 + std::vector > temp_track; //临时航迹 按临时航迹区存入 int Beam_num; //波位数 int beam_count; //波位计数 @@ -93,7 +90,7 @@ private: int last_beam_num = INT_MAX; //上一个波位号，用于判断是否搜索完一圈 - qint64 latest_timestamp = 0; //最新时间戳 + long long latest_timestamp = 0; //最新时间戳 struct RadarPara Work_Parameter; //工作参数 参数设置函数外部输入 @@ -115,6 +112,4 @@ private: }; - - #endif // DATA_PROCESS_H diff --git a/data_process_class_dll/data_process_class_dll.cpp b/data_process_class_dll/data_process_class_dll.cpp index 6fcef3e..3680fd9 100644 --- a/data_process_class_dll/data_process_class_dll.cpp +++ b/data_process_class_dll/data_process_class_dll.cpp @@ -1,4 +1,6 @@ +#ifndef DATA_PROCESS_CLASS_DLL_LIBRARY #define DATA_PROCESS_CLASS_DLL_LIBRARY +#endif #include "data_process_class_dll.h" #include "data_process.h" @@ -8,7 +10,6 @@ Data_process_class_dll::Data_process_class_dll() } */ - Data_process_class_dll *Data_Process_Factory::p = 0; Data_process_class_dll *Data_Process_Factory::GetB() diff --git a/data_process_class_dll/data_process_class_dll.h b/data_process_class_dll/data_process_class_dll.h index fe90c4d..8fad612 100644 --- a/data_process_class_dll/data_process_class_dll.h +++ b/data_process_class_dll/data_process_class_dll.h @@ -17,12 +17,12 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev float Snr; //目标信噪比 float RCS; //目标RCS float Encoder_value; //码盘值 - qint64 CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间 - qint64 GNSS_time; //GNSS时间 + long long CPI_time; //CPI时间 该波位无点迹输入时也需要给出当前时间 + long long GNSS_time; //GNSS时间 int Beam_index_azi_0; //上一个cpi波位 int Beam_index_aiz; //方位波位号（0-24） 该波位无输入点迹时 也需要给出当前波位 int Beam_index_elev; //俯仰波位号（0-13） - int PRI; //PRI(us) 给0 + int PRI; //PRI(us) 不允许为0 int Freq_index; //频点号 给0 int Point_Sum; //输入点迹总数 若该波位无点迹输入 置为0 int Point_Num; //点迹号(1,2,3,4...) @@ -34,7 +34,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT DataRev float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; - //输出航迹的数据结构 用户可见 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track { @@ -76,13 +75,11 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Track float track_rcs; //rcs int pitch_num; // 俯仰波位号 - qint64 GNSS_time; //GNSS时间 + long long GNSS_time; //GNSS时间 float speed_dim[129]; // 目标的十字星数据,目标点速度维129个点 float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; - - // 跟踪波束信息的结构体 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT TrackingBeam { @@ -106,8 +103,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT Target_direct_tracking int open_flag; // 1 引导跟踪 0 结束引导跟踪 }; - - // 雷达参数结构体 struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara { @@ -128,14 +123,12 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara float TAS_Height_4; //TAS跟踪高度4 (m) float TAS_Height_5; //TAS跟踪高度5 (m) - int track_prohibite_area_num; //禁止航迹起始的区域数目 float R_max_track_prohibited[30]; //最大距离 (m) float R_min_track_prohibited[30]; //最小距离 (m) float Azimuth_max_track_prohibited[30]; //最大方位角 (度) float Azimuth_min_track_prohibited[30]; //最小方位角 （度） - int TAS_prohibite_area_num; //禁止TAS的区域数目 float R_max_TAS_prohibited[30]; //最大距离 （m） float R_min_TAS_prohibited[30]; //最小距离 (m) @@ -144,7 +137,7 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara int cfar_th; //cfar门限 - int work_mode; //工作模式 0进程 1中程 3远程 + int work_mode; //工作模式 0进程 1中程 2远程 int north_angle; //北偏角 float V_MAX; //最大速度 @@ -154,7 +147,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara float DATA_RATE_MIDDLE; float DATA_RATE_FAR; - //数据关联参数 int track_start_point_num; //起批点数(典型值 3或4) float track_start_threshold; //起航波门大小(典型值 3) @@ -173,8 +165,6 @@ struct DATA_PROCESS_CLASS_DLLSHARED_EXPORT RadarPara }; - - class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll { @@ -187,7 +177,6 @@ public: return 0; } - //数据处理函数 virtual int track_process(struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 @@ -208,7 +197,7 @@ public: } //引导跟踪处理函数 - virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息 + virtual int direct_tracking_process(struct Target_direct_tracking Target_direct_info, //引导信息（当前接口未启用，保留空实现） struct DataRev Data_Input[150], //雷达的输入点迹 struct Track Trust_Track_Output[][10], //数据处理输出的航迹更新信息 主程序创建全局变量 数据处理函数更新其中数据 int *Trust_track_num_Output, //数据处理后输出的航迹数目 主程序创建全局变量 数据处理函数更新其中数据 @@ -220,7 +209,6 @@ public: return 0; } - //航迹清空函数 virtual int track_clear_all(void) { @@ -247,31 +235,25 @@ public: } //手动打跟踪波束 - virtual int tracking_point(float Azimuth) //方位角 + virtual int tracking_point(float Azimuth) //方位角（当前接口未启用，保留空实现） { return 0; } - - //参数设置初始化函数 virtual int track_process_parameters_initial(struct RadarPara Radar_Parameter) { return 0; } - //参数设置修改函数 virtual int track_process_parameters_modify(struct RadarPara Radar_Parameter) { return 0; } - - }; - //工厂类 用户可见 class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_Process_Factory { @@ -283,6 +265,4 @@ private: static Data_process_class_dll *p; }; - - #endif // DATA_PROCESS_CLASS_DLL_H diff --git a/data_process_class_dll/data_process_class_dll_global.h b/data_process_class_dll/data_process_class_dll_global.h index 4dcd7ca..1e39057 100644 --- a/data_process_class_dll/data_process_class_dll_global.h +++ b/data_process_class_dll/data_process_class_dll_global.h @@ -1,26 +1,12 @@ #ifndef DATA_PROCESS_CLASS_DLL_GLOBAL_H #define DATA_PROCESS_CLASS_DLL_GLOBAL_H - -#include - #if defined(DATA_PROCESS_CLASS_DLL_LIBRARY) -# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT Q_DECL_EXPORT +# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport) #else -# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT Q_DECL_IMPORT +# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllimport) #endif -//#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY) -//# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport) -//#else -//# define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport) -//#endif -//#if defined(DATA_PROCESS_CLASS_DLL_LIBRARY) -//#define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllexport) -//#else -//#define DATA_PROCESS_CLASS_DLLSHARED_EXPORT __declspec(dllimport) -//#endif #endif // DATA_PROCESS_CLASS_DLL_GLOBAL_H - diff --git a/data_process_class_dll/dot_coh.cpp b/data_process_class_dll/dot_coh.cpp index 72be4c5..39b4bf1 100644 --- a/data_process_class_dll/dot_coh.cpp +++ b/data_process_class_dll/dot_coh.cpp @@ -1,12 +1,11 @@ #include "dot_coh.h" -#include -#include"memory.h" -#include +#include +#include +#include using namespace std; - -int Dot_Coh::dot_coh_process(QVector *data_input, - QVector *point_recv, +int Dot_Coh::dot_coh_process(std::vector *data_input, + std::vector *point_recv, struct RadarPara Work_Parameter) { @@ -23,7 +22,7 @@ int Dot_Coh::dot_coh_process(QVector *data_input, float point_0_A=(*data_input)[loop_of_point].Amplitude; for (unsigned int i=loop_of_point+1;isize();i++) { - if((*data_input)[loop_of_point].Use_Flag!=1) + 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; @@ -33,7 +32,8 @@ int Dot_Coh::dot_coh_process(QVector *data_input, //凝聚条件： 距离、方位接近 if(Work_Parameter.work_mode == 0) { - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + 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 *data_input, point_0_F=(*data_input)[i].Azimuth; (*data_input)[loop_of_point].Use_Flag=1; } - else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + else if( (fabs(point_0_R-point_1_R)<=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)// { (*data_input)[i].Use_Flag=1; @@ -66,8 +66,6 @@ int Dot_Coh::dot_coh_process(QVector *data_input, (*data_input)[i].Use_Flag=1; } - - } } } @@ -76,7 +74,7 @@ int Dot_Coh::dot_coh_process(QVector *data_input, } //删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=data_input->begin(); Iter!=data_input->end();) { if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) @@ -98,21 +96,16 @@ int Dot_Coh::dot_coh_process(QVector *data_input, return 0; } - - - -int Dot_Coh::dot_coh_process_buff( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 +int Dot_Coh::dot_coh_process_buff( std::vector *data_input, //输入的点迹 + std::vector *point_recv, //输出点迹 struct RadarPara Work_Parameter //工作参数 ) { - - //1. data_input与data_input_buff进行凝聚 //1.1 data_input、data_input_buff中的数据放在一起 - QVector data_tmp; + std::vector data_tmp; for (int i=0;i *data_input, } - for (int i=0;isize();i++) { data_tmp.push_back((*data_input)[i]); @@ -137,9 +129,8 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, //1.2 把data_input、data_input_buff清空 - QVector().swap(data_input_buff); - QVector().swap(*data_input); - + std::vector().swap(data_input_buff); + std::vector().swap(*data_input); //1.3 对data_tmp进行凝聚 if(data_tmp.size()>1) @@ -155,7 +146,7 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, float point_0_A=data_tmp[loop_of_point].Amplitude; for (unsigned int i=loop_of_point+1;i *data_input, //凝聚条件： 距离、方位接近 if(Work_Parameter.work_mode == 0) { - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + 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 *data_input, point_0_F=data_tmp[i].Azimuth; data_tmp[loop_of_point].Use_Flag=1; } - else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)// + else if( (fabs(point_0_R-point_1_R)<=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)// { data_tmp[i].Use_Flag=1; @@ -206,7 +198,7 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, } //1.4 删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=data_tmp.begin(); Iter!=data_tmp.end();) { if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) @@ -233,9 +225,7 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, (*data_input).push_back(data_tmp[i]); } } - QVector().swap(data_tmp); - - + std::vector().swap(data_tmp); //2.data_input_buff数据输出给point_recv @@ -244,8 +234,7 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, (*point_recv).push_back(data_input_buff[i]); } - QVector().swap(data_input_buff); - + std::vector().swap(data_input_buff); //3.data_input数据输出给data_input_buff for (int i=0;isize();i++) @@ -253,16 +242,15 @@ int Dot_Coh::dot_coh_process_buff( QVector *data_input, data_input_buff.push_back((*data_input)[i]); } - - - return 0; } - - - Dot_Coh::Dot_Coh(void) { } + +void Dot_Coh::reset() +{ + data_input_buff.clear(); +} diff --git a/data_process_class_dll/dot_coh.h b/data_process_class_dll/dot_coh.h index a18666a..831b268 100644 --- a/data_process_class_dll/dot_coh.h +++ b/data_process_class_dll/dot_coh.h @@ -3,8 +3,7 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include -#include +#include using namespace std; @@ -14,29 +13,23 @@ class Dot_Coh public: // 点迹凝聚函数 - int dot_coh_process( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 + int dot_coh_process( std::vector *data_input, //输入的点迹 + std::vector *point_recv, //输出点迹 struct RadarPara Work_Parameter //工作参数 ); //点迹凝聚函数，缓存一帧，一边输出，一边进行滑窗凝聚 - int dot_coh_process_buff( QVector *data_input, //输入的点迹 - QVector *point_recv, //输出点迹 + int dot_coh_process_buff( std::vector *data_input, //输入的点迹 + std::vector *point_recv, //输出点迹 struct RadarPara Work_Parameter //工作参数 ); - - - //构造函数 Dot_Coh(); - - - + void reset(); private: - QVector data_input_buff; - + std::vector data_input_buff; }; diff --git a/data_process_class_dll/dot_coh_tas.cpp b/data_process_class_dll/dot_coh_tas.cpp index fa8a820..b6e30e8 100644 --- a/data_process_class_dll/dot_coh_tas.cpp +++ b/data_process_class_dll/dot_coh_tas.cpp @@ -1,13 +1,11 @@ #include "dot_coh_tas.h" -#include -#include"memory.h" -#include +#include +#include +#include using namespace std; - - -int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_input, //输入点迹 - QVector *point_recv_tas //输出点迹 +int Dot_Coh_TAS::dot_coh_tas_process(std::vector *data_input, //输入点迹 + std::vector *point_recv_tas //输出点迹 ) { if (data_input->size() == 0) { @@ -24,7 +22,7 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_in float point_0_A=(*data_input)[loop_of_point].Amplitude; for ( int i=loop_of_point+1;i<(*data_input).size();i++) { - if((*data_input)[loop_of_point].Use_Flag!=1) + 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; @@ -32,7 +30,8 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_in float point_1_A=(*data_input)[i].Amplitude; //凝聚条件： 距离、方位接近 - if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V) + 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 *data_in point_0_F=(*data_input)[i].Azimuth; (*data_input)[loop_of_point].Use_Flag=1; } - else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && fabs(point_0_F-point_1_F) <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V) + else if( (fabs(point_0_R-point_1_R)<=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) { (*data_input)[i].Use_Flag=1; @@ -51,10 +50,8 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_in } } - - //删除凝聚点 和 量程范围外点 - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=data_input->begin(); Iter!=data_input->end();) { if((*Iter).Use_Flag==1 || (*Iter).Range R_MAX ) @@ -68,14 +65,10 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_in } } - for (int i=0;isize();i++) { (*point_recv_tas).push_back((*data_input)[i]); } - - - return 1; } diff --git a/data_process_class_dll/dot_coh_tas.h b/data_process_class_dll/dot_coh_tas.h index 337f19b..c7975a6 100644 --- a/data_process_class_dll/dot_coh_tas.h +++ b/data_process_class_dll/dot_coh_tas.h @@ -2,18 +2,15 @@ #define DOT_COH_TAS_H #include "parameters.h" #include "struct.h" -#include - +#include class Dot_Coh_TAS { public: // 点迹凝聚函数 - int dot_coh_tas_process(QVector *data_input, //输入点迹 - QVector *point_recv_tas //输出点迹 + int dot_coh_tas_process(std::vector *data_input, //输入点迹 + std::vector *point_recv_tas //输出点迹 ); }; - - #endif // DOT_COH_TAS_H diff --git a/data_process_class_dll/kalman.cpp b/data_process_class_dll/kalman.cpp index 97f3bf4..01f42af 100644 --- a/data_process_class_dll/kalman.cpp +++ b/data_process_class_dll/kalman.cpp @@ -1,14 +1,18 @@ #include "kalman.h" #include "coor_trans.h" #include "parameters.h" -#include -#include "memory.h" -#include +#include +#include +#include using namespace std; - - +static double wrapAnglePi(double a) +{ + while (a > PI) a -= 2*PI; + while (a < -PI) a += 2*PI; + return a; +} void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]) @@ -31,7 +35,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta); R[1][0]=R[0][1]; - P[0][0]=R[0][0]; P[0][1]=R[0][0]/T; P[0][2]=R[0][1]; @@ -47,7 +50,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub P[2][2]=R[1][1]; P[2][3]=R[1][1]/T; - P[3][0]=R[0][1]/T; P[3][1]=2*R[0][1]/pow(T,2); P[3][2]=R[1][1]/T; @@ -55,7 +57,6 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub } - double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T) { @@ -111,8 +112,6 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T) Matrix2d S; S=H*P_pred*H.transpose()+R; - - //d Vector2d Z_presnet= Vector2d(Z[0],Z[1]); Vector2d delta_z; @@ -123,8 +122,6 @@ double kalman::d_cal_track_init(double Z[2],double X[4],double P[4][4],double T) } - - void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]) { double x0=Z0[0]; @@ -141,8 +138,6 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d X[4]=(y2-y1)/T2 ; X[5]=((y2-y1)/T2-(y1-y0)/T1)/((T2+T1)/2); - - double R0[2][2]; double R1[2][2]; double R2[2][2]; @@ -157,14 +152,12 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d R0[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta); R0[1][0]=R0[0][1]; - Coor_trans.cart2polar(Z1[0],Z1[1],&rho,&theta); R1[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta)); R1[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta)); R1[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta); R1[1][0]=R1[0][1]; - Coor_trans.cart2polar(Z2[0],Z2[1],&rho,&theta); R2[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta)); R2[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta)); @@ -200,9 +193,6 @@ void kalman::kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], d }; - - - double kalman::d_cal(double Z[2],double X[6],double P[6][6]) { VectorXd X_pred(6); @@ -241,13 +231,11 @@ double kalman::d_cal(double Z[2],double X[6],double P[6][6]) Matrix2d S; S=H*P_pred*H.transpose()+R; - //d Vector2d delta_z; delta_z=Z_mea-Z_pred; double d=delta_z.transpose()*S.inverse()*delta_z; - return d; }; @@ -273,7 +261,6 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6] for (int j=0;j<6;j++) P1(i,j)=P[i][j]; - VectorXd X_pred(6); MatrixXd P_pred(6,6); X_pred = F1*X1; @@ -318,15 +305,14 @@ double kalman::d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6] Vector3d Z_mea(Z[0],Z[1],Z[2]); Vector3d delta_z; delta_z=Z_mea-Z_pred; + delta_z(1)=wrapAnglePi(delta_z(1)); delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind; double d=delta_z.transpose()*S.inverse()*delta_z; return d; - } - double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind) { @@ -348,7 +334,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2], for (int j=0;j<6;j++) P1(i,j)=P[i][j]; - VectorXd X_pred(6); MatrixXd P_pred(6,6); Vector3d Z_mea(Z[0],Z[1],vr); @@ -356,7 +341,6 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2], X_pred = F1*X1; P_pred = F1*P1*F1.transpose()+Q1; - //Z(k+1|k) double x=X_pred[0]; double vx=X_pred[1]; @@ -396,18 +380,16 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2], Matrix3d S; S=H*P_pred*H.transpose()+R; - // bind_speed double v_bind=Bind_speed(prt, freq_ind); - //d Vector3d delta_z; delta_z=Z_mea-Z_pred; + delta_z(1)=wrapAnglePi(delta_z(1)); delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind; double d=delta_z.transpose()*S.inverse()*delta_z; - return d; }; @@ -435,7 +417,6 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6] VectorXd X1_pred(6); MatrixXd P1_pred(6,6); - X1_pred = F1*X1; P1_pred = F1*P1*F1.transpose()+Q1; @@ -446,11 +427,10 @@ void kalman::kalman_pred(double F[6][6], double Q[6][6] ,double X[6],double P[6] for(int j=0;j<6;j++) P_pred[i][j]=P1_pred(i,j); - }; void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],double P_cur[6][6],double Z[3], - double X_filter[6],double P_filter[6][6],double S_filter[2][2], + double X_filter[6],double P_filter[6][6],double S_filter[3][3], double prt,double freq_ind ) { MatrixXd F1(6,6); @@ -474,12 +454,9 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d VectorXd X_pred(6); MatrixXd P_pred(6,6); - X_pred = F1*X1; P_pred = F1*P1*F1.transpose()+Q1; - - Vector3d Z_mea(Z[0],Z[1],Z[2]); Vector3d Z_pred; @@ -521,10 +498,10 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d //bind_speed double v_bind=Bind_speed(prt, freq_ind); - //d Vector3d delta_z; delta_z=Z_mea-Z_pred; + delta_z(1)=wrapAnglePi(delta_z(1)); delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind; //X(k+1|k+1) @@ -536,10 +513,8 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d MatrixXd I; I.setIdentity(6, 6); - P=(I-K*H)*P_pred; - for (int i=0;i<6;i++) X_filter[i]=X(i); @@ -547,13 +522,12 @@ void kalman::kalman_filter_EKF(double F[6][6], double Q[6][6], double X_cur[6],d for (int j=0;j<6;j++) P_filter[i][j]=P(i,j); - for (int i=0;i<2;i++) - for (int j=0;j<2;j++) + for (int i=0;i<3;i++) + for (int j=0;j<3;j++) S_filter[i][j]=S(i,j); } - void kalman::kalman_filter(double F[6][6], double Q[6][6], double X_cur[6],double P_cur[6][6], double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]) @@ -579,16 +553,11 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6], VectorXd X_pred(6); MatrixXd P_pred(6,6); - X_pred = F1*X1; P_pred = F1*P1*F1.transpose()+Q1; - - Vector2d Z_mea(Z[0],Z[1]); - - //Z(k+1|k) MatrixXd H(2,6); H<< 1,0,0,0,0,0, @@ -614,7 +583,6 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6], Matrix2d S; S=H*P_pred*H.transpose()+R; - //kalmam gain MatrixXd K; @@ -629,10 +597,8 @@ void kalman::kalman_filter(double F[6][6], double Q[6][6], MatrixXd I; I.setIdentity(6, 6); - P=(I-K*H)*P_pred; - for (int i=0;i<6;i++) X_filter[i]=X(i); @@ -718,6 +684,7 @@ double kalman::d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],doubl Vector3d Z_presnet= Vector3d(Z[0],Z[1],Z[2]); Vector3d delta_z; delta_z=Z_presnet-Z_pred; + delta_z(1)=wrapAnglePi(delta_z(1)); delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind; double d=delta_z.transpose()*S.inverse()*delta_z; @@ -754,7 +721,6 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4] Vector3d Z_pred; Z_pred=H*X_pred; - //P(k+1|k) Matrix2d Q; Q<< 0.03*0.03, 0, @@ -803,6 +769,7 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4] Vector3d Z_presnet= Vector3d(Z[0],Z[1],vr); Vector3d delta_z; delta_z=Z_presnet-Z_pred; + delta_z(1)=wrapAnglePi(delta_z(1)); delta_z(2)=delta_z(2)-Round(delta_z(2)/v_bind)*v_bind; double d=delta_z.transpose()*S.inverse()*delta_z; @@ -810,11 +777,12 @@ double kalman::d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4] }; - double kalman::Bind_speed(double prt,double freq_ind) { double freq=FREQ0+freq_ind*0.02; + if (prt <= 0.0 || freq <= 0.0) + return 1.0; // 避免除零；正常流程 PRI 不允许为 0 return 150000.0/(freq*prt); diff --git a/data_process_class_dll/kalman.h b/data_process_class_dll/kalman.h index 9dcc9e8..3e244ba 100644 --- a/data_process_class_dll/kalman.h +++ b/data_process_class_dll/kalman.h @@ -2,12 +2,11 @@ #define KALMAN_H #include "parameters.h" #include "struct.h" -#include +#include #include using namespace Eigen; using namespace std; - class kalman { public: @@ -16,7 +15,6 @@ public: void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器 - double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind); @@ -34,7 +32,7 @@ public: void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]); void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3], - double X_filter[6],double P_filter[6][6],double S_filter[2][2], + double X_filter[6],double P_filter[6][6],double S_filter[3][3], double prt,double freq_ind ); double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度 diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index d068d76..896bc26 100644 --- a/data_process_class_dll/struct.h +++ b/data_process_class_dll/struct.h @@ -2,7 +2,7 @@ #define STRUCT_H #pragma once -#include +#include using namespace std; /********************************* 数据处理结构体 **********************************************/ @@ -16,8 +16,8 @@ struct PointRecv double Height; //高度 double snr; //信噪比 double RCS; //目标RCS - qint64 CPI_Time; //CPI时间 - qint64 GNSS_time; //GNSS时间 + long long CPI_Time; //CPI时间 + long long GNSS_time; //GNSS时间 int Point_index; //点迹号 1~50 int Point_Sum; //点迹总数 int Use_Flag; //点迹使用标志 1使用 0未使用 @@ -32,7 +32,6 @@ struct PointRecv float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; - // 可靠航迹数据结构 struct Trust_Track { @@ -41,8 +40,8 @@ struct Trust_Track double Amplitude; //幅度 double Height; //高度 - qint64 T_track; //航迹时间 - qint64 GNSS_time; //GNSS时间 + long long T_track; //航迹时间 + long long GNSS_time; //GNSS时间 int Track_Sum; //航迹总数 int Point_Index; //该航迹上的第几个点 @@ -62,8 +61,7 @@ struct Trust_Track int Track_section_idx; //航迹区号 - QVector Hight_smooth; //高度平滑 - + std::vector Hight_smooth; //高度平滑 double range_point; //关联上的点信息（距离、方位、俯仰） double azi_point; @@ -92,8 +90,6 @@ struct Trust_Track }; - - //临时航迹结构体 struct Temp_track { @@ -108,8 +104,8 @@ struct Temp_track double snr; //信噪比 double RCS; //RCS double Amp; //幅度 - qint64 T; //时间戳 - qint64 GNSS_time; //GNSS时间戳 + long long T; //时间戳 + long long GNSS_time; //GNSS时间戳 double d; //关联上的点的d @@ -124,11 +120,6 @@ struct Temp_track float range_dim[8]; // 目标的十字星数据,目标点距离维8个点 }; - - - - - // TAS跟踪目标结构体 struct Tracking_Target { @@ -137,7 +128,6 @@ struct Tracking_Target }; - //引导跟踪目标结构体 struct Direct_Tracking_Target { @@ -150,6 +140,4 @@ struct Direct_Tracking_Target }; - - #endif // STRUCT_H diff --git a/data_process_class_dll/tas_ctrl.cpp b/data_process_class_dll/tas_ctrl.cpp index 8bee9d2..7969f9e 100644 --- a/data_process_class_dll/tas_ctrl.cpp +++ b/data_process_class_dll/tas_ctrl.cpp @@ -1,17 +1,11 @@ -#include +#include "tas_ctrl.h" #include "coor_trans.h" -#include -#include"memory.h" -#include -#include +#include +#include +#include + using namespace std; - - - - - - TAS_Ctrl::TAS_Ctrl() { memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target)); @@ -19,13 +13,22 @@ TAS_Ctrl::TAS_Ctrl() tas_target_num=0; } -void TAS_Ctrl::tas_ctrl_process(QVector *trust_track, +void TAS_Ctrl::reset() +{ + memset(tas_target_queue,0,sizeof(tas_target_queue)); + tas_target_num=0; +} + + +void TAS_Ctrl::tas_ctrl_process(std::vector *trust_track, struct TrackingBeam *Tracking_beam, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, - qint64 latest_timestamp) + long long latest_timestamp) { + if (Trust_track_num_Output == 0) return; + *Trust_track_num_Output = 0; tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter); tas_beam_output(trust_track,Tracking_beam,latest_timestamp); @@ -40,8 +43,7 @@ void TAS_Ctrl::tas_ctrl_process(QVector *trust_track, memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target)); }; - -void TAS_Ctrl::tas_target_add(QVector *trust_track, +void TAS_Ctrl::tas_target_add(std::vector *trust_track, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter) @@ -67,6 +69,9 @@ void TAS_Ctrl::tas_target_add(QVector *trust_track, { if(tas_target_queue[j].empty_flag==0)// && j%2==0) { + if (*Trust_track_num_Output >= MAX_TRACK_NUM) + break; + //插入跟踪队列 tas_target_queue[j].Index=(*trust_track)[i].Track_Index; tas_target_queue[j].empty_flag=1; @@ -102,12 +107,8 @@ void TAS_Ctrl::tas_target_add(QVector *trust_track, } } - - }; - - //int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter) //{ @@ -128,7 +129,6 @@ void TAS_Ctrl::tas_target_add(QVector *trust_track, //}; - int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter) { @@ -142,10 +142,9 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct return 0; - } -void TAS_Ctrl::tas_target_del(QVector *trust_track, +void TAS_Ctrl::tas_target_del(std::vector *trust_track, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter) @@ -224,7 +223,6 @@ void TAS_Ctrl::tas_target_del(QVector *trust_track, // } - }; //int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter) @@ -246,18 +244,17 @@ void TAS_Ctrl::tas_target_del(QVector *trust_track, //}; - - -void TAS_Ctrl::tas_beam_output(QVector *trust_track, +void TAS_Ctrl::tas_beam_output(std::vector *trust_track, struct TrackingBeam *Tracking_beam, - int latest_timestamp) + long long latest_timestamp) { if(tas_target_queue[0].empty_flag==1) { - double H_track; - double X_now[6]; - int CPI_time = 0; + double H_track = 0; + double X_now[6] = {0}; + long long CPI_time = 0; + bool found_track = false; for (int i=0;isize();i++ ) { if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) @@ -266,9 +263,16 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, 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; } } + if (!found_track) + { + Tracking_beam->open_flag = 0; + return; + } + float delta_T = (latest_timestamp - CPI_time) / 1000.0f; //预测目标位置 计算跟踪波束波位号 俯仰角 @@ -286,7 +290,6 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, //目标俯仰角 double elev=asin(H_track/range)/PI*180; - // if(elev<=0) // elev=0; // else if(elev>=40) @@ -294,7 +297,6 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, // else // elev=elev; - Tracking_beam->Elev=elev; //跟踪波束类型 @@ -306,7 +308,7 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, //跟踪波束开关开启 Tracking_beam->open_flag=1; - //qDebug() << "TAS: range: " <Range << "azi: " <Azi <<"pit: " <Elev; + //std::cout << "TAS: range: " <Range << "azi: " <Azi <<"pit: " <Elev; } else { @@ -314,6 +316,4 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, Tracking_beam->open_flag=0; } - - } diff --git a/data_process_class_dll/tas_ctrl.h b/data_process_class_dll/tas_ctrl.h index 27580ef..a73b2cb 100644 --- a/data_process_class_dll/tas_ctrl.h +++ b/data_process_class_dll/tas_ctrl.h @@ -3,43 +3,42 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include +#include using namespace std; - - class TAS_Ctrl { public: TAS_Ctrl(); - void tas_ctrl_process( QVector *trust_track, + void reset(); + + void tas_ctrl_process( std::vector *trust_track, struct TrackingBeam *Tracking_beam, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, - qint64 latest_timestamp); - + long long latest_timestamp); private: //添加TAS目标 - void tas_target_add(QVector *trust_track, + void tas_target_add(std::vector *trust_track, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter); //删除TAS目标 - void tas_target_del(QVector *trust_track, + void tas_target_del(std::vector *trust_track, struct Track Trust_Track_Output[][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter); //TAS队列信息输出 - void tas_beam_output(QVector *trust_track, + void tas_beam_output(std::vector *trust_track, struct TrackingBeam *Tracking_beam, - int latest_timestamp); + long long latest_timestamp); //TAS 自动开启条件 int tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter); diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index cb1832e..69572b8 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -1,19 +1,18 @@ #include "track_asso.h" #include "kalman.h" #include "coor_trans.h" -#include -#include "memory.h" -#include +#include +#include +#include #include using namespace Eigen; using namespace std; - // //////////////////////////// 用point_recv中点迹与trust_track中的点迹进行关联， ///////////// // //////////////////////////// 航迹更新结果通过Trust_Track_Output[MAX_TRACK_NUM][10]和 Trust_track_num_Output输出 ///////////// -int Track_Asso:: track_asso_process(QVector *point_recv, //输入点迹 - QVector *trust_track, //航迹文件 +int Track_Asso:: track_asso_process(std::vector *point_recv, //输入点迹 + std::vector *trust_track, //航迹文件 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 struct RadarPara Work_Parameter //雷达参数 @@ -28,7 +27,6 @@ int Track_Asso:: track_asso_process(QVector *point_recv point_process[n-1].point_section_asso = 1; } - //IMM算法 model_interaction(trust_track); @@ -36,9 +34,8 @@ int Track_Asso:: track_asso_process(QVector *point_recv model_output(trust_track); - //删除关联上的点 - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter= point_process.begin(); Iter!=point_process.end();) { if((*Iter).Use_Flag==1 ) @@ -53,7 +50,7 @@ int Track_Asso:: track_asso_process(QVector *point_recv } //剩余点重新存入点迹 - QVector().swap((*point_recv)); + std::vector().swap((*point_recv)); for (int i=0;i *point_recv } //point_process清空 - QVector().swap(point_process); + std::vector().swap(point_process); //输出航迹 for (int i=0;isize();i++) @@ -93,18 +90,15 @@ int Track_Asso:: track_asso_process(QVector *point_recv 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].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].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; - double Direction_Angle; - Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]); - if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;} - if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;} + 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; //关联点信息 @@ -140,10 +134,9 @@ int Track_Asso:: track_asso_process(QVector *point_recv } //遍历所有航迹 进行多模型交互 -void Track_Asso:: model_interaction(QVector *trust_track) +void Track_Asso:: model_interaction(std::vector *trust_track) { - for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) { @@ -159,6 +152,9 @@ void Track_Asso:: model_interaction(QVector *trust_track) c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2]; c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2]; c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2]; + if (c[0] <= 1e-12) c[0] = 1e-12; + if (c[1] <= 1e-12) c[1] = 1e-12; + if (c[2] <= 1e-12) c[2] = 1e-12; u_t[0][0]=Pt[0][0]*u_last[0]/c[0]; u_t[1][0]=Pt[1][0]*u_last[1]/c[0]; @@ -170,7 +166,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) u_t[1][2]=Pt[1][2]*u_last[1]/c[2]; u_t[2][2]=Pt[2][2]*u_last[2]/c[2]; - double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6]; for (int i=0;i<6;i++) { @@ -179,7 +174,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) X3[i]=(*trust_track)[loop_of_track].X3[i]; } - for (int i=0;i<6;i++) { Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i]; @@ -187,7 +181,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i]; } - double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6]; double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6]; double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6]; @@ -195,7 +188,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) double X12[6][6],X22[6][6],X32[6][6]; double X13[6][6],X23[6][6],X33[6][6]; - for (int i=0;i<6;i++) { @@ -211,7 +203,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) } - for (int i=0;i<6;i++) for(int j=0;j<6;j++) { @@ -226,7 +217,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j]; } - double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6]; for (int i=0;i<6;i++) for(int j=0;j<6;j++) @@ -253,7 +243,6 @@ void Track_Asso:: model_interaction(QVector *trust_track) } } - // 产生 F Q 矩阵 void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter) { @@ -281,8 +270,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub F[5][4]=0; F[5][5]=exp(-alpha*delta_T); - - //模型1 //Q double q1=Work_Parameter.Model1_Q_slow; @@ -301,7 +288,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231; Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331; - //模型2 //Q double q2; @@ -343,7 +329,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232; Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332; - //模型3 //Q double q3; @@ -381,7 +366,6 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)); double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T)); - memset(Q3,0,36*sizeof(double)); Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133; Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233; @@ -391,10 +375,8 @@ void Track_Asso::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], doub Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233; Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333; - } - // 计算三个 d void Track_Asso::IMM_d_cal(double v_track, double X1[6], double P1[6][6], @@ -415,10 +397,7 @@ void Track_Asso::IMM_d_cal(double v_track, } - - - -void Track_Asso:: model_filter(QVector *trust_track,struct RadarPara Work_Parameter) +void Track_Asso:: model_filter(std::vector *trust_track,struct RadarPara Work_Parameter) { //存储关联信息 @@ -431,7 +410,7 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP int point_index; int track_index; }; - QVector associated_info; + std::vector associated_info; //遍历所有点迹航迹 计算点迹和航迹的统计距离 for (int loop_of_track = 0; loop_of_tracksize();loop_of_track++) @@ -443,7 +422,6 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP for (int loop_of_point = 0; loop_of_point *trust_track,struct RadarP // d_h = 1; // } -// qDebug() << delta_T<<" "< *trust_track,struct RadarP //小于关联门限 保存关联信息 if( - ( r_track<=500 && (d1*d1500&&r_track<1000) && (d1*d1500&&r_track<1000) && (d1*d1=1000 && (d1*d1 *trust_track,struct RadarP // } } - //最近邻法关联 int associated_num=associated_info.size(); for (int i=0;isize();i++) @@ -632,30 +609,57 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP 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); + if (delta_T <= 0) + { + // 乱序/重复时间戳：不作为有效关联，不标记点迹已使用、不刷新航迹新鲜度 + for (int ii=0;ii0) { double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6]; - double S1[2][2],S2[2][2],S3[2][2]; + double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}}; kalman Kalman; Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind ); - - Matrix2f S1_out,S2_out,S3_out; - for (int ii=0;ii<2;ii++) - for (int jj=0;jj<2;jj++) + Matrix3f S1_out,S2_out,S3_out; + 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]; } double det_S1=S1_out.determinant(); - double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1); + double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0; double det_S2=S2_out.determinant(); - double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2); + double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0; double det_S3=S3_out.determinant(); - double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3); + double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0; //更新模型概率 double u_last[3]; @@ -668,9 +672,14 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2]; c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2]; - (*trust_track)[track_index-1].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[track_index-1].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[track_index-1].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]; + if (u_den > 1e-12) + { + (*trust_track)[track_index-1].u[0]=Possibility1*c[0]/u_den; + (*trust_track)[track_index-1].u[1]=Possibility2*c[1]/u_den; + (*trust_track)[track_index-1].u[2]=Possibility3*c[2]/u_den; + } + // 若分母异常，保持上一拍模型概率 //更新 X1 X2 X3 P1 P2 P3 for(int ii=0;ii<6;ii++) @@ -691,7 +700,6 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP (*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_Time; (*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time; - //更新关联上的点迹信息 (*trust_track)[track_index-1].range_point=point_process[point_index-1].Range; (*trust_track)[track_index-1].azi_point=point_process[point_index-1].Azimuth/PI*180; @@ -746,7 +754,6 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP } } - //未关联上的航迹 进行外推 tws for (int i =0; i<(*trust_track).size();i++ ) { @@ -797,7 +804,6 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP 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].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间 @@ -805,12 +811,9 @@ void Track_Asso:: model_filter(QVector *trust_track,struct RadarP } } - } - - -void Track_Asso:: model_output(QVector *trust_track) +void Track_Asso:: model_output(std::vector *trust_track) { for(int loop_of_track=0; loop_of_tracksize(); loop_of_track++) { @@ -861,7 +864,6 @@ void Track_Asso:: model_output(QVector *trust_track) for (int j=0;j<6;j++) P_filter[i][j]=u_now[0]*(P1_filter[i][j]+X1X[i][j])+u_now[1]*(P2_filter[i][j]+X2X[i][j])+u_now[2]*(P3_filter[i][j]+X3X[i][j]); - //本地航迹文件更新 (*trust_track)[loop_of_track].X[0]=X_filter[0]; //位置 速度 (*trust_track)[loop_of_track].X[1]=X_filter[1]; @@ -910,22 +912,18 @@ void Track_Asso:: model_output(QVector *trust_track) } - - //高度维更新 void Track_Asso:: track_hight_update(int updata_track_index, //更新的航迹号 int asso_point_index, //点迹号 - QVector *trust_track //航迹 + std::vector *trust_track //航迹 ) { - if(updata_track_index<=trust_track->size() && asso_point_index<=point_process.size()) { (*trust_track)[updata_track_index-1].Hight_smooth.push_back(point_process[asso_point_index-1].Height); - int height_win_length; if((*trust_track)[updata_track_index-1].range_point <= 1000) @@ -971,12 +969,15 @@ void Track_Asso:: track_hight_update(int updata_ } }; - Track_Asso::Track_Asso() { Pt[0][0]=0.8;Pt[0][1]=0.15;Pt[0][2]=0.05; //IMM 转移概率 Pt[1][0]=0.3;Pt[1][1]=0.4;Pt[1][2]=0.3; Pt[2][0]=0.05;Pt[2][1]=0.15;Pt[2][2]=0.8; - }; + +void Track_Asso::reset() +{ + point_process.clear(); +} diff --git a/data_process_class_dll/track_asso.h b/data_process_class_dll/track_asso.h index 6e60855..a5d6441 100644 --- a/data_process_class_dll/track_asso.h +++ b/data_process_class_dll/track_asso.h @@ -1,44 +1,39 @@ #ifndef TRACK_ASSO_H #define TRACK_ASSO_H - #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include -#include +#include + using namespace std; - - /******************************* 航迹关联类 **************************************************/ class Track_Asso { public: - int track_asso_process(QVector *point_recv, //点迹文件 - QVector *trust_track, //航迹文件 + int track_asso_process(std::vector *point_recv, //点迹文件 + std::vector *trust_track, //航迹文件 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 struct RadarPara Work_Parameter //工作参数 ); - Track_Asso(); + void reset(); private: //要处理的点迹 - QVector point_process; - + std::vector point_process; //IMM - void model_interaction(QVector *trust_track); //模型交互 + void model_interaction(std::vector *trust_track); //模型交互 - - void model_filter(QVector *trust_track, //滤波 + void model_filter(std::vector *trust_track, //滤波 struct RadarPara Work_Parameter ); - void model_output(QVector *trust_track ); //模型输出 + void model_output(std::vector *trust_track ); //模型输出 void IMM_d_cal(double v_track, double X1[6], double P1[6][6], @@ -53,12 +48,10 @@ private: double Pt[3][3]; //模型转移概率 - - //高度维更新 void track_hight_update(int updata_track_index, //更新的航迹号 int asso_point_index, //点迹号 - QVector *trust_track //航迹 + std::vector *trust_track //航迹 ); }; #endif // TRACK_ASSO_H diff --git a/data_process_class_dll/track_asso_direct_tracking.cpp b/data_process_class_dll/track_asso_direct_tracking.cpp index 9f7dcc0..c25dab7 100644 --- a/data_process_class_dll/track_asso_direct_tracking.cpp +++ b/data_process_class_dll/track_asso_direct_tracking.cpp @@ -1,15 +1,14 @@ #include "track_asso_direct_tracking.h" #include "kalman.h" #include "coor_trans.h" -#include -#include "memory.h" -#include +#include +#include +#include #include using namespace Eigen; using namespace std; - -int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector *point_recv, //点迹 +int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(std::vector *point_recv, //点迹 Trust_Track *trust_track, //航迹 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 @@ -31,8 +30,7 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector().swap(point_process); - + std::vector().swap(point_process); //输出航迹 @@ -47,7 +45,12 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector 0.0) ? (*trust_track).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].Track_Index=(*trust_track).Track_Index; Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V = sqrt((*trust_track).X[1]*(*trust_track).X[1]+(*trust_track).X[4]*(*trust_track).X[4]); @@ -58,9 +61,8 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector0&&(*trust_track).X[4]<0){Direction_Angle=Direction_Angle+2*PI;} + Direction_Angle=atan2((*trust_track).X[4], (*trust_track).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; //关联点信息 @@ -77,12 +79,9 @@ int Track_Asso_Direct_Tracking::track_asso_process_direct_tracking(QVector associated_info; - + std::vector associated_info; //航迹信息 double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; @@ -316,7 +309,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, } } - //有关联 滤波 if (associated_info.size()>0) { @@ -348,7 +340,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter); double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6]; - double S1[2][2],S2[2][2],S3[2][2]; + double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}}; kalman Kalman; // Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1); // Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2); @@ -356,20 +348,20 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind ); - Matrix2f S1_out,S2_out,S3_out; - for (int ii=0;ii<2;ii++) - for (int jj=0;jj<2;jj++) + Matrix3f S1_out,S2_out,S3_out; + 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]; } double det_S1=S1_out.determinant(); - double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1); + double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0; double det_S2=S2_out.determinant(); - double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2); + double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0; double det_S3=S3_out.determinant(); - double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3); + double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0; //更新航迹 @@ -384,9 +376,14 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2]; c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2]; - (*trust_track).u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track).u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track).u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]; + if (u_den > 1e-12) + { + (*trust_track).u[0]=Possibility1*c[0]/u_den; + (*trust_track).u[1]=Possibility2*c[1]/u_den; + (*trust_track).u[2]=Possibility3*c[2]/u_den; + } + // 若分母异常，保持上一拍模型概率 //更新 X1 X2 X3 P1 P2 P3 for(int ii=0;ii<6;ii++) @@ -406,8 +403,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, //更新航迹时间 (*trust_track).T_track = point_process[point_index-1].CPI_Time; - - //更新关联上的点迹信息 (*trust_track).range_point=point_process[point_index-1].Range; (*trust_track).azi_point=point_process[point_index-1].Azimuth/PI*180; @@ -426,7 +421,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, track_hight_update(point_index,trust_track); } - //未关联上，航迹外推 else { @@ -454,7 +448,6 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track *trust_track, }; - void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track) { @@ -502,7 +495,6 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track) for (int jj=0;jj<6;jj++) P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]); - //本地航迹文件更新 (*trust_track).X[0]=X_filter[0]; //位置 速度 (*trust_track).X[1]=X_filter[1]; @@ -515,7 +507,6 @@ void Track_Asso_Direct_Tracking::model_output(Trust_Track *trust_track) for (int jj=0;jj<6;jj++) (*trust_track).P[ii][jj]=P_filter[ii][jj]; - } // 产生 F Q 矩阵 @@ -545,8 +536,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub F[5][4]=0; F[5][5]=exp(-alpha*delta_T); - - //模型1 //Q double q1=Work_Parameter.Model1_Q_slow; @@ -565,7 +554,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231; Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331; - //模型2 //Q double q2; @@ -598,7 +586,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232; Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332; - //模型3 //Q double q3; @@ -621,7 +608,6 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)); double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T)); - memset(Q3,0,36*sizeof(double)); Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133; Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233; @@ -631,10 +617,8 @@ void Track_Asso_Direct_Tracking::IMM_F_Q_gen(double v_track, double delta_T,doub Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233; Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333; - } - // 计算三个 d void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track, double X1[6], double P1[6][6], @@ -658,8 +642,6 @@ void Track_Asso_Direct_Tracking::IMM_d_cal(double v_track, } - - //高度维更新 void Track_Asso_Direct_Tracking::track_hight_update(int asso_point_index, //点迹号 Trust_Track *trust_track //航迹 @@ -668,7 +650,6 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point (*trust_track).Hight_smooth.push_back(point_process[asso_point_index-1].Height); - int height_win_length; if((*trust_track).range_point <= 1000) { @@ -687,7 +668,6 @@ void Track_Asso_Direct_Tracking::track_hight_update(int asso_point height_win_length = H_F_WIN_LEN+6; } - double sum=0; if((*trust_track).Hight_smooth.size() +#include using namespace std; - class Track_Asso_Direct_Tracking { public: - int track_asso_process_direct_tracking(QVector *point_recv, //点迹 + int track_asso_process_direct_tracking(std::vector *point_recv, //点迹 Trust_Track *trust_track, //航迹 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 @@ -18,18 +17,16 @@ class Track_Asso_Direct_Tracking ); Track_Asso_Direct_Tracking(); - + void reset(); private: //要处理的点迹 - QVector point_process; - + std::vector point_process; //IMM void model_interaction(Trust_Track *trust_track); //模型交互 - void model_filter(Trust_Track *trust_track, //滤波 struct RadarPara Work_Parameter ); @@ -55,5 +52,4 @@ class Track_Asso_Direct_Tracking ); }; - #endif // TRACK_ASSO_DIRECT_TRACKING_H diff --git a/data_process_class_dll/track_asso_tas.cpp b/data_process_class_dll/track_asso_tas.cpp index a83b701..21964f8 100644 --- a/data_process_class_dll/track_asso_tas.cpp +++ b/data_process_class_dll/track_asso_tas.cpp @@ -1,21 +1,20 @@ #include "track_asso_tas.h" #include "kalman.h" #include "coor_trans.h" -#include -#include "memory.h" -#include +#include +#include +#include #include using namespace Eigen; using namespace std; - -int Track_Asso_Tas::track_asso_process_tas(QVector *point_recv_tas, //点迹文件 - QVector *trust_track, //航迹文件 +int Track_Asso_Tas::track_asso_process_tas(std::vector *point_recv_tas, //点迹文件 + std::vector *trust_track, //航迹文件 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 struct RadarPara Work_Parameter, //工作参数 int tas_track_idx, -qint64 latest_timestamp //最新时间戳 +long long latest_timestamp //最新时间戳 ) { @@ -34,8 +33,7 @@ qint64 latest_timestamp //最新时间戳 model_output(trust_track,tas_track_idx); //point_process清空 - QVector().swap(point_process); - + std::vector().swap(point_process); //输出航迹 for (int i=0;isize();i++) @@ -53,7 +51,12 @@ qint64 latest_timestamp //最新时间戳 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].Elevation = asin((*trust_track)[i].Height/r)/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].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]); @@ -65,9 +68,8 @@ qint64 latest_timestamp //最新时间戳 Trust_Track_Output[*Trust_track_num_Output-1][0].z=(*trust_track)[i].Height+Work_Parameter.Height; double Direction_Angle; - Direction_Angle=atan((*trust_track)[i].X[4]/(*trust_track)[i].X[1]); - if((*trust_track)[i].X[1]<0){Direction_Angle=Direction_Angle+PI;} - if((*trust_track)[i].X[1]>0&&(*trust_track)[i].X[4]<0){Direction_Angle=Direction_Angle+2*PI;} + 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; //关联点信息 @@ -93,30 +95,23 @@ qint64 latest_timestamp //最新时间戳 return 0; - } - //进行多模型交互 -void Track_Asso_Tas:: model_interaction( QVector *trust_track, int tas_track_idx) +void Track_Asso_Tas:: model_interaction( std::vector *trust_track, int tas_track_idx) { - for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) { - if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) { - // if(point_process.size()>0) // { - // qDebug() << "model_interaction point_process :" < *trust_track, in c[0]=Pt[0][0]*u_last[0]+Pt[1][0]*u_last[1]+Pt[2][0]*u_last[2]; c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2]; c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2]; + if (c[0] <= 1e-12) c[0] = 1e-12; + if (c[1] <= 1e-12) c[1] = 1e-12; + if (c[2] <= 1e-12) c[2] = 1e-12; u_t[0][0]=Pt[0][0]*u_last[0]/c[0]; u_t[1][0]=Pt[1][0]*u_last[1]/c[0]; @@ -138,7 +136,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in u_t[1][2]=Pt[1][2]*u_last[1]/c[2]; u_t[2][2]=Pt[2][2]*u_last[2]/c[2]; - double Xo1_last[6],Xo2_last[6],Xo3_last[6],X1[6],X2[6],X3[6]; for (int i=0;i<6;i++) { @@ -147,7 +144,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in X3[i]=(*trust_track)[loop_of_track].X3[i]; } - for (int i=0;i<6;i++) { Xo1_last[i]=u_t[0][0]*X1[i]+u_t[1][0]*X2[i]+u_t[2][0]*X3[i]; @@ -155,7 +151,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in Xo3_last[i]=u_t[0][2]*X1[i]+u_t[1][2]*X2[i]+u_t[2][2]*X3[i]; } - double X1_sub_Xo1[6],X2_sub_Xo1[6],X3_sub_Xo1[6]; double X1_sub_Xo2[6],X2_sub_Xo2[6],X3_sub_Xo2[6]; double X1_sub_Xo3[6],X2_sub_Xo3[6],X3_sub_Xo3[6]; @@ -163,7 +158,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in double X12[6][6],X22[6][6],X32[6][6]; double X13[6][6],X23[6][6],X33[6][6]; - for (int i=0;i<6;i++) { @@ -179,7 +173,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in } - for (int i=0;i<6;i++) for(int j=0;j<6;j++) { @@ -194,7 +187,6 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in X33[i][j]=X3_sub_Xo3[i]*X3_sub_Xo3[j]; } - double Po1_last[6][6],Po2_last[6][6],Po3_last[6][6]; for (int i=0;i<6;i++) for(int j=0;j<6;j++) @@ -222,11 +214,10 @@ void Track_Asso_Tas:: model_interaction( QVector *trust_track, in } - -void Track_Asso_Tas::model_filter( QVector *trust_track, //滤波 +void Track_Asso_Tas::model_filter( std::vector *trust_track, //滤波 struct RadarPara Work_Parameter, int tas_track_idx, - qint64 latest_timestamp //最新时间戳 + long long latest_timestamp //最新时间戳 ) { //存储关联信息 @@ -238,24 +229,24 @@ void Track_Asso_Tas::model_filter( QVector *trust double d_min; int point_index; }; - QVector associated_info; - - + std::vector associated_info; //航迹信息 - double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6]; - double T_track; - double v_track; - double r_track; - double h_track; + double X1[6] = {0},X2[6] = {0},X3[6] = {0},P1[6][6] = {{0}},P2[6][6] = {{0}},P3[6][6] = {{0}}; + double T_track = 0; + double v_track = 0; + double r_track = 0; + double h_track = 0; + bool found_tas_track = false; for (int loop_of_track=0;loop_of_tracksize();loop_of_track++) { if((*trust_track)[loop_of_track].Track_Index == tas_track_idx) { + found_tas_track = true; // if(point_process.size()>0) // { - // qDebug() << "model_filter point_process :" < *trust } - + if (!found_tas_track) + return; // 计算量测和航迹统计距离 for (int loop_of_point = 0; loop_of_point *trust 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) + continue; double h_point = point_process[loop_of_point].Height; //计算统计距离d @@ -348,17 +342,15 @@ void Track_Asso_Tas::model_filter( QVector *trust d_h = 1; } - - // qDebug() << "T d" < *trust if (associated_info.size()>0) { - // qDebug() << "TAS asoooooo222" ; + // std::cout << "TAS asoooooo222" ; //找最近点 int min_index=1; @@ -409,7 +401,7 @@ void Track_Asso_Tas::model_filter( QVector *trust IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter); double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6]; - double S1[2][2],S2[2][2],S3[2][2]; + double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}}; kalman Kalman; // Kalman.kalman_filter(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1); // Kalman.kalman_filter(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2); @@ -417,20 +409,20 @@ void Track_Asso_Tas::model_filter( QVector *trust Kalman.kalman_filter_EKF(F,Q1,X1,P1,Z,X1_filter,P1_filter,S1, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q2,X2,P2,Z,X2_filter,P2_filter,S2, prt, freq_ind ); Kalman.kalman_filter_EKF(F,Q3,X3,P3,Z,X3_filter,P3_filter,S3, prt, freq_ind ); - Matrix2f S1_out,S2_out,S3_out; - for (int ii=0;ii<2;ii++) - for (int jj=0;jj<2;jj++) + Matrix3f S1_out,S2_out,S3_out; + 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]; } double det_S1=S1_out.determinant(); - double Possibility1=1/sqrt(2*PI*det_S1)*exp(-0.5*d1); + double Possibility1=(det_S1>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S1)*exp(-0.5*d1)):0.0; double det_S2=S2_out.determinant(); - double Possibility2=1/sqrt(2*PI*det_S2)*exp(-0.5*d2); + double Possibility2=(det_S2>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S2)*exp(-0.5*d2)):0.0; double det_S3=S3_out.determinant(); - double Possibility3=1/sqrt(2*PI*det_S3)*exp(-0.5*d3); + 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;isize();i++) @@ -448,9 +440,14 @@ void Track_Asso_Tas::model_filter( QVector *trust c[1]=Pt[0][1]*u_last[0]+Pt[1][1]*u_last[1]+Pt[2][1]*u_last[2]; c[2]=Pt[0][2]*u_last[0]+Pt[1][2]*u_last[1]+Pt[2][2]*u_last[2]; - (*trust_track)[i].u[0]=Possibility1*c[0]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[i].u[1]=Possibility2*c[1]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); - (*trust_track)[i].u[2]=Possibility3*c[2]/(Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]); + double u_den = Possibility1*c[0]+Possibility2*c[1]+Possibility3*c[2]; + if (u_den > 1e-12) + { + (*trust_track)[i].u[0]=Possibility1*c[0]/u_den; + (*trust_track)[i].u[1]=Possibility2*c[1]/u_den; + (*trust_track)[i].u[2]=Possibility3*c[2]/u_den; + } + // 若分母异常，保持上一拍模型概率 //更新 X1 X2 X3 P1 P2 P3 for(int ii=0;ii<6;ii++) @@ -471,8 +468,6 @@ void Track_Asso_Tas::model_filter( QVector *trust (*trust_track)[i].T_track = point_process[point_index-1].CPI_Time; (*trust_track)[i].GNSS_time = point_process[point_index-1].GNSS_time; - - //更新关联上的点迹信息 (*trust_track)[i].range_point=point_process[point_index-1].Range; (*trust_track)[i].azi_point=point_process[point_index-1].Azimuth/PI*180; @@ -505,6 +500,8 @@ void Track_Asso_Tas::model_filter( QVector *trust if((*trust_track)[i].Track_Index == tas_track_idx) { double delta_T = (latest_timestamp - T_track)/1000.0; + if (delta_T <= 0) + continue; double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6]; IMM_F_Q_gen( v_track, delta_T, F, Q1, Q2, Q3,Work_Parameter); @@ -528,13 +525,9 @@ void Track_Asso_Tas::model_filter( QVector *trust } - - } - - -void Track_Asso_Tas::model_output(QVector *trust_track, int tas_track_idx ) +void Track_Asso_Tas::model_output(std::vector *trust_track, int tas_track_idx ) { for(int i=0; isize(); i++) if((*trust_track)[i].Track_Index==tas_track_idx) @@ -584,7 +577,6 @@ void Track_Asso_Tas::model_output(QVector *trust_track, int tas_ for (int jj=0;jj<6;jj++) P_filter[ii][jj]=u_now[0]*(P1_filter[ii][jj]+X1X[ii][jj])+u_now[1]*(P2_filter[ii][jj]+X2X[ii][jj])+u_now[2]*(P3_filter[ii][jj]+X3X[ii][jj]); - //本地航迹文件更新 (*trust_track)[i].X[0]=X_filter[0]; //位置 速度 (*trust_track)[i].X[1]=X_filter[1]; @@ -604,12 +596,8 @@ void Track_Asso_Tas::model_output(QVector *trust_track, int tas_ } - } - - - // 产生 F Q 矩阵 void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], double Q1[6][6], double Q2[6][6], double Q3[6][6],struct RadarPara Work_Parameter) { @@ -637,8 +625,6 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], F[5][4]=0; F[5][5]=exp(-alpha*delta_T); - - //模型1 //Q double q1=Work_Parameter.Model1_Q_slow; @@ -657,7 +643,6 @@ void Track_Asso_Tas::IMM_F_Q_gen(double v_track, double delta_T,double F[6][6], Q1[4][3]=q1*q121; Q1[4][4]=q1*q221; Q1[4][5]=q1*q231; Q1[5][3]=q1*q131; Q1[5][4]=q1*q231; Q1[5][5]=q1*q331; - //模型2 //Q double q2; @@ -690,7 +675,6 @@ Q2[3][3]=q2*q112; Q2[3][4]=q2*q122; Q2[3][5]=q2*q132; Q2[4][3]=q2*q122; Q2[4][4]=q2*q222; Q2[4][5]=q2*q232; Q2[5][3]=q2*q132; Q2[5][4]=q2*q232; Q2[5][5]=q2*q332; - //模型3 //Q double q3; @@ -713,7 +697,6 @@ double q223=1/(2*pow(alpha,3))*(4*exp(-alpha*delta_T)-3-exp(-2*alpha*delta_T)+2* double q233=1/(2*pow(alpha,2))*(exp(-2*alpha*delta_T)+1-2*exp(-alpha*delta_T)); double q333=1/(2*alpha)*(1-exp(-2*alpha*delta_T)); - memset(Q3,0,36*sizeof(double)); Q3[0][0]=q3*q113; Q3[0][1]=q3*q123; Q3[0][2]=q3*q133; Q3[1][0]=q3*q123; Q3[1][1]=q3*q223; Q3[1][2]=q3*q233; @@ -723,10 +706,8 @@ Q3[3][3]=q3*q113; Q3[3][4]=q3*q123; Q3[3][5]=q3*q133; Q3[4][3]=q3*q123; Q3[4][4]=q3*q223; Q3[4][5]=q3*q233; Q3[5][3]=q3*q133; Q3[5][4]=q3*q233; Q3[5][5]=q3*q333; - } - // 计算三个 d void Track_Asso_Tas::IMM_d_cal(double v_track, double X1[6], double P1[6][6], @@ -750,12 +731,10 @@ struct RadarPara Work_Parameter) } - - //高度维更新 void Track_Asso_Tas::track_hight_update(int tas_track_idx, //更新的航迹号 int asso_point_index, //点迹号 - QVector *trust_track //航迹 + std::vector *trust_track //航迹 ) { for(int i=0; isize(); i++) @@ -763,7 +742,6 @@ void Track_Asso_Tas::track_hight_update(int tas_ { (*trust_track)[i].Hight_smooth.push_back(point_process[asso_point_index-1].Height); - int height_win_length; if((*trust_track)[i].range_point <= 1000) { @@ -782,7 +760,6 @@ void Track_Asso_Tas::track_hight_update(int tas_ height_win_length = H_F_WIN_LEN+6; } - double sum=0; if((*trust_track)[i].Hight_smooth.size() -#include -using namespace std; +#include +using namespace std; /******************************* 航迹关联类 **************************************************/ class Track_Asso_Tas { public: - int track_asso_process_tas(QVector *point_recv_tas, //点迹文件 - QVector *trust_track, //航迹文件 + int track_asso_process_tas(std::vector *point_recv_tas, //点迹文件 + std::vector *trust_track, //航迹文件 struct Track Trust_Track_Output[MAX_TRACK_NUM][10], //更新航迹信息 int *Trust_track_num_Output, //更新航迹数 struct RadarPara Work_Parameter, //工作参数 int tas_track_idx, - qint64 latest_timestamp //最新时间戳 + long long latest_timestamp //最新时间戳 ); - Track_Asso_Tas(); + void reset(); private: //要处理的点迹 - QVector point_process; - + std::vector point_process; //IMM - void model_interaction(QVector *trust_track, int tas_track_idx); //模型交互 + void model_interaction(std::vector *trust_track, int tas_track_idx); //模型交互 - - void model_filter(QVector *trust_track, //滤波 + void model_filter(std::vector *trust_track, //滤波 struct RadarPara Work_Parameter, int tas_track_idx, - qint64 latest_timestamp //最新时间戳 + long long latest_timestamp //最新时间戳 ); - void model_output(QVector *trust_track, int tas_track_idx ); //模型输出 + void model_output(std::vector *trust_track, int tas_track_idx ); //模型输出 void IMM_d_cal(double v_track, double X1[6], double P1[6][6], @@ -57,12 +54,9 @@ private: //高度维更新 void track_hight_update(int tas_track_index, //更新的航迹号 int asso_point_index, //点迹号 - QVector *trust_track //航迹 + std::vector *trust_track //航迹 ); - - }; - #endif // TRACK_ASSO_TAS_H diff --git a/data_process_class_dll/track_die.cpp b/data_process_class_dll/track_die.cpp index 6f26c72..f5c8117 100644 --- a/data_process_class_dll/track_die.cpp +++ b/data_process_class_dll/track_die.cpp @@ -1,25 +1,23 @@ #include "track_die.h" #include "kalman.h" #include "coor_trans.h" -#include -#include"memory.h" -#include +#include +#include +#include using namespace std; - -void Track_Die::track_die_process( QVector *trust_track, +void Track_Die::track_die_process( std::vector *trust_track, int Track_die_Index_Output[], int *Track_die_num_Output) { - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=trust_track->begin(); Iter!=trust_track->end();) { if( ((*Iter).Track_Mode == 0 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND) // ||((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) ||(*Iter).manual_delete_flag == 1) - { //输出消亡信息 *Track_die_num_Output=*Track_die_num_Output+1; diff --git a/data_process_class_dll/track_die.h b/data_process_class_dll/track_die.h index da9fac3..51181d4 100644 --- a/data_process_class_dll/track_die.h +++ b/data_process_class_dll/track_die.h @@ -3,20 +3,15 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include +#include using namespace std; - - class Track_Die { public: - void track_die_process( QVector *trust_track, + void track_die_process( std::vector *trust_track, int Track_die_Index_Output[], int *Track_die_num_Output); - - - }; #endif // TRACK_DIE_H diff --git a/data_process_class_dll/track_die_tas.cpp b/data_process_class_dll/track_die_tas.cpp index 8e50730..5d288af 100644 --- a/data_process_class_dll/track_die_tas.cpp +++ b/data_process_class_dll/track_die_tas.cpp @@ -1,27 +1,26 @@ #include "track_die_tas.h" #include "kalman.h" #include "coor_trans.h" -#include -#include"memory.h" -#include -#include +#include +#include +#include +#include + using namespace std; - -void Track_Die_Tas::track_die_process_tas( QVector *trust_track, +void Track_Die_Tas::track_die_process_tas( std::vector *trust_track, int Track_die_Index_Output[], int *Track_die_num_Output) { - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=trust_track->begin(); Iter!=trust_track->end();) { if( ((*Iter).Track_Mode == 1 && (*Iter).Extrapolate_round >= TRACK_DIE_ROUND_TAS) ||(*Iter).manual_delete_flag == 1) - { - qDebug() << "remove tas target :" << (*Iter).Track_Index; + std::cout << "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; diff --git a/data_process_class_dll/track_die_tas.h b/data_process_class_dll/track_die_tas.h index 83b4926..0220afb 100644 --- a/data_process_class_dll/track_die_tas.h +++ b/data_process_class_dll/track_die_tas.h @@ -3,20 +3,15 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include +#include using namespace std; - - class Track_Die_Tas { public: - void track_die_process_tas( QVector *trust_track, + void track_die_process_tas( std::vector *trust_track, int Track_die_Index_Output[], int *Track_die_num_Output); - - - }; #endif // TRACK_DIE_TAS_H diff --git a/data_process_class_dll/track_index_mangement.cpp b/data_process_class_dll/track_index_mangement.cpp index 97f5f60..4ed59d3 100644 --- a/data_process_class_dll/track_index_mangement.cpp +++ b/data_process_class_dll/track_index_mangement.cpp @@ -1,13 +1,17 @@ #include "track_index_mangement.h" -#include -#include "memory.h" -#include +#include +#include +#include #include using namespace std; +void Track_Ind_Mangement::reset() +{ + lastest_index = 0; +} -int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) +int Track_Ind_Mangement ::track_ind_get( std::vector *trust_track) { int isempty = 1; @@ -17,8 +21,6 @@ int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) isempty = 0; } - - if (isempty == 1) { lastest_index = 1; @@ -30,7 +32,9 @@ int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) int List[MAX_TRACK_INDEX]={0}; for (int i=0;isize();i++) { - List[(*trust_track)[i].Track_Index-1]=1; + int ti = (*trust_track)[i].Track_Index; + if (ti >= 1 && ti <= MAX_TRACK_INDEX) + List[ti-1]=1; } //分配航迹号 @@ -45,7 +49,7 @@ int Track_Ind_Mangement ::track_ind_get( QVector *trust_track) } } - for (int i=1;i *trust_track) } } - } else if(lastest_index == MAX_TRACK_INDEX) { - for (int i=1;i *trust_track) } - return 0; } diff --git a/data_process_class_dll/track_index_mangement.h b/data_process_class_dll/track_index_mangement.h index 266a880..b73f8fd 100644 --- a/data_process_class_dll/track_index_mangement.h +++ b/data_process_class_dll/track_index_mangement.h @@ -3,21 +3,19 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include +#include using namespace std; - - class Track_Ind_Mangement { public: - int track_ind_get( QVector *trust_track); - + int track_ind_get( std::vector *trust_track); + void reset(); private: - int lastest_index; + int lastest_index = 0; }; #endif // TRACK_INDEX_MANGEMENT_H diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index 16ef4e9..1c45868 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -1,17 +1,16 @@ #include "track_init.h" #include "kalman.h" #include "coor_trans.h" -#include -#include "memory.h" -#include +#include +#include +#include #include #include using namespace std; - -int Track_Init::track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, +int Track_Init::track_init_process_logic( std::vector *point_recv, //输入点迹 + std::vector *trust_track, //可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter @@ -25,7 +24,7 @@ int Track_Init::track_init_process_logic( QVector *p // || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692 // || (*point_recv)[i].CPI_Time == 30691692 || (*point_recv)[i].CPI_Time == 30691880) // { -// qDebug() << "azi=" << (*point_recv)[i].Azimuth +// std::cout << "azi=" << (*point_recv)[i].Azimuth // << "dis=" << (*point_recv)[i].Range // << "h=" << (*point_recv)[i].Height // << "v=" << (*point_recv)[i].Velocity @@ -34,13 +33,10 @@ int Track_Init::track_init_process_logic( QVector *p // } // } - - //取出待起航的点迹数据 for (int i=0;i<(*point_recv).size();i++) point_process.push_back((*point_recv)[i]); - //临时航迹的buff_round+1 for (int i=0;isize();i++) { @@ -59,7 +55,7 @@ int Track_Init::track_init_process_logic( QVector *p point_track_head_asso(temp_track,Work_Parameter); //删除关联上的点迹 - QVector ::iterator Iter; + std::vector ::iterator Iter; for (Iter=point_process.begin(); Iter!=point_process.end();) { if((*Iter).Use_Flag==1) @@ -74,15 +70,14 @@ int Track_Init::track_init_process_logic( QVector *p } //剩余点重新存入点迹 - QVector().swap((*point_recv)); + std::vector().swap((*point_recv)); for (int i=0;i *p temp_track_tmp.GNSS_time = point_process[i].GNSS_time; temp_track_tmp.buff_round = 1; // temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1; - temp_track->push_back( QVector ()); + temp_track->push_back( std::vector ()); int n=temp_track->size(); (*temp_track)[n-1].push_back(temp_track_tmp); - // if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time == 30682320) // { -// qDebug() << "point_process_azi=" << point_process[i].Azimuth +// std::cout << "point_process_azi=" << point_process[i].Azimuth // << "point_process_dis=" << point_process[i].Range // << "point_process_h=" << point_process[i].Height // << "point_process_v=" << point_process[i].Velocity @@ -120,28 +114,23 @@ int Track_Init::track_init_process_logic( QVector *p } //清空点迹 - QVector().swap(point_process); - QVector().swap((*point_recv)); - - - + std::vector().swap(point_process); + std::vector().swap((*point_recv)); //临时航迹满足起始长度 转为可靠航迹 tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter); - } //消亡临时航迹 tmp_track_die(temp_track); - -// qDebug() << "-----------------------------------------------"; +// std::cout << "-----------------------------------------------"; // for(int i=0;isize();i++) // { // for(int j=0;j<(*temp_track)[i].size();j++) // { -// qDebug() << "temp_track_azi=" << (*temp_track)[i][j].azi +// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi // << "temp_track_dis=" << (*temp_track)[i][j].r // << "temp_track_h=" << (*temp_track)[i][j].height // << "temp_track_v=" << (*temp_track)[i][j].vr @@ -149,18 +138,14 @@ int Track_Init::track_init_process_logic( QVector *p // } // } - return 0; } - - -void Track_Init::point_temp_track_asso(QVector > *temp_track, +void Track_Init::point_temp_track_asso(std::vector > *temp_track, struct RadarPara Work_Parameter) { - //关联信息 struct Asso_info { @@ -168,7 +153,7 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t int point_idx; double d; }; - QVector asso_info; + std::vector asso_info; for ( int i=0;i> *temp_t double y2=Z[0]*sin(Z[1]); double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2); - - // if(T_track_head == 33855816 && T_point == 33859004 ) // { -// qDebug() << "r_point=" << r_point +// std::cout << "r_point=" << r_point // << "h_point=" << h_point // << "T_point=" << T_point // << "r_temp_track=" << r_temp_track @@ -235,9 +218,6 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t // } - - - if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha0)// && 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; @@ -247,7 +227,7 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t asso_info.push_back(asso_info_tmp); (*temp_track)[j][L-1].asso_flag = 1; point_process[i].Use_Flag = 1; - //qDebug() << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point; + //std::cout << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point; } } } @@ -257,7 +237,7 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t // temp_track中加入新关联上的临时航迹 for (int i = 0 ; i ()); + (*temp_track).push_back(std::vector ()); //前L个点 int L = (*temp_track)[asso_info[i].track_idx-1].size(); @@ -268,7 +248,7 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t } //关联上的点 - Temp_track asso_track_info_tmp; + Temp_track asso_track_info_tmp = {}; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; @@ -300,9 +280,8 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t (*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp); } - //temp_track中删除关联上的临时航迹 - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { if((*Iter).size()>=2) @@ -324,11 +303,7 @@ void Track_Init::point_temp_track_asso(QVector > *temp_t } } - - - - -void Track_Init::point_track_head_asso( QVector > *temp_track, +void Track_Init::point_track_head_asso( std::vector > *temp_track, struct RadarPara Work_Parameter) { @@ -338,8 +313,7 @@ void Track_Init::point_track_head_asso( QVector > *temp_ int track_idx; int point_idx; }; - QVector asso_info; - + std::vector asso_info; //关联 for ( int i=0;i> *temp_ if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2) { - //点迹信息 double x_point, y_point, vr_point; coor_trans Coor_trans; @@ -371,7 +344,7 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // if(T_track_head == 30675948) // { -// qDebug() << "r_point=" << r_point +// std::cout << "r_point=" << r_point // << "h_point=" << h_point // << "T_point=" << T_point // << "x_track_head=" << x_track_head @@ -382,8 +355,8 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // if(T_point == 30682320) // { -// qDebug() <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"; -// qDebug() << "r_point=" << r_point +// std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"; +// std::cout << "r_point=" << r_point // << "h_point=" << h_point // << "T_point=" << T_point // << "x_track_head=" << x_track_head @@ -392,7 +365,6 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // << "T_track_head=" << T_track_head; // } - //距离差 double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); @@ -408,8 +380,6 @@ void Track_Init::point_track_head_asso( QVector > *temp_ 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 @@ -422,7 +392,7 @@ void Track_Init::point_track_head_asso( QVector > *temp_ // if(T_track_head == 30675948) // { -// qDebug() << "r_point=" << r_point +// std::cout << "r_point=" << r_point // << "h_point=" << h_point // << "T_point=" << T_point // << "x_track_head=" << x_track_head @@ -437,27 +407,24 @@ void Track_Init::point_track_head_asso( QVector > *temp_ asso_info.push_back(asso_info_tmp); (*temp_track)[j][0].asso_flag = 1; point_process[i].Use_Flag = 1; - //qDebug() << "v_track_head: " << v_track_head << "vr_point: " << vr_point; + //std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point; } - - } } } - // temp_track中加入新关联上的临时航迹 for (int i = 0 ; i ()); + (*temp_track).push_back(std::vector ()); //第一个点 (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; //第二个点 - struct Temp_track asso_track_info_tmp; + struct Temp_track asso_track_info_tmp = {}; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; @@ -489,10 +456,8 @@ void Track_Init::point_track_head_asso( QVector > *temp_ } - - //temp_track中删除关联上的航迹头 - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { if((*Iter)[0].asso_flag==1) @@ -509,8 +474,6 @@ void Track_Init::point_track_head_asso( QVector > *temp_ } - - ////////////////////////////////////////////计算三点间的夹角//////////////////////////////////////////////// double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2) { @@ -520,23 +483,19 @@ double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1, return alpha; - } - - - -void Track_Init::tmp_track_to_trust_track(QVector *trust_track, - QVector > *temp_track, +void Track_Init::tmp_track_to_trust_track(std::vector *trust_track, + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter) { - QVector > Track_to_start; + std::vector > Track_to_start; // 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中 - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { int L=(*Iter).size(); @@ -556,7 +515,7 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0 && snr_flag) //按长度查找TRUST_TRACK_POINT { //加入Track_to_start中 - Track_to_start.push_back(QVector ()); + Track_to_start.push_back(std::vector ()); for (int i = 0; i *trust_track, } } + if (Track_to_start.empty()) + return; + //2.两两比较Track_to_start中的航迹信息，删除重复的航迹 int L = Work_Parameter.track_start_point_num; - for (int i=0; i *trust_track, Track_to_start[i][0].asso_flag=2; } - } - } } } } - - QVector >::iterator Iter1; + std::vector >::iterator Iter1; for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();) { if( (*Iter1)[0].asso_flag == 2 ) @@ -619,15 +580,10 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, } } - - - - - - //3.Track_to_start中剩余的航迹起始为可靠航迹 - for (int i=0; isize() *trust_track, kalman Kalman; Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); - Trust_Track trust_track_tmp; + Trust_Track trust_track_tmp = {}; memcpy(trust_track_tmp.X, X, 6*sizeof(double)); memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); @@ -684,7 +640,6 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, (*trust_track).push_back(trust_track_tmp); - //输出航迹更新信息 *Trust_track_num_Output=*Trust_track_num_Output+1; for (int j=0;j *trust_track, 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].Elevation=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/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].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].pitch_num = Track_to_start[i][j].pitch_num; std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim)); @@ -712,8 +671,6 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, 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].track_time=(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; @@ -724,7 +681,12 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, 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].elev_point=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/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].vr_point=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; @@ -735,20 +697,23 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, } } - - QVector >().swap(Track_to_start); - + std::vector >().swap(Track_to_start); } - -void Track_Init::tmp_track_die(QVector > *temp_track) +void Track_Init::tmp_track_die(std::vector > *temp_track) { - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { int n=(*Iter).size(); + if (n <= 0) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + continue; + } if( (*Iter)[n-1].buff_round >2 || n>=10) { @@ -762,13 +727,9 @@ void Track_Init::tmp_track_die(QVector > *temp_track) } }; - - - int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter) { - for (int i=0;iWork_Parameter.R_min_track_prohibited[i] && aziWork_Parameter.Azimuth_min_track_prohibited[i]) @@ -779,13 +740,15 @@ int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara return 0; - } - - Track_Init::Track_Init() { +} +void Track_Init::reset() +{ + point_process.clear(); + track_index_mangement.reset(); } diff --git a/data_process_class_dll/track_init.h b/data_process_class_dll/track_init.h index f129cb4..49d4c26 100644 --- a/data_process_class_dll/track_init.h +++ b/data_process_class_dll/track_init.h @@ -4,49 +4,44 @@ #include "track_index_mangement.h" #include "parameters.h" #include "struct.h" -#include -#include +#include + using namespace std; - - - /******************************* 航迹起始类 **************************************************/ class Track_Init { public: //航迹起始逻辑法 - int track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, + int track_init_process_logic( std::vector *point_recv, //输入点迹 + std::vector *trust_track, //可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter ); Track_Init(); + void reset(); private: + std::vector point_process; //要处理的点迹 - QVector point_process; //要处理的点迹 - - - void point_temp_track_asso(QVector > *temp_track, + void point_temp_track_asso(std::vector > *temp_track, struct RadarPara Work_Parameter); //临时航迹与点迹关联 - - void point_track_head_asso( QVector > *temp_track, + void point_track_head_asso( std::vector > *temp_track, struct RadarPara Work_Parameter); //航迹头与点迹关联 - void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, + void tmp_track_to_trust_track( std::vector *trust_track, //临时航迹转可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter); - void tmp_track_die(QVector > *temp_track);//临时航迹消亡 + void tmp_track_die(std::vector > *temp_track);//临时航迹消亡 double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 diff --git a/data_process_class_dll/track_init_direct_tracking.cpp b/data_process_class_dll/track_init_direct_tracking.cpp index 481fddc..63b2f40 100644 --- a/data_process_class_dll/track_init_direct_tracking.cpp +++ b/data_process_class_dll/track_init_direct_tracking.cpp @@ -1,16 +1,16 @@ #include "track_init_direct_tracking.h" #include "kalman.h" #include "coor_trans.h" -#include -#include "memory.h" -#include +#include +#include +#include #include #include using namespace std; -int Track_Init_Direct_Tracking::track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, +int Track_Init_Direct_Tracking::track_init_process_logic( std::vector *point_recv, //输入点迹（当前接口未启用，保留空实现） + std::vector *trust_track, //可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, @@ -18,14 +18,11 @@ int Track_Init_Direct_Tracking::track_init_process_logic( QVector ) { - - return 0; } - -void Track_Init_Direct_Tracking::point_temp_track_asso(QVector > *temp_track, +void Track_Init_Direct_Tracking::point_temp_track_asso(std::vector > *temp_track, struct RadarPara Work_Parameter) //临时航迹与点迹关联 { //关联信息 @@ -34,7 +31,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector asso_info; + std::vector asso_info; for ( int i=0;i=5 ) @@ -86,7 +82,6 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector 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; @@ -101,12 +96,10 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector ()); + (*temp_track).push_back(std::vector ()); //前L个点 int L = (*temp_track)[asso_info[i].track_idx-1].size(); @@ -117,7 +110,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { if((*Iter).size()>=2) @@ -171,7 +163,7 @@ void Track_Init_Direct_Tracking::point_temp_track_asso(QVector > *temp_track, +void Track_Init_Direct_Tracking::point_track_head_asso( std::vector > *temp_track, struct RadarPara Work_Parameter) //航迹头与点迹关联 { //关联上的信息 @@ -180,8 +172,7 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector asso_info; - + std::vector asso_info; //关联 for ( int i=0;i= 2) { - //点迹信息 double x_point, y_point, vr_point; coor_trans Coor_trans; @@ -211,7 +201,6 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector =1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) ) && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E @@ -245,24 +232,21 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector ()); + (*temp_track).push_back(std::vector ()); //第一个点 (*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]); (*temp_track)[(*temp_track).size()-1][0].asso_flag = 0; //第二个点 - struct Temp_track asso_track_info_tmp; + struct Temp_track asso_track_info_tmp = {}; asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range; asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth; asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height; @@ -293,10 +277,8 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { if((*Iter)[0].asso_flag==1) @@ -313,20 +295,23 @@ void Track_Init_Direct_Tracking::point_track_head_asso( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, +void Track_Init_Direct_Tracking::tmp_track_to_trust_track( std::vector *trust_track, //临时航迹转可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, int track_ID) { //temp_track中的临时航迹转为可靠航迹 - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { int L=(*Iter).size(); + if (L < 3) + { + ++Iter; + continue; + } double range=(*Iter)[L-1].r; double azi=(*Iter)[L-1].azi/PI*180; if( L==Work_Parameter.track_start_point_num) //按长度查找TRUST_TRACK_POINT @@ -342,7 +327,7 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector double T2=((*Iter)[L-1].T-(*Iter)[L-2].T)/1000.0; kalman Kalman; Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P); - Trust_Track trust_track_tmp; + Trust_Track trust_track_tmp = {}; memcpy(trust_track_tmp.X, X, 6*sizeof(double)); memcpy(trust_track_tmp.P, P, 6*6*sizeof(double)); @@ -426,17 +411,21 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector } } - } - -void Track_Init_Direct_Tracking::tmp_track_die(QVector > *temp_track) +void Track_Init_Direct_Tracking::tmp_track_die(std::vector > *temp_track) { - QVector >::iterator Iter; + std::vector >::iterator Iter; for (Iter=temp_track->begin(); Iter!=temp_track->end();) { int n=(*Iter).size(); + if (n <= 0) + { + temp_track->erase(Iter); + Iter=temp_track->begin(); + continue; + } if( (*Iter)[n-1].buff_round >1 || n>=10) { temp_track->erase(Iter); @@ -449,9 +438,6 @@ void Track_Init_Direct_Tracking::tmp_track_die(QVector > } } - - - double Track_Init_Direct_Tracking::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2) { double D12[2]={x1-x0,y1-y0}; @@ -460,11 +446,14 @@ double Track_Init_Direct_Tracking::alpha_cal_track_init(double x0,double y0,doub return alpha; - } Track_Init_Direct_Tracking::Track_Init_Direct_Tracking() { +} +void Track_Init_Direct_Tracking::reset() +{ + point_process.clear(); } diff --git a/data_process_class_dll/track_init_direct_tracking.h b/data_process_class_dll/track_init_direct_tracking.h index 1132e2d..b72a353 100644 --- a/data_process_class_dll/track_init_direct_tracking.h +++ b/data_process_class_dll/track_init_direct_tracking.h @@ -3,16 +3,15 @@ #include "data_process_class_dll.h" #include "parameters.h" #include "struct.h" -#include - +#include class Track_Init_Direct_Tracking { public: //航迹起始逻辑法 - int track_init_process_logic( QVector *point_recv, //输入点迹 - QVector *trust_track, //可靠航迹 - QVector > *temp_track, + int track_init_process_logic( std::vector *point_recv, //输入点迹 + std::vector *trust_track, //可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, @@ -20,35 +19,30 @@ public: ); Track_Init_Direct_Tracking(); - + void reset(); private: + std::vector point_process; //要处理的点迹 - QVector point_process; //要处理的点迹 - - - void point_temp_track_asso(QVector > *temp_track, + void point_temp_track_asso(std::vector > *temp_track, struct RadarPara Work_Parameter); //临时航迹与点迹关联 - - void point_track_head_asso( QVector > *temp_track, + void point_track_head_asso( std::vector > *temp_track, struct RadarPara Work_Parameter); //航迹头与点迹关联 - void tmp_track_to_trust_track( QVector *trust_track, //临时航迹转可靠航迹 - QVector > *temp_track, + void tmp_track_to_trust_track( std::vector *trust_track, //临时航迹转可靠航迹 + std::vector > *temp_track, struct Track Trust_Track_Output[MAX_TRACK_NUM][10], int *Trust_track_num_Output, struct RadarPara Work_Parameter, int track_ID ); - void tmp_track_die(QVector > *temp_track);//临时航迹消亡 + void tmp_track_die(std::vector > *temp_track);//临时航迹消亡 double alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2); //计算夹角 - }; - #endif // TRACK_INIT_DIRECT_TRACKING_H diff --git a/requirements.md b/requirements.md new file mode 100644 index 0000000..c221066 --- /dev/null +++ b/requirements.md @@ -0,0 +1,16 @@ +# 雷达数据处理项目更改需求说明 + +逻辑BUG整理 + +理解代码逻辑，找出代码中的逻辑BUG，包括但不限于数组越界、内存泄漏、逻辑异常等问题，输出一份BUG报告，记录问题以及修复建议，由我来统一决定如何修复。 + +已经发现的BUG示例： + +```c++ +//track_init.cpp:534 +... +for (int i=0; i