diff --git a/.gitignore b/.gitignore index a8f818e..526a81d 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,5 @@ debug/ release/ build/ rdp_logs/ +design_docs/ .mnemon/ \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index f63656e..26b8a65 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -42,6 +42,7 @@ - 修复 MSVC `/W3` 下全部 C4819 / C4018 / C4244 编译警告：源码文件改为带 BOM 的 UTF-8 编码；signed/unsigned 不匹配处改用 `size_t` 循环变量或显式 `static_cast` 比较；double/float、__int64/double 等窄化转换改为 `static_cast` 显式转换，保持原有计算逻辑不变（涉及 `data_process.cpp`、`dot_coh.cpp`、`dot_coh_tas.cpp`、`tas_ctrl.cpp`、`track_asso.cpp`、`track_asso_tas.cpp`、`track_init.cpp`、`track_init_direct_tracking.cpp`、`track_index_mangement.cpp` 等）。 - 修复 `TAS_Ctrl::tas_beam_output()` 把航迹向量下标 `earliest_track_idx` 误赋给 `Tracking_beam->TAS_track_index` 的问题（1521592）：现输出实际航迹号 `(*trust_track)[earliest_track_idx].Track_Index`，避免跟踪波束批号与目标不符。 +- 修正 README.md（第 2、5.2、8.1、8.3、9.3、13 节）与 `docs/data_process_flow_report.md`（第 0、1、6.1~6.4 节、附录）中航迹起始的步骤顺序描述（纯文档勘误，无代码改动）：两份文档原写作「航迹头（1 点）→ 点-航迹头关联（2 点）→ 点-临时航迹关联（≥3 点）→ 起批」，与 `track_init.cpp`（L49-126）每圈的实际执行顺序相反；现更正为「① 点迹先与 ≥2 点临时航迹关联（延长已有临时航迹）→ ② 再与 1 点航迹头关联（两点卡尔曼初始化出 2 点临时航迹）→ ③ 两个阶段都结束后统一删除被消费（`Use_Flag==1`）的点迹、剩余点迹逐条转为新航迹头 → ④ 长度达标的临时航迹经三点卡尔曼初始化转为可靠航迹」，并同步修正流程图与状态机；另补充两点此前的描述缺失：两个关联阶段互不排斥（航迹头阶段不检查临时航迹阶段留下的 `Use_Flag`，同一点迹可同时延长一条临时航迹并开启一条 2 点临时航迹）、两类关联都不做一对一分配（同一条临时航迹可被同圈多个点延长）。 ### Removed diff --git a/README.md b/README.md index 197c67f..db46f11 100644 --- a/README.md +++ b/README.md @@ -53,7 +53,7 @@ - **双模式处理管线**：data_preprocess 按输入类型与扫描圈完成标志返回 0/1/2/3（TWS 以方位波位号 == 0 判为一圈完成，机扫转台反转不再误判），track_process 据此调度 TWS 凝聚/关联/起始或 TAS 凝聚/关联。 - **两帧滑窗点迹凝聚**：TWS 点迹先与上一帧缓存合并凝聚，再进入关联，兼顾跨波位（CPI）相邻点迹的分裂问题；近程模式增加速度维约束。 -- **逻辑法航迹起始**：航迹头（单点）→ 点-头速度法关联（两点）→ 点-临时航迹 EKF 波门 + 折角约束关联（三点及以上）→ 三点卡尔曼初始化转可靠航迹；带起批屏蔽区与重复航迹去重。 +- **逻辑法航迹起始**：每圈按固定顺序推进——① 未关联点迹先与 ≥2 点临时航迹做 EKF 波门 + 折角约束关联（延长临时航迹），② 再与 1 点航迹头做速度法关联（两点卡尔曼初始化出 2 点临时航迹），③ 两步都做完后删除被消费的点迹、剩余点迹转成新航迹头，④ 长度达标的临时航迹经三点卡尔曼初始化转可靠航迹；带起批屏蔽区与重复航迹去重。 - **3 模型 IMM 滤波**：三个 Singer 模型（低/中/高过程噪声），量测为「距离-方位-径向速度」三维的 EKF，多普勒残差按盲速折叠，似然用 3×3 新息协方差。 - **最近邻数据关联**：TWS 按航迹距离分段收紧模型 3 波门并附加距离门限；TAS 附加按距离分段的高度门限。ΔT ≤ 0（点迹时间戳不晚于航迹时间）的候选不更新航迹状态、仅消费该点迹（标记已使用），避免其继续起批而生成虚假 TWS 航迹。关联输出的航迹同时给出**靠近/远离方向标志**：`Flag_Direction = vr_point > 0 ? 1 : 0`（1 靠近 / 0 远离），TWS、TAS 两通道一致。 - **高度维独立平滑**：按距离分段选择滑动平均窗长（5/6/7/9 点），抑制俯仰测量跳变。 @@ -253,7 +253,7 @@ classDiagram | Dot_Coh_TAS | TAS 点迹凝聚（单帧，距离/方位/速度三维门限） | | Track_Asso | TWS 点航关联 + IMM 滤波：模型交互 → 门限筛选（NN）→ 各模型 EKF → 概率更新 → 模型输出；未关联航迹外推；高度平滑；ΔT≤0（乱序/重复时间戳）的候选取出后不更新滤波、不刷新航迹，仅消费该点迹 | | Track_Asso_Tas | TAS 航迹关联（只处理指定批号 tas_track_idx，门限与 TWS 略有差异） | -| Track_Init | 逻辑法航迹起始：航迹头维护、点-头关联（两点）、点-临时航迹关联（三点及以上）、起批（三点卡尔曼初始化）、屏蔽区判断、临时航迹消亡 | +| Track_Init | 逻辑法航迹起始，每圈顺序固定：点-临时航迹关联（延长 ≥2 点临时航迹）→ 点-航迹头关联（两点起始）→ 剩余点转新航迹头 → 长度达标者起批（三点卡尔曼初始化）；另含起批屏蔽区判断与临时航迹消亡 | | Track_Die / Track_Die_Tas | 可靠航迹消亡：外推轮数超限或手动删除标志置位时输出消亡批号并删除 | | TAS_Ctrl | TAS 波束控制：手动目标入队（tas_target_add）、失效目标出队（tas_target_del）、跟踪波束预测输出（tas_beam_output：数据率门控 + 队内选择最早 CPI 目标）、调度时刻记录（last_track_time） | | Track_Ind_Mangement | 航迹号 1~500 的顺序分配与回绕复用 | @@ -358,7 +358,7 @@ flowchart TD C --> D["dot_coh 两帧滑窗凝聚 → point_recv"] D --> E["track_asso: IMM 交互 → 点航波门筛选（NN）→ EKF 滤波 → 模型输出"] E --> F["track_die: 外推超限/手动删除 → 输出消亡批号"] - F --> G["track_init: 点-临时航迹关联 → 点-航迹头关联 → 起批转可靠航迹"] + F --> G["track_init: 点-临时航迹关联 → 点-航迹头关联 → 剩余点转新航迹头 → 起批转可靠航迹"] G --> H["输出 Trust_Track_Output（含起批历史点）"] H --> I["Beam_Ctrl: 手动 TAS 目标入队 / 波束输出"] ``` @@ -382,20 +382,23 @@ flowchart TD ```mermaid flowchart TD - A["未关联点迹"] --> B["转航迹头 Temp_track(1点) buff_round=1"] - B --> C{"下一帧点迹"} - C --> D["点-航迹头关联：V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT、径向速度同号、dis ≥ |vr_point|·ΔT 且 dis ≥ |v_track_head|·ΔT → 两点卡尔曼初始化（航迹头→2 点临时航迹）"] - D --> E{"后续点迹"} - E --> F["点-临时航迹关联：EKF 统计距离波门 d²≤64、折角<60°、径向速度同号、|Δr|≥5m"] - F --> G{"长度 ≥ track_start_point_num(3~9) 且 ≤10"} - G -- 是 --> H["屏蔽区判断 → 重复航迹去重（保留末点 d 更小者）→ 三点卡尔曼初始化"] - H --> I["转可靠航迹：分配航迹号 1~500，IMM 三模型同状态初始化 u=1/3"] - I --> J["输出前 track_start_point_num 个历史点 Trust_Track_Output[i][0..L-1]"] - G -- 否 --> K{"buff_round>2 或 长度≥10 ?"} - K -- 是 --> L["临时航迹消亡"] - K -- 否 --> E + A["本圈未关联点迹（point_recv → point_process）"] --> A1["临时航迹所有点的 buff_round +1"] + A1 --> B["① 点-临时航迹关联（≥2 点临时航迹、末点 buff_round≥2）：EKF 统计距离波门 d²≤64、折角<60°、径向速度同号、|Δr|≥5m → 复制原航迹并追加新点"] + B --> C["② 点-航迹头关联（1 点航迹头、buff_round≥2）：V_MIN·ΔT ≤ dis ≤ V_MAX·ΔT、径向速度同号、dis ≥ |vr_point|·ΔT 且 dis ≥ |v_track_head|·ΔT → 两点卡尔曼初始化（航迹头 + 新点 → 2 点临时航迹）"] + C --> D["③ 删除 Use_Flag==1 的点迹，剩余点全部转为新航迹头 Temp_track（1 点、buff_round=1）"] + D --> E{"④ 长度 ≥ track_start_point_num(3~9) 且 ≤10 且不在起批屏蔽区 ?"} + E -- 是 --> F["重复航迹去重（保留末点 d 更小者）→ 三点卡尔曼初始化"] + F --> G["转可靠航迹：分配航迹号 1~500，IMM 三模型同状态初始化 u=1/3"] + G --> H["输出前 track_start_point_num 个历史点 Trust_Track_Output[i][0..L-1]"] + E -- 否 --> I["留在临时航迹表，下一圈继续关联"] + H --> J["⑤ 临时航迹消亡：末点 buff_round>2 或 长度≥10"] + I --> J ``` +> 顺序说明：`track_init_process_logic()` 中四步的先后顺序固定、不可颠倒——先延长已有临时航迹，再与航迹头配对，两步都走完才生成新航迹头，最后才把长度达标的临时航迹转为可靠航迹（因此当圈新建的 2 点临时航迹不可能当圈起批，长度刚达标的临时航迹则可以当圈起批）；临时航迹消亡在第 ⑤ 步、即全部起始动作之后执行。 +> +> 第 ① ② 步**互不排斥**：两者都只把命中的点迹标记 `Use_Flag=1`，航迹头关联并不检查临时航迹关联留下的标记，因此同一点迹可能既延长一条临时航迹、又开启一条新的两点临时航迹；两类关联也都不做一对一分配（同一条临时航迹可被本圈多个点延长，同一个航迹头也可与本圈多个点配对）。只有走完两步后才统一删除 `Use_Flag==1` 的点迹，其余点迹成为新航迹头。 + --- ## 9. 算法实现详解 @@ -447,7 +450,34 @@ $$ ### 9.3 航迹起始（逻辑法） -#### (1) 点-航迹头关联（两点起始） +每圈（model=1）在 `track_init_process_logic()` 中按**固定顺序**执行下列四步，顺序不可颠倒： + +1. **点-临时航迹关联**：本圈未关联点迹优先延长已有的 ≥2 点临时航迹（判据见 (1)）； +2. **点-航迹头关联**：随后点迹与 1 点航迹头配对，产生新的 2 点临时航迹（判据见 (2)）； +3. **剩余点迹转航迹头**：前两步只把命中的点迹标记为已使用（`Use_Flag=1`），两步都做完后才统一删除这些点迹，其余点迹逐条转为新航迹头（见 (3)）； +4. **转可靠航迹（起批）**：长度达标的临时航迹经三点卡尔曼初始化转为可靠航迹（见 (4)）；起批动作完成后才消亡不满足条件的临时航迹。 + +> 两点补充：① 第 1、2 步**互不排斥**——航迹头关联不检查临时航迹关联留下的 `Use_Flag`，因此同一点迹可能既延长一条临时航迹、又开启一条新的两点临时航迹；两类关联也都不做一对一分配（同一条临时航迹可被本圈多个点延长，同一个航迹头也可与本圈多个点配对）。② 每圈入口先把**所有**临时航迹所有点的 `buff_round` 加 1 再做关联，新建点（航迹头或被关联上的新点）`buff_round = 1`，因此当圈新建的 2 点临时航迹要到下一圈末点 `buff_round` 变为 2 之后才参与第 1 步。 + +#### (1) 点-临时航迹关联（延长 ≥2 点临时航迹） + +对 ≥ 2 点的临时航迹（末点 buff_round ≥ 2），用航迹末点状态做 EKF 一步预测，计算三维（距离-方位-径向速度）马氏统计距离 d（见 9.5.3），并要求 |r_point − r_temp| ≥ 5 m，同时满足： + +$$ +d^2 \le TRACK\_START\_THRESHOLD^2 = 64,\qquad +\alpha < ALPHA\_START = 60°,\qquad +v_{r,point}\cdot v_{r,track}>0 +$$ + +折角 α 为最近三点连线夹角（度）： + +$$ +\alpha=\arccos\left(\frac{\vec{D_{12}}\cdot\vec{D_{23}}}{\|\vec{D_{12}}\|\,\|\vec{D_{23}}\|}\right)\cdot\frac{180}{\pi} +$$ + +关联成功后**复制原临时航迹并追加该点迹**，新航迹的 4 维状态用最近两点做**两点卡尔曼初始化**（见 9.5.2）重新得到。 + +#### (2) 点-航迹头关联（两点起始） 对单点航迹头（buff_round ≥ 2），量测点需同时满足以下四个条件（7d5a56f 起的判据）： @@ -462,25 +492,13 @@ $$ > 历史行为（已注释停用）：原先按扫描体制与距离分段收紧速度门限——近程 r≥1000 用全速、r<1000 用半速；中远程 r≥2000 用全速、r<2000 取下限 1/2·V_min·ΔT / 上限 1/3·V_max·ΔT。 -关联成功用 **两点卡尔曼初始化**（见 9.5.2）。 +关联成功用 **两点卡尔曼初始化**（见 9.5.2）生成 2 点临时航迹：第 1 点为原航迹头，第 2 点为该点迹。 -#### (2) 点-临时航迹关联（三点及以上） +#### (3) 剩余点迹转航迹头 -对 ≥ 2 点的临时航迹，用航迹末点状态做 EKF 一步预测，计算三维（距离-方位-径向速度）马氏统计距离 d（见 9.5.3），并要求 |r_point − r_head| ≥ 5 m，同时满足： +第 (1)(2) 步结束后统一删除 `Use_Flag==1` 的点迹；剩余点迹回写 `point_recv`（随后被整体清空），并**逐条新建 1 点临时航迹（航迹头）**：位置直接取点迹的直角坐标、速度置 0、`buff_round = 1`。新航迹头当圈不再参与任何关联，只能从下一圈开始按第 (2) 步配对。 -$$ -d^2 \le TRACK\_START\_THRESHOLD^2 = 64,\qquad -\alpha < ALPHA\_START = 60°,\qquad -v_{r,point}\cdot v_{r,track}>0 -$$ - -折角 α 为最近三点连线夹角（度）： - -$$ -\alpha=\arccos\left(\frac{\vec{D_{12}}\cdot\vec{D_{23}}}{\|\vec{D_{12}}\|\,\|\vec{D_{23}}\|}\right)\cdot\frac{180}{\pi} -$$ - -#### (3) 转可靠航迹（起批） +#### (4) 转可靠航迹（起批） 临时航迹长度 N 满足 track_start_point_num ≤ N ≤ 10（起批点数由参数给定，典型 3~4，接口层校验 3~9）且不在起批屏蔽区内（屏蔽区为距离-方位矩形区域，最多 30 个）时进入起批队列；同源（前三点时间/距离相同）的重复航迹只保留 d 较小者（比较下标 `track_start_point_num-1` 处的 d）。起批时令 L = track_start_point_num，用航迹中下标 L-3、L-2、L-1 的三个点做 **三点卡尔曼初始化** 得到 6 维状态（航迹长度恰为 L 时即最近三点），构造 Trust_Track： @@ -851,7 +869,7 @@ $$ - 起批判据对马氏距离平方再次平方（`d*d ≤ TRACK_START_THRESHOLD²`），实际门限为 `d ≤ 8`，按字面 `d² ≤ 64` 理解会严重放宽；调 `TRACK_START_THRESHOLD` 时需注意这一层。 - 折角计算 `acos` 无定义域钳位，浮点越界产生 NaN 时条件恒假（静默丢弃该关联）。 - 起批输出数组行号可能越过容量：容量判据用 `trust_track->size() < MAX_TRACK_INDEX`，而输出行计数在 track_asso 阶段（track_die 删除航迹之前）已推进，极端场景（当圈消亡 ≥ 2 条且起批占满配额）可写到 `MAX_TRACK_NUM` 之外。 -- 起批输出有多个字段未赋值（见 9.3(3) 与 docs/track_init_readme.md 第 14 节），宿主不应依赖这些字段。 +- 起批输出有多个字段未赋值（见 9.3(4) 与 docs/track_init_readme.md 第 14 节），宿主不应依赖这些字段。 --- @@ -895,7 +913,7 @@ $$ |---|---| | CHANGELOG.md | 变更日志（Keep a Changelog 格式，按 git 提交记录维护） | | docs/data_process_flow_report.md | 数据处理流程与关键逻辑说明（按当前源码整理：前处理与判圈、凝聚、TWS/TAS 关联与滤波、航迹起始、外推消亡、TAS 波束控制、参数脉络；含与本文档的历史差异提示） | -| docs/data_process_flow_report.html | 上文同内容的 HTML 渲染版 | +| docs/data_process_flow_report.html | 上文同内容的 HTML 渲染版；⚠ 该文件的航迹起始步骤顺序仍是修正前的旧表述（未随 md 同步，本次未改动），以 md 与源码为准 | | docs/track_init_readme.md | 航迹起始（Track Init）专项说明：分阶段流程、起始条件汇总、调参直觉、输出字段映射（含未赋值字段）、已知风险与代码索引 | | archived_docs/BUG_REPORT.md | 全量逻辑 BUG 分析（P0~P3 共 37 项，含位置、说明、建议、处置结论）——过期留档，仅作历史参考 | | archived_docs/BUG_FIX_REPORT.md | 修复明细、修复状态汇总表与 MSVC 2013 x86 编译验证记录——过期留档（对应 01d28e0 时期） | diff --git a/data_process_class_dll/parameters.h b/data_process_class_dll/parameters.h index 844d353..7aad751 100644 --- a/data_process_class_dll/parameters.h +++ b/data_process_class_dll/parameters.h @@ -52,7 +52,7 @@ #define TRACK_DIE_ROUND 4 // 航迹消亡时间 #define TRACK_DIE_ROUND_TAS 96 // -#define ASSO_THORD 3 +#define ASSO_THORD 4 #define H_F_WIN_LEN 3 @@ -101,7 +101,7 @@ #define TRACK_DIE_ROUND 4 // 航迹消亡时间 #define TRACK_DIE_ROUND_TAS 5 -#define ASSO_THORD 3 +#define ASSO_THORD 4 #define H_F_WIN_LEN 3 diff --git a/data_process_class_dll/tas_ctrl.cpp b/data_process_class_dll/tas_ctrl.cpp index 0a4e247..fce72c5 100644 --- a/data_process_class_dll/tas_ctrl.cpp +++ b/data_process_class_dll/tas_ctrl.cpp @@ -263,6 +263,7 @@ void TAS_Ctrl::tas_beam_output(std::vector *trust_track long long CPI_time = 0; long long earliest_CPI_time = LLONG_MAX; float delta_T = 0; + float delta_T_last_track = 0; double H_track = 0; int earliest_track_idx=-1; int earliest_tas_queue_idx=-1; @@ -281,13 +282,14 @@ void TAS_Ctrl::tas_beam_output(std::vector *trust_track if((*trust_track)[track_idx].Track_Index == tas_target_queue[tas_idx].Index) { CPI_time = (*trust_track)[track_idx].T_track; - delta_T = (latest_timestamp - CPI_time) / 1000.0f; - - //查找最早的CPI时间, 航迹时间与当前最新时间戳的差值大于 DATA_RATE_TAS 时才输出TAS跟踪波束 - if(CPI_time < earliest_CPI_time && (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f > Work_Parameter.DATA_RATE_TAS && delta_T > Work_Parameter.DATA_RATE_TAS) + delta_T_last_track = (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f; + + //查找CPI时间最早，且满足TAS数据率的目标 + if(CPI_time < earliest_CPI_time && (delta_T_last_track > Work_Parameter.DATA_RATE_TAS)) { earliest_CPI_time = CPI_time; H_track = (*trust_track)[track_idx].Height; + delta_T = (latest_timestamp - CPI_time) / 1000.0f; earliest_track_idx = track_idx; earliest_tas_queue_idx = tas_idx; @@ -295,7 +297,6 @@ void TAS_Ctrl::tas_beam_output(std::vector *trust_track for(int ii=0;ii<6;ii++){ X_now[ii]=(*trust_track)[track_idx].X[ii]; } - } break; diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp index ee0e377..4bb9cbc 100644 --- a/data_process_class_dll/track_asso.cpp +++ b/data_process_class_dll/track_asso.cpp @@ -417,157 +417,108 @@ void Track_Asso:: model_filter(std::vector *trust_track,struct Ra //遍历所有点迹航迹 计算点迹和航迹的统计距离 for (size_t loop_of_track = 0; loop_of_tracksize();loop_of_track++) { -// if((*trust_track)[loop_of_track].manual_tracking_flag==0) -// { - (*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1 + (*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1 - for (size_t loop_of_point = 0; loop_of_point=1000) -// { -// if(abs(h_track-h_point)<=200) -// { -// d_h = 1; -// } -// else -// { -// d_h = 0; -// } - -// } -// else if(r_track>=2000 && r_track<3000 ) -// { -// if(abs(h_track-h_point)<=200) -// { -// d_h = 1; -// } -// else -// { -// d_h = 0; -// } -// } -// else if(r_track>=3000 && r_track<5000) -// { -// if(abs(h_track-h_point)<=300) -// { -// d_h = 1; -// } -// else -// { -// d_h = 0; -// } -// } -// else -// { -// d_h = 1; -// } - -// std::cout << delta_T<<" "< 3000 && abs(p_track - p_point)*180/PI <= 7) || \ -// (r_track > 1000 && r_track <= 3000 && abs(p_track - p_point)*180/PI <= 5) || \ -// (r_track <= 1000 && abs(p_track - p_point)*180/PI <= 10)){ -// d_p = true; -// } - - //计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差 - double& t_x0 = (*trust_track)[loop_of_track].X[0]; - double& t_y0 = (*trust_track)[loop_of_track].X[3]; - double a_track = atan2(t_y0, t_x0); //航迹方位角 - double& r_point = point_process[loop_of_point].Range; //点迹距离 - double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差 - if(a_delta > PI){ - a_delta = 2*PI - a_delta; - } - - double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta)); - double VT = v_track*delta_T; - - bool d_r = true; - - if(VT < 100 && R > 300){ - //VT<100m，使用固定门限300m - d_r = false; - }else if((VT>=100 && VT<=300 && R/VT>3)){ - d_r = false; - }else if(VT>300 && R > 1200){ - d_r = false; - } - - //小于关联门限 保存关联信息 - if( - ( r_track<=500 && (d1*d1500&&r_track<1000) && (d1*d1=1000 && (d1*d1(loop_of_point)+1; - associated_info_tmp.track_index=static_cast(loop_of_track)+1; - if(d1<=d2&&d1<=d3) - associated_info_tmp.d_min=d1; - else if(d2<=d1&& d2<=d3) - associated_info_tmp.d_min=d2; - else - associated_info_tmp.d_min=d3; - - associated_info.push_back(associated_info_tmp); - } + if( (r_track <= 500 && \ + (d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \ + ((r_track > 500 && r_track < 1000) && \ + (d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 100.0)) || \ + (r_track >= 1000 && \ + (d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\ + ){ + thd_d = true; } + + //判定高度门限，thd_h + bool thd_h = false; + double delta_h = abs(h_track-h_point); + + if( (r_track <= 1000 && delta_h <= 150) || \ + ((r_track > 1000 && r_track <= 2000) && delta_h <= 200) || \ + ((r_track > 2000 && r_track <= 3000) && delta_h <= 250) || \ + ((r_track > 3000 && r_track <= 5000) && delta_h <= 300) || \ + r_track > 5000 + ){ + thd_h = true; + } + + //判定距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差 + double& t_x0 = (*trust_track)[loop_of_track].X[0]; + double& t_y0 = (*trust_track)[loop_of_track].X[3]; + double a_track = atan2(t_y0, t_x0); //航迹方位角 + double& r_point = point_process[loop_of_point].Range; //点迹距离 + double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差 + if(a_delta > PI){ + a_delta = 2*PI - a_delta; + } + + double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta)); + double VT = v_track*delta_T; + + bool thd_r = true; + + if(VT < 100 && R > 300){ + //VT<100m，使用固定门限300m + thd_r = false; + }else if((VT>=100 && VT<=300 && R/VT>3)){ + thd_r = false; + }else if(VT>300 && R > 1200){ + thd_r = false; + } + + //小于关联门限 保存关联信息 + if(thd_d && thd_h && thd_r) + { + //std::cout << "asooooooooooo"; + + struct asso_info associated_info_tmp; + associated_info_tmp.d1=d1; + associated_info_tmp.d2=d2; + associated_info_tmp.d3=d3; + associated_info_tmp.point_index=static_cast(loop_of_point)+1; + associated_info_tmp.track_index=static_cast(loop_of_track)+1; + if(d1<=d2&&d1<=d3) + associated_info_tmp.d_min=d1; + else if(d2<=d1&& d2<=d3) + associated_info_tmp.d_min=d2; + else + associated_info_tmp.d_min=d3; + + associated_info.push_back(associated_info_tmp); + } + } // } } diff --git a/data_process_class_dll/track_asso_tas.cpp b/data_process_class_dll/track_asso_tas.cpp index c112e93..3452a67 100644 --- a/data_process_class_dll/track_asso_tas.cpp +++ b/data_process_class_dll/track_asso_tas.cpp @@ -289,65 +289,28 @@ void Track_Asso_Tas::model_filter( std::vector *t double d1,d2,d3; IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter); - //计算高度门限 - double d_h; - if (r_track<1000) - { - if(abs(h_track-h_point)<=150) - { - d_h = 1; - } - else - { - d_h = 0; - } + //判定统计距离门限，thd_d + bool thd_d = false; - } - else if (r_track<2000 && r_track>=1000) - { - if(abs(h_track-h_point)<=200) - { - d_h = 1; - } - else - { - d_h = 0; - } - - } - else if(r_track>=2000 && r_track<3000 ) - { - if(abs(h_track-h_point)<=200) - { - d_h = 1; - } - else - { - d_h = 0; - } - } - else if(r_track>=3000 && r_track<5000) - { - if(abs(h_track-h_point)<=300) - { - d_h = 1; - } - else - { - d_h = 0; - } - } - else - { - d_h = 1; + if(d1*d1 1000 && r_track <= 2000) && delta_h <= 200) || \ + ((r_track > 2000 && r_track <= 3000) && delta_h <= 250) || \ + ((r_track > 3000 && r_track <= 5000) && delta_h <= 300) || \ + r_track > 5000 + ){ + thd_h = true; + } //小于关联门限 保存关联信息 - if((d1*d1 - data_process_flow_report - - - - - - - - - - - - - - - - - -
- -

