first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,685 @@
+# 多目标追踪过滤器（TargetTracker）技术路线
+
+> 本文档将 `drone_controller/target_tracker.py` 的实现抽象为与编程语言无关的技术路线，
+> 可作为 C++（或任意语言）重新实现的规格说明。所有算法细节、参数默认值均以现有代码
+> 实际实现为准，未包含代码中不存在的功能。
+
+---
+
+## 目录
+
+1. [概述与系统定位](#1-概述与系统定位)
+2. [输入输出接口定义](#2-输入输出接口定义)
+3. [核心数据结构](#3-核心数据结构)
+4. [卡尔曼滤波算法细节](#4-卡尔曼滤波算法细节)
+5. [检测-轨迹关联流程](#5-检测-轨迹关联流程)
+6. [轨迹生命周期与 ID 管理](#6-轨迹生命周期与-id-管理)
+7. [主流程伪代码（每帧处理）](#7-主流程伪代码每帧处理)
+8. [关键参数表](#8-关键参数表)
+9. [C++ 移植要点与模块划分建议](#9-c-移植要点与模块划分建议)
+
+---
+
+## 1. 概述与系统定位
+
+### 1.1 模块职责
+
+本模块是一个**轻量级多目标追踪过滤器**，位于无人机视觉闭环控制链路中
+"目标检测"与"飞控闭环"之间，承担三类职责：
+
+1. **平滑**：检测器（YOLO 推理）输出的边界框逐帧抖动，通过恒定速度卡尔曼滤波
+   输出平滑后的目标中心点与边界框，供下游速度闭环（PID）消费。
+2. **抗短暂丢失（Coast 预测）**：目标短暂消失（如 2~5 帧）时，不立即判丢，
+   而是用滤波器预测值继续输出目标位置（`is_predicted = true`），
+   避免控制输出突变导致机体抽搐。
+3. **ID 稳定与主目标锁定**：多目标场景下通过"中心距离 + IoU 加权代价"的贪婪
+   关联维持稳定 ID；选定主目标（primary）后持续锁定，直到该轨道真正死亡，
+   避免被新出现的高置信度目标抢走导致"甩头"。
+
+### 1.2 在系统中的位置
+
+```
+ 摄像头/RTSP ──► NPU 推理(YOLO) ──► 后处理检测框
+                                        │
+                                        ▼
+                              ┌──────────────────┐
+                              │  TargetTracker   │  ◄── 每帧调用 update()
+                              │  (本模块)         │
+                              └──────────────────┘
+                                        │ 平滑中心点 / 框 / 状态标志
+                       ┌────────────────┼────────────────┐
+                       ▼                ▼                ▼
+                 20Hz 控制线程      Coast 期间         tracked=false
+                 PID → 速度指令    清空 PID 积分       触发零速刹车
+```
+
+- **调用时机**：由推理主循环在每一帧检测完成后调用一次 `update()`；
+  结果由独立的控制线程（约 20Hz 速度闭环）读取消费。
+- **上游行为约定**（来自调用方，供移植时对齐）：
+  - 输出 `is_predicted == true`（Coast 中）时，调用方会清空 PID 积分项，
+    防止积分饱和（windup）；
+  - 输出 `tracked == false`（彻底丢失）时，调用方执行零速刹车；
+  - 停止视觉闭环时调用方会调用 `reset()` 清空全部追踪状态。
+
+### 1.3 设计特点
+
+- 恒定速度模型，8 维状态向量，纯线性卡尔曼滤波（无 UKF/EKF）。
+- 关联采用**贪婪最邻近**（按轨道置信度降序逐一匹配），不使用匈牙利算法；
+  代价函数为中心距离与 (1−IoU) 的加权和。
+- 所有坐标使用**原始像素坐标**（与检测后处理输出空间一致），不做归一化。
+- 时间步长以"帧"为单位（dt = 1），与帧率无关，参数按约 30fps、640×480 画面调优。
+
+---
+
+## 2. 输入输出接口定义
+
+### 2.1 主要接口
+
+| 接口 | 说明 |
+|------|------|
+| `构造(max_lost_frames, max_association_dist, min_hits, dist_weight, iou_weight)` | 创建追踪器，参数见[第 8 节](#8-关键参数表) |
+| `update(detections, frame_shape) → TrackResult` | 每帧调用一次，输入检测列表，输出主目标追踪结果 |
+| `get_last_result() → TrackResult` | 获取上一帧结果快照（线程安全读取场景使用） |
+| `reset()` | 清空所有轨道、ID 计数器、锁定状态与缓存结果 |
+| `active_count() → int` | 当前存活轨道数量 |
+| `active_tracks() → {track_id: Tracklet}` | 当前所有轨道的只读快照（浅复制） |
+
+### 2.2 输入：detections
+
+每帧检测列表，每个元素为 6 元组：
+
+```
+[x1, y1, x2, y2, conf, cls_id]
+```
+
+| 字段 | 类型 | 含义 |
+|------|------|------|
+| x1, y1 | 浮点 | 边界框左上角（像素坐标） |
+| x2, y2 | 浮点 | 边界框右下角（像素坐标） |
+| conf | 浮点 | 检测置信度 ∈ [0,1] |
+| cls_id | 整数 | 类别编号 |
+
+- 允许传入空列表（表示本帧无任何检测），此时所有轨道进入 Coast 计数。
+- `frame_shape = (height, width)`，可选。仅在"无实测轨道、只有 Coast 轨道"
+  时用于选取主目标的兜底排序（选最接近画面中心的轨道）；未提供时按
+  640×480 的画面中心 (320, 240) 计算。
+
+### 2.3 输出：TrackResult
+
+`update()` 每帧返回一个结构体（代码中以字典表示），字段如下：
+
+| 字段 | 类型 | 含义 |
+|------|------|------|
+| tracked | 布尔 | 是否存在有效主目标。false 时调用方应刹车 |
+| primary_id | 整数或空 | 主目标轨道 ID；无目标时为空 |
+| center | (cx, cy) 或空 | 平滑后的目标中心点（像素坐标） |
+| box | [x1,y1,x2,y2] 或空 | 平滑后的边界框 |
+| confidence | 浮点 | 当前置信度（Coast 期间继承最近一次实测的置信度） |
+| is_predicted | 布尔 | true = 当前帧处于 Coast（仅有预测、无实测） |
+| lost_frames | 整数 | 主目标连续未命中帧数 |
+| n_active | 整数 | 当前活跃轨道总数 |
+| raw | [x1,y1,x2,y2] 或空 | 关联到的原始检测框；Coast 时为空 |
+
+丢失（无主目标）时的返回值：`tracked=false`，其余定位字段为空，`lost_frames=0`。
+
+---
+
+## 3. 核心数据结构
+
+模块由三层结构组成：
+
+```
+TargetTracker（多目标管理器）
+ └── Tracklet × N（单条轨道：生命周期 + 辅助状态）
+      └── KalmanBoxFilter（单个 8 维恒定速度卡尔曼滤波器）
+```
+
+### 3.1 KalmanBoxFilter（单目标滤波器）
+
+| 成员 | 类型 | 含义 |
+|------|------|------|
+| x | 数组[8] | 状态向量 [cx, cy, w, h, vx, vy, vw, vh] |
+| P | 矩阵 8×8 | 状态协方差 |
+| F | 矩阵 8×8 | 状态转移矩阵（常量） |
+| H | 矩阵 4×8 | 观测矩阵（常量） |
+| initialized | 布尔 | 是否已用首次观测初始化 |
+
+### 3.2 Tracklet（轨道）
+
+| 成员 | 类型 | 含义 |
+|------|------|------|
+| track_id | 整数 | 全局唯一轨道 ID |
+| kf | KalmanBoxFilter | 本轨道的滤波器 |
+| lost_count | 整数 | 当前连续未匹配帧数 |
+| max_lost | 整数 | 最大允许连续丢失帧数（构造时注入） |
+| confidence | 浮点 | 最近一次匹配的置信度 |
+| cls_id | 整数 | 最近一次匹配的类别 ID |
+| age | 整数 | 轨道已存在的总帧数（创建时为 1） |
+| hits | 整数 | 累计成功匹配次数（创建时为 1） |
+| last_center | (cx, cy) | 最近一次中心点（实测或预测） |
+| last_box | [x1,y1,x2,y2] 或空 | 最近一次边界框（实测或预测） |
+
+派生状态（只读属性）：
+
+- `is_dead := lost_count > max_lost`（可回收）
+- `is_coasting := lost_count > 0`（处于纯预测状态）
+
+### 3.3 TargetTracker（管理器）
+
+| 成员 | 类型 | 含义 |
+|------|------|------|
+| max_lost_frames 等 5 项 | 浮点/整数 | 配置参数，见第 8 节 |
+| next_id | 整数 | 下一个可分配的轨道 ID（从 0 递增，不复用） |
+| tracks | 哈希表 {track_id → Tracklet} | 所有存活轨道 |
+| frame_count | 整数 | 已处理帧数（仅统计用） |
+| locked_primary_id | 整数或空 | 当前锁定的主目标轨道 ID |
+| last_result | TrackResult | 上一帧结果缓存（供跨线程读取） |
+
+---
+
+## 4. 卡尔曼滤波算法细节
+
+### 4.1 状态空间模型
+
+状态向量（8 维）与观测向量（4 维）：
+
+\[
+\mathbf{x} = [c_x,\ c_y,\ w,\ h,\ v_x,\ v_y,\ v_w,\ v_h]^T,
+\qquad
+\mathbf{z} = [c_x,\ c_y,\ w,\ h]^T
+\]
+
+其中 \(c_x, c_y\) 为框中心像素坐标，\(w, h\) 为框宽高，后四项为对应速度
+（单位：像素/帧）。
+
+**状态转移**（恒定速度模型，dt = 1 帧）：
+
+\[
+\mathbf{x}_k = F\,\mathbf{x}_{k-1},\quad
+F = \begin{bmatrix} I_4 & I_4 \\ 0 & I_4 \end{bmatrix}
+\]
+
+即前 4 维（位置/尺寸）各自加上对应速度：`F[i][i+4] = 1 (i=0..3)`，其余为单位阵。
+
+**观测模型**（直接观测位置与尺寸，不观测速度）：
+
+\[
+\mathbf{z}_k = H\,\mathbf{x}_k,\quad
+H = [\,I_4 \mid 0\,] \in \mathbb{R}^{4\times 8}
+\]
+
+### 4.2 噪声参数（经验值，针对 640×480 画面调优）
+
+| 常量 | 值 | 用途 |
+|------|----|------|
+| STD_POS | 1/50 = 0.02 | 位置过程噪声比例系数 |
+| STD_VEL | 1/200 = 0.005 | 速度过程噪声比例系数 |
+| INI_POS | 2.0 | 初始位置协方差（对角） |
+| INI_VEL | 100.0 | 初始速度协方差（对角） |
+
+**关键设计：噪声与目标尺寸成比例（动态 Q/R）。**
+预测时过程噪声 Q 按当前状态中的目标宽高缩放：
+
+\[
+\sigma_{pos} = [\tfrac{w}{50},\ \tfrac{h}{50},\ \tfrac{w}{50},\ \tfrac{h}{50}],
+\quad
+\sigma_{vel} = [\tfrac{w}{200},\ \tfrac{h}{200},\ \tfrac{w}{200},\ \tfrac{h}{200}]
+\]
+
+\[
+Q = \mathrm{diag}\big(\sigma_{pos}^2,\ \sigma_{vel}^2\big)
+\]
+
+（Q 为 8×8 对角阵，前 4 个对角元来自 \(\sigma_{pos}\)，后 4 个来自 \(\sigma_{vel}\)；
+每个分量先平方再上对角。）
+
+更新时观测噪声 R 同样按当前状态的宽高缩放，并额外乘 2 增强平滑：
+
+\[
+R = \mathrm{diag}\big(2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2,\ 2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2\big)
+\]
+
+> 实现注意：R 中使用 `abs(w)`、`abs(h)` 防御负尺寸；Q 中直接使用状态值。
+> 该差异与代码保持一致即可，正常情形下尺寸为正，二者等价。
+
+### 4.3 初始化（initiate）
+
+用首次观测 \([c_x, c_y, w, h]\) 初始化：
+
+- 状态：\(\mathbf{x} = [c_x, c_y, w, h, 0, 0, 0, 0]^T\)（速度置零）
+- 协方差：\(P = \mathrm{diag}(2, 2, 2, 2, 100, 100, 100, 100)\)
+  （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
+
+### 4.4 预测（predict）
+
+未初始化时返回空。否则：
+
+\[
+\mathbf{x} \leftarrow F\,\mathbf{x}, \qquad
+P \leftarrow F\,P\,F^T + Q
+\]
+
+返回预测的位置/尺寸 \([c_x, c_y, w, h]\)（状态向量前 4 维的副本）。
+
+### 4.5 更新（update）
+
+未初始化时直接返回。给定观测 \(\mathbf{z} = [c_x, c_y, w, h]\)：
+
+\[
+S = H\,P\,H^T + R
+\]
+
+\[
+K = P\,H^T\,S^{-1}
+\]
+
+\[
+\mathbf{y} = \mathbf{z} - H\,\mathbf{x} \quad \text{（残差 / innovation）}
+\]
+
+\[
+\mathbf{x} \leftarrow \mathbf{x} + K\,\mathbf{y}, \qquad
+P \leftarrow (I_8 - K\,H)\,P
+\]
+
+> 实现注意：S 为 4×4 矩阵，代码中使用通用矩阵求逆。C++ 中可用
+> 4×4 专用求逆（如高斯消元或 LDL 分解），无需引入大型线性代数库。
+> 代码未对位置/尺寸做画面边界钳制，也未做负尺寸约束，移植时保持一致
+> （若需增强鲁棒性可另行添加，但属于行为变更）。
+
+---
+
+## 5. 检测-轨迹关联流程
+
+### 5.1 代价函数
+
+对每条轨道（预测后的中心 \((p_x, p_y)\)、预测框 \(B_t\)）与每个检测
+（中心 \((d_x, d_y)\)、检测框 \(B_d\)），定义加权代价：
+
+\[
+\mathrm{cost} = w_{dist}\cdot \frac{\lVert (p_x,p_y)-(d_x,d_y) \rVert_2}{D_{max}}
+\;+\; w_{iou}\cdot (1 - \mathrm{IoU}(B_t, B_d))
+\]
+
+- \(\lVert\cdot\rVert_2\) 为中心点欧氏距离（像素）；
+- \(D_{max}\) 为距离软阈值（`max_association_dist`，默认 200 px），
+  用于把距离项归一化到与 IoU 项同量级；实际使用时取
+  \(\max(D_{max},\ 10^{-3})\) 防止除零；
+- 默认 \(w_{dist} = w_{iou} = 0.5\)。
+
+**匹配有效性判据**：仅当 `cost < 1.0` 时视为有效匹配
+（实现上：每条轨道的最优代价初始化为 1.0，只有严格小于 1.0 的候选才会被记录）。
+
+### 5.2 IoU 计算
+
+轴对齐框 \([x_1,y_1,x_2,y_2]\) 的标准交并比：
+
+\[
+\mathrm{IoU}(A,B) = \frac{\mathrm{area}(A \cap B)}{\mathrm{area}(A) + \mathrm{area}(B) - \mathrm{area}(A \cap B)}
+\]
+
+交集宽高需与 0 取最大值；任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
+
+### 5.3 贪婪关联算法（非匈牙利）
+
+按**轨道置信度从高到低**排序后逐一贪心匹配（高置信轨道优先抢占检测）：
+
+```
+ASSOCIATE(tracks, detections):
+    若 tracks 或 detections 为空：
+        返回 matches=[], unmatched_tracks=全部轨道, unmatched_dets=全部检测
+
+    预计算每个检测的中心与框
+
+    sorted_tracks ← 按 track.confidence 降序排序的轨道列表
+    assigned_dets ← 空集合;  assigned_tracks ← 空集合;  matches ← []
+
+    对 sorted_tracks 中每条轨道 t：
+        若 t.id ∈ assigned_tracks：跳过
+        best_cost ← 1.0;  best_det ← 空
+        对每个未分配的检测 d：
+            dist ← 欧氏距离(t.last_center, d.center)
+            norm_dist ← dist / max(Dmax, 1e-3)
+            iou ← IoU(t.last_box, d.box)
+            cost ← w_dist × norm_dist + w_iou × (1 − iou)
+            若 cost < best_cost：
+                best_cost ← cost;  best_det ← d
+        若 best_det 非空：
+            matches.append((t.id, best_det.idx))
+            assigned_dets.add(best_det.idx);  assigned_tracks.add(t.id)
+
+    unmatched_tracks ← tracks 中不在 assigned_tracks 的轨道
+    unmatched_dets   ← detections 中不在 assigned_dets 的检测
+    返回 (matches, unmatched_tracks, unmatched_dets)
+```
+
+复杂度：O(M×N)（M 条轨道 × N 个检测），另有 O(M log M) 排序。
+在机载小目标数量（通常个位数）下开销可忽略。
+
+> 说明：代码明确选择贪心而非匈牙利最优匹配——贪心实现简单、无第三方依赖，
+> 配合"置信度优先 + 距离/IoU 加权"在无人机跟随单主目标的场景下已足够稳定。
+
+---
+
+## 6. 轨迹生命周期与 ID 管理
+
+### 6.1 生命周期状态
+
+代码中没有显式状态枚举，但由字段组合可归纳出四个阶段：
+
+| 阶段 | 判定条件 | 行为 |
+|------|----------|------|
+| 新建（未确认） | `hits < min_hits` | 正常参与关联与预测，但主目标选举时优先级低于已确认轨道 |
+| 已确认 | `hits ≥ min_hits` | 主目标选举的优先候选池 |
+| Coast（滑行预测） | `0 < lost_count ≤ max_lost` | 无实测，输出滤波器预测值，`is_predicted=true` |
+| 死亡 | `lost_count > max_lost` | 当帧即从轨道表中删除 |
+
+注意两点（与代码一致）：
+
+1. **min_hits 不阻止轨道参与关联或对外输出**，它只影响"重新选择主目标"时
+   的候选池优先级（见 6.3）。这与 SORT 中 tentative 轨道不输出的做法不同。
+2. **死亡判定使用严格大于**：`lost_count > max_lost`，即默认配置下允许
+   连续 8 帧 Coast，第 9 帧未命中才回收。
+
+### 6.2 ID 分配策略
+
+- 全局计数器 `next_id` 从 0 开始，每创建一个新轨道自增 1；
+- **ID 永不复用**；`reset()` 时计数器归零；
+- 未匹配到任何现有轨道的检测，每个都会创建一个新轨道（无预过滤，
+  不要求检测置信度下限——上游检测器已完成阈值过滤）。
+
+### 6.3 主目标（primary）锁定与选举
+
+对外每帧只输出**一个**主目标，供控制回路使用。选择规则：
+
+```
+SELECT_PRIMARY:
+    若轨道表为空：清除锁定，返回 tracked=false
+
+    ① 锁定延续：若 locked_primary_id 非空且该轨道仍存活：
+        直接以该轨道输出（Coast 中照样输出，is_predicted=true）
+        —— 不被其他更高置信度目标抢占
+
+    ② 否则（无锁定 / 锁定目标已死亡被回收）→ 重新选举并锁定：
+        measured  ← 所有 lost_count == 0 的轨道（本帧有实测）
+        coasting  ← 其余存活轨道
+
+        若 measured 非空：
+            confirmed ← measured 中 hits ≥ min_hits 者
+            pool ← confirmed（若为空则退回 measured）
+            chosen ← pool 中 confidence 最大者
+        否则若 coasting 非空：
+            chosen ← coasting 中离画面中心最近者（欧氏距离平方排序）
+            is_predicted ← true
+        否则：
+            清除锁定，返回 tracked=false
+
+        locked_primary_id ← chosen.id
+        以 chosen 输出结果
+```
+
+锁定的**唯一解除途径**：锁定轨道死亡被回收时清除 `locked_primary_id`
+（在轨道删除的同一处处理），或调用 `reset()`。
+
+### 6.4 生命周期状态图
+
+```mermaid
+graph TB
+    A[未匹配检测] --> B[新建轨道 hits=1 lost=0]
+    B -->|每帧匹配成功| C{hits >= min_hits?}
+    C -->|否, 选举低优先级| B
+    C -->|是, 已确认| D[正常跟踪 lost=0]
+    D -->|某帧未匹配| E[Coast lost_count+1 输出预测值]
+    E -->|重新匹配| D
+    E -->|lost_count > max_lost| F[死亡回收 删除轨道]
+    B -->|连续未匹配| E
+    F -->|若是锁定主目标| G[清除锁定 下帧重新选举]
+```
+
+---
+
+## 7. 主流程伪代码（每帧处理）
+
+`update(detections, frame_shape)` 的完整单帧流程：
+
+```
+UPDATE(detections, frame_shape):
+    frame_count += 1
+
+    # ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
+    对 tracks 中每条轨道 t：
+        t.predict()          # 卡尔曼预测；更新 t.last_center / t.last_box；age += 1
+
+    # ---- 步骤 2：构造检测中心表并关联 ----
+    若 detections 非空：
+        det_centers ← []
+        对每个检测 i = [x1,y1,x2,y2,conf,cls]：
+            cx ← (x1+x2)/2;  cy ← (y1+y2)/2
+            w  ← x2−x1;      h  ← y2−y1
+            det_centers.append((i, cx, cy, w, h, conf, cls))
+        (matches, unmatched_tracks, unmatched_dets) ← ASSOCIATE(tracks, det_centers)
+    否则：
+        matches ← [];  unmatched_tracks ← 全部轨道;  unmatched_dets ← []
+
+    # ---- 步骤 3：用匹配到的检测更新轨道 ----
+    对每个匹配 (tid, did)：
+        由 det_centers[did] 重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
+        tracks[tid].update(raw_box, conf, cls)
+        # 内部：框→[cx,cy,w,h] → kf.update；lost_count←0；hits+1；age+1
+        #       更新 last_center / last_box / confidence / cls_id
+
+    # ---- 步骤 4：未匹配轨道计丢失，死亡者回收 ----
+    对每个 tid ∈ unmatched_tracks：
+        tracks[tid].mark_miss()                 # lost_count+1, age+1
+        若 tracks[tid].is_dead：                 # lost_count > max_lost
+            若 tid == locked_primary_id：locked_primary_id ← 空
+            从 tracks 中删除 tid
+
+    # ---- 步骤 5：未匹配检测 → 创建新轨道 ----
+    对每个 did ∈ unmatched_dets：
+        以 (next_id, [cx,cy,w,h], conf, cls, max_lost=max_lost_frames) 创建 Tracklet
+        new_track.last_box ← 由中心/宽高重建的框      # 保证创建当帧即可参与输出
+        tracks[next_id] ← new_track
+        next_id += 1
+
+    # ---- 步骤 6：选取/延续主目标，组装输出 ----
+    result ← SELECT_PRIMARY(frame_shape)
+    last_result ← result          # 缓存供跨线程读取
+    返回 result
+```
+
+其中 `Tracklet.predict()` 与 `Tracklet.update()`：
+
+```
+TRACKLET_PREDICT():
+    pred ← kf.predict()
+    若 pred 非空：
+        last_center ← (pred.cx, pred.cy)
+        last_box    ← center_size → [x1,y1,x2,y2]
+        age += 1
+    返回 (last_center, last_box)
+
+TRACKLET_UPDATE(box=[x1,y1,x2,y2], conf, cls):
+    cx ← (x1+x2)/2;  cy ← (y1+y2)/2
+    w  ← x2−x1;      h  ← y2−y1
+    kf.update([cx, cy, w, h])
+    last_center ← (cx, cy);  last_box ← box
+    confidence ← conf;  cls_id ← cls
+    lost_count ← 0;  hits += 1;  age += 1
+```
+
+每帧处理的时序图：
+
+```mermaid
+graph TB
+    A[接收本帧检测] --> B[全部轨道卡尔曼预测]
+    B --> C{有检测?}
+    C -->|是| D[中心距离+IoU 贪婪关联]
+    C -->|否| E[全部轨道标记 miss]
+    D --> F[匹配轨道: 卡尔曼更新]
+    D --> G[未匹配轨道: miss 计数]
+    D --> H[未匹配检测: 新建轨道]
+    G --> I{lost > max_lost?}
+    I -->|是| J[回收轨道 必要时解除锁定]
+    I -->|否| K[主目标选择/锁定延续]
+    E --> I
+    F --> K
+    H --> K
+    J --> K
+    K --> L[输出 TrackResult]
+```
+
+---
+
+## 8. 关键参数表
+
+以下默认值全部取自代码实际实现。
+
+### 8.1 追踪器配置参数
+
+| 参数 | 默认值 | 含义与调优建议 |
+|------|--------|----------------|
+| max_lost_frames | 8 | 最大连续丢失帧数（约 250~300ms @ 30fps）。越大越能容忍遮挡，但 Coast 漂移时间越长；判丢后下游刹车 |
+| max_association_dist | 200.0（像素） | 中心距离软阈值，用于归一化距离代价。目标运动快/帧率低时调大 |
+| min_hits | 3 | 轨道"确认"所需累计匹配次数，仅影响主目标选举优先级 |
+| dist_weight | 0.5 | 关联代价中归一化距离项权重 |
+| iou_weight | 0.5 | 关联代价中 (1−IoU) 项权重。两项建议之和保持 1.0，使 cost<1.0 判据语义稳定 |
+
+> 工程部署时以上参数可由外部配置文件注入（项目中即通过配置文件的
+> `target_tracker` 段传入，未配置项回落到代码默认值）。
+
+### 8.2 卡尔曼滤波内部常量
+
+| 常量 | 值 | 含义 |
+|------|----|------|
+| dt | 1.0 | 状态转移步长，以"帧"为单位 |
+| STD_POS | 1/50 | 位置过程/观测噪声比例系数（乘以目标宽高） |
+| STD_VEL | 1/200 | 速度过程噪声比例系数（乘以目标宽高） |
+| INI_POS | 2.0 | 初始位置协方差 |
+| INI_VEL | 100.0 | 初始速度协方差 |
+| R 缩放因子 | 2.0 | 观测噪声额外加倍，增强输出平滑 |
+| 匹配代价上限 | 1.0 | cost ≥ 1.0 判为不可匹配 |
+| Coast 兜底画面中心 | (320, 240) | frame_shape 缺省时按 640×480 计算 |
+
+---
+
+## 9. C++ 移植要点与模块划分建议
+
+### 9.1 模块 / 类划分草案
+
+与代码三层结构一一对应：
+
+```
+target_tracker/
+ ├── kalman_box_filter.h/.cpp   // class KalmanBoxFilter
+ ├── tracklet.h/.cpp            // struct/class Tracklet
+ └── target_tracker.h/.cpp      // class TargetTracker + struct TrackResult
+```
+
+```cpp
+// ---- 基础几何量 ----
+struct BoxXYXY  { double x1, y1, x2, y2; };
+struct BoxCXCYWH{ double cx, cy, w, h;  };
+
+// ---- 单目标滤波器 ----
+class KalmanBoxFilter {
+    double x[8];        // 状态向量
+    double P[8][8];     // 协方差
+    static constexpr double F[8][8] = /* I4 右上 I4 */;
+    static constexpr double H[4][8] = /* [I4 | 0] */;
+    bool initialized = false;
+
+    void initiate(const BoxCXCYWH& z);
+    bool predict(BoxCXCYWH& out);      // 未初始化返回 false
+    void update(const BoxCXCYWH& z);
+};
+
+// ---- 轨道 ----
+struct Tracklet {
+    int track_id;
+    KalmanBoxFilter kf;
+    int lost_count = 0;
+    int max_lost;
+    double confidence;
+    int cls_id;
+    int age = 1;
+    int hits = 1;
+    std::pair<double,double> last_center;
+    std::optional<BoxXYXY> last_box;   // 首帧预测前可能为空（创建时即赋实测框）
+
+    bool is_dead() const      { return lost_count > max_lost; }
+    bool is_coasting() const  { return lost_count > 0; }
+};
+
+// ---- 输出结构 ----
+struct TrackResult {
+    bool tracked = false;
+    std::optional<int> primary_id;
+    std::pair<double,double> center;   // tracked=false 时无效
+    std::optional<BoxXYXY> box;
+    double confidence = 0.0;
+    bool is_predicted = false;
+    int lost_frames = 0;
+    int n_active = 0;
+    std::optional<BoxXYXY> raw;
+};
+
+// ---- 管理器 ----
+class TargetTracker {
+public:
+    struct Config {
+        int    max_lost_frames      = 8;
+        double max_association_dist = 200.0;
+        int    min_hits             = 3;
+        double dist_weight          = 0.5;
+        double iou_weight           = 0.5;
+    };
+    explicit TargetTracker(const Config& cfg);
+    TrackResult update(const std::vector<Detection>& dets,
+                       std::optional<FrameShape> frame_shape);
+    TrackResult last_result() const;   // 需要跨线程时加锁/原子快照
+    void reset();
+};
+```
+
+### 9.2 STL 容器选择
+
+| 代码中的结构 | C++ 建议 |
+|--------------|----------|
+| 轨道哈希表 `{id: Tracklet}` | `std::unordered_map<int, Tracklet>`；若需按 ID 有序遍历可用 `std::map` |
+| 关联中的已分配集合 | `std::unordered_set<int>`，或目标数量少时直接用 `std::vector<bool>` 位标记 |
+| 按置信度排序的轨道视图 | 复制 ID/指针到 `std::vector` 后 `std::sort`（不直接排哈希表） |
+| 检测列表 | `std::vector<Detection>`，`Detection` 为 POD 结构体 |
+| 结果缓存 | `TrackResult` 值类型；跨线程用互斥锁保护的拷贝，或双缓冲原子交换 |
+
+### 9.3 数值计算
+
+- 矩阵规模极小（8×8、4×8、4×4），**无需引入 Eigen/OpenCV**：
+  用固定大小二维数组或 `std::array<double, 64>` 手写矩阵乘法即可。
+- 唯一需要通用实现的是 4×4 矩阵求逆（S 矩阵）。可用高斯-约当消元或
+  利用 S 接近对称正定的特性做 Cholesky/LDL 分解；建议加极小量
+  正则化（对角加 1e-9）防奇异。
+- 距离计算用 `std::hypot`；IoU 为纯标量运算。
+- 全程 `double` 精度，与代码一致（代码为 float64）。
+
+### 9.4 行为一致性检查清单
+
+移植后建议用以下用例对齐行为：
+
+1. **空检测帧序列**：连续输入空检测，主目标应输出 `is_predicted=true`
+   共 `max_lost_frames` 帧，第 `max_lost_frames + 1` 帧 `tracked=false`
+   （因死亡判定为 `lost_count > max_lost`）。
+2. **ID 单调性**：多次进出画面的目标应获得严格递增、不复用的 ID。
+3. **锁定不被抢占**：锁定目标 Coast 期间出现更高置信度新目标，
+   `primary_id` 不变；锁定目标死亡后切换到新目标。
+4. **min_hits 语义**：新轨道（hits 未达 min_hits）在无其他已确认实测轨道时
+   也可被选为主目标（候选池回退逻辑）。
+5. **cost 边界**：恰好 `cost = 1.0` 不构成匹配（严格小于）。
+6. **置信度优先**：两轨道竞争同一检测时，置信度高者获胜。
+
+### 9.5 线程模型注意
+
+原系统中追踪器在推理主线程更新，结果由 20Hz 控制线程读取。C++ 移植若
+保持该模型，仅需保护 `last_result` 的读写（互斥锁或无锁双缓冲），
+追踪器内部状态不必加锁（单线程写）。若改为多线程调用 `update()`，
+则需整体加锁。