雷达数据处理流程与关键逻辑说明

-
-

适用对象：本仓库 data_process_class_dll（X256 雷达数据处理动态链接库）

-
-
-

0. 一页速览

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
环节输入处理输出
① 前处理一帧原始点迹（TWS 或 TAS）单位换算、缓存、判圈处理模式码 0/1/2/3
② 调度模式码分流到 TWS / TAS 通道—
③ 凝聚单 CPI 或相邻两 CPI 点迹同类点合并（保幅度大者）凝聚点迹表
④ 关联凝聚点迹 + 已有航迹IMM 交互 → 波门筛选 → 最近邻一对一 → EKF 更新更新航迹 + 剩余点迹
⑤ 起始剩余（未关联）点迹逻辑法：航迹头 → 2 点 → ≥3 点 → 起批新可靠航迹
⑥ 外推/消亡未关联航迹按数据率预测一帧；超限则消亡补点航迹 / 消亡批号
⑦ 波束控制TAS 队列 + 航迹数据率门控 + 队列调度 → 位置外推跟踪波束指令
-

贯穿全流程的三条主线

-
    -
  1. 时间主线：所有时间戳为 64 位毫秒（CPI/GNSS），参与运算时除 1000 转秒；ΔT = 点迹时间 − 航迹时间，ΔT 的正负决定"有效更新 / 只消费不更新 / 跳过"。
    2. -
  2. 状态主线：每条可靠航迹维护一套 IMM（3 个 Singer 模型）状态 [x, vx, ax, y, vy, ay]，x/y 两轴解耦，量测为「距离-方位-径向速度」三维 EKF。
    3. -
  3. 生命周期主线：可靠航迹靠"外推轮数"消亡；临时航迹靠"缓存圈数 buff_round"消亡。
    4. -
-
-

1. 总体数据流

-
flowchart TD - IN["宿主逐波位/逐CPI送入 DataRev[150]"] --> DP["data_preprocess 前处理与判圈"] - DP -->|"model=2 TWS未满圈"| M2["滑窗凝聚（只凝聚，不关联）"] - DP -->|"model=1 TWS满一圈"| M1["TWS 通道"] - DP -->|"model=3 TAS数据"| M3["TAS 通道"] - - M2 --> BUF["本帧凝聚结果留作下一圈缓存"] - M2 --> PR["上一帧凝聚点 → point_recv（参与下一圈关联）"] - - M1 --> A1["IMM 交互 + 波门筛选 + 最近邻关联 + EKF 更新"] - A1 --> A2["未关联航迹外推 / 超限消亡"] - A2 --> A3["剩余点迹 → 逻辑法航迹起始"] - A3 --> OUT1["输出更新航迹 + 起批航迹历史点 + 消亡批号"] - - M3 --> B1["TAS 凝聚（单帧）"] - B1 --> B2["仅对指定批号航迹做关联/EKF 或外推"] - B2 --> B3["TAS 超时消亡"] - B3 --> OUT2["输出该批号航迹"] - - OUT1 --> BC["Beam_Ctrl 波束控制"] - OUT2 --> BC - BC --> OUT3["TrackingBeam：距离/方位/俯仰/批号"] -
-

2. 入口前处理与"判圈"

-

2.1 前处理动作

-

对每一个输入点迹统一做下面几件事（TWS 与 TAS 相同）：

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
动作说明
单位换算方位 度 → 弧度（内部统一弧度，输出再转度）
高度折算Height = Range · sin(Elevation)（由量测俯仰直接折算，不经坐标滤波）
字段搬运幅度、SNR、RCS、时间戳、频点号、PRI、俯仰波位、十字星数据（speed_dim[129]/range_dim[8]）全部透传
使用标记全部置 0（未使用）
数量保护TWS：min(Point_Sum, 150)；TAS：clamp(Point_Sum, 0, 150)
全局时间每次调用都用本帧首点的 CPI 时间刷新"最新时间戳"，供 TAS 外推/波束使用
-

2.2 判圈逻辑（决定走哪条通道）

-
flowchart TD - A["收到一帧点迹"] --> B{"point_type ?"} - B -->|"0 = TWS"| C["写入 TWS 缓存"] - C --> D{"方位波位号 Beam_index_aiz == 0 ?"} - D -->|"是"| E["return 1：满一圈，执行 TWS 航迹处理"] - D -->|"否"| F["return 2：未满圈，仅做滑窗凝聚"] - B -->|"1 = TAS"| G["写入 TAS 缓存<br/>记录 TAS 批号与本帧时间戳"] - G --> H["return 3：TAS 处理"] - B -->|"其它"| I["return 0：无效数据，丢弃"] -

要点

-
    -
  • 判圈条件由"波位号由大变小"改为 波位号为 0（7d5a56f），避免机扫转台反转时误判一圈完成（代码中保留了旧判断的注释）。
    • -
  • 波位号 0 是每圈的起始波位，因此每圈恰好触发一次 model=1。
    • -
  • 清空（track_clear_all）后圈状态复位，下一次调用必然判为"满一圈"。
    • -
  • TAS 数据不参与判圈，每来一帧就处理一次（TAS 是调度的跟踪波位，本身就是单目标 CPI）。
    • -
-

2.3 调度：model 与动作的对应

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
model触发实际执行的动作（顺序固定）
2TWS 未满圈两帧滑窗凝聚；缓存本帧点迹
1TWS 满一圈① 关联/滤波/外推 ② 航迹消亡 ③ 航迹起始
3TAS 帧① TAS 凝聚 ② TAS 关联/外推 ③ TAS 消亡
0非法类型无
-
-

顺序很关键：消散在起始之前，消亡腾出的资源不会与本圈更新冲突；起始在最后，只有"没被任何可靠航迹用掉"的点迹才有资格起批。

-
-
-

3. 点迹凝聚

-

3.1 目的与判据

-

同一目标在相邻波位/相邻 CPI 上会分裂成多个点，需先合并（保留幅度最大者，其余标记删除）。

- - - - - - - - - - - - - - - - - - - - - - - -
模式距离门限方位门限速度门限
近程 work_mode==0≤ 80 m≤ 6°（按环绕最小角差）≤ 2 m/s
中/远程 work_mode!=0≤ 80 m≤ 6°不约束
-

方位差按环绕处理：

-
Δθ = min( |θ0 − θ1| , 2π − |θ0 − θ1| )
-

3.2 合并规则（链式比较）

-
flowchart TD - A["两两比较点迹 (0, i)"] --> B{"距离/方位/速度<br/>均在门限内?"} - B -- 否 --> Z["跳过"] - B -- 是 --> C{"幅度：点0 < 点i ?"} - C -- 是 --> D["点0 标记删除<br/>并把点i 的 R/方位/V/幅度<br/>接管为当前基准值"] - C -- 否 --> E["点i 标记删除"] - D --> F["后续比较以幅度更大者为基准（链式凝聚）"] - E --> F - F --> G["统一删除：被标记点 + 量程外点<br/>r < 100 m 或 r > 100000 m"] -

3.3 TWS 两帧滑窗（关键机制）

-
flowchart TD - A["上一帧缓存点（区号=1）"] --> C["合并成同一张表"] - B["本帧新点（区号=2）"] --> C - C --> D["整表凝聚 + 量程过滤"] - D --> E{"点的区号"} - E -->|"1（较早的点）"| F["输出到 point_recv<br/>→ 本圈参与关联/起始"] - E -->|"2（本帧的点）"| G["留在缓存中<br/>→ 与下一帧合并后再输出"] - F --> H["缓存清空，本帧剩余点成为新缓存"] -

为什么这样设计

-
    -
  • 相邻两个 CPI 的同一目标点会落在不同帧里，单帧凝聚无法合并 → 延迟一帧输出，换取跨 CPI 的分裂点合并；
    • -
  • 代价是关联/起始用的是"上一帧"的点，时间上滞后一圈，但由于后续所有 ΔT 都按真实 CPI 时间戳计算，滤波本身不受影响；
    • -
  • 因此 TWS 通道的正常节奏是：未满圈 model=2 只做凝聚 + 缓存；满圈 model=1 用上一帧凝聚结果做关联与起始。
    • -
-
-

TAS 凝聚为单帧不缓存，门限恒为「距离 ≤ 80 m、方位 ≤ 6°、速度 ≤ 2 m/s」，之后同样做量程过滤。

-
-
-

4. TWS 点航关联与滤波（model=1 的核心）

-

4.1 处理骨架

-
flowchart TD - A["全部可靠航迹"] --> A1["IMM 交互（mix）"] - A1 --> B["滤波前先统一置 point_flag=0（本圈默认未关联实点）"] - B --> C["逐航迹 × 逐点：算 ΔT、三个模型马氏距离 d1/d2/d3"] - C --> D{"波门判定<br/>距离分段 + 距离门限 d_r + 俯仰门限 d_p"} - D -- 通过 --> E["记入候选表（含 d_min = min(d1,d2,d3)）"] - D -- 不通过 --> F["丢弃该候选对"] - E --> G["最近邻：反复取全表 d_min 最小者"] - G --> H["该航迹本圈候选全部移除<br/>该点迹候选全部移除<br/>（一对一）"] - H --> I{"ΔT > 0 ?"} - I -- 是 --> J["三模型 EKF 更新 → 模型概率更新 → 融合输出<br/>更新航迹时间/点信息/高度平滑<br/>Extrapolate_round=0, point_flag=1"] - I -- 否 --> K["不更新状态、不刷新航迹时间<br/>但把该点迹标记为已使用（消费掉）"] - J --> L{"还有候选?"} - K --> L - L -- 有 --> G - L -- 无 --> M["未关联航迹（point_flag==0 且非手动TAS）按数据率外推"] - M --> N["输出全部航迹（更新行 + 补点行）"] -

4.2 波门（三步都通过才算候选）

-

（1）统计距离波门——按航迹距离分段收紧高机动模型：

- - - - - - - - - - - - - - - - - - - - - - - - - -
航迹距离 r_track模型 1 / 2模型 3
≤ 500 md² < 9d² < 9/1000（≈ 关闭，抑制近程高机动模型的乱关联）
500 ~ 1000 md² < 9d² < 9/100（收紧）
≥ 1000 md² < 9d² < 9
-
-

⚠ 门限语义：代码里 d 已经是马氏距离平方，判据又写作 d*d < 9，等效于 d < 3（约对应 3 自由度卡方分布 95% 分位的量级）。调整 ASSO_THORD 时务必记住这一层"二次方"。

-
-

（2）距离门限 d_r——抑制远距离突跳。用航迹-点迹弦距 R 与航迹位移 VT = v_track·ΔT 判断：

-
VT < 100  且 R > 300      → 拒绝（慢速目标不允许 300 m 以上的跳变）
-100 ≤ VT ≤ 300 且 R/VT > 3 → 拒绝（允许的跳变与速度位移成比例）
-VT > 300  且 R > 1200     → 拒绝（高速目标给固定 1200 m 上限）
-

（3）俯仰门限 d_p：分距离段 7°/5°/10° 的逻辑当前被注释停用，恒为通过。

-

4.3 最近邻（NN）选取规则

-
    -
  • 候选对按 d_min = min(d1, d2, d3) 排序，每轮取全表最小值；
    • -
  • 选中后，候选表中"该航迹的全部候选"和"该点迹的全部候选"一并删除 → 每条航迹每圈最多关联一个点，每个点最多被一条航迹使用；
    • -
  • 直到候选表为空或遍历完航迹数。
    • -
-

4.4 ΔT ≤ 0 的特殊处理（当前行为）

-

ΔT = （点迹 CPI 时间 − 航迹 T_track）/1000 ≤ 0，典型场景是"航迹刚被 TAS 点迹更新过，本圈 TWS 点迹时间更早"或"重复时间戳"。

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
环节行为
是否参与最近邻比较参与（仍可能与最小 d 一起被选中）
IMM/EKF 更新不执行，X1/X2/X3、P1/P2/P3、模型概率均保持原值
航迹时间/点信息不刷新（T_track、GNSS_time、关联点信息、幅度/SNR/RCS 都不变）
新鲜度标志point_flag 保持 0、Extrapolate_round 不归零、associate_point_number 不递增
本圈后续该航迹本圈不再关联其它点迹；由于 point_flag 仍为 0，在随后的外推环节被推进一帧
点迹归属该点迹被标记"已使用"消费掉 → 不会再进入航迹起始，避免生成虚假 TWS 航迹
-

4.5 关联成功后的更新内容

-
    -
  1. 三个模型的 EKF 状态/协方差更新；
    2. -
  2. 模型概率按新息协方差似然更新（见第 5 节）；
    3. -
  3. 航迹时间 T_track / GNSS 时间跟进到该点迹；
    4. -
  4. 关联点信息（距离/方位/俯仰/径向速度/PRI）、幅度、SNR、RCS、俯仰波位、十字星数据刷新；
    5. -
  5. 高度维平滑（见 4.6）；
    6. -
  6. Extrapolate_round = 0、point_flag = 1、associate_point_number += 1。
    7. -
-

4.6 高度平滑

-

关联实点后把点迹高度压入滑窗，输出高度取最近 w 个点的均值（不足 w 个则全平均）：

- - - - - - - - - - - - - - - - - - - - - - - - - -
点迹距离窗口长度 w
≤ 1000 m5
1000 ~ 2000 m6
2000 ~ 4000 m7
> 4000 m9
-
-

距离越远俯仰测量跳变越大，窗口随之放长；仅 TWS/TAS 关联到实点时才刷新，外推补点不改变高度。

-
-

4.7 未关联航迹的外推

-

对 point_flag == 0 且 manual_tracking_flag == 0 的航迹（含 ΔT≤0 被消费的情形）：

-
    -
  • 推算步长取运行时数据率：work_mode 0/1/2 → DATA_RATE_SHORT / MIDDLE / FAR（秒）；
    • -
  • 用各子模型的 Singer F、Q 推进一帧（不做量测更新）；
    • -
  • T_track、GNSS_time 各推进 ΔT×1000 毫秒；
    • -
  • Extrapolate_round += 1，point_flag = 0（输出为补点/虚点）。
    • -
-
-

5. IMM + EKF 关键逻辑

-

5.1 IMM 框架

-
flowchart LR - A["上一拍三模型状态/协方差 + 模型概率 u"] --> B["交互 mix：算归一化常数 c̄j 与混合概率"] - B --> C["得到三个混合初值 X0j / P0j"] - C --> D["各子模型独立预测（Singer F 和 Qj）"] - D --> E["各子模型 EKF 量测更新 → 新息协方差 Sj、马氏距离 dj"] - E --> F["似然 Λj = N(dj; 0, Sj)，模型概率 μj' ∝ Λj·c̄j"] - F --> G["融合输出：X̂ = Σμj'X̂j<br/>P = Σμj'[Pj + (X̂j−X̂)(X̂j−X̂)ᵀ]"] -
    -
  • 状态布局 [x, vx, ax, y, vy, ay]，x/y 两轴解耦；
    • -
  • 模型转移概率矩阵固定：
    Pt = [ 0.8  0.15  0.05
-       0.3  0.4   0.3
-       0.05 0.15  0.8 ]
-
    -
    • -
  • 机动相关时间常数 α = 0.2；
    • -
  • 数值保护：归一化常数 c̄j ≤ 1e-12 时钳位；似然分母（det S）≤ 1e-12 时该模型似然置 0；概率分母异常时保持上一拍模型概率；EKF 似然用 3×3 新息协方差（保证与量测维数一致）。
    • -
-

5.2 三个 Singer 模型的过程噪声强度分档

-

按航迹速度 v_track 切换 q 值（模型 1 恒用 slow）：

- - - - - - - - - - - - - - - - - - - - - - - - - -
模型TWS 通道TAS 通道
模型 1（低机动）Model1_Q_slow（恒定）同左
模型 2（中机动）v≤5：slow/20；5<v≤30：slow；v>30：fastv≤5：slow/20；50<v≤100：10·fast；其余：fast
模型 3（高机动）v≤5：slow/20；5<v≤20：slow；20<v≤50：fast；50<v≤100：2·fast；v>100：10·fastv≤5：slow/20；50<v≤100：10·fast；其余：fast
-
-

语义：低速目标按"低机动"处理（把中/高机动模型噪声压到极小，避免噪声被当成机动），速度越高逐步放大高机动模型的过程噪声。Model1_Q_fast 字段不参与计算。

-
-

5.3 量测与残差修正（EKF）

-

量测向量 Z = [r, θ, v_r]，量测函数由预测状态给出：

-
h(X) = [ √(x²+y²),  atan2(y,x) 归一化到 [0,2π),  −(x·vx + y·vy)/√(x²+y²) ]
-

在预测点处线性化得到雅可比 H（3×6），量测噪声 R = diag(σr², σa², σv²)（10 m / 0.02 rad / 2 m/s），S = H·P·Hᵀ + R。

-

两处残差修正（关键）

-
    -
  1. -

    方位环绕：νθ 归一到 [−π, π)，避免 0°/360° 附近的假大残差；

    -
    2. -
  2. -

    多普勒盲速折叠：νv ← νv − round(νv / v_bind) · v_bind，其中

    -
    v_bind = 150000 / (f_GHz · PRI_µs),   f_GHz = 9.2 + 0.02 · 频点号
-

    PRI ≤ 0 或频率非法时返回安全值 1.0（接口约定 PRI 单位 µs 且不允许为 0）。

    -
    3. -
-

判据（关联）与模型似然共用同一个 d = νᵀS⁻¹ν，且滤波函数把 3×3 新息协方差 S 一并回传给似然使用。

-

5.4 航迹初始化

- - - - - - - - - - - - - - - - - - - - - - - - - -
场景维数做法
两点（航迹头 + 1 点，临时航迹）4 维 [x, vx, y, vy]位置取末点，速度取两点差分；协方差按 Z1 处的转换量测协方差 R 填充，速度项按 R/T、2R/T² 传播
三点（起批，可靠航迹）6 维位置取末点、速度取末两点差分、加速度取两段速度差分；协方差按 R0/R1/R2 误差传播构造
起批时 IMM 初始化—三个子模型用同一状态/协方差，模型概率 u = [1/3, 1/3, 1/3]
-

转换量测协方差（极坐标量测转直角坐标时的等效噪声，含方位非线性引起的附加项，由 σr=10 m、σa=0.02 rad 决定）用于上述初始化与临时航迹统计距离计算。

-
-

约束：两点/三点初始化对时间差 T 无除零保护，因此起批路径要求时间戳严格递增、不允许重复。

-
-
-

6. 航迹起始（逻辑法）

-

6.1 状态机

-
flowchart TD - A["本圈未被可靠航迹关联的点迹"] --> B["全部转成'航迹头'（1 点，buff_round=1）"] - B --> C{"下一圈的点迹"} - C --> D["点-航迹头关联（两点起始）<br/>速度区间 + 多普勒一致性"] - D -->|命中| E["两点卡尔曼初始化 → 2 点临时航迹"] - D -->|未命中| B - E --> F{"后续圈的点迹"} - F --> G["点-临时航迹关联（三点及以上）<br/>EKF 统计距离 + 折角 + 多普勒同号"] - G -->|命中| H["航迹延长（复制原航迹 + 追加新点）"] - G -->|未命中| F - H --> I{"长度 ≥ 起批点数<br/>且不在起批屏蔽区?"} - I -- 是 --> J["同源去重（保留末点 d 更小者）"] - J --> K["三点卡尔曼初始化 → 分配航迹号 1~500<br/>IMM 三模型同状态，u=1/3"] - K --> L["输出该航迹的历史点（前 N 个点）"] - I -- 否 --> F - F --> M{"末点 buff_round > 2<br/>或长度 ≥ 10 ?"} - M -- 是 --> N["临时航迹消亡（不对外输出）"] -

6.2 关联判据汇总

-

阶段一：点 ↔ 航迹头（两点起始）

-

用航迹头位置与点迹的平面距离 dis 与 ΔT（秒）：

-
dis ≤ V_MAX · ΔT                      平均速度不超上限
-dis ≥ V_MIN · ΔT                      平均速度不低于下限（滤除静止/慢速杂波）
-v_r,point · v_r,head > 0              两点多普勒同号（同接近/同远离）
-dis ≥ |v_r,point| · ΔT                几何位移不小于"点迹自身多普勒"位移
-dis ≥ |v_r,head|  · ΔT                同上，用航迹头的多普勒约束
-
    -
  • 后两条是多普勒-几何一致性约束，用于剔除"切向高速短航迹"（7d5a56f 新增）：切向飞行时真实几何位移大而多普勒径向速度小，纯速度区间门限会放进虚假航迹；
    • -
  • 判据隐含要求 ΔT > 0（ΔT ≤ 0 时 dis ≤ V_MAX·ΔT 必然不成立）；
    • -
  • 近程/中远程分支当前取值相同（都是 V_MAX），实际不做距离分段。
    • -
-

阶段二：点 ↔ ≥2 点临时航迹（三点及以上）

-
|r_point − r_末点| ≥ 5 m                              距离差硬门限（近距点不参与）
-d ≤ 8（代码写作 d² ≤ TRACK_START_THRESHOLD²=64）        3 维 EKF 统计距离波门
-α < ALPHA_START = 60°                                  折角：最近三点连线夹角
-v_r,point · v_r,末点 > 0                               多普勒同号
-
    -
  • d 由 4 维 CV 状态一步预测 + 「距离-方位-径向速度」EKF 得到（过程噪声 0.03²、同样做方位环绕与盲速折叠）；
    • -
  • 折角 α 为向量 D12（前一点→末点）与 D23（末点→新点）的夹角；α 越小越接近直线；
    • -
  • 同样存在"门限对马氏距离平方再平方"的语义，实际生效为 d ≤ 8。
    • -
-

6.3 临时航迹的"圈数"生命周期

-

buff_round 只在起始流程里自增，并且每次调用（= 每圈）先把所有临时航迹所有点的 buff_round +1，再做关联：

- - - - - - - - - - - - - - - - - - - - - -
动作效果
新点加入buff_round = 1
关联资格只看末点，要求 buff_round ≥ 2 → 上一圈加入的点本圈才能被延续，同一航迹每圈最多吸收一个点
消亡条件末点 buff_round > 2（连续 2 圈无新点）或长度 ≥ 10
-

时间轴示例（以第 k 圈加入新点为起点）

-
第 k   圈：新点加入 → 末点 buff_round=1（本圈不再关联）
-第 k+1 圈：+1 → 2 → 允许关联；成功则新点 1，失败则 2（不 >2，存活）
-第 k+2 圈：+1 → 3 → 仍允许关联；成功则新点 1，失败则 3>2 → 消亡
-

即：允许中间空 1 圈，最大关联间隔 2 圈；生命周期完全由"圈数"驱动，与绝对时间无关。

-

6.4 航迹分裂与去重

-
    -
  • 关联阶段不做一对一分配：一个点满足多条临时航迹条件时，每条都各自复制出一条新航迹；多条点命中同一条航迹时也各自复制 → 同一圈会产生大量同源临时航迹；
    • -
  • 起批前做同源去重：比较两条候选航迹的前三个点，只要有任一点的"时间 T + 距离 r"完全相同即判为同源，保留末点统计距离 d 更小者；
    • -
  • 这样既保证不漏关联（允许分裂试探），又不会让同一批点重复起批。
    • -
-

6.5 起批与输出

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
条件说明
长度track_start_point_num ≤ L ≤ 10（接口层校验 3~9，典型 3 或 4）
位置末点不落在起批屏蔽区（距离-方位矩形，最多 30 个，用严格不等号判断）
容量可靠航迹数 < 500 且能分配到一个未占用航迹号
初始化用最后三个点做三点卡尔曼初始化 → 6 维状态；三模型同状态、u=1/3
输出该航迹的前 N 个点（N = 起批点数）作为历史点一次性输出，每条航迹最多 10 个历史点
-
-

达到长度即当圈起批，没有"连续 M 圈确认"的环节；若被屏蔽区拦住，航迹会继续在临时航迹表里增长，直到离开屏蔽区（此时长度已大于起批点数）或长度达到 10 被判消亡。

-
-
-

7. 外推、消亡与航迹号管理

-

7.1 消亡条件

- - - - - - - - - - - - - - - - - - - - - -
通道条件（满足其一即消亡）
TWS 航迹Track_Mode==0 且 Extrapolate_round ≥ 4
TAS 航迹Track_Mode==1 且 Extrapolate_round ≥ TRACK_DIE_ROUND_TAS（相扫 5 / 机扫 96）
手动删除manual_delete_flag == 1（两种通道通用，由外部删除接口置位）
-

消亡时把批号写入消亡输出数组并从航迹表删除；临时航迹消亡不对外输出（起批失败的中间态对宿主不可见）。

-

7.2 航迹号分配

-
flowchart TD - A["需要起批"] --> B{"航迹表为空?"} - B -- 是 --> C["分配 1"] - B -- 否 --> D["用航迹表建立 1~500 占用位图"] - D --> E["从 lastest_index+1 向上找第一个空闲号（含 500）"] - E --> F{"找到?"} - F -- 是 --> G["分配并更新 lastest_index"] - F -- 否 --> H["从 1 回绕扫一遍找空闲号"] - H --> I{"找到?"} - I -- 是 --> G - I -- 否 --> J["返回 0：全满，本候选不起批"] -

特点：优先顺序递增分配（便于人工按批号识别新老），只有走到表尾才回绕复用早期空号。

-
-

8. TAS 通道（model=3）

-

8.1 与 TWS 通道的差异

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
项TWSTAS
凝聚相邻两帧滑窗，近程多速度维约束单帧，恒有速度维约束
处理对象全部可靠航迹 × 全部点仅指定批号的这一条航迹 × 全部点
俯仰/高度门限俯仰门限当前恒通过高度门限生效（按距离分段）
ΔT ≤ 0参与选取但只消费点迹、不更新直接跳过该点（并记告警日志）
外推时间按 work_mode 数据率固定步长按 最新时间戳 − 航迹时间
输出全部航迹仅该批号航迹
-

8.2 TAS 关联与更新

-
flowchart TD - A["TAS 本帧点迹 + 指定批号航迹"] --> B{"该批号航迹存在?"} - B -- 否 --> Z["直接返回（不做任何处理）"] - B -- 是 --> C["IMM 交互；航迹 point_flag 置 0"] - C --> D["逐点：ΔT ≤ 0 → 记告警并跳过"] - D --> E["逐点：三模型统计距离 d1/d2/d3 + 高度门限"] - E --> F{"d² < 9（任一模型）且高度门限通过?"} - F -- 是 --> G["记入候选表"] - F -- 否 --> H["丢弃"] - G --> I["取 d_min 最小的一点"] - I --> J["三模型 EKF 更新 → 概率更新 → 融合<br/>刷新时间/点信息/高度平滑"] - J --> K["输出该批号航迹"] - E --> L{"候选表为空?"} - L -- 是 --> M["按 ΔT = 最新时间戳 − 航迹时间 外推一帧<br/>（ΔT ≤ 0 则跳过）"] - M --> K -

高度门限（|h_航迹 − h_点迹| 上限，按航迹距离分段）：

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
航迹距离门限
≤ 1000 m150 m
1000 ~ 2000 m200 m
2000 ~ 3000 m200 m
3000 ~ 5000 m300 m
> 5000 m不约束
-

TAS 后续处理：外推不受 work_mode 数据率约束，而是直接推进到"本帧最新时间戳"，保证 TAS 航迹时间与波束调度时刻同步。

-
-

9. TAS 波束控制（外部接口 Beam_Ctrl）

-

9.1 队列管理

-
flowchart TD - A["Beam_Ctrl 调用"] --> B["输出航迹计数清零"] - B --> C["入队 tas_target_add"] - C --> D["出队 tas_target_del"] - D --> E["波束输出 tas_beam_output"] - E --> F["输出 TrackingBeam"] -

入队条件（只支持手动转 TAS）：

-
    -
  • 该航迹 manual_tracking_flag == 1（由外部 tracking_start 置位）；
    • -
  • 航迹当前 Track_Mode == 0（还不是 TAS）；
    • -
  • 队列未满（相扫最多 4 个 / 机扫最多 1 个）；
    • -
  • 不在 TAS 禁止区（距离-方位矩形）。
    • -
-

入队动作：写批号、标记非空、last_track_time = T_track、航迹 Track_Mode 置 1，并立即输出一次该航迹更新（通知宿主跟踪状态变化）。

-

出队条件：队列中的批号在航迹表中已不存在，或不再处于"手动 + TAS"状态 → 清空该队列项。

-
-

自动开启/退出 TAS 的逻辑（按距离/速度/高度阈值）已全部注释停用，只支持手动。

-
-

9.2 波束输出调度（关键）

-
flowchart TD - A["遍历 TAS 队列"] --> B["按批号在航迹表中找到该航迹"] - B --> C{"距上次波束输出 > DATA_RATE_TAS 秒<br/>且 最新时间戳 − 航迹时间 > DATA_RATE_TAS 秒?"} - C -- 否 --> A - C -- 是 --> D["在合格目标中选 CPI 时间最早者"] - D --> E["位置外推：x + vx·ΔT, y + vy·ΔT"] - E --> F["Range = √(xp²+yp²)，Azi = atan2(yp,xp)<br/>Elev = asin(Height / Range)"] - F --> G["open_flag=1, type=1, 批号=真实航迹号<br/>更新该目标 last_track_time = 最新时间戳"] - C -.->|"无任何合格目标"| H["open_flag = 0（其余字段保持旧值）"] -

要点

-
    -
  • 门控用两个条件：距上次输出的时间、航迹时间与最新时间戳之差都要大于 DATA_RATE_TAS 秒（单位是秒，由宿主经参数初始化传入，须为 > 0 的有效值）→ 保证 TAS 目标的数据率不随 Beam_Ctrl 的调用频率变化；
    • -
  • 多目标时选队列中 CPI 时间最早的目标，等价于"按最久未更新者优先"的轮转；
    • -
  • 原来的"每次调用整队循环移位"已停用；
    • -
  • 俯仰用航迹高度直接算（叠加雷达平台高度的处理只影响对外输出的 z，不参与波束俯仰）。
    • -
-
-

10. 参数脉络（决定行为的旋钮）

-

10.1 编译期宏（随扫描体制切换）

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
宏相扫机扫作用点
SIGMA_R / A / E / V10 / 0.02 / 0.2 / 2.0同左量测噪声、EKF 的 R、转换量测协方差
DOT_COH_RANGE / V / AZI80 / 2 / 6同左凝聚门限（距离 m / 速度 m/s / 方位 °）
R_MIN / R_MAX100 / 100000同左有效量程（凝聚后过滤）
TRACK_START_THRESHOLD8同左起始统计距离波门（d ≤ 8）
ALPHA_START60同左起始折角上限（°）
ASSO_THORD3同左关联波门（d² < 9，等效 d < 3）
TRACK_DIE_ROUND44TWS 消亡外推轮数
TRACK_DIE_ROUND_TAS596TAS 消亡外推轮数
MAX_TRACK_NUM / MAX_TRACK_INDEX500 / 500同左航迹数上限 / 航迹号上限
TAS_QUEUE_LENGTH / MAX_TAS_NUM4 / 41 / 1TAS 队列长度 / 最大 TAS 目标数
H_F_WIN_LEN3同左高度平滑窗长基数（实际 +2/+3/+4/+6）
FREQ09.2 GHz同左盲速计算的基准频率
RDP_LOG_ENABLE / RDP_LOG_DIR——日志落盘开关与目录
-

10.2 运行时参数（宿主传入，随任务变化）

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
参数影响环节
work_mode（0 近程/1 中程/2 远程）凝聚是否带速度维约束；TWS 外推步长；关联距离门限分支
track_start_point_num（校验 3~9）起批所需点数
V_MAX / V_MIN航迹头-点关联的速度区间
DATA_RATE_SHORT / MIDDLE / FARTWS 未关联航迹外推步长（秒）
DATA_RATE_TAS（秒，须 > 0）TAS 波束输出间隔门控
Height输出 z = 航迹高度 + 平台高度
Sys_delay输出位置做 x + vx·Sys_delay 的系统延迟补偿
Model1/2/3_Q_slow/fast三个 Singer 模型的过程噪声强度（分档见 5.2）
track_prohibite_area_*（≤30 组）起批屏蔽区（距离 m + 方位 °）
TAS_prohibite_area_*（≤30 组）TAS 禁止区
-
-

track_start_threshold / track_asso_threshold / track_asso_threshold_tas 三个字段当前未被实现读取，实际门限来自编译期宏。

-
-
-

11. 关键约定与注意事项

-

时序与时间戳

-
    -
  • 时间戳为 64 位毫秒；T_track 输出为秒（/1000）；所有 ΔT 用秒参与运算；
    • -
  • TWS 关联中 ΔT ≤ 0 的候选"只消费点迹、不更新航迹"（点迹不会再去起批）；
    • -
  • TAS 关联中 ΔT ≤ 0 直接跳过该点；TAS 外推 ΔT ≤ 0 跳过本轮；
    • -
  • 两点/三点卡尔曼初始化对 T 无除零保护 → 起批路径的时间戳必须严格递增。
    • -
-

量纲与坐标

-
    -
  • 外部接口角度为度，内部统一弧度，输出再度；
    • -
  • 平面坐标为水平投影，z = 目标高度 + 雷达平台高度；
    • -
  • 点高度由 R·sin(俯仰) 直接折算，不参与滤波状态（由高度平滑单独维护）。
    • -
-

门限语义

-
    -
  • 关联与起始的波门写作 d² < 门限²，而 d 已是马氏距离平方 → 实际等效 d < 门限；
    • -
  • 起始的"距离差 ≥ 5 m"为硬编码，对切向远距离慢速目标可能造成漏关联；
    • -
  • 起批去重只比较前三个点，若起批点数 > 3，共享前三点后分叉的两条航迹会被判为同源。
    • -
-

容量与边界

-
    -
  • 点迹数上限 150（前处理已限幅）；航迹数/航迹号上限 500，全满时新目标不再起批；
    • -
  • 屏蔽区/禁止区的组数按 30 组数组定义，配置数量未做上限校验；
    • -
  • 高度平滑缓存只增不减，长时运行下每条航迹的内存占用会持续增长。
    • -
-

未启用

-
    -
  • 引导跟踪（direct tracking）、手动打跟踪波束接口为空实现；
    • -
  • TAS 自动开启/退出逻辑停用，只支持手动转/停 TAS；
    • -
  • tracking_stop 后，队列项要等到下一次 Beam_Ctrl 才真正移除。
    • -
-
-

附录：与现有 README.md 的差异提示

-

现有 README 的 7.2 / 8.3 / 9.3 节滞后于当前源码，落地实现以本文为准：

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
项README 描述当前源码
满圈判据当前方位波位号 < 上一帧波位号方位波位号 == 0（7d5a56f，适应机扫转台反转）
点-航迹头判据按距离分段收紧速度门限（半速、1/2~1/3 速）该分支注释停用；改为统一速度区间 + 两条多普勒-几何一致性约束（7d5a56f）
起始输出历史点"临时航迹的全部 L 个点"实际输出前 track_start_point_num 个点
起始输入过滤"track_init 跳过 Use_Flag==1 的点"上游关联已删除已用点，起始流程内部不再按 Use_Flag 过滤输入
日志开关"RDP_LOG_ENABLE 默认开启"parameters.h 中该宏当前为注释状态（不落盘，日志进控制台）
-
-

建议：README 与 track_init_readme.md（航迹起始专项说明，内容与当前源码一致）可择机对齐到本文的流程描述。

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