first commit

.idea/.gitignore

@@ -0,0 +1,5 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/

.idea/code_drone-test.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

.idea/inspectionProfiles/Project_Default.xml

@@ -0,0 +1,27 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
+      <option name="ignoredErrors">
+        <list>
+          <option value="N801" />
+          <option value="N806" />
+        </list>
+      </option>
+    </inspection_tool>
+    <inspection_tool class="PyStubPackagesAdvertiser" enabled="true" level="WARNING" enabled_by_default="true">
+      <option name="ignoredPackages">
+        <list>
+          <option value="scipy" />
+        </list>
+      </option>
+    </inspection_tool>
+    <inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
+      <option name="ignoredIdentifiers">
+        <list>
+          <option value="moviepy.*" />
+        </list>
+      </option>
+    </inspection_tool>
+  </profile>
+</component>

.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/code_drone-test.iml" filepath="$PROJECT_DIR$/.idea/code_drone-test.iml" />
+    </modules>
+  </component>
+</project>

.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md

@@ -0,0 +1,49 @@
+# C++ → Python 移植计划（reference-c++ → utils）
+
+## 范围与环境
+- 移植 `reference-c++/target_tracker` 与 `reference-c++/visual_track_control` 两个子项目为 Python，放入 `utils/`；**不改动 reference-c++ 任何文件**。
+- Python 解释器：`D:\program file\conda\envs\pytorch\python.exe`（3.12.13，mavsdk 3.15.3 / numpy 2.4.3 已装）。需向该环境 `pip install pytest`。
+- 行为保真优先：单位、符号约定、限幅顺序、超龄/判丢/Coast 语义、异常消息文本全部与 C++ 版及规格文档一致。
+
+## 目录布局
+```
+utils/
+├── README.md                      # 唯一文档（结构、运行、MAVSDK 映射决策、实机接线片段）
+├── conftest.py                    # pytest sys.path 注入，使两包可顶层导入
+├── target_tracker/                # 包1：追踪库（仅依赖 stdlib + numpy）
+│   ├── __init__.py                # 导出 TargetTracker/TrackResult/Detection/FrameShape 等
+│   ├── kalman_box_filter.py       # 8 维恒速卡尔曼（numpy 矩阵；Q 用状态值、R 用 abs×2 的规格差异保留）
+│   ├── tracklet.py                # 轨道 + ToCenterSize/ToXYXY/ComputeIoU
+│   ├── target_tracker.py          # 六步主流程 + 贪婪关联 + 主目标锁定；LastResult 用 threading.Lock
+│   └── tests/                     # pytest 移植 27 条（kalman 5 / tracklet 12 / tracker 10，含 §9.4 六条验收）
+├── visual_track_control/          # 包2：控制律（不 import target_tracker）
+│   ├── __init__.py
+│   ├── vtc_types.py               # TrackObservation/ControlSnapshot/ControlOutput/枚举（frozen dataclass）
+│   ├── vtc_config.py              # stdlib json 加载，校验异常消息与 C++ 一致（含字段路径）
+│   ├── line_of_sight.py / pid_controller.py / rate_limiter.py / staleness_checker.py / frame_transform.py
+│   ├── tracking_control_law.py    # 八步单周期主流程，logging 仅模式切换打点
+│   ├── offboard_sender.py         # OffboardSender 协议 + YawIntegrator + LoopbackSender（演示/测试）
+│   ├── mavsdk_sender.py           # MAVSDK 实现（mavsdk 懒加载，其余模块零感知）
+│   ├── config/track_control.json  # 从 C++ 原样拷贝
+│   └── tests/                     # pytest 移植 8 套
+└── examples/
+    └── track_control_demo.py      # 唯一同时 import 两包的胶水点
+```
+
+## 关键设计
+1. **解耦边界**：C++ 版 ControlSnapshot 内嵌 tracker::TrackResult；Python 改为控制律自有的最小结构 `TrackObservation(tracked, is_predicted, center)`。visual_track_control 全程不出现 target_tracker 导入，demo 负责字段拷贝。两包内部一律相对导入、可独立搬移；mavsdk 只在 mavsdk_sender.py 内懒加载。
+2. **MAVSDK 映射（已确认方案）**：统一 `set_velocity_ned(VelocityNedYaw)`。
+   - 速率式：`YawIntegrator` 把 yaw_rate 积分为绝对航向（wrap180，dt 取发送侧单调时钟差）→ VelocityNedYaw(vn, ve, vd, 积分航向)；
+   - 位置式：直接用 out.yaw_deg；刹车：零速 + 保持积分航向；降级：不发送。
+   - 控制律输出仍与 C++ 一致（机体系计算限幅→输出端旋转为 NED），NED 分量直接对应 MAVSDK 字段；OffboardError 捕获后返回 False（对齐 C++ Send 布尔语义）。offboard.start()/stop() 生命周期不属于 sender，README 给接线片段。
+3. **数值**：全程 Python float（双精度），卡尔曼用 numpy（4×4 求逆 + 1e-9 正则化）；保留检测入口 NaN/Inf 过滤、置信度 NaN→−∞ 排序等防御逻辑；时间戳 `time.monotonic_ns()//10**6`。
+4. **MAVSDK sender 测试**：用 fake offboard 对象（鸭子类型）验证分发与积分行为，不 import mavsdk；gateway 相关 C++ 字节级用例改为验证 sender 收到的 VelocityNedYaw 内容。
+
+## 验证与交付
+1. `pip install pytest`（pytorch 环境）→ 两包测试全绿；
+2. 运行 demo（asyncio，20Hz，三阶段：正常 6s → Coast 4s → 判丢 3s，演示判丢阈值 60 帧），输出语义与 C++ 版一致；
+3. mavsdk_sender 冒烟测试（fake offboard，不连实机）；
+4. 撰写 utils/README.md。
+
+## 实施顺序
+pip 安装 pytest → target_tracker 包+测试 → visual_track_control 包+测试 → sender 层+测试 → demo → README → 汇总报告。

docs/物理追踪思路.md

@@ -0,0 +1,371 @@
+# 视觉跟踪控制律设计（FlightController 实现期规格）
+
+## 1. 文档状态与范围
+
+### 1.1 文档状态
+
+- 本文档为**实现期规格文档**，由早期草稿《物理追踪实现路线 v1.0》重写而来：
+  原文档中与既有设计体系冲突的表述（摄像头型号、飞控直连、控制时机、
+  拦截设备绑定、雷达距离语义等）已全部修正，合理的控制思路
+  （水平角控制、垂直控制、距离控制）保留并纳入本规格。
+- 当前相关代码均为骨架占位实现，与 `docs/开发进度.md`（第 6 步未开始）一致：
+  - `src/control/flight_controller.cpp`（IFlightController Stub）
+  - `src/state_machine/mission_state_machine.cpp`（IMissionStateMachine Stub）
+  - `src/perception/target_estimator.cpp`（ITargetEstimator Stub）
+- 本文档中所有阈值、限幅值、PID 参数**只列占位，不填具体数值**，
+  一律标注"待定/配置化"（需求分析.md NFR-007）。
+
+### 1.2 范围与状态机对应
+
+本文档定义视觉跟踪阶段（位置→速度控制切换之后）的控制律：
+
+| 状态机阶段 | 本文档覆盖内容 |
+|------------|----------------|
+| S5 GPS_APPROACH | 仅提供上下文（位置引导阶段，控制律不在本文档范围） |
+| S6 VISUAL_HANDOVER | 切换后首条限幅指令的产生要求（切换条件本身见 状态机设计.md S6） |
+| S7 VISUAL_TRACKING | **本文档核心**：水平航向、垂直/高度、距离三路控制律 |
+| S8 OBSTACLE_HOLD | 控制律输出被障碍策略接管（限速/刹停），见第 9 章 |
+
+### 1.3 文档定位与引用关系
+
+本文档定位为**视觉跟踪控制律设计（FlightController 实现期规格）**。
+与以下内容重叠的部分一律引用、不复述：
+
+| 引用文档 | 引用章节 | 本文档不重复的内容 |
+|----------|----------|--------------------|
+| `docs/状态机设计.md` | S6、S7、§4.2、§7 | 切换守卫条件、目标丢失转移、可替换策略接口 |
+| `docs/感知与避障设计.md` | §3、§5、§6、§9 | 标定流程、稳定锁定、雷达关联条件、降级原则 |
+| `docs/视觉追踪技术路线.md` | §1.2、§2 | TargetTracker 算法细节、Coast/判丢行为约定 |
+| `docs/系统架构设计.md` | §6、§8 | 数据流、控制单出口、时间与控制周期 |
+| `docs/数据接口文档.md` | §2.2、§3、§4.9~§4.12、§5 | 消息字段定义、接口签名、Topic 一览 |
+| `docs/需求分析.md` | FR-033、FR-041、FR-051、NFR-007、NFR-008、§7 | 需求条文本身 |
+| `include/common/types.h` | 全文 | 消息类型与 Topic 常量的权威定义 |
+| `target_tracker/target_tracker.md` | §2、§7 | 追踪库接口与未来接线指引 |
+
+## 2. 输入输出与数据流
+
+### 2.1 输入
+
+控制律在状态机与控制线程中按固定周期消费以下快照（Topic 名称常量见
+`include/common/types.h` 的 `drone::common::topics` 命名空间）：
+
+| Topic 常量 | 消息类型 | 用途 |
+|------------|----------|------|
+| `kEstimatedTarget` | `TargetState` | 目标估计状态（位置/速度/置信度/最近观测时间），控制律主输入 |
+| `kFlightState` | `FlightStateSnapshot` | 本机姿态、速度、局部位置，用于坐标转换与反馈 |
+| `kLaserRange` | `LaserRangeSample` | 机头前向单点距离；**仅在相机—雷达标定完成且空间关联成立时**才可作为目标距离，否则仅为前向安全距离（需求分析.md FR-041） |
+| `kGroundTarget` | `GroundStationTarget` | 地面站目标状态，用于丢失校验与降级回退（非视觉控制主输入） |
+
+### 2.2 输出与控制链路
+
+控制律不直接写飞控。链路严格遵循 系统架构设计.md §6 的控制单出口约束：
+
+```text
+状态机与控制线程（控制律，本文档范围）
+  -> ControlIntent（控制意图，kControlIntent）
+  -> IFlightController（意图 -> 设定值转换与限幅）
+  -> Px4Setpoint（kPx4Setpoint）
+  -> IPx4Link（PX4 通信线程）
+  -> 飞控串口
+```
+
+- 只有 PX4 通信线程可以写飞控串口；控制律只提交意图
+  （系统架构设计.md §6、数据接口文档.md §4.9）。
+- 视觉跟踪阶段主要使用：
+  - `ControlIntentType::kVelocityHeading`（速度与航向意图）：
+    `target_x/target_y/target_z` 为速度分量（坐标系由 `header.frame_id` 给出），
+    `yaw_deg` 为目标航向，`max_speed_mps` 携带速度限制；
+  - `ControlIntentType::kBrakeHover`（刹停/悬停意图）：目标丢失判丢、
+    障碍制动等场景；
+  - `ControlIntentType::kManualOverride`（禁止自动输出/人工接管意图）：
+    遥控器接管时停止任务控制输出。
+- `Px4Setpoint` 使用 `SetpointType::kVelocity`（速度设定值，分量写入
+  `x`、`y`、`z`）或 `SetpointType::kAttitude`（姿态微调，仅限垂直小误差
+  支路，见第 5 章）；航向写入 `yaw_deg` 或 `yaw_rate_dps`；
+  无效设定值（如刹停完成）也需发送，避免过期指令（`valid` 标志语义见
+  `include/common/types.h`）。
+
+### 2.3 坐标系编码
+
+所有空间数据消息显式使用 `MessageHeader.frame_id` 编码
+（数据接口文档.md §2.2、`include/common/types.h` 文件头约定）：
+
+```text
+0 = 未知
+1 = WGS84 经纬度
+2 = 机体（前右下）
+3 = 局部NED
+4 = 局部ENU
+```
+
+控制律内部计算统一在**机体坐标系（frame_id = 2）**下进行
+（视线角→机体前/右/下分量），输出的速度意图若采用局部系
+（frame_id = 3），必须随 `FlightStateSnapshot` 的姿态/航向完成旋转，
+并在 `header.frame_id` 中显式标注；禁止混用未标注坐标系的数据。
+
+## 3. 坐标与标定前提
+
+### 3.1 转换链路
+
+```text
+目标像素位置 (u, v)
+  ->（相机内参 + 畸变校正）-> 归一化像平面方向
+  ->（相机→机体固定外参）-> 机体视线方向
+  ->（结合当前机体姿态）-> 水平偏航误差 / 垂直俯仰误差
+  ->（雷达关联成立时叠加距离）-> 机体系相对位置 (Δ前, Δ右, Δ下)
+```
+
+视觉像素只有结合相机模型和对应时刻的机体姿态，才能转换为目标相对机体
+的视线方向（感知与避障设计.md §3）。
+
+### 3.2 标定前提与待定项
+
+实现视觉控制前必须完成（感知与避障设计.md §3、需求分析.md §7）：
+
+- 相机内参与畸变标定（**待定**：相机内参、畸变参数未标定）；
+- 相机坐标系到机体坐标系的固定外参（**待定**：外参未标定）；
+- 激光波束在相机图像中的投影位置或区域（**待定**：相机—雷达标定未完成）；
+- 图像、检测、光流、雷达和 PX4 姿态的时间对齐；
+- 目标和本机高度基准（**待定**，需求分析.md §7）。
+
+摄像头为 SIYI A8mini，经网口以 RTSP 输出 H.265、1080p、25 FPS 视频
+（需求分析.md §3、开发思路.md）；画面宽度 `W`、高度 `H` 与帧率以实际
+解码配置为准。标定完成前，控制律只允许在 SITL/台架中以录制数据回放
+验证数学链路，不得上实飞。
+
+## 4. 水平航向控制
+
+### 4.1 目标与原理
+
+目标：让机头在水平方向对准目标。由目标像素水平偏移反求偏航修正量，
+经限幅后作为航向指令。
+
+设目标中心像素横坐标偏移为 `x_offset`（目标中心相对画面中心，右正），
+图像宽度为 `W`，相机**水平视场角**为 `θ_h`（注意：`θ_h` 仅表示水平
+视场角，与俯仰姿态角无关，不得复用同一符号）。
+
+针孔模型下的精确表述：
+
+```
+β = atan( x_offset * 2 * tan(θ_h / 2) / W )
+```
+
+当目标靠近画面中心（|x_offset| 相对 W 较小）时，可用线性近似：
+
+```
+β ≈ x_offset * ( θ_h / W )
+```
+
+线性近似的适用条件：
+
+- 偏移量处于画面中心附近（具体适用范围**待定**，由标定与实测确定）；
+- 已完成畸变校正（畸变在画面边缘放大近似误差）；
+- 大偏移场景应使用针孔模型表述，或直接采用归一化像平面方向。
+
+机体姿态变化时，像素偏移同时包含本机转动与目标运动两部分，
+控制律需结合 `FlightStateSnapshot` 的姿态与角速度做补偿
+（实现期细化，补偿结构**待定**）。
+
+### 4.2 输出映射
+
+- 航向指令映射到 `ControlIntentType::kVelocityHeading` 的 `yaw_deg`，
+  或由 `Px4Setpoint` 的 `yaw_deg` / `yaw_rate_dps` 表达；
+- 采用位置式（β→目标航向）还是速率式（β→偏航角速度）由实现期
+  按 PX4 1.17.0 Offboard 行为确定（**待定**）；
+- 偏航角速度与方向变化率必须限幅（需求分析.md FR-033），
+  限幅值见第 8 章。
+
+## 5. 垂直 / 高度控制
+
+### 5.1 误差来源
+
+垂直误差由两个来源合成：
+
+1. 像素垂直偏移：目标中心相对画面中心的纵坐标偏移 `y_offset`（下正），
+   图像高度 `H`，相机**垂直视场角** `θ_v`。与第 4 章同理，垂直视线角：
+
+   ```
+   α = atan( y_offset * 2 * tan(θ_v / 2) / H )        （针孔模型）
+   α ≈ y_offset * ( θ_v / H )                          （中心附近线性近似）
+   ```
+
+2. 距离信息：只有雷达与目标完成空间关联（见 3.2 与第 6.3 节）时，
+   才可由测距距离 `d` 换算高度差：
+
+   ```
+   Δh = d * sin(α)
+   ```
+
+   （等价于早期草稿中的 `Δh = d × cos(90° − α)`；此处 α 为垂直视线角，
+   与水平视场角 θ_h、垂直视场角 θ_v 明确区分命名。）
+
+雷达未关联时，垂直通道只能使用角度误差做姿态/速率级控制，
+不得假设已知高度差。
+
+### 5.2 控制策略
+
+垂直控制按角度误差大小分两支（沿用原草稿思路，阈值待定）：
+
+- **角度误差较小**：直接微调姿态角（`SetpointType::kAttitude`），
+  仅作辅助，必须有严格姿态角与姿态变化率限幅（需求分析.md FR-033）；
+- **角度误差较大**：调整高度，使用速度设定值（`SetpointType::kVelocity`
+  的 `z` 分量，局部 NED 下为垂直速度）趋近目标高度。
+
+角度误差切换阈值、姿态角限幅、垂直速度限幅均**待定**（第 8 章）。
+
+## 6. 距离控制
+
+### 6.1 目标
+
+距离控制使目标进入拦截射程。**射程进入条件由拦截设备接口
+（`IInterceptionDevice`）与配置给出**，本文档不假设任何具体拦截方式；
+具体捕网或其他拦截方式不得进入通用逻辑（需求分析.md FR-051、
+状态机设计.md §7）。射程 `d_exp` 为配置项，数值**待定**。
+
+### 6.2 控制律
+
+设目标距离为 `d`（来源见 6.3），射程为 `d_exp`：
+
+```
+距离误差：Δd = d - d_exp
+前向速度指令 = PID(Δd)
+```
+
+- 对 `Δd` 做 PID 得到机体前向速度指令，采用速度指令实现
+  （`ControlIntentType::kVelocityHeading` 的速度分量 →
+  `SetpointType::kVelocity`）；
+- `d > d_exp` 时前向接近，`d < d_exp` 时减速/后退，接近末端的速度
+  必须按限幅约束收敛；
+- PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部**待定/配置化**。
+
+### 6.3 距离来源与降级
+
+- 雷达距离**不得直接当作目标距离**：`LaserRangeSample` 表达机头前向
+  单点距离，不得自动解释为任意视觉目标的距离
+  （`include/common/types.h` 注释、通信与数据定义.md §7）。
+  仅当相机—雷达标定完成、视觉区域覆盖雷达投影点、时间足够接近且
+  测量质量有效时，才可关联为目标距离（需求分析.md FR-041、
+  感知与避障设计.md §3）；
+- 关联失败时，雷达距离仅作为**前向安全距离**，供障碍策略
+  （第 9 章）使用；距离通道此时按配置降级：保持当前速度、
+  限速接近或退出近距操作（具体降级动作**待定**）；
+- 视觉估计的距离趋势（目标估计器输出）可作为辅助输入，
+  融合权重**待定**。
+
+### 6.4 与追踪库的行为衔接
+
+沿用 视觉追踪技术路线.md §1.2 的上游行为约定：
+
+- 追踪库输出 `is_predicted == true`（Coast 预测中）时，距离与航向
+  PID 的**积分项清空**，防止积分饱和；
+- 追踪库输出 `tracked == false`（判丢）时，执行**零速刹车**
+  （`ControlIntentType::kBrakeHover`），并按 状态机设计.md §4.2
+  的目标丢失转移处理；
+- 停止视觉闭环时调用追踪库 `Reset()` 清空状态。
+
+## 7. 控制周期与时序
+
+### 7.1 固定控制周期
+
+- 控制线程按**固定周期持续运行**，不等待任意传感器消息后被动执行；
+  视觉跟踪控制律在每个控制周期内持续执行，而非"检测完成后才进行"
+  （系统架构设计.md §8）；
+- 每次控制计算使用同一时刻附近的 PX4、视觉、雷达和目标数据快照；
+- 超龄数据只能触发降级或告警，不能继续生成正常控制指令
+  （超龄判定使用 `MessageHeader.receive_time_ms` + `valid_for_ms`）；
+- 控制周期频率与各输入超龄阈值**待定/配置化**（系统架构设计.md §8）。
+
+### 7.2 与 target_tracker 的衔接
+
+目标追踪库（`target_tracker/`，已实现，见 `target_tracker/target_tracker.md`）
+承担像素级平滑、Coast 与主目标锁定：
+
+- 推理侧每帧调用 `TargetTracker::Update()`，控制线程（约 20Hz 速度闭环）
+  读取 `TrackResult`（`LastResult()` 线程安全拷贝）；
+- 控制线程消费 `TrackResult` 的平滑中心点计算第 4/5 章角度误差，
+  并执行 6.4 的 Coast/判丢约定；
+- `TrackResult` 属于追踪库自有类型，**尚未进入
+  `include/common/types.h`**；`target_tracker/target_tracker.md` §7
+  提议新增 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
+  该提议当前**未实现**，接入主工程时需先在数据接口文档与
+  types.h 中正式定义。
+
+## 8. 限幅、阈值与配置化
+
+所有阈值集中于此并进入配置文件，不得散落在代码中
+（需求分析.md NFR-007）。视觉跟踪的速度、加速度和方向变化
+必须限幅（需求分析.md FR-033）。
+
+| 项目 | 说明 | 状态 |
+|------|------|------|
+| 水平/垂直角度误差切换阈值 | 姿态微调与高度/速率控制的分支条件 | 待定 |
+| 射程 `d_exp` | 由拦截设备接口与配置给出 | 待定 |
+| 距离通道 PID 参数 | Kp / Ki / Kd、积分限幅 | 待定 |
+| 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 |
+| 垂直速度限幅 | 上升/下降速度上限 | 待定 |
+| 姿态角限幅 | 姿态微调支路的角与角速率上限 | 待定 |
+| 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 |
+| 加速度限幅 | 各轴加速度上限 | 待定 |
+| 控制周期频率 | 控制线程固定周期 | 待定 |
+| 各输入超龄阈值 | 视觉/雷达/姿态/目标估计 | 待定 |
+| 线性近似适用范围 | 画面中心偏移比例 | 待定 |
+
+首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
+（状态机设计.md S6）。
+
+## 9. 异常与降级
+
+### 9.1 目标丢失
+
+- 判丢后执行零速刹车并调用 `ITargetLostStrategy`：短时预测
+  （预测时长与不确定度有上限）、最新有效 `kGroundTarget` 校验、
+  整机转向搜索或退回 `GPS_APPROACH`（状态机设计.md §4.2、
+  感知与避障设计.md §5）；
+- 卡尔曼预测不得无限期生成控制指令（需求分析.md FR-034）。
+
+### 9.2 传感器降级
+
+按 感知与避障设计.md §9 的降级原则，与本文档控制律相关者：
+
+| 故障 | 视觉跟踪阶段动作 |
+|------|------------------|
+| 摄像头失效 | 退出视觉跟踪 |
+| 光流失效 | 降低跟踪可信度和速度 |
+| 雷达失效 | 降低速度或退出近距操作；距离通道仅保留角度级控制 |
+| 地面站目标过期 | 可按策略短时视觉跟踪 |
+| PX4 姿态过期 | 禁止空间转换和控制，立即退出正常控制 |
+
+### 9.3 障碍物规避
+
+前向危险时控制律输出被障碍策略接管：清除视觉速度意图，限速、刹停并
+悬停（S8 OBSTACLE_HOLD，状态机设计.md S8、感知与避障设计.md §7）。
+雷达未关联时的前向安全距离即该策略的主输入。危险解除后必须重新
+验证入口条件才可恢复视觉控制（需求分析.md FR-044）。
+
+## 10. 待定项与验证计划
+
+### 10.1 待定项清单
+
+| 待定项 | 依据 |
+|--------|------|
+| 相机内参、畸变参数、相机—机体—雷达外参 | 需求分析.md §7 |
+| 目标和本机高度基准 | 需求分析.md §7 |
+| 第 8 章全部阈值、限幅与 PID 参数 | 需求分析.md §7、NFR-007 |
+| 雷达—目标空间关联的具体判据参数 | 感知与避障设计.md §3 |
+| 航向控制位置式/速率式选择 | PX4 1.17.0 Offboard 实测 |
+| 机体姿态补偿结构 | 实现期细化 |
+| `TrackedTarget` / `kTrackedTarget` 消息定义 | target_tracker.md §7 提议，未入库 |
+
+### 10.2 验证路径
+
+按 需求分析.md NFR-008 与 系统架构设计.md §11 逐级验证：
+
+1. **开发机单元测试**：控制律纯函数（角度换算、PID、限幅）用
+   Google Test 覆盖；录制数据回放验证像素→视线角数学链路；
+2. **PX4 SITL**：在仿真中验证设定值流、限幅行为、丢失/降级分支；
+3. **串口台架**：真实飞控 + 模拟数据，验证时序、超龄判定与
+   控制权仲裁；
+4. **受控实飞**：按状态机设计验证 S6→S7 切换与跟踪效果。
+
+必测故障项包含旧目标位置、旧视频帧、雷达异常、遥控器接管、
+算力板控制输出中断（系统架构设计.md §11）。

docs/视觉追踪技术路线.md

@@ -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()`，
+则需整体加锁。

reference-c++/target_tracker/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.16)
+
+# =============================================================================
+# target_tracker —— 视觉追踪库（独立动态库工程）
+#
+# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。
+# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。
+# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。
+# =============================================================================
+
+project(target_tracker VERSION 1.0.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# ---- 动态库目标 ----
+add_library(target_tracker SHARED
+    src/kalman_box_filter.cpp
+    src/tracklet.cpp
+    src/target_tracker.cpp)
+target_include_directories(target_tracker PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+target_compile_features(target_tracker PUBLIC cxx_std_17)
+
+# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加
+# visibility=hidden 与导出宏），全部公共类直接对外导出。
+set_target_properties(target_tracker PROPERTIES
+    VERSION 1.0.0
+    SOVERSION 1)
+
+# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic）
+if(MSVC)
+    target_compile_options(target_tracker PRIVATE /W4 /permissive-)
+else()
+    target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic)
+endif()
+
+# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ----
+include(CTest)
+if(BUILD_TESTING)
+    find_package(GTest QUIET)
+    if(GTest_FOUND)
+        include(GoogleTest)
+
+        # 测试可执行文件直接链接共享库 target_tracker；
+        # BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行
+        set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN")
+
+        # 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为
+        add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp)
+        target_link_libraries(kalman_box_filter_test PRIVATE
+            target_tracker GTest::gtest_main)
+        if(MSVC)
+            target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-)
+        else()
+            target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic)
+        endif()
+        gtest_discover_tests(kalman_box_filter_test)
+
+        # 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数
+        add_executable(tracklet_test tests/tracklet_test.cpp)
+        target_link_libraries(tracklet_test PRIVATE
+            target_tracker GTest::gtest_main)
+        if(MSVC)
+            target_compile_options(tracklet_test PRIVATE /W4 /permissive-)
+        else()
+            target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic)
+        endif()
+        gtest_discover_tests(tracklet_test)
+
+        # 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例
+        add_executable(target_tracker_test tests/target_tracker_test.cpp)
+        target_link_libraries(target_tracker_test PRIVATE
+            target_tracker GTest::gtest_main)
+        if(MSVC)
+            target_compile_options(target_tracker_test PRIVATE /W4 /permissive-)
+        else()
+            target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic)
+        endif()
+        gtest_discover_tests(target_tracker_test)
+    else()
+        message(STATUS "GoogleTest not found; target_tracker tests will not be built")
+    endif()
+endif()

reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h

@@ -0,0 +1,87 @@
+#pragma once
+
+// =============================================================================
+// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器
+//
+// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量）
+//   状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1）
+//   观测向量 z[4] = [cx, cy, w, h]（§4.1）
+//   时间步长 dt = 1 帧，与帧率无关（§1.3）
+//
+// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算，
+// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。
+// =============================================================================
+
+#include <array>
+#include <optional>
+
+namespace drone {
+namespace tracker {
+
+/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）
+struct BoxCXCYWH {
+    double cx = 0.0;  // 框中心 x（像素）
+    double cy = 0.0;  // 框中心 y（像素）
+    double w = 0.0;   // 框宽（像素）
+    double h = 0.0;   // 框高（像素）
+};
+
+/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4）
+class KalmanBoxFilter {
+public:
+    KalmanBoxFilter() = default;
+
+    /// 是否已用首次观测初始化（§3.1 initialized 字段）
+    bool Initialized() const { return initialized_; }
+
+    /// 用首次观测 [cx, cy, w, h] 初始化（§4.3）：
+    /// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。
+    void Initiate(const BoxCXCYWH& z);
+
+    /// 预测推进一帧（§4.4）：未初始化返回 std::nullopt；
+    /// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。
+    std::optional<BoxCXCYWH> Predict();
+
+    /// 观测更新（§4.5）：未初始化时直接返回；
+    /// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。
+    void Update(const BoxCXCYWH& z);
+
+    /// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用）
+    const std::array<double, 8>& State() const { return x_; }
+
+    // ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ----
+    static constexpr double kStdPos = 1.0 / 50.0;   // STD_POS：位置噪声比例系数
+    static constexpr double kStdVel = 1.0 / 200.0;  // STD_VEL：速度噪声比例系数
+    static constexpr double kIniPos = 2.0;          // INI_POS：初始位置协方差
+    static constexpr double kIniVel = 100.0;        // INI_VEL：初始速度协方差
+    static constexpr double kRScale = 2.0;          // R 额外缩放因子（增强平滑，§8.2）
+
+private:
+    std::array<double, 8> x_{};                // 状态向量（§3.1）
+    std::array<std::array<double, 8>, 8> P_{}; // 状态协方差 8×8（§3.1）
+    bool initialized_ = false;                 // 是否已初始化（§3.1）
+
+    // 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1
+    static constexpr std::array<std::array<double, 8>, 8> kF = {{
+        {{1, 0, 0, 0, 1, 0, 0, 0}},
+        {{0, 1, 0, 0, 0, 1, 0, 0}},
+        {{0, 0, 1, 0, 0, 0, 1, 0}},
+        {{0, 0, 0, 1, 0, 0, 0, 1}},
+        {{0, 0, 0, 0, 1, 0, 0, 0}},
+        {{0, 0, 0, 0, 0, 1, 0, 0}},
+        {{0, 0, 0, 0, 0, 0, 1, 0}},
+        {{0, 0, 0, 0, 0, 0, 0, 1}},
+    }};
+
+    // 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列），
+    // 直接观测位置与尺寸、不观测速度
+    static constexpr std::array<std::array<double, 8>, 4> kH = {{
+        {{1, 0, 0, 0, 0, 0, 0, 0}},
+        {{0, 1, 0, 0, 0, 0, 0, 0}},
+        {{0, 0, 1, 0, 0, 0, 0, 0}},
+        {{0, 0, 0, 1, 0, 0, 0, 0}},
+    }};
+};
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/include/target_tracker/target_tracker.h

@@ -0,0 +1,123 @@
+#pragma once
+
+// =============================================================================
+// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口
+//
+// 对应规格：docs/视觉追踪技术路线.md
+//   §2 输入输出接口定义（Detection / TrackResult）
+//   §3.3 TargetTracker（管理器）
+//   §5 检测-轨迹关联（贪婪最邻近，§5.3）
+//   §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
+//   §7 主流程（每帧 Update 六步）
+//
+// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对
+// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。
+// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。
+// =============================================================================
+
+#include <mutex>
+#include <optional>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "tracklet.h"
+
+namespace drone {
+namespace tracker {
+
+/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]）
+struct Detection {
+    double x1 = 0.0;      // 边界框左上角 x（像素坐标）
+    double y1 = 0.0;      // 边界框左上角 y
+    double x2 = 0.0;      // 边界框右下角 x
+    double y2 = 0.0;      // 边界框右下角 y
+    double conf = 0.0;    // 检测置信度 ∈ [0,1]
+    int cls_id = 0;       // 类别编号
+};
+
+/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道"
+/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。
+struct FrameShape {
+    int height = 0;
+    int width = 0;
+};
+
+/// 每帧追踪输出（§2.3）
+struct TrackResult {
+    bool tracked = false;                       // 是否存在有效主目标；false 时调用方应刹车
+    std::optional<int> primary_id;              // 主目标轨道 ID；无目标时为空
+    std::pair<double, double> center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效）
+    std::optional<BoxXYXY> box;                 // 平滑后的边界框
+    double confidence = 0.0;                    // 当前置信度（Coast 期间继承最近实测值）
+    bool is_predicted = false;                  // true = Coast（仅预测、无实测）
+    int lost_frames = 0;                        // 主目标连续未命中帧数
+    int n_active = 0;                           // 当前活跃轨道总数
+    std::optional<BoxXYXY> raw;                 // 关联到的原始检测框；Coast 时为空（§2.3）
+};
+
+/// 多目标追踪过滤器（§1.1 / §3.3）
+class TargetTracker {
+public:
+    /// 追踪器配置参数（§8.1，默认值取自规格）
+    struct Config {
+        int max_lost_frames = 8;          // 最大连续丢失帧数（严格大于才判死，§6.1）
+        double max_association_dist = 200.0;  // 中心距离软阈值 Dmax（像素，§5.1）
+        int min_hits = 3;                 // 轨道"确认"所需累计匹配次数（§6.1）
+        double dist_weight = 0.5;         // 关联代价距离项权重（§5.1）
+        double iou_weight = 0.5;          // 关联代价 (1−IoU) 项权重（§5.1）
+    };
+
+    /// 默认配置构造（等价于 Config{}，§8.1 默认值）
+    TargetTracker();
+
+    /// 指定配置构造
+    explicit TargetTracker(const Config& cfg);
+
+    /// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸，
+    /// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。
+    TrackResult Update(const std::vector<Detection>& detections,
+                       std::optional<FrameShape> frame_shape = std::nullopt);
+
+    /// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5）
+    TrackResult LastResult() const;
+
+    /// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2）
+    void Reset();
+
+    /// 当前存活轨道数量（§2.1）
+    int ActiveCount() const;
+
+    /// 当前所有轨道的只读快照（§2.1；值拷贝）
+    std::unordered_map<int, Tracklet> ActiveTracks() const;
+
+    /// 配置只读访问
+    const Config& GetConfig() const { return config_; }
+
+private:
+    /// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引)
+    struct AssociationResult {
+        std::vector<std::pair<int, int>> matches;
+        std::vector<int> unmatched_track_ids;
+        std::vector<int> unmatched_det_indices;
+    };
+
+    /// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配，
+    /// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于
+    /// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。
+    AssociationResult AssociateDetections(const std::vector<Detection>& detections);
+
+    /// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY）
+    TrackResult SelectPrimary(std::optional<FrameShape> frame_shape);
+
+    Config config_;                                   // 配置参数（§3.3）
+    int next_id_ = 0;                                 // 下一个可分配 ID，从 0 递增不复用（§6.2）
+    std::unordered_map<int, Tracklet> tracks_;        // 所有存活轨道（§3.3）
+    long long frame_count_ = 0;                       // 已处理帧数（仅统计用，§3.3）
+    std::optional<int> locked_primary_id_;            // 当前锁定的主目标 ID（§3.3）
+    TrackResult last_result_;                         // 上一帧结果缓存（§3.3）
+    mutable std::mutex last_result_mutex_;            // 仅保护 last_result_（§9.5）
+};
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/include/target_tracker/tracklet.h

@@ -0,0 +1,93 @@
+#pragma once
+
+// =============================================================================
+// tracklet.h —— 轨道（Tracklet）与基础几何工具
+//
+// 对应规格：docs/视觉追踪技术路线.md
+//   §3.2 Tracklet（轨道）字段定义与派生状态
+//   §5.2 IoU 计算
+//   §7   主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE
+//
+// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于
+// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY
+// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。
+// 本库只依赖 C++ 标准库，无任何第三方依赖。
+// =============================================================================
+
+#include <optional>
+#include <utility>
+
+#include "kalman_box_filter.h"
+
+namespace drone {
+namespace tracker {
+
+// ---- 基础几何量（§9.1 模块/类划分草案） ----
+
+/// 轴对齐边界框（像素坐标，左上/右下角表示）
+struct BoxXYXY {
+    double x1 = 0.0;
+    double y1 = 0.0;
+    double x2 = 0.0;
+    double y2 = 0.0;
+};
+
+/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换）
+BoxCXCYWH ToCenterSize(const BoxXYXY& box);
+
+/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换）
+BoxXYXY ToXYXY(const BoxCXCYWH& box);
+
+/// 标准轴对齐 IoU（§5.2）：
+/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
+double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b);
+
+// =============================================================================
+// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）
+// =============================================================================
+struct Tracklet {
+    /// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）：
+    /// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0，
+    /// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。
+    Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
+             int max_lost_frames);
+
+    int track_id = 0;                 // 全局唯一轨道 ID（§3.2）
+    KalmanBoxFilter kf;               // 本轨道的滤波器（§3.2）
+    int lost_count = 0;               // 当前连续未匹配帧数（§3.2）
+    int max_lost = 0;                 // 最大允许连续丢失帧数（构造时注入，§3.2）
+    double confidence = 0.0;          // 最近一次匹配的置信度（§3.2）
+    int cls_id = 0;                   // 最近一次匹配的类别 ID（§3.2）
+    int age = 1;                      // 轨道已存在的总帧数（创建时为 1，§3.2）
+    int hits = 1;                     // 累计成功匹配次数（创建时为 1，§3.2）
+    std::pair<double, double> last_center{0.0, 0.0};  // 最近一次中心点（实测或预测）
+    std::optional<BoxXYXY> last_box;  // 最近一次边界框（实测或预测，§3.2）
+    std::optional<BoxXYXY> last_raw_box;  // 最近一次实测关联到的原始检测框；
+                                          // Coast 期间保留不更新，仅内部参考；
+                                          // 对外输出语义按 §2.3：Coast 时 raw 置空
+
+    // ---- 派生状态（§3.2 只读属性） ----
+
+    /// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2）
+    bool IsDead() const { return lost_count > max_lost; }
+
+    /// is_coasting := lost_count > 0（处于纯预测状态，§3.2）
+    bool IsCoasting() const { return lost_count > 0; }
+
+    // ---- 生命周期方法（§7 末尾伪代码） ----
+
+    /// TRACKLET_PREDICT：卡尔曼预测推进一帧。
+    /// 预测成功时更新 last_center / last_box 且 age+1。
+    void Predict();
+
+    /// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update；
+    /// lost_count 清零、hits+1、age+1，更新
+    /// last_center / last_box / last_raw_box / confidence / cls_id。
+    void Update(const BoxXYXY& box, double conf, int cls);
+
+    /// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。
+    void MarkMiss();
+};
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/src/kalman_box_filter.cpp

@@ -0,0 +1,235 @@
+// =============================================================================
+// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现
+//
+// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、
+// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。
+// 关键公式逐行注释并标注对照文档小节号。
+// =============================================================================
+
+#include "target_tracker/kalman_box_filter.h"
+
+#include <cmath>
+#include <utility>
+
+namespace drone {
+namespace tracker {
+namespace {
+
+// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ----
+
+/// 8×8 矩阵乘法：C = A·B
+std::array<std::array<double, 8>, 8> MatMul8(const std::array<std::array<double, 8>, 8>& a,
+                                               const std::array<std::array<double, 8>, 8>& b) {
+    std::array<std::array<double, 8>, 8> c{};
+    for (int i = 0; i < 8; ++i) {
+        for (int k = 0; k < 8; ++k) {
+            if (a[i][k] == 0.0) continue;  // 稀疏常量矩阵加速，结果不变
+            for (int j = 0; j < 8; ++j) {
+                c[i][j] += a[i][k] * b[k][j];
+            }
+        }
+    }
+    return c;
+}
+
+/// 8×8 矩阵转置：R = Aᵀ
+std::array<std::array<double, 8>, 8> MatTranspose8(
+    const std::array<std::array<double, 8>, 8>& a) {
+    std::array<std::array<double, 8>, 8> r{};
+    for (int i = 0; i < 8; ++i) {
+        for (int j = 0; j < 8; ++j) {
+            r[i][j] = a[j][i];
+        }
+    }
+    return r;
+}
+
+/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。
+/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。
+/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。
+bool Invert4x4(const std::array<std::array<double, 4>, 4>& m,
+               std::array<std::array<double, 4>, 4>& out) {
+    constexpr double kRegularization = 1e-9;  // 正则化极小量（§9.3）
+
+    // 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵
+    std::array<std::array<double, 8>, 4> aug{};
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0);
+            aug[i][j + 4] = (i == j) ? 1.0 : 0.0;
+        }
+    }
+
+    for (int col = 0; col < 4; ++col) {
+        // 部分主元：选取当前列绝对值最大行交换，提高数值稳定性
+        int pivot = col;
+        for (int r = col + 1; r < 4; ++r) {
+            if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) {
+                pivot = r;
+            }
+        }
+        if (std::abs(aug[pivot][col]) < 1e-12) {
+            return false;  // 主元退化（正则化后理论上不可达）
+        }
+        if (pivot != col) {
+            std::swap(aug[pivot], aug[col]);
+        }
+        // 主元行归一化
+        const double d = aug[col][col];
+        for (int j = 0; j < 8; ++j) {
+            aug[col][j] /= d;
+        }
+        // 消去其余行（高斯-约当：一次消成对角单位阵）
+        for (int r = 0; r < 4; ++r) {
+            if (r == col) continue;
+            const double f = aug[r][col];
+            if (f == 0.0) continue;
+            for (int j = 0; j < 8; ++j) {
+                aug[r][j] -= f * aug[col][j];
+            }
+        }
+    }
+
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            out[i][j] = aug[i][j + 4];
+        }
+    }
+    return true;
+}
+
+}  // namespace
+
+void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) {
+    // §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零）
+    x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}};
+
+    // §4.3 协方差：P = diag(2,2,2,2,100,100,100,100)
+    // （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
+    for (auto& row : P_) row.fill(0.0);
+    for (int i = 0; i < 4; ++i) {
+        P_[i][i] = kIniPos;       // INI_POS = 2.0（§4.2）
+        P_[i + 4][i + 4] = kIniVel;  // INI_VEL = 100.0（§4.2）
+    }
+    initialized_ = true;
+}
+
+std::optional<BoxCXCYWH> KalmanBoxFilter::Predict() {
+    // §4.4：未初始化返回空
+    if (!initialized_) {
+        return std::nullopt;
+    }
+
+    // §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放
+    //   σ_pos = [w/50, h/50, w/50, h/50]
+    //   σ_vel = [w/200, h/200, w/200, h/200]
+    // （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致）
+    const double w = x_[2];
+    const double h = x_[3];
+    const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos};
+    const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel};
+    std::array<std::array<double, 8>, 8> q{};
+    for (int i = 0; i < 4; ++i) {
+        q[i][i] = std_pos[i] * std_pos[i];       // 前 4 对角元来自 σ_pos（各分量先平方）
+        q[i + 4][i + 4] = std_vel[i] * std_vel[i];  // 后 4 对角元来自 σ_vel
+    }
+
+    // §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度）
+    std::array<double, 8> x_new{};
+    for (int i = 0; i < 8; ++i) {
+        double sum = 0.0;
+        for (int j = 0; j < 8; ++j) {
+            sum += kF[i][j] * x_[j];
+        }
+        x_new[i] = sum;
+    }
+    x_ = x_new;
+
+    // §4.4 协方差传播：P ← F·P·Fᵀ + Q
+    P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF));
+    for (int i = 0; i < 8; ++i) {
+        for (int j = 0; j < 8; ++j) {
+            P_[i][j] += q[i][j];
+        }
+    }
+
+    // §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本）
+    return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]};
+}
+
+void KalmanBoxFilter::Update(const BoxCXCYWH& z) {
+    // §4.5：未初始化直接返回
+    if (!initialized_) {
+        return;
+    }
+
+    // §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²)
+    // （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑；
+    //  该差异与规格 §4.2 注记保持一致）
+    const double w = std::abs(x_[2]);
+    const double h = std::abs(x_[3]);
+    std::array<std::array<double, 4>, 4> r{};
+    const double rw = kRScale * (w * kStdPos) * (w * kStdPos);
+    const double rh = kRScale * (h * kStdPos) * (h * kStdPos);
+    r[0][0] = rw;
+    r[1][1] = rh;
+    r[2][2] = rw;
+    r[3][3] = rh;
+
+    // §4.5 新息协方差：S = H·P·Hᵀ + R（4×4）
+    // H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块
+    std::array<std::array<double, 4>, 4> s{};
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            s[i][j] = P_[i][j] + r[i][j];
+        }
+    }
+
+    // §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异）
+    std::array<std::array<double, 4>, 4> s_inv{};
+    if (!Invert4x4(s, s_inv)) {
+        return;  // 正则化后理论不可达；防御性跳过本次更新
+    }
+
+    // §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4）
+    // H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4）
+    std::array<std::array<double, 4>, 8> k{};
+    for (int i = 0; i < 8; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            double sum = 0.0;
+            for (int l = 0; l < 4; ++l) {
+                sum += P_[i][l] * s_inv[l][j];
+            }
+            k[i][j] = sum;
+        }
+    }
+
+    // §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维）
+    const double z_vec[4] = {z.cx, z.cy, z.w, z.h};
+    double y[4];
+    for (int i = 0; i < 4; ++i) {
+        y[i] = z_vec[i] - x_[i];
+    }
+
+    // §4.5 状态更新：x ← x + K·y
+    for (int i = 0; i < 8; ++i) {
+        double sum = 0.0;
+        for (int j = 0; j < 4; ++j) {
+            sum += k[i][j] * y[j];
+        }
+        x_[i] += sum;
+    }
+
+    // §4.5 协方差更新：P ← (I8 − K·H)·P
+    // K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维）
+    std::array<std::array<double, 8>, 8> ikh{};
+    for (int i = 0; i < 8; ++i) {
+        for (int j = 0; j < 8; ++j) {
+            ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0);
+        }
+    }
+    P_ = MatMul8(ikh, P_);
+}
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/src/target_tracker.cpp

@@ -0,0 +1,312 @@
+// =============================================================================
+// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现
+//
+// 对应规格：docs/视觉追踪技术路线.md
+//   §5 检测-轨迹关联（贪婪最邻近，§5.3）
+//   §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
+//   §7 主流程六步（UPDATE 伪代码）
+//   §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁）
+// =============================================================================
+
+#include "target_tracker/target_tracker.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace drone {
+namespace tracker {
+
+TargetTracker::TargetTracker() : TargetTracker(Config{}) {}
+
+TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {}
+
+TrackResult TargetTracker::Update(const std::vector<Detection>& detections,
+                                  std::optional<FrameShape> frame_shape) {
+    // §7：frame_count += 1（仅统计用）
+    ++frame_count_;
+
+    // ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ----
+    // 防御性增强：上游检测器理论上已过滤，但库自身必须免疫——
+    // 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出
+    std::vector<Detection> valid_detections;
+    valid_detections.reserve(detections.size());
+    for (const Detection& det : detections) {
+        if (std::isfinite(det.x1) && std::isfinite(det.y1) &&
+            std::isfinite(det.x2) && std::isfinite(det.y2) &&
+            std::isfinite(det.conf)) {
+            valid_detections.push_back(det);
+        }
+    }
+
+    // ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
+    // （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1）
+    for (auto& kv : tracks_) {
+        kv.second.Predict();
+    }
+
+    // ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ----
+    AssociationResult assoc;
+    if (!valid_detections.empty()) {
+        assoc = AssociateDetections(valid_detections);
+    } else {
+        // 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支）
+        for (const auto& kv : tracks_) {
+            assoc.unmatched_track_ids.push_back(kv.first);
+        }
+    }
+
+    // ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ----
+    for (const auto& match : assoc.matches) {
+        const int track_id = match.first;
+        const Detection& det = valid_detections[static_cast<size_t>(match.second)];
+        // §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
+        // （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原）
+        const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2};
+        tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id);
+    }
+
+    // ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ----
+    for (const int track_id : assoc.unmatched_track_ids) {
+        auto it = tracks_.find(track_id);
+        if (it == tracks_.end()) {
+            continue;
+        }
+        it->second.MarkMiss();  // lost_count+1、age+1
+        if (it->second.IsDead()) {  // lost_count > max_lost（严格大于，§6.1）
+            // 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3）
+            if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) {
+                locked_primary_id_.reset();
+            }
+            tracks_.erase(it);
+        }
+    }
+
+    // ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ----
+    for (const int det_idx : assoc.unmatched_det_indices) {
+        const Detection& det = valid_detections[static_cast<size_t>(det_idx)];
+        // §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box，
+        // 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2）
+        tracks_.emplace(next_id_,
+                        Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}),
+                                 det.conf, det.cls_id, config_.max_lost_frames));
+        ++next_id_;
+    }
+
+    // ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ----
+    TrackResult result = SelectPrimary(frame_shape);
+    {
+        std::lock_guard<std::mutex> lock(last_result_mutex_);
+        last_result_ = result;  // 缓存供跨线程读取（§9.5）
+    }
+    return result;
+}
+
+TrackResult TargetTracker::LastResult() const {
+    // §9.5：LastResult() 返回受互斥锁保护的拷贝
+    std::lock_guard<std::mutex> lock(last_result_mutex_);
+    return last_result_;
+}
+
+void TargetTracker::Reset() {
+    // §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果
+    tracks_.clear();
+    next_id_ = 0;  // §6.2：reset() 时计数器归零
+    frame_count_ = 0;
+    locked_primary_id_.reset();  // 锁定的唯一解除途径之二（§6.3）
+    std::lock_guard<std::mutex> lock(last_result_mutex_);
+    last_result_ = TrackResult{};
+}
+
+int TargetTracker::ActiveCount() const {
+    return static_cast<int>(tracks_.size());
+}
+
+std::unordered_map<int, Tracklet> TargetTracker::ActiveTracks() const {
+    return tracks_;  // 值拷贝只读快照（§2.1）
+}
+
+TargetTracker::AssociationResult TargetTracker::AssociateDetections(
+    const std::vector<Detection>& detections) {
+    AssociationResult result;
+
+    // §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort，
+    // 不直接排哈希表，§9.2）
+    std::vector<int> sorted_ids;
+    sorted_ids.reserve(tracks_.size());
+    for (const auto& kv : tracks_) {
+        sorted_ids.push_back(kv.first);
+    }
+    std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) {
+        const double raw_a = tracks_.at(a).confidence;
+        const double raw_b = tracks_.at(b).confidence;
+        // NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较
+        // 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序，
+        // 等价传递性恒成立
+        const double ca = std::isfinite(raw_a)
+                              ? raw_a
+                              : -std::numeric_limits<double>::infinity();
+        const double cb = std::isfinite(raw_b)
+                              ? raw_b
+                              : -std::numeric_limits<double>::infinity();
+        if (ca != cb) return ca > cb;  // 置信度从高到低
+        return a < b;                  // 平局按 ID 升序作次级键，保证确定性
+    });
+
+    std::vector<bool> det_assigned(detections.size(), false);
+
+    for (const int track_id : sorted_ids) {
+        const Tracklet& track = tracks_.at(track_id);
+        // §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录，
+        // 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据）
+        double best_cost = 1.0;
+        int best_det = -1;
+        for (size_t d = 0; d < detections.size(); ++d) {
+            if (det_assigned[d]) continue;  // 跳过已分配检测
+            const Detection& det = detections[d];
+            // §5.1：中心点欧氏距离（像素）
+            const double det_cx = (det.x1 + det.x2) / 2.0;
+            const double det_cy = (det.y1 + det.y2) / 2.0;
+            const double dist =
+                std::hypot(track.last_center.first - det_cx,
+                           track.last_center.second - det_cy);
+            // §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零
+            const double norm_dist =
+                dist / std::max(config_.max_association_dist, 1e-3);
+            // §5.2：预测框与检测框的 IoU
+            const double iou = track.last_box.has_value()
+                                   ? ComputeIoU(*track.last_box,
+                                                {det.x1, det.y1, det.x2, det.y2})
+                                   : 0.0;
+            // §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU)
+            const double cost = config_.dist_weight * norm_dist +
+                                config_.iou_weight * (1.0 - iou);
+            if (cost < best_cost) {  // 严格小于才更新（§5.1 / §5.3）
+                best_cost = cost;
+                best_det = static_cast<int>(d);
+            }
+        }
+        if (best_det >= 0) {
+            result.matches.emplace_back(track_id, best_det);
+            det_assigned[static_cast<size_t>(best_det)] = true;
+        } else {
+            result.unmatched_track_ids.push_back(track_id);
+        }
+    }
+
+    // §5.3：未分配检测收集
+    for (size_t d = 0; d < detections.size(); ++d) {
+        if (!det_assigned[d]) {
+            result.unmatched_det_indices.push_back(static_cast<int>(d));
+        }
+    }
+    return result;
+}
+
+TrackResult TargetTracker::SelectPrimary(std::optional<FrameShape> frame_shape) {
+    TrackResult result;
+    result.n_active = static_cast<int>(tracks_.size());
+
+    // §6.3：轨道表为空 → 清除锁定，返回 tracked=false
+    if (tracks_.empty()) {
+        locked_primary_id_.reset();
+        return result;
+    }
+
+    // §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出，
+    // is_predicted=true；不被其他更高置信度目标抢占）
+    Tracklet* chosen = nullptr;
+    bool is_predicted = false;
+    if (locked_primary_id_.has_value()) {
+        auto it = tracks_.find(*locked_primary_id_);
+        if (it != tracks_.end()) {
+            chosen = &it->second;
+            is_predicted = it->second.IsCoasting();
+        }
+    }
+
+    // §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
+    if (chosen == nullptr) {
+        // measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道
+        std::vector<Tracklet*> measured;
+        std::vector<Tracklet*> coasting;
+        for (auto& kv : tracks_) {
+            if (kv.second.IsCoasting()) {
+                coasting.push_back(&kv.second);
+            } else {
+                measured.push_back(&kv.second);
+            }
+        }
+
+        if (!measured.empty()) {
+            // confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
+            // （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1）
+            std::vector<Tracklet*> pool;
+            for (Tracklet* t : measured) {
+                if (t->hits >= config_.min_hits) {
+                    pool.push_back(t);
+                }
+            }
+            if (pool.empty()) {
+                pool = measured;
+            }
+            // chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序
+            // 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则）
+            chosen = pool.front();
+            for (Tracklet* t : pool) {
+                if (t->confidence > chosen->confidence) {
+                    chosen = t;
+                }
+            }
+            is_predicted = false;
+        } else if (!coasting.empty()) {
+            // 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序）
+            // §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240)
+            const double center_x =
+                (frame_shape.has_value() && frame_shape->width > 0)
+                    ? frame_shape->width / 2.0
+                    : 320.0;
+            const double center_y =
+                (frame_shape.has_value() && frame_shape->height > 0)
+                    ? frame_shape->height / 2.0
+                    : 240.0;
+            double best_dist_sq = std::numeric_limits<double>::infinity();
+            for (Tracklet* t : coasting) {
+                const double dx = t->last_center.first - center_x;
+                const double dy = t->last_center.second - center_y;
+                const double dist_sq = dx * dx + dy * dy;
+                if (dist_sq < best_dist_sq) {
+                    best_dist_sq = dist_sq;
+                    chosen = t;
+                }
+            }
+            if (chosen == nullptr) {
+                // NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN，
+                // dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用；
+                // 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达
+                chosen = coasting.front();
+            }
+            is_predicted = true;  // Coast 兜底：仅预测、无实测
+        } else {
+            // 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
+            locked_primary_id_.reset();
+            return result;
+        }
+        locked_primary_id_ = chosen->track_id;  // 选举后锁定（§6.3）
+    }
+
+    // ---- 组装输出（§2.3 九字段） ----
+    result.tracked = true;
+    result.primary_id = chosen->track_id;
+    result.center = chosen->last_center;
+    result.box = chosen->last_box;
+    result.confidence = chosen->confidence;  // Coast 期间继承最近实测置信度
+    result.is_predicted = is_predicted;
+    result.lost_frames = chosen->lost_count;
+    // §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守）
+    result.raw = is_predicted ? std::nullopt : chosen->last_raw_box;
+    return result;
+}
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/src/tracklet.cpp

@@ -0,0 +1,101 @@
+// =============================================================================
+// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现
+//
+// 对应规格：docs/视觉追踪技术路线.md
+//   §5.2 IoU 计算
+//   §7   TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码
+//   §3.2 Tracklet 字段语义
+// =============================================================================
+
+#include "target_tracker/tracklet.h"
+
+#include <algorithm>
+
+namespace drone {
+namespace tracker {
+
+BoxCXCYWH ToCenterSize(const BoxXYXY& box) {
+    // §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
+    return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
+                     box.x2 - box.x1, box.y2 - box.y1};
+}
+
+BoxXYXY ToXYXY(const BoxCXCYWH& box) {
+    // §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
+    return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0,
+                   box.cx + box.w / 2.0, box.cy + box.h / 2.0};
+}
+
+double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) {
+    // §5.2：任一框为空（宽或高 ≤ 0）返回 0
+    const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
+    const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
+    if (area_a <= 0.0 || area_b <= 0.0) {
+        return 0.0;
+    }
+
+    // 交集宽高与 0 取最大值（§5.2）
+    const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1);
+    const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1);
+    if (inter_w <= 0.0 || inter_h <= 0.0) {
+        return 0.0;  // 交集面积 ≤ 0 返回 0（§5.2）
+    }
+
+    const double inter_area = inter_w * inter_h;
+    const double union_area = area_a + area_b - inter_area;
+    if (union_area <= 0.0) {
+        return 0.0;  // 并集 ≤ 0 返回 0（§5.2）
+    }
+    // IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2）
+    return inter_area / union_area;
+}
+
+Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
+                   int max_lost_frames)
+    : track_id(id),
+      max_lost(max_lost_frames),
+      confidence(conf),
+      cls_id(cls) {
+    // §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角）
+    kf.Initiate(measurement);
+    // §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出）
+    last_center = {measurement.cx, measurement.cy};
+    last_box = ToXYXY(measurement);
+    // 创建即源于实测检测：同步记录原始检测框（Coast 期间保留，
+    // 对外输出按 §2.3 在 Coast 时置空）
+    last_raw_box = last_box;
+    // age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2）
+}
+
+void Tracklet::Predict() {
+    // §7 TRACKLET_PREDICT：卡尔曼预测推进一帧
+    const auto pred = kf.Predict();
+    if (pred.has_value()) {
+        // 预测成功：更新 last_center / last_box，age += 1
+        last_center = {pred->cx, pred->cy};
+        last_box = ToXYXY(*pred);
+        ++age;
+    }
+}
+
+void Tracklet::Update(const BoxXYXY& box, double conf, int cls) {
+    // §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
+    kf.Update(ToCenterSize(box));
+    last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0};
+    last_box = box;
+    last_raw_box = box;      // 实测关联到的原始检测框（Coast 期间保留不更新）
+    confidence = conf;
+    cls_id = cls;
+    lost_count = 0;          // 匹配成功：连续丢失计数清零
+    ++hits;                  // 累计匹配次数 +1
+    ++age;                   // 帧龄 +1
+}
+
+void Tracklet::MarkMiss() {
+    // §7 步骤 4：未匹配计丢失 lost_count+1、age+1
+    ++lost_count;
+    ++age;
+}
+
+}  // namespace tracker
+}  // namespace drone

reference-c++/target_tracker/target_tracker.md

@@ -0,0 +1,217 @@
+# target_tracker 视觉追踪库实现文档
+
+> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器：
+> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。
+> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"），
+> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。
+
+---
+
+## 一、功能职责与边界
+
+**做什么**：
+
+- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志
+  （`TrackResult`），供下游速度闭环（PID）消费。
+- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。
+- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`），
+  避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。
+- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID；
+  主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。
+
+**不做什么**（边界）：
+
+- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）；
+- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖；
+- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）；
+- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。
+
+## 二、接口与数据流
+
+命名空间 `drone::tracker`，头文件保护 `#pragma once`。
+
+| 头文件 | 内容 |
+|--------|------|
+| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` |
+| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` |
+| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` |
+
+主要接口签名：
+
+```cpp
+class KalmanBoxFilter {
+    void Initiate(const BoxCXCYWH& z);              // 首次观测初始化（§4.3）
+    std::optional<BoxCXCYWH> Predict();             // 未初始化返回 nullopt（§4.4）
+    void Update(const BoxCXCYWH& z);                // 观测更新（§4.5）
+    bool Initialized() const;
+    const std::array<double, 8>& State() const;     // [cx,cy,w,h,vx,vy,vw,vh]
+};
+
+struct Tracklet {
+    Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
+             int max_lost_frames);
+    bool IsDead() const;      // lost_count > max_lost（严格大于）
+    bool IsCoasting() const;  // lost_count > 0
+    void Predict();           // kf.predict 非空时更新 last_center/last_box、age+1
+    void Update(const BoxXYXY& box, double conf, int cls);  // 匹配更新
+    void MarkMiss();          // lost+1、age+1
+    // 字段：track_id / kf / lost_count / max_lost / confidence / cls_id /
+    //       age / hits / last_center / last_box / last_raw_box
+};
+
+class TargetTracker {
+    struct Config {  // §8.1 默认值
+        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;
+    };
+    TargetTracker();                               // 默认配置
+    explicit TargetTracker(const Config& cfg);
+    TrackResult Update(const std::vector<Detection>& detections,
+                       std::optional<FrameShape> frame_shape = std::nullopt);
+    TrackResult LastResult() const;                // 线程安全拷贝
+    void Reset();                                  // 清全部状态且 next_id 归零
+    int ActiveCount() const;
+    std::unordered_map<int, Tracklet> ActiveTracks() const;  // 只读快照
+};
+```
+
+数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入
+`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box /
+confidence / is_predicted / lost_frames / n_active / raw）。丢失时
+`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。
+
+## 三、关键实现点
+
+### 3.1 卡尔曼滤波（对照技术路线 §4）
+
+- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。
+- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`；
+  矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。
+- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、
+  `kRScale=2.0`（§4.2/§8.2）。
+- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h
+  缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格
+  §4.2 注记严格保持一致。
+- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。
+- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆，
+  消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、
+  x←x+K(z−Hx)、P←(I−KH)P（§4.5）。
+
+### 3.2 贪婪关联（对照 §5）
+
+- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`，
+  距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格
+  小于才匹配**（cost=1.0 恰好不匹配，§5.1）。
+- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配；
+  排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。
+
+### 3.3 主目标选举三级回退（对照 §6.3）
+
+1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出
+   `is_predicted=true`），不被高置信度目标抢占；
+2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits
+   的已确认池取 confidence 最大；池空则退回全部 measured；
+3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方，
+   `frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`；
+   都没有则 `tracked=false`。
+
+锁定仅在轨道死亡删除处或 `Reset()` 时解除。
+
+### 3.4 主流程六步（对照 §7）
+
+`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道
+update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定
+目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box，
+保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并
+缓存 last_result。
+
+### 3.5 线程模型（对照 §9.5）
+
+单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把
+`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁；
+若未来改为多线程调用 `Update()` 需整体加锁。
+
+### 3.6 符号导出说明
+
+当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。
+日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补
+`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数，
+否则下游链接将缺失符号。
+
+## 四、日志行为
+
+本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程，
+接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流，
+本库内部不新增日志。
+
+## 五、测试方式
+
+构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。
+
+```bash
+cd /mnt/d/ProgramData/drone_test/drone_test
+cmake -S target_tracker -B target_tracker/build
+cmake --build target_tracker/build -j$(nproc)
+ctest --test-dir target_tracker/build --output-on-failure
+```
+
+测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 /
+target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖
+技术路线 §9.4 六条验收用例：
+
+1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧
+   （is_predicted=true），第 max_lost_frames+1 帧 tracked=false；
+2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零；
+3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定，
+   锁定目标死亡后切换；
+4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits<min_hits）
+   可当选主目标；
+5. `CostExactlyOneDoesNotMatch`：纯距离项构造 cost=1.0 恰好不匹配，
+   并附 cost<1.0 对照帧；
+6. `HigherConfidenceTrackWinsDetection`：两轨道竞争同一检测，置信度高者获胜。
+
+其余为补充行为测试（丢失输出字段约定、Coast 兜底选举、锁定延续、
+默认画面中心、IoU 边界、滤波器收敛性等）。
+
+## 六、排查 / 修改要点
+
+- **主目标"甩头"**：确认是否误改锁定解除逻辑——锁定只能在轨道死亡删除处
+  或 `Reset()` 解除；选举分支不得覆盖存活锁定。
+- **Coast 时间不对**：死亡判定为 `lost_count > max_lost`（严格大于），
+  默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。
+- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否
+  匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。
+- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且
+  乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。
+- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH`
+  定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。
+- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。
+
+## 七、未来接线指引（接入主工程视频链路）
+
+本库以独立动态库交付，主工程接入建议采用：
+
+1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧
+   到达为帧节拍驱动追踪（保证与推理节奏一致）；
+2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条
+   DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence
+   聚合成 `std::vector<Detection>` 后调用 `Update()`；
+3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与
+   `kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为
+   `TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast
+   （is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车
+   （技术路线 §1.2 上游行为约定）。
+
+主工程链接该 so 的 CMake 片段示例：
+
+```cmake
+# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程）
+add_subdirectory(target_tracker)
+
+target_link_libraries(drone_control PRIVATE target_tracker)
+# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录
+# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致）
+```

reference-c++/target_tracker/tests/kalman_box_filter_test.cpp

@@ -0,0 +1,124 @@
+// =============================================================================
+// kalman_box_filter_test.cpp —— KalmanBoxFilter 单元测试
+//
+// 覆盖 docs/视觉追踪技术路线.md §4 行为：
+//   - 初始化状态正确（§4.3）
+//   - 静止目标多帧收敛到观测（§4.5）
+//   - 匀速目标速度收敛且预测超前一个 dt（§4.4）
+//   - 未初始化 predict 返回 nullopt（§4.4）
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "target_tracker/kalman_box_filter.h"
+
+using drone::tracker::BoxCXCYWH;
+using drone::tracker::KalmanBoxFilter;
+
+namespace {
+
+// 未初始化时 predict 返回 nullopt（§4.4）；update 直接返回不改变状态（§4.5）
+TEST(KalmanBoxFilterTest, UninitializedPredictReturnsNullopt) {
+    KalmanBoxFilter kf;
+    EXPECT_FALSE(kf.Initialized());
+    EXPECT_FALSE(kf.Predict().has_value());
+
+    // 未初始化时 update 无副作用
+    kf.Update({100.0, 100.0, 20.0, 20.0});
+    EXPECT_FALSE(kf.Initialized());
+    EXPECT_FALSE(kf.Predict().has_value());
+    for (const double v : kf.State()) {
+        EXPECT_DOUBLE_EQ(v, 0.0);
+    }
+}
+
+// 初始化状态正确（§4.3）：速度置零、位置/尺寸取首次观测
+TEST(KalmanBoxFilterTest, InitiateSetsStateCorrectly) {
+    KalmanBoxFilter kf;
+    kf.Initiate({320.0, 240.0, 40.0, 30.0});
+    EXPECT_TRUE(kf.Initialized());
+
+    const std::array<double, 8> expected = {
+        {320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0}};
+    for (size_t i = 0; i < expected.size(); ++i) {
+        EXPECT_DOUBLE_EQ(kf.State()[i], expected[i]) << "index=" << i;
+    }
+
+    // 初始化后 predict 返回状态前 4 维副本（速度为零 → 位置不变，§4.4）
+    const auto pred = kf.Predict();
+    ASSERT_TRUE(pred.has_value());
+    EXPECT_NEAR(pred->cx, 320.0, 1e-9);
+    EXPECT_NEAR(pred->cy, 240.0, 1e-9);
+    EXPECT_NEAR(pred->w, 40.0, 1e-9);
+    EXPECT_NEAR(pred->h, 30.0, 1e-9);
+}
+
+// 静止目标：多帧重复观测同一框，滤波输出应收敛到观测值（§4.5）
+TEST(KalmanBoxFilterTest, StaticTargetConvergesToObservation) {
+    KalmanBoxFilter kf;
+    const BoxCXCYWH z{100.0, 150.0, 40.0, 60.0};
+    kf.Initiate({104.0, 156.0, 42.0, 62.0});  // 带初始偏差，验证收敛能力
+
+    for (int i = 0; i < 50; ++i) {
+        kf.Predict();
+        kf.Update(z);
+    }
+
+    EXPECT_NEAR(kf.State()[0], z.cx, 0.5);  // cx
+    EXPECT_NEAR(kf.State()[1], z.cy, 0.5);  // cy
+    EXPECT_NEAR(kf.State()[2], z.w, 0.5);   // w
+    EXPECT_NEAR(kf.State()[3], z.h, 0.5);   // h
+    // 静止目标速度应收敛到约 0
+    EXPECT_NEAR(kf.State()[4], 0.0, 0.1);
+    EXPECT_NEAR(kf.State()[5], 0.0, 0.1);
+}
+
+// 匀速目标：速度估计收敛到真实速度，且 predict 超前一个 dt（§4.4）
+TEST(KalmanBoxFilterTest, ConstantVelocityConvergesAndPredictLeadsOneDt) {
+    KalmanBoxFilter kf;
+    const double vx = 3.0;   // 像素/帧
+    const double vy = 2.0;   // 像素/帧
+
+    // 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
+    BoxCXCYWH z{100.0, 100.0, 30.0, 30.0};
+    kf.Initiate(z);
+    for (int i = 0; i < 60; ++i) {
+        kf.Predict();
+        z.cx += vx;
+        z.cy += vy;
+        kf.Update(z);
+    }
+
+    // 速度收敛到真实速度（像素/帧）
+    EXPECT_NEAR(kf.State()[4], vx, 0.3) << "vx 未收敛";
+    EXPECT_NEAR(kf.State()[5], vy, 0.3) << "vy 未收敛";
+    // 位置贴合当前观测
+    EXPECT_NEAR(kf.State()[0], z.cx, 1.5);
+    EXPECT_NEAR(kf.State()[1], z.cy, 1.5);
+
+    // 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
+    const auto pred = kf.Predict();
+    ASSERT_TRUE(pred.has_value());
+    EXPECT_NEAR(pred->cx, z.cx + vx, 1.0) << "预测未超前一个 dt";
+    EXPECT_NEAR(pred->cy, z.cy + vy, 1.0) << "预测未超前一个 dt";
+}
+
+// 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
+TEST(KalmanBoxFilterTest, SmallTargetNoiseScalingStable) {
+    KalmanBoxFilter kf;
+    const BoxCXCYWH z{50.0, 60.0, 4.0, 4.0};  // 小目标（4×4 像素）
+    kf.Initiate(z);
+    for (int i = 0; i < 30; ++i) {
+        kf.Predict();
+        kf.Update(z);
+    }
+    EXPECT_NEAR(kf.State()[0], z.cx, 0.5);
+    EXPECT_NEAR(kf.State()[1], z.cy, 0.5);
+    EXPECT_TRUE(std::isfinite(kf.State()[2]));
+    EXPECT_TRUE(std::isfinite(kf.State()[3]));
+}
+
+}  // namespace

reference-c++/target_tracker/tests/target_tracker_test.cpp

@@ -0,0 +1,402 @@
+// =============================================================================
+// target_tracker_test.cpp —— TargetTracker 单元测试
+//
+// 逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
+//   1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
+//   2. ID 严格递增不复用（含 Reset 后归零）
+//   3. 锁定不被抢占；锁定目标死亡后切换到新目标
+//   4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
+//   5. cost=1.0 恰好不构成匹配（严格小于）
+//   6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <limits>
+#include <optional>
+#include <vector>
+
+#include "target_tracker/target_tracker.h"
+
+using drone::tracker::Detection;
+using drone::tracker::FrameShape;
+using drone::tracker::TargetTracker;
+using drone::tracker::TrackResult;
+
+namespace {
+
+Detection MakeDet(double x1, double y1, double x2, double y2, double conf,
+                  int cls = 0) {
+    return Detection{x1, y1, x2, y2, conf, cls};
+}
+
+// ============================================================================
+// 验收用例 1（§9.4-1）：空检测帧序列
+// 连续输入空检测，主目标输出 is_predicted=true 共 max_lost_frames 帧，
+// 第 max_lost_frames + 1 帧 tracked=false（死亡判定 lost_count > max_lost）。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, EmptyFramesCoastThenLost) {
+    TargetTracker tracker;  // 默认配置 max_lost_frames = 8
+    ASSERT_EQ(tracker.GetConfig().max_lost_frames, 8);
+
+    // 第 1 帧：给出一个检测，建立轨道并锁定为主目标
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
+    ASSERT_TRUE(r.tracked);
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_FALSE(r.is_predicted);   // 当帧有实测
+    EXPECT_EQ(r.lost_frames, 0);
+    EXPECT_EQ(r.n_active, 1);
+    ASSERT_TRUE(r.box.has_value());
+    ASSERT_TRUE(r.raw.has_value()); // 当帧关联到原始检测框
+
+    // 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=true）
+    for (int i = 1; i <= 8; ++i) {
+        r = tracker.Update({});
+        ASSERT_TRUE(r.tracked) << "第 " << i << " 帧 Coast 期间不应判丢";
+        EXPECT_TRUE(r.is_predicted) << "第 " << i << " 帧应为纯预测输出";
+        EXPECT_EQ(r.lost_frames, i);
+        EXPECT_TRUE(r.primary_id.has_value());
+        EXPECT_TRUE(r.box.has_value());     // Coast 期间继续输出预测框
+        EXPECT_FALSE(r.raw.has_value());    // Coast 时 raw 为空（§2.3）
+        EXPECT_DOUBLE_EQ(r.confidence, 0.9); // 继承最近实测置信度（§2.3）
+    }
+
+    // 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=false
+    r = tracker.Update({});
+    EXPECT_FALSE(r.tracked);
+    EXPECT_FALSE(r.primary_id.has_value());
+    EXPECT_FALSE(r.box.has_value());
+    EXPECT_EQ(r.lost_frames, 0);  // 丢失时 lost_frames=0（§2.3）
+    EXPECT_EQ(r.n_active, 0);
+}
+
+// ============================================================================
+// 验收用例 2（§9.4-2）：ID 单调性
+// 多次进出画面的目标获得严格递增、不复用的 ID；Reset 后计数器归零。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, IdStrictlyIncreasingNoReuse) {
+    TargetTracker::Config cfg;
+    cfg.max_lost_frames = 1;  // 缩短 Coast 窗口，加速轨道死亡
+    TargetTracker tracker(cfg);
+
+    // 目标 A 出现 → id 0
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    const int id_a = *r.primary_id;
+    EXPECT_EQ(id_a, 0);
+
+    // 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
+    tracker.Update({});
+    r = tracker.Update({});
+    EXPECT_FALSE(r.tracked);
+    EXPECT_EQ(tracker.ActiveCount(), 0);
+
+    // 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
+    r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    const int id_b = *r.primary_id;
+    EXPECT_EQ(id_b, 1);
+    EXPECT_GT(id_b, id_a);
+
+    // 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
+    r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9),
+                        MakeDet(400.0, 300.0, 430.0, 330.0, 0.8)});
+    EXPECT_EQ(tracker.ActiveCount(), 2);
+    int max_id = 0;
+    for (const auto& kv : tracker.ActiveTracks()) {
+        max_id = std::max(max_id, kv.first);
+    }
+    EXPECT_EQ(max_id, 2);
+
+    // Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
+    tracker.Reset();
+    EXPECT_EQ(tracker.ActiveCount(), 0);
+    EXPECT_FALSE(tracker.LastResult().tracked);
+    r = tracker.Update({MakeDet(50.0, 50.0, 70.0, 70.0, 0.9)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_EQ(*r.primary_id, 0);
+}
+
+// ============================================================================
+// 验收用例 3（§9.4-3）：锁定不被抢占
+// 锁定目标 Coast 期间出现更高置信度新目标，primary_id 不变；
+// 锁定目标死亡后切换到新目标。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, LockNotStolenAndSwitchAfterDeath) {
+    TargetTracker::Config cfg;
+    cfg.max_lost_frames = 3;
+    TargetTracker tracker(cfg);
+
+    // 低置信度目标 A 建立并锁定
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.4)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    const int id_a = *r.primary_id;
+
+    // A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
+    r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_EQ(*r.primary_id, id_a) << "Coast 期间锁定不应被高置信度目标抢占";
+    EXPECT_TRUE(r.is_predicted);
+    EXPECT_EQ(tracker.ActiveCount(), 2);  // A（Coast）与 B 并存
+
+    // 继续 Coast，锁定依旧延续
+    r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
+    EXPECT_EQ(*r.primary_id, id_a);
+    EXPECT_TRUE(r.is_predicted);
+
+    // A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
+    for (int i = 0; i < 2; ++i) {
+        r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
+    }
+    // A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
+    ASSERT_TRUE(r.tracked);
+    EXPECT_NE(*r.primary_id, id_a) << "锁定目标死亡后应切换到新目标";
+    EXPECT_FALSE(r.is_predicted);
+
+    // 后续 B 持续作为主目标
+    r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_NE(*r.primary_id, id_a);
+}
+
+// ============================================================================
+// 验收用例 4（§9.4-4）：min_hits 回退
+// 新轨道（hits 未达 min_hits）在无其他已确认实测轨道时也可当选主目标。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, MinHitsFallbackAllowsNewTrack) {
+    TargetTracker::Config cfg;
+    cfg.min_hits = 5;  // 抬高确认门槛，保证测试期间轨道始终未确认
+    TargetTracker tracker(cfg);
+
+    // 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
+    TrackResult r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
+    ASSERT_TRUE(r.tracked) << "无已确认轨道时新轨道应能当选主目标";
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_EQ(*r.primary_id, 0);
+    EXPECT_FALSE(r.is_predicted);
+
+    // 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
+    for (int i = 0; i < 3; ++i) {
+        r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
+        ASSERT_TRUE(r.tracked);
+        EXPECT_EQ(*r.primary_id, 0);
+    }
+}
+
+// ============================================================================
+// 验收用例 5（§9.4-5）：cost 边界
+// 恰好 cost = 1.0 不构成匹配（仅严格小于才匹配）。
+// 构造：w_dist=1、w_iou=0（纯距离项），中心距离恰等于 Dmax → cost=1.0；
+// 同时给出略小于阈值的对照帧证明边界两侧行为正确。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, CostExactlyOneDoesNotMatch) {
+    TargetTracker::Config cfg;
+    cfg.max_lost_frames = 1;
+    cfg.max_association_dist = 200.0;
+    cfg.dist_weight = 1.0;  // 纯距离项，IoU 项权重为 0 且两框零交集
+    cfg.iou_weight = 0.0;
+    TargetTracker tracker(cfg);
+
+    // 建立轨道：中心 (100,100)
+    TrackResult r = tracker.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    const int id_a = *r.primary_id;
+
+    // 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
+    // （预测后轨道中心速度为 0，仍位于 (100,100)；两框零交集 IoU=0 无影响）
+    r = tracker.Update({MakeDet(90.0, 290.0, 110.0, 310.0, 0.9)});
+    // 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
+    EXPECT_EQ(tracker.ActiveCount(), 2) << "cost=1.0 不应匹配，应新建轨道";
+    // 原轨道（锁定）进入 Coast：lost_frames=1
+    EXPECT_TRUE(r.tracked);
+    EXPECT_TRUE(r.is_predicted);
+    EXPECT_EQ(*r.primary_id, id_a);
+    EXPECT_EQ(r.lost_frames, 1);
+
+    // 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
+    TargetTracker tracker2(cfg);
+    r = tracker2.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
+    const int id_b = *r.primary_id;
+    r = tracker2.Update({MakeDet(90.0, 289.0, 110.0, 309.0, 0.9)});
+    EXPECT_EQ(tracker2.ActiveCount(), 1) << "cost<1.0 应匹配成功，不应新建轨道";
+    ASSERT_TRUE(r.primary_id.has_value());
+    EXPECT_EQ(*r.primary_id, id_b);
+    EXPECT_FALSE(r.is_predicted);  // 实测命中
+}
+
+// ============================================================================
+// 验收用例 6（§9.4-6）：置信度优先
+// 两轨道竞争同一检测时，置信度高者获胜。
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, HigherConfidenceTrackWinsDetection) {
+    TargetTracker tracker;
+
+    // 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.5),
+                                    MakeDet(180.0, 100.0, 200.0, 120.0, 0.9)});
+    ASSERT_EQ(tracker.ActiveCount(), 2);
+
+    // 找出两条轨道 ID（按置信度区分）
+    int id_low = -1;
+    int id_high = -1;
+    for (const auto& kv : tracker.ActiveTracks()) {
+        if (kv.second.confidence > 0.8) {
+            id_high = kv.first;
+        } else {
+            id_low = kv.first;
+        }
+    }
+    ASSERT_NE(id_low, -1);
+    ASSERT_NE(id_high, -1);
+
+    // 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
+    // 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
+    r = tracker.Update({MakeDet(140.0, 100.0, 160.0, 120.0, 0.7)});
+
+    const auto tracks = tracker.ActiveTracks();
+    ASSERT_EQ(tracks.size(), 2u);
+    // 获胜者：命中（lost=0、hits=2、置信度更新）
+    EXPECT_EQ(tracks.at(id_high).lost_count, 0) << "高置信度轨道应获胜";
+    EXPECT_EQ(tracks.at(id_high).hits, 2);
+    EXPECT_DOUBLE_EQ(tracks.at(id_high).confidence, 0.7);
+    // 失败者：未匹配进入 Coast（lost=1、hits 不变）
+    EXPECT_EQ(tracks.at(id_low).lost_count, 1) << "低置信度轨道应落选";
+    EXPECT_EQ(tracks.at(id_low).hits, 1);
+    EXPECT_DOUBLE_EQ(tracks.at(id_low).confidence, 0.5);
+}
+
+// ============================================================================
+// NaN/Inf 防御（评审修复项）：坐标或置信度含非有限值的检测被丢弃，
+// 不崩溃、不新建轨道、不污染主目标输出
+// ============================================================================
+TEST(TargetTrackerAcceptanceTest, NonFiniteDetectionsAreDiscarded) {
+    TargetTracker tracker;
+    const double nan = std::numeric_limits<double>::quiet_NaN();
+    const double inf = std::numeric_limits<double>::infinity();
+
+    // 第 1 帧：建立正常轨道
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
+    ASSERT_TRUE(r.tracked);
+
+    // 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
+    // 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
+    r = tracker.Update({MakeDet(nan, 100.0, 140.0, 140.0, 0.9),
+                        MakeDet(100.0, -inf, 140.0, 140.0, 0.9),
+                        MakeDet(100.0, 100.0, nan, 140.0, 0.9),
+                        MakeDet(102.0, 102.0, 142.0, 142.0, nan),
+                        MakeDet(101.0, 101.0, 141.0, 141.0, 0.9)});
+    EXPECT_EQ(tracker.ActiveCount(), 1) << "非有限检测不应新建轨道";
+    ASSERT_TRUE(r.tracked);
+    EXPECT_FALSE(r.is_predicted);  // 正常检测命中，实测输出
+    // 主目标输出未被 NaN 污染
+    EXPECT_TRUE(std::isfinite(r.center.first));
+    EXPECT_TRUE(std::isfinite(r.center.second));
+    ASSERT_TRUE(r.box.has_value());
+    EXPECT_TRUE(std::isfinite(r.box->x1) && std::isfinite(r.box->y1));
+    EXPECT_TRUE(std::isfinite(r.box->x2) && std::isfinite(r.box->y2));
+    EXPECT_TRUE(std::isfinite(r.confidence));
+    ASSERT_TRUE(r.raw.has_value());  // 实测帧 raw 有值
+
+    // 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
+    r = tracker.Update({MakeDet(nan, nan, nan, nan, nan)});
+    EXPECT_TRUE(r.tracked);
+    EXPECT_TRUE(r.is_predicted);
+    EXPECT_EQ(r.lost_frames, 1);
+    EXPECT_FALSE(r.raw.has_value());  // Coast 时 raw 为空（§2.3）
+    EXPECT_TRUE(std::isfinite(r.center.first));
+
+    // 持续 NaN 检测直至轨道死亡回收，全程不崩溃
+    for (int i = 0; i < 10; ++i) {
+        r = tracker.Update({MakeDet(nan, 0.0, 10.0, 10.0, 0.9)});
+    }
+    EXPECT_FALSE(r.tracked);
+    EXPECT_EQ(tracker.ActiveCount(), 0);
+}
+
+// ============================================================================
+// 补充行为测试（§2.3 / §6.3 / §9.5）
+// ============================================================================
+
+// 丢失时返回值约定：tracked=false、定位字段空、lost_frames=0（§2.3）
+TEST(TargetTrackerBehaviorTest, LostResultFieldsAreEmpty) {
+    TargetTracker tracker;
+    TrackResult r = tracker.Update({});  // 空帧序列起点：无任何轨道
+    EXPECT_FALSE(r.tracked);
+    EXPECT_FALSE(r.primary_id.has_value());
+    EXPECT_FALSE(r.box.has_value());
+    EXPECT_FALSE(r.raw.has_value());
+    EXPECT_EQ(r.lost_frames, 0);
+    EXPECT_EQ(r.n_active, 0);
+    EXPECT_DOUBLE_EQ(r.confidence, 0.0);
+
+    // LastResult 返回同一快照（§9.5 跨线程读取入口）
+    const TrackResult cached = tracker.LastResult();
+    EXPECT_FALSE(cached.tracked);
+}
+
+// Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
+// 注意锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支，
+// 故采用错帧创建 + 缩短 max_lost 的方式构造场景。
+TEST(TargetTrackerBehaviorTest, CoastingFallbackChoosesNearestToFrameCenter) {
+    TargetTracker::Config cfg;
+    cfg.max_lost_frames = 2;
+    TargetTracker tracker(cfg);
+
+    // F1：角落目标（高置信度）先建立并被锁定
+    tracker.Update({MakeDet(10.0, 10.0, 30.0, 30.0, 0.95)});
+    // F2：画面中心附近目标建立；角落目标丢失(1)
+    tracker.Update({MakeDet(300.0, 220.0, 340.0, 260.0, 0.9)});
+    // F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
+    tracker.Update({MakeDet(600.0, 450.0, 620.0, 470.0, 0.85)});
+    // F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
+    // 其余两条 Coast 存活 → 重新选举选离画面中心最近者
+    TrackResult r = tracker.Update({}, FrameShape{480, 640});
+    ASSERT_TRUE(r.tracked);
+    EXPECT_TRUE(r.is_predicted);
+    EXPECT_NEAR(r.center.first, 320.0, 25.0);   // 靠近画面中心 (320,240) 者当选
+    EXPECT_NEAR(r.center.second, 240.0, 25.0);
+    EXPECT_EQ(r.n_active, 2);                    // 角落轨道已回收
+}
+
+// 锁定延续优先于置信度选举：锁定轨道实测时即使置信度较低也保持主目标（§6.3 ①）
+TEST(TargetTrackerBehaviorTest, LockedPrimaryKeptWhileMeasured) {
+    TargetTracker tracker;
+    TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.4)});
+    ASSERT_TRUE(r.primary_id.has_value());
+    const int locked_id = *r.primary_id;
+
+    // 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
+    for (int i = 0; i < 5; ++i) {
+        r = tracker.Update({MakeDet(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
+                            MakeDet(400.0, 300.0, 430.0, 330.0, 0.95)});
+        ASSERT_TRUE(r.primary_id.has_value());
+        EXPECT_EQ(*r.primary_id, locked_id);
+        EXPECT_FALSE(r.is_predicted);
+    }
+}
+
+// frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）：
+// 两条 Coast 轨道分别位于默认中心两侧，当选者应为更近的一侧
+TEST(TargetTrackerBehaviorTest, DefaultFrameCenterIs320x240) {
+    TargetTracker::Config cfg;
+    cfg.max_lost_frames = 2;
+    TargetTracker tracker(cfg);
+
+    // F1：角落目标先建立并被锁定（高置信度）
+    tracker.Update({MakeDet(0.0, 0.0, 20.0, 20.0, 0.95)});
+    // F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
+    tracker.Update({MakeDet(270.0, 230.0, 290.0, 250.0, 0.9)});
+    // F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
+    tracker.Update({MakeDet(490.0, 230.0, 510.0, 250.0, 0.85)});
+    // F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
+    // （不传 frame_shape → 按默认画面中心 (320,240)；两候选间距足够大，
+    // 速度估计漂移不影响最近者判定）
+    TrackResult r = tracker.Update({});
+    ASSERT_TRUE(r.tracked);
+    EXPECT_TRUE(r.is_predicted);
+    EXPECT_NEAR(r.center.first, 280.0, 30.0);   // 左侧更近，当选
+    EXPECT_NEAR(r.center.second, 240.0, 30.0);
+}
+
+}  // namespace

reference-c++/target_tracker/tests/tracklet_test.cpp

@@ -0,0 +1,177 @@
+// =============================================================================
+// tracklet_test.cpp —— Tracklet 与几何工具单元测试
+//
+// 覆盖 docs/视觉追踪技术路线.md：
+//   - IoU 边界（空框 / 零交集 / 完全重合=1，§5.2）
+//   - IsDead / IsCoasting 判定（§3.2 派生状态，§6.1 严格大于）
+//   - Predict / Update / MarkMiss 计数语义（§7 末尾伪代码）
+//   - BoxXYXY ↔ BoxCXCYWH 互转（§7）
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include "target_tracker/tracklet.h"
+
+using drone::tracker::BoxCXCYWH;
+using drone::tracker::BoxXYXY;
+using drone::tracker::ComputeIoU;
+using drone::tracker::ToCenterSize;
+using drone::tracker::ToXYXY;
+using drone::tracker::Tracklet;
+
+namespace {
+
+// ---- 框表示互转（§7） ----
+
+TEST(BoxConversionTest, RoundTripPreservesValues) {
+    const BoxXYXY box{10.0, 20.0, 50.0, 80.0};
+    const BoxCXCYWH c = ToCenterSize(box);
+    EXPECT_DOUBLE_EQ(c.cx, 30.0);   // (10+50)/2
+    EXPECT_DOUBLE_EQ(c.cy, 50.0);   // (20+80)/2
+    EXPECT_DOUBLE_EQ(c.w, 40.0);    // 50−10
+    EXPECT_DOUBLE_EQ(c.h, 60.0);    // 80−20
+
+    const BoxXYXY back = ToXYXY(c);
+    EXPECT_DOUBLE_EQ(back.x1, box.x1);
+    EXPECT_DOUBLE_EQ(back.y1, box.y1);
+    EXPECT_DOUBLE_EQ(back.x2, box.x2);
+    EXPECT_DOUBLE_EQ(back.y2, box.y2);
+}
+
+// ---- IoU 边界（§5.2） ----
+
+TEST(IoUTest, IdenticalBoxesReturnOne) {
+    const BoxXYXY a{10.0, 10.0, 30.0, 30.0};
+    EXPECT_NEAR(ComputeIoU(a, a), 1.0, 1e-12);  // 完全重合 = 1
+}
+
+TEST(IoUTest, DisjointBoxesReturnZero) {
+    const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
+    const BoxXYXY b{20.0, 20.0, 30.0, 30.0};
+    EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0);  // 零交集
+}
+
+TEST(IoUTest, TouchingEdgesReturnZero) {
+    // 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
+    const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
+    const BoxXYXY b{10.0, 0.0, 20.0, 10.0};   // 右边缘相接
+    EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0);
+    const BoxXYXY c{10.0, 10.0, 20.0, 20.0};  // 角点相接
+    EXPECT_DOUBLE_EQ(ComputeIoU(a, c), 0.0);
+}
+
+TEST(IoUTest, EmptyBoxReturnsZero) {
+    const BoxXYXY normal{0.0, 0.0, 10.0, 10.0};
+    const BoxXYXY zero_area{5.0, 5.0, 5.0, 5.0};    // 零面积
+    const BoxXYXY inverted{10.0, 10.0, 0.0, 0.0};   // 反向（负宽高）
+    EXPECT_DOUBLE_EQ(ComputeIoU(normal, zero_area), 0.0);
+    EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, normal), 0.0);
+    EXPECT_DOUBLE_EQ(ComputeIoU(normal, inverted), 0.0);
+    EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, zero_area), 0.0);
+}
+
+TEST(IoUTest, PartialOverlapIsCorrect) {
+    // A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
+    const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
+    const BoxXYXY b{5.0, 5.0, 15.0, 15.0};
+    EXPECT_NEAR(ComputeIoU(a, b), 25.0 / 175.0, 1e-12);
+    EXPECT_NEAR(ComputeIoU(b, a), 25.0 / 175.0, 1e-12);  // 对称性
+}
+
+// ---- 构造与派生状态（§3.2 / §6.1） ----
+
+TEST(TrackletTest, ConstructionInitialFields) {
+    const BoxCXCYWH meas{100.0, 100.0, 20.0, 20.0};
+    Tracklet t(7, meas, 0.9, 1, 8);
+
+    EXPECT_EQ(t.track_id, 7);
+    EXPECT_EQ(t.lost_count, 0);
+    EXPECT_EQ(t.max_lost, 8);
+    EXPECT_DOUBLE_EQ(t.confidence, 0.9);
+    EXPECT_EQ(t.cls_id, 1);
+    EXPECT_EQ(t.age, 1);   // 创建时为 1（§3.2）
+    EXPECT_EQ(t.hits, 1);  // 创建时为 1（§3.2）
+    EXPECT_DOUBLE_EQ(t.last_center.first, 100.0);
+    EXPECT_DOUBLE_EQ(t.last_center.second, 100.0);
+    ASSERT_TRUE(t.last_box.has_value());  // 创建当帧即赋 last_box（§7 步骤 5）
+    EXPECT_DOUBLE_EQ(t.last_box->x1, 90.0);
+    EXPECT_DOUBLE_EQ(t.last_box->y1, 90.0);
+    EXPECT_DOUBLE_EQ(t.last_box->x2, 110.0);
+    EXPECT_DOUBLE_EQ(t.last_box->y2, 110.0);
+    EXPECT_FALSE(t.IsDead());
+    EXPECT_FALSE(t.IsCoasting());
+}
+
+// IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
+TEST(TrackletTest, IsDeadUsesStrictlyGreater) {
+    Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
+    for (int i = 0; i < 8; ++i) {
+        t.MarkMiss();
+        EXPECT_FALSE(t.IsDead()) << "lost_count=" << t.lost_count
+                                 << " 未超过 max_lost 不应判死";
+    }
+    EXPECT_EQ(t.lost_count, 8);
+    t.MarkMiss();  // lost_count = 9 > 8
+    EXPECT_TRUE(t.IsDead());
+}
+
+TEST(TrackletTest, IsCoastingWhenLostPositive) {
+    Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
+    EXPECT_FALSE(t.IsCoasting());
+    t.MarkMiss();
+    EXPECT_TRUE(t.IsCoasting());  // lost_count > 0 即 Coast（§3.2）
+}
+
+// ---- 生命周期方法计数语义（§7） ----
+
+TEST(TrackletTest, PredictAdvancesAgeAndUpdatesLastState) {
+    Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
+    const int age_before = t.age;
+    t.Predict();  // 速度为零：位置不变，但 age+1、last_box 更新为预测框
+    EXPECT_EQ(t.age, age_before + 1);
+    ASSERT_TRUE(t.last_box.has_value());
+    EXPECT_NEAR(t.last_center.first, 100.0, 1e-9);
+    EXPECT_NEAR(t.last_center.second, 100.0, 1e-9);
+    // hits / lost_count / confidence 不受预测影响
+    EXPECT_EQ(t.hits, 1);
+    EXPECT_EQ(t.lost_count, 0);
+    EXPECT_DOUBLE_EQ(t.confidence, 0.9);
+}
+
+TEST(TrackletTest, UpdateResetsLostAndIncrementsHits) {
+    Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
+    t.MarkMiss();
+    t.MarkMiss();
+    ASSERT_EQ(t.lost_count, 2);
+    ASSERT_TRUE(t.IsCoasting());
+
+    const int age_before = t.age;
+    const BoxXYXY box{150.0, 160.0, 170.0, 180.0};
+    t.Update(box, 0.75, 2);
+
+    EXPECT_EQ(t.lost_count, 0);  // 匹配成功清零（§7）
+    EXPECT_EQ(t.hits, 2);        // hits+1
+    EXPECT_EQ(t.age, age_before + 1);  // age+1
+    EXPECT_DOUBLE_EQ(t.confidence, 0.75);
+    EXPECT_EQ(t.cls_id, 2);
+    EXPECT_DOUBLE_EQ(t.last_center.first, 160.0);   // (150+170)/2
+    EXPECT_DOUBLE_EQ(t.last_center.second, 170.0);  // (160+180)/2
+    ASSERT_TRUE(t.last_box.has_value());
+    EXPECT_DOUBLE_EQ(t.last_box->x1, box.x1);
+    ASSERT_TRUE(t.last_raw_box.has_value());  // 实测框同步记录
+    EXPECT_DOUBLE_EQ(t.last_raw_box->x2, box.x2);
+    EXPECT_FALSE(t.IsCoasting());
+}
+
+TEST(TrackletTest, MarkMissIncrementsLostAndAge) {
+    Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
+    const int age_before = t.age;
+    t.MarkMiss();
+    EXPECT_EQ(t.lost_count, 1);
+    EXPECT_EQ(t.age, age_before + 1);
+    // MarkMiss 不影响 hits / confidence
+    EXPECT_EQ(t.hits, 1);
+    EXPECT_DOUBLE_EQ(t.confidence, 0.9);
+}
+
+}  // namespace

reference-c++/visual_track_control/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.16)
+
+# =============================================================================
+# visual_track_control —— 视觉跟踪控制律静态库（独立子工程）
+#
+# 实现 docs/物理追踪思路.md 的水平航向控制（§4）、垂直/高度双支路控制（§5）、
+# 距离 PID（§6）、全量限幅（§8）、Coast 清积分与判丢刹车（§6.4）、
+# 超龄降级（§7.1、§9.2）。
+#
+# 构建约束：
+#   - 与主工程完全解耦：根 CMakeLists.txt 零改动，可独立构建；
+#   - 仅依赖 third_party：nlohmann（单头文件）、spdlog（源码）、
+#     c_library_v2-master（MAVLink 纯头文件）；
+#   - target_tracker 为 SHARED 库且零源码修改，原样 add_subdirectory 复用，
+#     TrackResult 出现在本库公共头（vtc_types.h），故以 PUBLIC 链接；
+#   - MAVLink include 目录设为 SYSTEM PRIVATE：压告警噪音且不泄漏给下游，
+#     全工程仅 src/mavlink_command_gateway.cpp 一个 TU 包含 mavlink.h
+#     （MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义）。
+# =============================================================================
+
+project(visual_track_control LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# target_tracker 是 SHARED 库：BUILD_RPATH 指向构建目录，示例无需
+# LD_LIBRARY_PATH 即可直接运行（部署时需随带 libtarget_tracker.so.1）。
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_BINARY_DIR}/target_tracker;$ORIGIN")
+
+# ---- 依赖：target_tracker（零改动复用；其测试块由 -DBUILD_TESTING=OFF 压制） ----
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../target_tracker
+                 ${CMAKE_BINARY_DIR}/target_tracker)
+
+# ---- 依赖：spdlog（守卫引入，避免挂载进根构建时重复定义；关闭示例/测试/动态库/安装） ----
+if(NOT TARGET spdlog::spdlog)
+    set(SPDLOG_BUILD_EXAMPLE    OFF CACHE BOOL "" FORCE)
+    set(SPDLOG_BUILD_EXAMPLE_HO OFF CACHE BOOL "" FORCE)
+    set(SPDLOG_BUILD_TESTS      OFF CACHE BOOL "" FORCE)
+    set(SPDLOG_BUILD_TESTS_HO   OFF CACHE BOOL "" FORCE)
+    set(SPDLOG_BUILD_SHARED     OFF CACHE BOOL "" FORCE)
+    set(SPDLOG_INSTALL          OFF CACHE BOOL "" FORCE)
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third_party/spdlog
+                     ${CMAKE_BINARY_DIR}/spdlog)
+endif()
+
+# ---- 静态库本体 ----
+add_library(visual_track_control STATIC
+    src/vtc_config.cpp
+    src/pid_controller.cpp
+    src/line_of_sight.cpp
+    src/tracking_control_law.cpp
+    src/mavlink_command_gateway.cpp)
+
+target_include_directories(visual_track_control PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+# nlohmann JSON：单头文件，仅配置加载器（启动期）使用，不进入热路径
+target_include_directories(visual_track_control PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/nlohmann)
+
+# MAVLink 纯头文件库：SYSTEM 压告警、PRIVATE 不泄漏（公共头无任何 mavlink_* 类型）
+target_include_directories(visual_track_control SYSTEM PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/c_library_v2-master)
+
+target_link_libraries(visual_track_control
+    PUBLIC  target_tracker
+    PRIVATE spdlog::spdlog)
+target_compile_features(visual_track_control PUBLIC cxx_std_17)
+
+# 编译告警：与主工程一致
+if(MSVC)
+    target_compile_options(visual_track_control PRIVATE /W4 /permissive-)
+else()
+    target_compile_options(visual_track_control PRIVATE -Wall -Wextra -Wpedantic)
+endif()
+
+# ---- 示例可执行文件（合成检测 → 追踪 → 控制律 → MAVLink 字节流打印） ----
+add_executable(track_control_demo examples/track_control_demo.cpp)
+target_link_libraries(track_control_demo PRIVATE visual_track_control spdlog::spdlog)
+target_compile_definitions(track_control_demo PRIVATE
+    VTC_DEMO_DEFAULT_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json")
+if(MSVC)
+    target_compile_options(track_control_demo PRIVATE /W4 /permissive-)
+else()
+    target_compile_options(track_control_demo PRIVATE -Wall -Wextra -Wpedantic)
+endif()
+
+# ---- 单元测试（Google Test） ----
+# 独立选项：构建本子工程时需 -DBUILD_TESTING=OFF 压制 target_tracker 的测试，
+# 故本库测试开关不复用 BUILD_TESTING，二者互不干扰。
+# 开发机未安装 GTest 时自动跳过（不影响库与示例构建）。
+option(VTC_BUILD_TESTS "构建 visual_track_control 单元测试" ON)
+if(VTC_BUILD_TESTS)
+    find_package(GTest QUIET)
+    if(GTest_FOUND)
+        enable_testing()
+        include(GoogleTest)
+        add_executable(vtc_unit_tests
+            tests/rate_limiter_test.cpp
+            tests/staleness_checker_test.cpp
+            tests/frame_transform_test.cpp
+            tests/line_of_sight_test.cpp
+            tests/pid_controller_test.cpp
+            tests/tracking_control_law_test.cpp
+            tests/vtc_config_test.cpp
+            tests/mavlink_command_gateway_test.cpp)
+        target_link_libraries(vtc_unit_tests PRIVATE
+            visual_track_control GTest::gtest GTest::gtest_main)
+        target_compile_definitions(vtc_unit_tests PRIVATE
+            VTC_TEST_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json"
+            VTC_TEST_TMP_DIR="${CMAKE_CURRENT_BINARY_DIR}")
+        if(MSVC)
+            target_compile_options(vtc_unit_tests PRIVATE /W4 /permissive-)
+        else()
+            target_compile_options(vtc_unit_tests PRIVATE -Wall -Wextra -Wpedantic)
+        endif()
+        gtest_discover_tests(vtc_unit_tests)
+    else()
+        message(STATUS "未找到 GTest：跳过 vtc_unit_tests 单元测试")
+    endif()
+endif()

reference-c++/visual_track_control/config/track_control.json

@@ -0,0 +1,48 @@
+{
+    "_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。",
+
+    "control": {
+        "frequency_hz": 20.0,
+        "visual_stale_ms": 200,
+        "radar_stale_ms": 500,
+        "attitude_stale_ms": 300
+    },
+
+    "camera": {
+        "image_width": 1920,
+        "image_height": 1080,
+        "fov_h_deg": 60.0,
+        "fov_v_deg": 35.0
+    },
+
+    "heading": {
+        "mode": "rate",
+        "gain": 1.0,
+        "yaw_rate_limit_dps": 30.0,
+        "yaw_slew_limit_dps2": 60.0
+    },
+
+    "vertical": {
+        "vz_gain_mps_per_deg": 0.1,
+        "vz_limit_mps": 2.0
+    },
+
+    "distance": {
+        "d_exp_m": 10.0,
+        "kp": 0.5,
+        "ki": 0.05,
+        "kd": 0.1,
+        "integral_limit": 2.0,
+        "derivative_filter_coef": 0.2,
+        "approach_velocity_limit_mps": 3.0,
+        "retreat_velocity_limit_mps": 1.0,
+        "no_distance_action": "hold",
+        "no_distance_approach_limit_mps": 0.5
+    },
+
+    "accel_limit": {
+        "ax_mps2": 2.0,
+        "ay_mps2": 2.0,
+        "az_mps2": 1.5
+    }
+}

reference-c++/visual_track_control/examples/track_control_demo.cpp

@@ -0,0 +1,225 @@
+// =============================================================================
+// track_control_demo.cpp —— 视觉跟踪控制律演示程序
+//
+// 数据流（模拟单线程整合的完整链路）：
+//   合成运动检测框 → TargetTracker::Update()（模拟推理线程每帧调用）
+//   → LastResult() + 打戳 → ControlSnapshot → TrackingControlLaw::Update()
+//   → MavlinkCommandGateway → LoggingTransport（字节数 + 十六进制摘要）
+//
+// 依次演示三种分支（对应物理追踪思路 §6.4）：
+//   1. 正常跟踪：持续喂入合成检测框，输出速度航向意图（垂直/距离通道
+//      统一为速度分量）；
+//   2. Coast：停止喂检测 → is_predicted==true → 清积分，继续预测跟踪；
+//   3. 判丢刹车：连续丢帧超过追踪库阈值 → tracked==false → 零速刹车。
+//
+// 运行方式：
+//   track_control_demo [配置文件路径]   # 缺省使用编译期内置路径
+// =============================================================================
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <cstdio>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <spdlog/spdlog.h>
+#include <target_tracker/target_tracker.h>
+
+#include "visual_track_control/i_command_transport.h"
+#include "visual_track_control/mavlink_command_gateway.h"
+#include "visual_track_control/tracking_control_law.h"
+#include "visual_track_control/vtc_config.h"
+#include "visual_track_control/vtc_types.h"
+
+#ifndef VTC_DEMO_DEFAULT_CONFIG
+#define VTC_DEMO_DEFAULT_CONFIG "config/track_control.json"
+#endif
+
+namespace {
+
+// ---- 演示时长（秒）：总时长有限，自动退出 ----
+constexpr int kNormalPhaseSeconds = 6;   // 阶段1：正常跟踪
+constexpr int kCoastPhaseSeconds = 4;    // 阶段2：Coast（停止喂检测）
+constexpr int kLostPhaseSeconds = 3;     // 阶段3：判丢刹车
+
+// 追踪库判丢阈值放大：默认 8 帧在 20Hz 下 Coast 仅 0.4 秒，不便于观察；
+// 演示专用放宽到 60 帧（约 3 秒），正式参数以追踪库规格为准。
+constexpr int kDemoMaxLostFrames = 60;
+
+const char* ModeName(drone::vtc::ControlMode mode) {
+    switch (mode) {
+        case drone::vtc::ControlMode::kVelocityHeading: return "速度航向";
+        case drone::vtc::ControlMode::kBrakeHover:      return "零速刹车";
+        case drone::vtc::ControlMode::kDegradedHold:    return "降级保持";
+    }
+    return "未知";
+}
+
+/// 演示用传输实现：统计字节数并打印十六进制摘要（按秒级节流，防止刷屏）
+class LoggingTransport : public drone::vtc::ICommandTransport {
+public:
+    bool Send(const std::uint8_t* data, std::size_t len) override {
+        ++msg_count_;
+        total_bytes_ += len;
+
+        const auto now_ms =
+            std::chrono::duration_cast<std::chrono::milliseconds>(
+                std::chrono::steady_clock::now().time_since_epoch())
+                .count();
+        if (now_ms - last_print_ms_ >= 1000) {
+            // 十六进制摘要：取包头前 12 字节
+            std::string hex;
+            char buf[4];
+            const std::size_t n = len < 12 ? len : 12;
+            for (std::size_t i = 0; i < n; ++i) {
+                std::snprintf(buf, sizeof(buf), "%02X ", data[i]);
+                hex += buf;
+            }
+            spdlog::info("[Transport] 累计 {} 帧 / {} 字节; 本帧 {} 字节, 包头: {}",
+                         msg_count_, total_bytes_, len, hex);
+            last_print_ms_ = now_ms;
+        }
+        return true;
+    }
+
+private:
+    std::int64_t last_print_ms_ = 0;
+    std::uint64_t msg_count_ = 0;
+    std::uint64_t total_bytes_ = 0;
+};
+
+/// 合成运动检测框：从画面右下向中心缓慢漂移（模拟目标被持续锁定）
+std::vector<drone::tracker::Detection> SynthesizeDetections(double elapsed_s,
+                                                            int width, int height) {
+    const double center_x = width * 0.5 + 160.0 - 20.0 * elapsed_s;
+    const double center_y = height * 0.5 + 80.0 - 10.0 * elapsed_s;
+    const double half_w = 70.0;
+    const double half_h = 50.0;
+
+    drone::tracker::Detection det;
+    det.x1 = center_x - half_w;
+    det.y1 = center_y - half_h;
+    det.x2 = center_x + half_w;
+    det.y2 = center_y + half_h;
+    det.conf = 0.9;
+    det.cls_id = 0;
+    return {det};
+}
+
+}  // namespace
+
+int main(int argc, char** argv) {
+    // ---- 启动期：一次性加载配置（缺字段/非法值直接抛异常终止） ----
+    const std::string config_path = argc > 1 ? argv[1] : VTC_DEMO_DEFAULT_CONFIG;
+    drone::vtc::TrackingConfig cfg;
+    try {
+        cfg = drone::vtc::TrackingConfig::LoadFromJson(config_path);
+    } catch (const std::exception& e) {
+        spdlog::error("配置加载失败: {}", e.what());
+        return 1;
+    }
+    spdlog::info("配置加载成功: {} (控制频率 {} Hz)", config_path,
+                 cfg.control.frequency_hz);
+
+    // ---- 构造：追踪库 / 控制律 / 传输 / 网关 ----
+    drone::tracker::TargetTracker::Config tracker_cfg;
+    tracker_cfg.max_lost_frames = kDemoMaxLostFrames;  // 演示专用判丢阈值
+    drone::tracker::TargetTracker tracker(tracker_cfg);
+
+    drone::vtc::TrackingControlLaw control_law(cfg);
+    LoggingTransport transport;
+    drone::vtc::MavlinkCommandGateway gateway(transport, /*sys=*/1, /*comp=*/1,
+                                              /*target_sys=*/1, /*target_comp=*/1);
+
+    const int width = cfg.camera.image_width;
+    const int height = cfg.camera.image_height;
+    const int period_ms = static_cast<int>(1000.0 / cfg.control.frequency_hz);
+    const int normal_ticks = kNormalPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
+    const int coast_ticks = kCoastPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
+    const int lost_ticks = kLostPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
+    const int total_ticks = normal_ticks + coast_ticks + lost_ticks;
+
+    spdlog::info("演示开始: 正常跟踪 {}s → Coast {}s → 判丢刹车 {}s (共 {} 拍, {} ms/拍)",
+                 kNormalPhaseSeconds, kCoastPhaseSeconds, kLostPhaseSeconds,
+                 total_ticks, period_ms);
+
+    drone::tracker::FrameShape frame_shape;
+    frame_shape.height = height;
+    frame_shape.width = width;
+
+    const char* current_phase = nullptr;
+
+    // ---- 主循环：按配置频率固定节拍（§7.1） ----
+    for (int tick = 0; tick < total_ticks; ++tick) {
+        const double elapsed_s = static_cast<double>(tick) / cfg.control.frequency_hz;
+        // 模拟单调时钟：1 秒偏移保证取样时间戳有效
+        const std::int64_t now_ms = 1000 + static_cast<std::int64_t>(tick) * period_ms;
+
+        // ---- 阶段划分 ----
+        const char* phase = tick < normal_ticks ? "正常跟踪"
+                            : tick < normal_ticks + coast_ticks ? "Coast"
+                                                                : "判丢刹车";
+        if (phase != current_phase) {
+            spdlog::info("===== 阶段切换: {} =====", phase);
+            current_phase = phase;
+        }
+
+        // ---- 模拟推理线程：仅正常跟踪阶段喂入检测框 ----
+        // 阶段2/3 停止喂检测：先 Coast（is_predicted），后判丢（tracked==false）
+        std::vector<drone::tracker::Detection> detections;
+        if (tick < normal_ticks) {
+            detections = SynthesizeDetections(elapsed_s, width, height);
+        }
+        tracker.Update(detections, frame_shape);
+
+        // ---- 控制线程：LastResult() + 打戳（TrackResult 无时间戳，§7.2） ----
+        drone::vtc::ControlSnapshot snapshot;
+        snapshot.track = tracker.LastResult();
+        snapshot.track_time_ms = now_ms;
+
+        // 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径）
+        snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s;
+        snapshot.associated_to_target = (tick < normal_ticks) &&
+                                        snapshot.radar_distance_m > 0.0;
+        snapshot.radar_time_ms = now_ms;
+
+        // 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
+        snapshot.attitude_present = true;
+        snapshot.attitude_time_ms = now_ms;
+
+        // ---- 控制律单周期计算 ----
+        const drone::vtc::ControlOutput output = control_law.Update(snapshot, now_ms);
+
+        // ---- 按模式经网关发送 ----
+        const auto time_boot_ms = static_cast<std::uint32_t>(now_ms);
+        switch (output.mode) {
+            case drone::vtc::ControlMode::kVelocityHeading:
+                gateway.SendVelocityHeading(output, time_boot_ms);
+                break;
+            case drone::vtc::ControlMode::kBrakeHover:
+                gateway.SendBrakeHover(time_boot_ms);
+                break;
+            case drone::vtc::ControlMode::kDegradedHold:
+                break;  // 降级保持：不发送指令（仅告警，§7.1）
+        }
+
+        // ---- 半秒一次的摘要打印（演示观测用，非生产热路径） ----
+        if (tick % 10 == 0) {
+            spdlog::info(
+                "[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} "
+                "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}",
+                elapsed_s, phase, ModeName(output.mode),
+                output.coast_active ? "(Coast)" : "", snapshot.track.tracked,
+                output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps,
+                snapshot.radar_distance_m, snapshot.associated_to_target);
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
+    }
+
+    control_law.Reset();  // 停止视觉闭环：清全部状态（§6.4）
+    spdlog::info("演示结束");
+    return 0;
+}

reference-c++/visual_track_control/include/visual_track_control/frame_transform.h

@@ -0,0 +1,44 @@
+#pragma once
+
+// =============================================================================
+// frame_transform.h —— 机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转
+//
+// 对应规格：docs/物理追踪思路.md
+//   §2.3 控制律内部计算统一在机体坐标系；输出的速度意图若采用局部系，
+//        必须随姿态完成旋转并显式标注坐标系，禁止混用未标注坐标系的数据。
+//
+// 实现要点：
+//   - 速度设定值只随**航向角 ψ**（绕下轴旋转）旋转：水平速度分量的
+//     姿态耦合项由飞控内环处理，这是 PX4 offboard 速度设定值的标准做法；
+//     滚转/俯仰不参与（对速度指令为二阶小量）；
+//   - 旋转为正交变换：合速度大小不变，因此"限幅在机体系完成、旋转在
+//     输出端执行"不破坏 §8 限幅约束；
+//   - 无状态纯函数（零分配、无虚函数、无锁），可独立单元测试。
+//
+// 正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
+//   - 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东 ✓
+//   - 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东 ✓
+// =============================================================================
+
+#include <cmath>
+
+namespace drone::vtc {
+
+/// 机体系速度（前/右/下）→ 局部 NED（北/东/地），绕下轴按航向角 ψ 旋转：
+///   v_north =  v_forward·cosψ − v_right·sinψ
+///   v_east  =  v_forward·sinψ + v_right·cosψ
+///   v_down  =  v_down（不变）
+/// 参数以引用传入并就地写出旋转结果。
+inline void RotateBodyToNed(double& v_forward, double& v_right, double& v_down,
+                            double yaw_rad) {
+    (void)v_down;  // 垂直分量不受绕下轴旋转影响；保留参数以保持接口对称
+    const double c = std::cos(yaw_rad);
+    const double s = std::sin(yaw_rad);
+    const double north = v_forward * c - v_right * s;
+    const double east = v_forward * s + v_right * c;
+    v_forward = north;
+    v_right = east;
+    // v_down 不变
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h

@@ -0,0 +1,31 @@
+#pragma once
+
+// =============================================================================
+// i_command_transport.h —— 命令传输层抽象接口
+//
+// 设计目标（双重解耦）：
+//   - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现，
+//     后续重写传输只换实现，不动打包与控制律；
+//   - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。
+//
+// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）：
+// 网关打包后交由实现方决定如何送达。
+// =============================================================================
+
+#include <cstddef>
+#include <cstdint>
+
+namespace drone::vtc {
+
+/// 命令传输接口：只吃字节流
+class ICommandTransport {
+public:
+    virtual ~ICommandTransport() = default;
+
+    /// 发送一帧已打包的命令字节流。
+    /// 返回 true = 已交付底层链路；false = 发送失败（调用方可计数/告警）。
+    /// 实现方保证：不长期持有 data 指针（同步拷贝或同步写出）。
+    virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h

@@ -0,0 +1,38 @@
+#pragma once
+
+// =============================================================================
+// line_of_sight.h —— 像素偏移 → 视线角（针孔模型）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §4.1 水平视线角 β：
+//        针孔模型 β = atan( x_offset · 2·tan(θ_h/2) / W )
+//        （线性近似 β ≈ x_offset · ( θ_h / W ) 仅在 |x_offset| 很小时成立）
+//   §5.1 垂直视线角 α 同理（θ_v、H）
+//
+// 实现要点：
+//   - 全程使用精确针孔模型：规格中的线性近似与针孔式在切换边界两侧
+//     函数值不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制
+//     环路注入突变；atan 计算代价可忽略，故直接全程采用精确式，
+//     linear_ratio 配置项随之移除（§8 待定项收敛）；
+//   - 构造期预计算针孔系数，热路径仅一次乘加 + atan
+//     （零分配、无虚函数、无锁）。
+// =============================================================================
+
+namespace drone::vtc {
+
+/// 像素偏移 → 视线角（弧度）转换器；水平/垂直各构造一个实例
+class PixelToAngle {
+public:
+    /// size_px：画面尺寸（W 或 H，像素，必须 > 0）；
+    /// fov_rad：视场角（θ_h 或 θ_v，弧度，必须 ∈ (0, π)）。
+    /// 非法参数抛 std::invalid_argument（构造期校验）。
+    PixelToAngle(double size_px, double fov_rad);
+
+    /// 偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）
+    double Convert(double offset_px) const;
+
+private:
+    double pinhole_scale_;    // 2·tan(θ/2) / size：针孔模型系数
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h

@@ -0,0 +1,65 @@
+#pragma once
+
+// =============================================================================
+// mavlink_command_gateway.h —— MAVLink 命令打包网关（对外声明）
+//
+// 对应规格：docs/物理追踪思路.md §2.2
+//   - kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
+//   - kBrakeHover      → ID 84 全零速度
+//   （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
+//
+// 关键构建约束（务必遵守）：
+//   - MAVLink 头（common/mavlink.h）只允许被本库的
+//     src/mavlink_command_gateway.cpp 一个 TU 包含：
+//     MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义；
+//   - 因此本头文件不出现任何 mavlink_* 类型，打包/发送缓冲用
+//     固定尺寸字节存储承载（尺寸在 .cpp 中 static_assert 校验）。
+//
+// 线程与资源：无线程、无 IO、公共方法非虚；成员固定缓冲，热路径零堆分配。
+// =============================================================================
+
+#include <cstddef>
+#include <cstdint>
+
+#include "visual_track_control/i_command_transport.h"
+#include "visual_track_control/vtc_types.h"
+
+namespace drone::vtc {
+
+/// MAVLink 命令打包网关：控制意图 → MAVLink 消息字节流 → 传输接口
+class MavlinkCommandGateway {
+public:
+    /// transport：字节流传输实现（注入，生命周期由调用方保证）；
+    /// system_id/component_id：本机 MAVLink 身份；
+    /// target_system/target_component：飞控端身份。
+    MavlinkCommandGateway(ICommandTransport& transport, std::uint8_t system_id,
+                          std::uint8_t component_id, std::uint8_t target_system,
+                          std::uint8_t target_component);
+
+    /// 速度与航向意图 → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式 type_mask）。
+    /// 掩码选择：`yaw_rate_dps` 非零 → 角速率掩码；`yaw_deg` 非零 → 航向角掩码；
+    /// 两者均为零 → 角速率掩码配零角速率（保持当前航向，避免误发绝对航向 0）。
+    bool SendVelocityHeading(const ControlOutput& output, std::uint32_t time_boot_ms);
+
+    /// 零速刹车 → ID 84 全零速度（判丢等场景，§6.4）。
+    bool SendBrakeHover(std::uint32_t time_boot_ms);
+
+    // 固定缓冲尺寸（.cpp 中与实际类型大小做 static_assert 校验）：
+    static constexpr std::size_t kMsgBufferSize = 512;  // ≥ sizeof(mavlink_message_t)
+    static constexpr std::size_t kTxBufferSize = 512;   // ≥ MAVLINK_MAX_PACKET_LEN
+
+private:
+    ICommandTransport& transport_;  // 字节流传输实现（注入）
+
+    // MAVLink 身份
+    std::uint8_t system_id_;
+    std::uint8_t component_id_;
+    std::uint8_t target_system_;
+    std::uint8_t target_component_;
+
+    // 固定缓冲：热路径零堆分配（对齐 8 满足 mavlink_message_t 对齐要求）
+    alignas(8) std::uint8_t msg_storage_[kMsgBufferSize];  // mavlink_message_t 承载
+    std::uint8_t tx_buffer_[kTxBufferSize];                // 发送字节缓冲
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/pid_controller.h

@@ -0,0 +1,59 @@
+#pragma once
+
+// =============================================================================
+// pid_controller.h —— 位置式 PID 控制器（纯算法构件）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §6.2 距离控制律：前向速度指令 = PID(Δd)，Δd = d − d_exp
+//   §8   PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化
+//
+// 特性（热路径零分配、无虚函数、无锁）：
+//   - 位置式输出：out = Kp·e + Ki·∫e + Kd·d(e)/dt；
+//   - 积分钳位：积分项限幅在 ±integral_limit；
+//   - anti-windup（条件积分）：输出饱和且本拍积分增量仍推深饱和时冻结积分，
+//     误差反号后允许积分回卷，防止积分饱和与残留；
+//   - 微分一阶低通：derivative_filter_coef ∈ [0,1)，0 = 不滤波；
+//   - ResetIntegral() / Reset() 均 O(1)（Coast 清积分，§6.4）。
+// =============================================================================
+
+namespace drone::vtc {
+
+/// PID 参数（全部来自配置，代码不写死任何数值）
+struct PidParams {
+    double kp = 0.0;  // 比例增益
+    double ki = 0.0;  // 积分增益
+    double kd = 0.0;  // 微分增益
+    double integral_limit = 0.0;        // 积分项限幅（<=0 表示不限幅）
+    double derivative_filter_coef = 0.0;  // 微分一阶低通系数 ∈ [0,1)：越大滤波越强
+    double output_limit = 0.0;          // 输出限幅（<=0 表示不限幅）
+};
+
+/// 位置式 PID 控制器
+class PidController {
+public:
+    explicit PidController(const PidParams& params);
+
+    /// 单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
+    /// dt_s <= 0 时保持上次输出（异常节拍保护）。
+    double Update(double error, double dt_s);
+
+    /// 清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出
+    void ResetIntegral();
+
+    /// 清空全部内部状态（积分、微分历史、上次输出）
+    void Reset();
+
+    /// 上次输出（只读）
+    double LastOutput() const { return last_output_; }
+
+private:
+    PidParams params_;
+
+    double integral_ = 0.0;            // 积分累计（已钳位）
+    double prev_error_ = 0.0;          // 上次误差（微分用）
+    double filtered_derivative_ = 0.0; // 低通后的微分值
+    double last_output_ = 0.0;         // 上次输出
+    bool has_prev_error_ = false;      // 是否存在有效微分历史
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h

@@ -0,0 +1,56 @@
+#pragma once
+
+// =============================================================================
+// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §8   所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033）
+//   S6   首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
+//        （状态机设计.md S6）
+//
+// 特性（热路径零分配、无虚函数、无锁）：
+//   - Clamp()：静态限幅；
+//   - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时
+//     天然实现"首条指令突变抑制"（S6）。
+// =============================================================================
+
+#include <algorithm>
+
+namespace drone::vtc {
+
+/// 限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）
+inline double Clamp(double v, double lo, double hi) {
+    return std::min(std::max(v, lo), hi);
+}
+
+/// 变化率限幅器：限制输出每秒最大变化量（单位由调用方决定，
+/// 如 度/秒^2 用于航向、米/秒^2 用于速度）
+class SlewRateLimiter {
+public:
+    /// rate_limit：每秒最大变化量（<=0 表示冻结输出）；initial：初始输出值
+    explicit SlewRateLimiter(double rate_limit, double initial = 0.0)
+        : rate_limit_(rate_limit), output_(initial) {}
+
+    /// 朝 target 推进一个节拍：单步变化不超过 rate_limit·dt_s。
+    /// dt_s <= 0 或 rate_limit <= 0 时保持当前输出（异常节拍保护）。
+    double Limit(double target, double dt_s) {
+        if (dt_s <= 0.0 || rate_limit_ <= 0.0) {
+            return output_;
+        }
+        const double max_step = rate_limit_ * dt_s;
+        output_ = Clamp(target, output_ - max_step, output_ + max_step);
+        return output_;
+    }
+
+    /// 复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）
+    void Reset(double value = 0.0) { output_ = value; }
+
+    /// 上次输出（只读）
+    double LastOutput() const { return output_; }
+
+private:
+    double rate_limit_;  // 每秒最大变化量
+    double output_;      // 当前输出
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h

@@ -0,0 +1,38 @@
+#pragma once
+
+// =============================================================================
+// staleness_checker.h —— 输入超龄判定（单调毫秒时钟）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令；
+//        超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。
+//
+// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。
+// =============================================================================
+
+#include <cstdint>
+
+namespace drone::vtc {
+
+/// 输入超龄判定器（无状态，仅提供静态判定）
+class StalenessChecker {
+public:
+    /// sample_time_ms：取样单调时钟毫秒（<=0 视为从未取样，直接判超龄）；
+    /// now_ms：当前单调时钟毫秒；
+    /// valid_for_ms：有效期（毫秒，配置给定）。
+    /// 返回 true = 已超龄。
+    static bool IsStale(std::int64_t sample_time_ms, std::int64_t now_ms,
+                        std::int64_t valid_for_ms) {
+        if (sample_time_ms <= 0) {
+            return true;  // 从未取样：视为超龄（触发降级）
+        }
+        if (now_ms < sample_time_ms) {
+            // 时间戳不一致（时钟源错配/打戳错误）：年龄不可信。
+            // 单一单调时钟下不应发生；宁可降级也不信任不可核实的取样。
+            return true;
+        }
+        return (now_ms - sample_time_ms) > valid_for_ms;
+    }
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h

@@ -0,0 +1,63 @@
+#pragma once
+
+// =============================================================================
+// tracking_control_law.h —— 视觉跟踪主控律（TrackingControlLaw）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §4  水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
+//   §5  垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
+//   §6  距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3）
+//   §6.4 Coast 清积分、判丢零速刹车
+//   §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
+//   §8   统一限幅
+//   §2.3 输出端随机体航向把机体系速度旋转为局部 NED
+//
+// 线程模型：单线程调用（状态机与控制线程），成员无锁、无堆对象；
+// 日志仅状态切换点（刹车进出、降级切换），高频热路径不打日志。
+// =============================================================================
+
+#include <cstdint>
+
+#include "visual_track_control/line_of_sight.h"
+#include "visual_track_control/pid_controller.h"
+#include "visual_track_control/rate_limiter.h"
+#include "visual_track_control/vtc_config.h"
+#include "visual_track_control/vtc_types.h"
+
+namespace drone::vtc {
+
+/// 视觉跟踪主控律：消费控制快照，输出限幅后的控制意图
+class TrackingControlLaw {
+public:
+    /// cfg：启动期加载的全量配置（值拷贝持有，运行期不再触碰配置源）
+    explicit TrackingControlLaw(const TrackingConfig& cfg);
+
+    /// 单周期控制计算（§7.1 固定周期持续运行）。
+    /// now_ms：当前单调时钟毫秒；内部据此推算 dt 并做超龄判定。
+    /// 流程：①超龄判定 → ②判丢刹车 → ③Coast 清积分 → ④像素→视线角
+    ///       → ⑤水平航向 → ⑥垂直速度 → ⑦距离通道 → ⑧机体系→NED 旋转输出
+    ControlOutput Update(const ControlSnapshot& snapshot, std::int64_t now_ms);
+
+    /// 停止视觉闭环时清全部状态（积分、限幅器历史、节拍时钟，§6.4）
+    void Reset();
+
+private:
+    TrackingConfig cfg_;  // 配置值拷贝（运行期只读）
+
+    // ---- 预计算构件（构造期完成，热路径零分配） ----
+    PixelToAngle pixel_to_beta_;   // 水平：x_offset → β（§4.1）
+    PixelToAngle pixel_to_alpha_;  // 垂直：y_offset → α（§5.1）
+    PidController distance_pid_;   // 距离通道 PID（§6.2）
+
+    // ---- 变化率限幅器（§8 / S6 首条指令限幅） ----
+    SlewRateLimiter yaw_deg_limiter_;    // 位置式航向变化率（度/秒^2）
+    SlewRateLimiter yaw_rate_limiter_;   // 速率式角速度变化率（度/秒^2）
+    SlewRateLimiter vx_limiter_;         // 前向加速度限幅（米/秒^2）
+    SlewRateLimiter vz_limiter_;         // 垂直加速度限幅（米/秒^2）
+
+    // ---- 节拍与状态（仅打日志用，不参与数学） ----
+    std::int64_t last_now_ms_ = 0;                       // 上次计算时刻
+    ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志）
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/vtc_config.h

@@ -0,0 +1,113 @@
+#pragma once
+
+// =============================================================================
+// vtc_config.h —— 视觉跟踪控制律配置（TrackingConfig + LoadFromJson）
+//
+// 对应规格：docs/物理追踪思路.md
+//   §8  限幅、阈值与配置化（所有阈值集中配置，不得散落在代码中，
+//       需求分析.md NFR-007）
+//   §4  水平航向控制（模式、增益、限幅）
+//   §5  垂直/高度控制（垂直速度增益与限幅；原双支路已统一为速度支路）
+//   §6  距离控制（射程、PID、限幅、无距离降级动作）
+//   §7  控制周期与时序（频率、各输入超龄阈值）
+//
+// 设计约束：
+//   - 全部数值待定：本结构默认值与示例配置均为占位，标定前仅限回放验证；
+//   - LoadFromJson 启动期一次性解析：缺字段/类型错/非法值抛
+//     std::runtime_error 并携带字段名（初始化抛异常规范）；
+//   - 热路径绝不触碰 JSON 对象，不做热更新（调参重启即可）。
+// =============================================================================
+
+#include <cstdint>
+#include <string>
+
+namespace drone::vtc {
+
+/// 航向通道模式（§4.2：位置式/速率式按 PX4 1.11.3 Offboard 实测选择）
+///
+/// 默认速率式：yaw_rate 指令天然是相对量，无坐标系歧义。位置式输出为
+/// 绝对 NED 航向（当前航向 + 限幅修正量合成），依赖新鲜自机姿态；
+/// 姿态接线未完成前不得用于实飞。
+enum class HeadingMode : std::uint8_t {
+    kRate,      // 速率式（默认）：β → 偏航角速度，输出 yaw_rate_dps
+    kPosition,  // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
+};
+
+/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定）
+enum class NoDistanceAction : std::uint8_t {
+    kHold,         // 保持：前向速度指令清零（安全占位语义）
+    kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
+    kExit,         // 退出近距操作：输出降级保持（kDegradedHold）
+};
+
+/// control 组：控制周期与各输入超龄阈值（§7）
+struct ControlGroupConfig {
+    double frequency_hz = 20.0;             // 控制频率（Hz，§7.1 待定）
+    std::int64_t visual_stale_ms = 200;     // 视觉追踪结果超龄阈值（毫秒）
+    std::int64_t radar_stale_ms = 500;      // 雷达距离超龄阈值（毫秒）
+    std::int64_t attitude_stale_ms = 300;   // 自机姿态超龄阈值（毫秒）
+};
+
+/// camera 组：画面与视场角（§3.2、§4.1、§5.1；内参/畸变未标定，占位）
+struct CameraGroupConfig {
+    int image_width = 1920;    // 画面宽度 W（像素）
+    int image_height = 1080;   // 画面高度 H（像素）
+    double fov_h_deg = 60.0;   // 水平视场角 θ_h（度，仅水平，§4.1）
+    double fov_v_deg = 35.0;   // 垂直视场角 θ_v（度，§5.1）
+};
+
+/// heading 组：水平航向通道（§4）
+struct HeadingGroupConfig {
+    HeadingMode mode = HeadingMode::kRate;  // 默认速率式（§4.2，见 HeadingMode 注释）
+    double gain = 1.0;                  // 增益：视线角(度) → 航向指令（无量纲占位）
+    double yaw_rate_limit_dps = 30.0;   // 偏航角速度上限（度/秒，FR-033）
+    double yaw_slew_limit_dps2 = 60.0;  // 方向变化率上限（度/秒^2，FR-033、S6 首条限幅）
+};
+
+/// vertical 组：垂直/高度控制（§5）
+///
+/// 规格 §5.2 原设"小误差姿态微调 / 大误差垂直速度"双支路，实测评审后
+/// 统一为速度支路（见 ControlMode 注释）：切换阈值与姿态微调参数随之移除。
+/// 小误差经 vz_gain 线性映射为小垂直速度，行为连续且无模式翻转。
+struct VerticalGroupConfig {
+    double vz_gain_mps_per_deg = 0.1;    // 垂直速度增益：α(度) → 垂直速度(米/秒)
+    double vz_limit_mps = 2.0;           // 垂直速度限幅（米/秒）
+};
+
+/// distance 组：距离通道（§6）
+struct DistanceGroupConfig {
+    double d_exp_m = 10.0;          // 射程 d_exp（米，由拦截设备接口给出，§6.1 待定）
+    double kp = 0.0;                // PID 比例（§6.2 待定）
+    double ki = 0.0;                // PID 积分
+    double kd = 0.0;                // PID 微分
+    double integral_limit = 0.0;    // 积分项限幅
+    double derivative_filter_coef = 0.0;  // 微分一阶低通系数 ∈ [0,1)：0=不滤波
+    double approach_velocity_limit_mps = 3.0;  // 接近速度限幅（米/秒，§8）
+    double retreat_velocity_limit_mps = 1.0;   // 后退速度限幅（米/秒，§8）
+    NoDistanceAction no_distance_action = NoDistanceAction::kHold;  // 无距离降级（§6.3）
+    double no_distance_approach_limit_mps = 0.5;  // kSlowApproach 限速值（米/秒）
+};
+
+/// accel_limit 组：各轴加速度限幅（§8，待定）
+struct AccelLimitConfig {
+    double ax_mps2 = 2.0;  // 前向加速度限幅（米/秒^2）
+    double ay_mps2 = 2.0;  // 侧向加速度限幅（米/秒^2，预留）
+    double az_mps2 = 1.5;  // 垂直加速度限幅（米/秒^2）
+};
+
+/// 视觉跟踪控制律全量配置（§8：所有阈值集中于此）
+struct TrackingConfig {
+    ControlGroupConfig control;
+    CameraGroupConfig camera;
+    HeadingGroupConfig heading;
+    VerticalGroupConfig vertical;
+    DistanceGroupConfig distance;
+    AccelLimitConfig accel_limit;
+
+    /// 从 JSON 文件一次性加载配置（启动期调用）。
+    /// 缺字段 / 类型错误 / 非法取值均抛 std::runtime_error，
+    /// 异常消息携带字段名（如 "heading.gain"）。
+    static TrackingConfig LoadFromJson(const std::string& path);
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/vtc_types.h

@@ -0,0 +1,102 @@
+#pragma once
+
+// =============================================================================
+// vtc_types.h —— 视觉跟踪控制律的中立输入/输出结构
+//
+// 对应规格：docs/物理追踪思路.md
+//   §2   输入输出与数据流（控制意图枚举语义）
+//   §6.4 与追踪库的行为衔接（tracked / is_predicted 语义）
+//   §7.2 与 target_tracker 的衔接（TrackResult 无时间戳，由调用方在
+//        LastResult() 瞬间打戳）
+//
+// 设计约束：
+//   - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
+//     控制律热路径零堆分配；
+//   - 本库不依赖主工程 include/ 的任何类型，独立可编译；
+//   - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作
+//     目标距离（§6.3、需求分析.md FR-041）。
+// =============================================================================
+
+#include <cstdint>
+
+#include <target_tracker/target_tracker.h>
+
+namespace drone::vtc {
+
+/// 控制输出模式（对应物理追踪思路 §2.2 的控制意图语义）
+///
+/// 注意：规格 §5.2 原设"垂直小误差姿态微调支路"（kAttitudeTrim →
+/// SET_ATTITUDE_TARGET），实测评审后取消——姿态微调支路会把整个输出切成
+/// 姿态消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息
+/// 类型逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路（小误差
+/// 对应小 vz），全周期单一 offboard 消息。
+enum class ControlMode : std::uint8_t {
+    kVelocityHeading,  // 速度与航向意图：速度分量 + yaw（§4.2、§5、§6.2 主输出）
+    kBrakeHover,       // 零速刹车/悬停：判丢等场景（§6.4、§9.1）
+    kDegradedHold,     // 降级保持：超龄/退出近距操作，禁止正常控制（§7.1、§9.2）
+};
+
+/// 降级原因（仅 kDegradedHold 模式有意义）
+enum class DegradedReason : std::uint8_t {
+    kNone,             // 未降级
+    kAttitudeStale,    // PX4 姿态过期：禁止空间转换和控制（§9.2）
+    kVisionStale,      // 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
+    kNoDistanceExit,   // 无距离关联且配置为退出近距操作（§6.3）
+};
+
+/// 控制律输入快照：调用方在同一控制周期内采集并打戳
+///
+/// 注意：TrackResult 本身不含时间戳（§7.2），调用方必须在读取
+/// LastResult() 的瞬间用单调时钟打戳填入 track_time_ms；
+/// 超龄判定一律使用"取样时间 + valid_for_ms"（§7.1）。
+struct ControlSnapshot {
+    // ---- 视觉追踪结果（控制律主输入） ----
+    drone::tracker::TrackResult track;  // 追踪库结果（值拷贝）
+    std::int64_t track_time_ms = 0;     // 取样单调时钟（毫秒），<=0 视为从未取样
+
+    // ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ----
+    double radar_distance_m = 0.0;      // 机头前向单点距离（米）
+    bool associated_to_target = false;  // 相机—雷达空间关联成立标志
+    std::int64_t radar_time_ms = 0;     // 取样单调时钟（毫秒）
+
+    // ---- 自机姿态/速度 ----
+    // 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转
+    // （§2.3）与位置式绝对航向合成；其余字段预留，接入主工程时由
+    // FlightStateSnapshot 填充（机体补偿见 §4.1）。
+    bool attitude_present = false;      // 姿态源当前是否可用
+    std::int64_t attitude_time_ms = 0;  // 姿态取样单调时钟（毫秒）
+    double roll_rad = 0.0;              // 横滚（弧度，FRD）
+    double pitch_rad = 0.0;             // 俯仰（弧度，FRD）
+    double yaw_rad = 0.0;               // 航向（弧度，NED）
+    double yaw_rate_radps = 0.0;        // 偏航角速度（弧度/秒，机体补偿预留，§4.1）
+    double velocity_x_mps = 0.0;        // 自机速度分量（预留）
+    double velocity_y_mps = 0.0;
+    double velocity_z_mps = 0.0;
+};
+
+/// 控制律输出：限幅后的控制意图（§8：所有输出必须经过限幅）
+///
+/// 坐标语义：速度分量在机体系（前/右/下，§2.3）内计算与限幅，输出前
+/// 已随自机航向（ControlSnapshot.yaw_rad）旋转为局部 NED（北/东/地），
+/// 与网关使用的 MAV_FRAME_LOCAL_NED 一致；因此旋转后 vy_mps 可能非零
+/// （机体系侧向恒为 0，旋转不改变合速度大小，限幅在旋转前完成）。
+struct ControlOutput {
+    ControlMode mode = ControlMode::kDegradedHold;  // 输出模式
+    bool valid = false;                             // 指令是否可用于发送
+
+    // ---- 速度分量（局部 NED：北/东/地，米/秒；由机体系随航向旋转得到） ----
+    double vx_mps = 0.0;   // 北向（距离通道输出经旋转，§6.2）
+    double vy_mps = 0.0;   // 东向（机体系恒 0，航向旋转后可非零）
+    double vz_mps = 0.0;   // 垂直（NED 下正，垂直速度支路输出，§5.2）
+
+    // ---- 航向通道（§4.2：位置式输出 yaw_deg，速率式输出 yaw_rate_dps） ----
+    double yaw_deg = 0.0;       // 目标绝对航向（度，NED，右转为正；仅位置式，
+                                // 由当前航向 + 限幅修正量合成并回绕 [-180,180)）
+    double yaw_rate_dps = 0.0;  // 偏航角速度指令（度/秒；速率式为默认，天然相对量）
+
+    // ---- 状态标注 ----
+    DegradedReason degraded_reason = DegradedReason::kNone;  // 降级原因
+    bool coast_active = false;  // 本周期处于 Coast（is_predicted==true，§6.4）
+};
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/src/line_of_sight.cpp

@@ -0,0 +1,42 @@
+// =============================================================================
+// line_of_sight.cpp —— 像素偏移 → 视线角实现（全程针孔模型）
+//
+// 对应规格：docs/物理追踪思路.md §4.1（水平 β）、§5.1（垂直 α）：
+//   针孔模型：angle = atan( offset · 2·tan(θ/2) / size )
+//
+// 原设计的线性近似双路切换已取消：线性式与针孔式在切换边界两侧不连续
+// （θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入突变；
+// atan 代价可忽略，全程采用精确针孔式。
+//
+// 构造期预计算针孔系数，Convert 热路径仅一次乘加 + atan。
+// =============================================================================
+
+#include "visual_track_control/line_of_sight.h"
+
+#include <cmath>
+#include <stdexcept>
+
+namespace drone::vtc {
+
+namespace {
+constexpr double kPi = 3.14159265358979323846;
+}
+
+PixelToAngle::PixelToAngle(double size_px, double fov_rad) {
+    // 构造期校验：非法参数直接抛异常（初始化抛异常规范）
+    if (!(size_px > 0.0)) {
+        throw std::invalid_argument("PixelToAngle: size_px 必须为正");
+    }
+    if (!(fov_rad > 0.0 && fov_rad < kPi)) {
+        throw std::invalid_argument("PixelToAngle: fov_rad 必须在 (0, π) 内");
+    }
+
+    // 预计算针孔系数（§4.1 公式）
+    pinhole_scale_ = 2.0 * std::tan(fov_rad / 2.0) / size_px;  // 2·tan(θ/2) / size
+}
+
+double PixelToAngle::Convert(double offset_px) const {
+    return std::atan(offset_px * pinhole_scale_);  // 针孔模型，全偏移范围精确
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/src/mavlink_command_gateway.cpp

@@ -0,0 +1,119 @@
+// =============================================================================
+// mavlink_command_gateway.cpp —— MAVLink 命令打包网关实现
+//
+// 对应规格：docs/物理追踪思路.md §2.2
+//   kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
+//   kBrakeHover      → ID 84 全零速度
+//   （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
+//
+// 关键构建约束：本文件是全工程唯一包含 common/mavlink.h 的 TU。
+// MAVLINK_HELPER 默认空宏（mavlink_helpers.h），多 TU 包含会多重定义。
+//
+// 打包策略：
+//   - 使用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入），
+//     不使用 _pack_chan 通道 API；
+//   - mavlink_msg_to_send_buffer 输出字节流 → ICommandTransport::Send；
+//   - 消息/发送缓冲为成员固定持有，热路径零堆分配。
+//
+// 坐标系与语义说明（待实测确认）：
+//   - coordinate_frame 取 MAV_FRAME_LOCAL_NED；速度分量由主控律在输出端
+//     随机体航向旋转为局部 NED（北/东/地，物理追踪思路 §2.3），本文件
+//     直接填充不再做坐标变换；
+//   - 速度模式 type_mask 需以 PX4 1.11.3 Offboard 实测为准
+//     （物理追踪思路 §10.1）。
+// =============================================================================
+
+#include "visual_track_control/mavlink_command_gateway.h"
+
+#include <cmath>
+
+#include "common/mavlink.h"  // 全工程唯一包含点（见文件头构建约束）
+
+namespace drone::vtc {
+namespace {
+
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kDegToRad = kPi / 180.0;
+
+// ---- SET_POSITION_TARGET_LOCAL_NED（ID 84）type_mask 常量 ----
+// 掩码位语义：置位 = 忽略该字段。
+// 基础位：忽略位置 x/y/z（bit0-2）与加速度 afx/afy/afz（bit6-8）= 0x01C7。
+constexpr std::uint16_t kMaskVelocityBase = 0x01C7;
+// 速度 + 航向角模式：忽略 yaw_rate（bit11）
+constexpr std::uint16_t kMaskVelocityYaw = kMaskVelocityBase | 0x0800;
+// 速度 + 偏航角速率模式：忽略 yaw（bit10）
+constexpr std::uint16_t kMaskVelocityYawRate = kMaskVelocityBase | 0x0400;
+// 刹车模式：速度全零，yaw 与 yaw_rate 均忽略（bit10、bit11）
+constexpr std::uint16_t kMaskBrake = kMaskVelocityBase | 0x0C00;
+
+/// 固定存储解释为消息结构（打包函数只写入字节，不读取旧值）
+inline mavlink_message_t* AsMessage(std::uint8_t* storage) {
+    return reinterpret_cast<mavlink_message_t*>(storage);
+}
+
+}  // namespace
+
+// 固定缓冲尺寸校验：编译期保证存储足够（头文件不暴露 mavlink 类型）
+static_assert(sizeof(mavlink_message_t) <= MavlinkCommandGateway::kMsgBufferSize,
+              "msg_storage_ 不足以容纳 mavlink_message_t");
+static_assert(MAVLINK_MAX_PACKET_LEN <= MavlinkCommandGateway::kTxBufferSize,
+              "tx_buffer_ 不足以容纳最大 MAVLink 数据包");
+
+MavlinkCommandGateway::MavlinkCommandGateway(ICommandTransport& transport,
+                                             std::uint8_t system_id,
+                                             std::uint8_t component_id,
+                                             std::uint8_t target_system,
+                                             std::uint8_t target_component)
+    : transport_(transport),
+      system_id_(system_id),
+      component_id_(component_id),
+      target_system_(target_system),
+      target_component_(target_component) {}
+
+bool MavlinkCommandGateway::SendVelocityHeading(const ControlOutput& output,
+                                                std::uint32_t time_boot_ms) {
+    auto* msg = AsMessage(msg_storage_);
+
+    // 掩码三分支（A2 修复）：
+    //   角速率非零 → 速率掩码（速率式控制律常态，§4.2）；
+    //   航向角非零 → 航向掩码（位置式控制律）；
+    //   两者均零   → 速率掩码配零角速率：保持当前航向。
+    // 旧实现把"零角速率"落到航向掩码，等于误发绝对航向 0（指向正北）。
+    const bool has_yaw_rate = std::fabs(output.yaw_rate_dps) > 1.0e-9;
+    const bool has_yaw = std::fabs(output.yaw_deg) > 1.0e-9;
+    const std::uint16_t type_mask =
+        (!has_yaw_rate && has_yaw) ? kMaskVelocityYaw : kMaskVelocityYawRate;
+
+    mavlink_msg_set_position_target_local_ned_pack(
+        system_id_, component_id_, msg,
+        time_boot_ms, target_system_, target_component_,
+        MAV_FRAME_LOCAL_NED, type_mask,
+        0.0F, 0.0F, 0.0F,  // 位置字段：忽略
+        static_cast<float>(output.vx_mps), static_cast<float>(output.vy_mps),
+        static_cast<float>(output.vz_mps),
+        0.0F, 0.0F, 0.0F,  // 加速度字段：忽略
+        static_cast<float>(output.yaw_deg * kDegToRad),
+        static_cast<float>(output.yaw_rate_dps * kDegToRad));
+
+    const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
+    return transport_.Send(tx_buffer_, len);
+}
+
+bool MavlinkCommandGateway::SendBrakeHover(std::uint32_t time_boot_ms) {
+    auto* msg = AsMessage(msg_storage_);
+
+    // 判丢零速刹车（§6.4）：速度全零，位置/加速度/航向字段全部忽略
+    mavlink_msg_set_position_target_local_ned_pack(
+        system_id_, component_id_, msg,
+        time_boot_ms, target_system_, target_component_,
+        MAV_FRAME_LOCAL_NED, kMaskBrake,
+        0.0F, 0.0F, 0.0F,
+        0.0F, 0.0F, 0.0F,
+        0.0F, 0.0F, 0.0F,
+        0.0F, 0.0F);
+
+    const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
+    return transport_.Send(tx_buffer_, len);
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/src/pid_controller.cpp

@@ -0,0 +1,81 @@
+// =============================================================================
+// pid_controller.cpp —— 位置式 PID 控制器实现
+//
+// 对应规格：docs/物理追踪思路.md
+//   §6.2 前向速度指令 = PID(Δd)
+//   §6.4 Coast 时积分项清空（ResetIntegral）
+//
+// anti-windup 策略（条件积分）：输出达到饱和限幅且本拍积分增量仍在
+// 推深饱和时，冻结本步积分累加（不采纳候选积分值），防止积分饱和；
+// 误差反号后增量反向、允许积分回卷，避免饱和残留。
+// =============================================================================
+
+#include "visual_track_control/pid_controller.h"
+
+#include <cmath>
+
+#include "visual_track_control/rate_limiter.h"
+
+namespace drone::vtc {
+
+PidController::PidController(const PidParams& params) : params_(params) {}
+
+double PidController::Update(double error, double dt_s) {
+    if (dt_s <= 0.0) {
+        return last_output_;  // 异常节拍保护：保持上次输出
+    }
+
+    // ---- 积分：候选值先钳位（积分限幅） ----
+    double integral_candidate = integral_ + error * dt_s;
+    if (params_.integral_limit > 0.0) {
+        integral_candidate =
+            Clamp(integral_candidate, -params_.integral_limit, params_.integral_limit);
+    }
+
+    // ---- 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） ----
+    double derivative = 0.0;
+    if (has_prev_error_) {
+        derivative = (error - prev_error_) / dt_s;
+        const double coef = params_.derivative_filter_coef;
+        derivative = coef * filtered_derivative_ + (1.0 - coef) * derivative;
+    }
+    filtered_derivative_ = derivative;
+    prev_error_ = error;
+    has_prev_error_ = true;
+
+    // ---- 位置式合成 ----
+    const double integral_term = params_.ki * integral_candidate;
+    const double raw = params_.kp * error + integral_term + params_.kd * derivative;
+
+    // ---- anti-windup（条件积分，C1 修复） ----
+    // 冻结判据基于"本拍积分增量方向"（error·dt）：输出饱和且本拍增量仍
+    // 在推深饱和时才冻结。不采用旧实现"积分项与输出同向"判据（误差反号
+    // 后无法回卷），也不用 raw·error（大 kp 项会掩盖饱和方向、拖延回卷）。
+    const bool saturated = params_.output_limit > 0.0 && std::fabs(raw) > params_.output_limit;
+    const bool increment_pushes_deeper = (error * dt_s) * raw > 0.0;
+    if (!(saturated && increment_pushes_deeper)) {
+        integral_ = integral_candidate;
+    }
+
+    // ---- 输出限幅 ----
+    if (params_.output_limit > 0.0) {
+        last_output_ = Clamp(raw, -params_.output_limit, params_.output_limit);
+    } else {
+        last_output_ = raw;
+    }
+    return last_output_;
+}
+
+void PidController::ResetIntegral() {
+    integral_ = 0.0;  // §6.4：Coast 清积分，防积分饱和
+}
+
+void PidController::Reset() {
+    integral_ = 0.0;
+    prev_error_ = 0.0;
+    filtered_derivative_ = 0.0;
+    last_output_ = 0.0;
+    has_prev_error_ = false;
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/src/tracking_control_law.cpp

@@ -0,0 +1,239 @@
+// =============================================================================
+// tracking_control_law.cpp —— 视觉跟踪主控律实现
+//
+// 对应规格：docs/物理追踪思路.md
+//   §4.1/§4.2 水平航向控制：像素偏移 → β → 增益 → slew → 角速度限幅，
+//             位置式输出 yaw_deg / 速率式输出 yaw_rate_dps（按配置）
+//   §5.1/§5.2 垂直控制：统一速度支路 α → 垂直速度设定值（原"小误差姿态
+//             微调/大误差垂直速度"双支路已合并：姿态微调会把输出切成姿态
+//             消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
+//             vtc_types.h ControlMode 注释）
+//   §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）；
+//             未关联/超龄 → 按配置降级动作（hold / slow_approach / exit）
+//   §6.4      Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车
+//   §7.1      超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
+//   §8        所有输出统一限幅；首条指令经变化率限幅（S6）
+//   §2.3      输出端随机体航向把机体系速度旋转为局部 NED
+//
+// 日志纪律：仅状态切换点（刹车进出、降级切换）打印；热路径零日志、零分配。
+// =============================================================================
+
+#include "visual_track_control/tracking_control_law.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <spdlog/spdlog.h>
+
+#include "visual_track_control/frame_transform.h"
+#include "visual_track_control/staleness_checker.h"
+
+namespace drone::vtc {
+namespace {
+
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kRadToDeg = 180.0 / kPi;
+constexpr double kDegToRad = kPi / 180.0;
+constexpr double kMaxDtS = 0.5;  // 单步 dt 上限（秒）：异常节拍保护
+
+/// 角度回绕到 [-180, 180)（位置式绝对航向合成用）
+double WrapDeg180(double deg) {
+    double d = std::fmod(deg + 180.0, 360.0);
+    if (d < 0.0) {
+        d += 360.0;
+    }
+    return d - 180.0;
+}
+
+/// 控制模式可读名（仅状态切换日志使用）
+const char* ModeName(ControlMode mode) {
+    switch (mode) {
+        case ControlMode::kVelocityHeading: return "速度航向";
+        case ControlMode::kBrakeHover:      return "零速刹车";
+        case ControlMode::kDegradedHold:    return "降级保持";
+    }
+    return "未知";
+}
+
+/// 由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者
+PidParams BuildDistancePidParams(const TrackingConfig& cfg) {
+    PidParams p;
+    p.kp = cfg.distance.kp;
+    p.ki = cfg.distance.ki;
+    p.kd = cfg.distance.kd;
+    p.integral_limit = cfg.distance.integral_limit;
+    p.derivative_filter_coef = cfg.distance.derivative_filter_coef;
+    p.output_limit = std::max(cfg.distance.approach_velocity_limit_mps,
+                              cfg.distance.retreat_velocity_limit_mps);
+    return p;
+}
+
+}  // namespace
+
+TrackingControlLaw::TrackingControlLaw(const TrackingConfig& cfg)
+    : cfg_(cfg),
+      pixel_to_beta_(cfg.camera.image_width, cfg.camera.fov_h_deg * kDegToRad),
+      pixel_to_alpha_(cfg.camera.image_height, cfg.camera.fov_v_deg * kDegToRad),
+      distance_pid_(BuildDistancePidParams(cfg)),
+      yaw_deg_limiter_(cfg.heading.yaw_slew_limit_dps2),
+      yaw_rate_limiter_(cfg.heading.yaw_slew_limit_dps2),
+      vx_limiter_(cfg.accel_limit.ax_mps2),
+      vz_limiter_(cfg.accel_limit.az_mps2) {}
+
+ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
+                                         std::int64_t now_ms) {
+    // ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
+    // 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率：
+    // 单拍误差由限幅器与超龄判定兜底，不引入额外状态。
+    double dt_s = 1.0 / cfg_.control.frequency_hz;
+    if (last_now_ms_ > 0 && now_ms > last_now_ms_) {
+        dt_s = std::min(static_cast<double>(now_ms - last_now_ms_) / 1000.0, kMaxDtS);
+    }
+    last_now_ms_ = now_ms;
+
+    ControlOutput out;
+    out.mode = ControlMode::kVelocityHeading;  // 正常路径默认模式，分支可覆盖
+
+    // 状态切换日志辅助：仅模式变化时打印（刹车进出、降级切换等）
+    auto FinishAndLog = [&](const char* reason) {
+        if (out.mode != last_mode_) {
+            spdlog::info("[ControlLaw] 状态切换: {} -> {} ({})",
+                         ModeName(last_mode_), ModeName(out.mode), reason);
+            last_mode_ = out.mode;
+        }
+        return out;
+    };
+
+    // ---- ① 超龄判定（§7.1 / §9.2） ----
+    // 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2）
+    if (!snapshot.attitude_present ||
+        StalenessChecker::IsStale(snapshot.attitude_time_ms, now_ms,
+                                  cfg_.control.attitude_stale_ms)) {
+        out.mode = ControlMode::kDegradedHold;
+        out.degraded_reason = DegradedReason::kAttitudeStale;
+        return FinishAndLog("姿态过期/缺失，禁止控制");
+    }
+    // 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
+    if (StalenessChecker::IsStale(snapshot.track_time_ms, now_ms,
+                                  cfg_.control.visual_stale_ms)) {
+        out.mode = ControlMode::kDegradedHold;
+        out.degraded_reason = DegradedReason::kVisionStale;
+        return FinishAndLog("视觉追踪结果超龄");
+    }
+
+    // ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
+    if (!snapshot.track.tracked) {
+        if (last_mode_ != ControlMode::kBrakeHover) {
+            // 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
+            distance_pid_.Reset();
+            yaw_deg_limiter_.Reset();
+            yaw_rate_limiter_.Reset();
+            vx_limiter_.Reset();
+            vz_limiter_.Reset();
+        }
+        out.mode = ControlMode::kBrakeHover;  // 速度分量保持默认全零
+        out.valid = true;
+        return FinishAndLog("判丢，零速刹车");
+    }
+
+    // ---- ③ Coast：is_predicted == true → 清积分防饱和（§6.4） ----
+    if (snapshot.track.is_predicted) {
+        distance_pid_.ResetIntegral();
+        // 航向通道为纯比例增益映射（§4.2），无积分项，无需清理
+        out.coast_active = true;
+    }
+
+    // ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ----
+    // x_offset = center.first − W/2（右正），y_offset = center.second − H/2（下正）
+    const double x_offset =
+        snapshot.track.center.first - cfg_.camera.image_width * 0.5;
+    const double y_offset =
+        snapshot.track.center.second - cfg_.camera.image_height * 0.5;
+    const double beta_deg = pixel_to_beta_.Convert(x_offset) * kRadToDeg;
+    const double alpha_deg = pixel_to_alpha_.Convert(y_offset) * kRadToDeg;
+
+    // ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ----
+    if (cfg_.heading.mode == HeadingMode::kRate) {
+        // 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033）
+        const double target_rate = cfg_.heading.gain * beta_deg;
+        const double slewed = yaw_rate_limiter_.Limit(target_rate, dt_s);
+        out.yaw_rate_dps = Clamp(slewed, -cfg_.heading.yaw_rate_limit_dps,
+                                 cfg_.heading.yaw_rate_limit_dps);
+    } else {
+        // 位置式：β → 航向修正量，变化率限幅（方向变化率上限，FR-033）后
+        // 与当前航向合成为绝对 NED 航向——ID 84 的 yaw 字段是绝对值，
+        // 直接发修正量会让机体转向"修正角"方向（A2 修复）。姿态已在 ①
+        // 通过超龄校验，yaw_rad 可用。
+        const double target_yaw = cfg_.heading.gain * beta_deg;
+        const double correction_deg = yaw_deg_limiter_.Limit(target_yaw, dt_s);
+        out.yaw_deg = WrapDeg180(snapshot.yaw_rad * kRadToDeg + correction_deg);
+    }
+
+    // ---- ⑥ 垂直速度通道（§5：统一速度支路） ----
+    // 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。
+    // 小误差自然映射为小垂直速度：无需单独的切换阈值与姿态微调支路，
+    // 输出模式保持 kVelocityHeading，距离通道前向速度不会被丢弃（A1 修复）。
+    {
+        const double vz_target = cfg_.vertical.vz_gain_mps_per_deg * alpha_deg;
+        const double slewed = vz_limiter_.Limit(vz_target, dt_s);  // 加速度限幅（§8）
+        out.vz_mps = Clamp(slewed, -cfg_.vertical.vz_limit_mps,
+                           cfg_.vertical.vz_limit_mps);
+    }
+
+    // ---- ⑦ 距离通道（§6.2 / §6.3） ----
+    // 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
+    const bool distance_available =
+        snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 &&
+        !StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms,
+                                   cfg_.control.radar_stale_ms);
+    if (distance_available) {
+        // Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
+        const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m;
+        const double vx_raw = distance_pid_.Update(delta_d, dt_s);
+        const double slewed = vx_limiter_.Limit(vx_raw, dt_s);  // 加速度限幅（§8）
+        out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps,
+                           cfg_.distance.approach_velocity_limit_mps);
+    } else {
+        // 无可用目标距离：清积分防饱和，按配置降级动作（§6.3）
+        distance_pid_.ResetIntegral();
+        switch (cfg_.distance.no_distance_action) {
+            case NoDistanceAction::kHold:
+                // 保持：前向指令清零（安全占位语义；接入主工程后可改为保持当前速度）
+                out.vx_mps = 0.0;
+                vx_limiter_.Reset(0.0);
+                break;
+            case NoDistanceAction::kSlowApproach:
+                // 限速接近：缓速前向，同样经加速度限幅
+                out.vx_mps = vx_limiter_.Limit(
+                    cfg_.distance.no_distance_approach_limit_mps, dt_s);
+                break;
+            case NoDistanceAction::kExit:
+                // 退出近距操作：整体降级保持
+                out.mode = ControlMode::kDegradedHold;
+                out.degraded_reason = DegradedReason::kNoDistanceExit;
+                return FinishAndLog("距离未关联，退出近距操作");
+        }
+    }
+
+    // ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
+    // 到此为止各通道限幅均在机体系完成（旋转为正交变换，合速度不变，
+    // 限幅约束不被破坏）；输出端随快照航向旋转为北/东/地，与网关的
+    // MAV_FRAME_LOCAL_NED 一致。姿态已在 ① 通过超龄校验，yaw_rad 可用。
+    RotateBodyToNed(out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad);
+
+    out.valid = true;
+    return FinishAndLog(out.coast_active ? "Coast 预测跟踪" : "正常跟踪");
+}
+
+void TrackingControlLaw::Reset() {
+    // 停止视觉闭环时清全部状态（§6.4）
+    distance_pid_.Reset();
+    yaw_deg_limiter_.Reset();
+    yaw_rate_limiter_.Reset();
+    vx_limiter_.Reset();
+    vz_limiter_.Reset();
+    last_now_ms_ = 0;
+    last_mode_ = ControlMode::kDegradedHold;
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/src/vtc_config.cpp

@@ -0,0 +1,225 @@
+// =============================================================================
+// vtc_config.cpp —— 配置加载器实现（启动期一次性解析）
+//
+// 对应规格：docs/物理追踪思路.md §8（阈值集中配置化，NFR-007）。
+//
+// 实现要点：
+//   - 仅在启动期调用一次：缺字段 / 类型错误 / 非法取值抛
+//     std::runtime_error，异常消息携带完整字段名（如 "heading.gain"）；
+//   - 热路径绝不触碰 JSON：解析结果拷贝进 TrackingConfig 后即释放；
+//   - 本文件是全库唯一包含 nlohmann/json 的 TU。
+// =============================================================================
+
+#include "visual_track_control/vtc_config.h"
+
+#include <fstream>
+#include <stdexcept>
+#include <string>
+
+#include <nlohmann/json.hpp>
+
+namespace drone::vtc {
+namespace {
+
+using Json = nlohmann::json;
+
+/// 读取并解析 JSON 文件；失败抛异常并携带路径
+Json ParseConfigFile(const std::string& path) {
+    std::ifstream ifs(path);
+    if (!ifs) {
+        throw std::runtime_error("配置文件打开失败: " + path);
+    }
+    try {
+        return Json::parse(ifs);
+    } catch (const std::exception& e) {
+        throw std::runtime_error("配置文件解析失败: " + path + " (" + e.what() + ")");
+    }
+}
+
+/// 取必需的对象分组；缺失或类型错抛异常
+const Json& RequireSection(const Json& root, const char* section) {
+    const auto it = root.find(section);
+    if (it == root.end() || !it->is_object()) {
+        throw std::runtime_error(std::string("配置缺失分组或类型错误: ") + section);
+    }
+    return *it;
+}
+
+/// 取必需的数值字段；缺失/类型错抛异常并携带字段名
+double RequireNumber(const Json& section, const char* section_name, const char* key) {
+    const auto it = section.find(key);
+    if (it == section.end() || !it->is_number()) {
+        throw std::runtime_error(std::string("配置缺失字段或类型错误: ") +
+                                 section_name + "." + key);
+    }
+    return it->get<double>();
+}
+
+/// 取必需的整数字段
+std::int64_t RequireInteger(const Json& section, const char* section_name,
+                            const char* key) {
+    const auto it = section.find(key);
+    if (it == section.end() || !it->is_number_integer()) {
+        throw std::runtime_error(std::string("配置缺失字段或类型错误(需整数): ") +
+                                 section_name + "." + key);
+    }
+    return it->get<std::int64_t>();
+}
+
+/// 取必需的字符串字段
+std::string RequireString(const Json& section, const char* section_name,
+                          const char* key) {
+    const auto it = section.find(key);
+    if (it == section.end() || !it->is_string()) {
+        throw std::runtime_error(std::string("配置缺失字段或类型错误(需字符串): ") +
+                                 section_name + "." + key);
+    }
+    return it->get<std::string>();
+}
+
+/// 校验字段为正数；非法抛异常并携带字段名
+double RequirePositive(double value, const char* field_name) {
+    if (!(value > 0.0)) {
+        throw std::runtime_error(std::string("配置非法(必须为正): ") + field_name);
+    }
+    return value;
+}
+
+/// 校验字段为非负数
+double RequireNonNegative(double value, const char* field_name) {
+    if (value < 0.0) {
+        throw std::runtime_error(std::string("配置非法(不得为负): ") + field_name);
+    }
+    return value;
+}
+
+/// 校验字段在开区间 (lo, hi)
+double RequireRange(double value, double lo, double hi, const char* field_name) {
+    if (!(value > lo && value < hi)) {
+        throw std::runtime_error(std::string("配置非法(超出范围): ") + field_name);
+    }
+    return value;
+}
+
+ControlGroupConfig LoadControl(const Json& root) {
+    const Json& s = RequireSection(root, "control");
+    ControlGroupConfig c;
+    c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"),
+                                     "control.frequency_hz");
+    c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms");
+    c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms");
+    c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms");
+    if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) {
+        throw std::runtime_error("配置非法: control.*_stale_ms 必须为正");
+    }
+    return c;
+}
+
+CameraGroupConfig LoadCamera(const Json& root) {
+    const Json& s = RequireSection(root, "camera");
+    CameraGroupConfig c;
+    c.image_width = static_cast<int>(
+        RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width"));
+    c.image_height = static_cast<int>(
+        RequirePositive(RequireInteger(s, "camera", "image_height"), "camera.image_height"));
+    c.fov_h_deg = RequireRange(RequireNumber(s, "camera", "fov_h_deg"), 0.0, 180.0,
+                               "camera.fov_h_deg");
+    c.fov_v_deg = RequireRange(RequireNumber(s, "camera", "fov_v_deg"), 0.0, 180.0,
+                               "camera.fov_v_deg");
+    return c;
+}
+
+HeadingGroupConfig LoadHeading(const Json& root) {
+    const Json& s = RequireSection(root, "heading");
+    HeadingGroupConfig h;
+    const std::string mode = RequireString(s, "heading", "mode");
+    if (mode == "position") {
+        h.mode = HeadingMode::kPosition;
+    } else if (mode == "rate") {
+        h.mode = HeadingMode::kRate;
+    } else {
+        throw std::runtime_error("配置非法: heading.mode 仅支持 position / rate");
+    }
+    h.gain = RequireNonNegative(RequireNumber(s, "heading", "gain"), "heading.gain");
+    h.yaw_rate_limit_dps = RequirePositive(
+        RequireNumber(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps");
+    h.yaw_slew_limit_dps2 = RequirePositive(
+        RequireNumber(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2");
+    return h;
+}
+
+VerticalGroupConfig LoadVertical(const Json& root) {
+    const Json& s = RequireSection(root, "vertical");
+    VerticalGroupConfig v;
+    v.vz_gain_mps_per_deg = RequireNonNegative(
+        RequireNumber(s, "vertical", "vz_gain_mps_per_deg"), "vertical.vz_gain_mps_per_deg");
+    v.vz_limit_mps = RequirePositive(RequireNumber(s, "vertical", "vz_limit_mps"),
+                                     "vertical.vz_limit_mps");
+    return v;
+}
+
+DistanceGroupConfig LoadDistance(const Json& root) {
+    const Json& s = RequireSection(root, "distance");
+    DistanceGroupConfig d;
+    d.d_exp_m = RequirePositive(RequireNumber(s, "distance", "d_exp_m"), "distance.d_exp_m");
+    d.kp = RequireNonNegative(RequireNumber(s, "distance", "kp"), "distance.kp");
+    d.ki = RequireNonNegative(RequireNumber(s, "distance", "ki"), "distance.ki");
+    d.kd = RequireNonNegative(RequireNumber(s, "distance", "kd"), "distance.kd");
+    d.integral_limit = RequireNonNegative(RequireNumber(s, "distance", "integral_limit"),
+                                          "distance.integral_limit");
+    d.derivative_filter_coef = RequireRange(
+        RequireNumber(s, "distance", "derivative_filter_coef"), -0.000001, 1.0,
+        "distance.derivative_filter_coef");
+    if (d.derivative_filter_coef < 0.0) {
+        d.derivative_filter_coef = 0.0;  // 容许 -0.0
+    }
+    d.approach_velocity_limit_mps = RequirePositive(
+        RequireNumber(s, "distance", "approach_velocity_limit_mps"),
+        "distance.approach_velocity_limit_mps");
+    d.retreat_velocity_limit_mps = RequirePositive(
+        RequireNumber(s, "distance", "retreat_velocity_limit_mps"),
+        "distance.retreat_velocity_limit_mps");
+    const std::string action = RequireString(s, "distance", "no_distance_action");
+    if (action == "hold") {
+        d.no_distance_action = NoDistanceAction::kHold;
+    } else if (action == "slow_approach") {
+        d.no_distance_action = NoDistanceAction::kSlowApproach;
+    } else if (action == "exit") {
+        d.no_distance_action = NoDistanceAction::kExit;
+    } else {
+        throw std::runtime_error(
+            "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit");
+    }
+    d.no_distance_approach_limit_mps = RequireNonNegative(
+        RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
+        "distance.no_distance_approach_limit_mps");
+    return d;
+}
+
+AccelLimitConfig LoadAccelLimit(const Json& root) {
+    const Json& s = RequireSection(root, "accel_limit");
+    AccelLimitConfig a;
+    a.ax_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ax_mps2"),
+                                "accel_limit.ax_mps2");
+    a.ay_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ay_mps2"),
+                                "accel_limit.ay_mps2");
+    a.az_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "az_mps2"),
+                                "accel_limit.az_mps2");
+    return a;
+}
+
+}  // namespace
+
+TrackingConfig TrackingConfig::LoadFromJson(const std::string& path) {
+    const Json root = ParseConfigFile(path);
+    TrackingConfig cfg;
+    cfg.control = LoadControl(root);
+    cfg.camera = LoadCamera(root);
+    cfg.heading = LoadHeading(root);
+    cfg.vertical = LoadVertical(root);
+    cfg.distance = LoadDistance(root);
+    cfg.accel_limit = LoadAccelLimit(root);
+    return cfg;
+}
+
+}  // namespace drone::vtc

reference-c++/visual_track_control/tests/frame_transform_test.cpp

@@ -0,0 +1,56 @@
+// =============================================================================
+// frame_transform_test.cpp —— RotateBodyToNed 单元测试
+//
+// 覆盖（物理追踪思路 §2.3，A4）：
+//   零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。
+// =============================================================================
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/frame_transform.h"
+
+namespace {
+
+using drone::vtc::RotateBodyToNed;
+
+constexpr double kPi = 3.14159265358979323846;
+
+TEST(FrameTransformTest, ZeroYawIsIdentity) {
+    double x = 1.0, y = 0.5, z = -2.0;
+    RotateBodyToNed(x, y, z, 0.0);
+    EXPECT_NEAR(x, 1.0, 1e-12);   // 北 = 前
+    EXPECT_NEAR(y, 0.5, 1e-12);   // 东 = 右
+    EXPECT_NEAR(z, -2.0, 1e-12);
+}
+
+TEST(FrameTransformTest, YawEast90MapsForwardToEast) {
+    double x = 2.0, y = 0.0, z = 0.0;
+    RotateBodyToNed(x, y, z, kPi / 2.0);  // 机头朝东
+    EXPECT_NEAR(x, 0.0, 1e-9);  // 北向 ≈ 0
+    EXPECT_NEAR(y, 2.0, 1e-9);  // 东向 = 前向速度
+}
+
+TEST(FrameTransformTest, RightIsEastWhenHeadingNorth) {
+    double x = 0.0, y = 1.0, z = 0.0;
+    RotateBodyToNed(x, y, z, 0.0);
+    EXPECT_NEAR(x, 0.0, 1e-12);
+    EXPECT_NEAR(y, 1.0, 1e-12);
+}
+
+TEST(FrameTransformTest, DownComponentUnchanged) {
+    double x = 1.0, y = 1.0, z = 3.5;
+    RotateBodyToNed(x, y, z, kPi / 3.0);
+    EXPECT_DOUBLE_EQ(z, 3.5);
+}
+
+TEST(FrameTransformTest, PreservesHorizontalNorm) {
+    // 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
+    double x = 3.0, y = -4.0, z = 1.0;
+    const double norm_before = std::hypot(x, y);
+    RotateBodyToNed(x, y, z, 0.7);
+    EXPECT_NEAR(std::hypot(x, y), norm_before, 1e-9);
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/line_of_sight_test.cpp

@@ -0,0 +1,69 @@
+// =============================================================================
+// line_of_sight_test.cpp —— PixelToAngle 单元测试
+//
+// 覆盖（物理追踪思路 §4.1 / §5.1，C2）：
+//   零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、
+//   与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
+// =============================================================================
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/line_of_sight.h"
+
+namespace {
+
+using drone::vtc::PixelToAngle;
+
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kRadToDeg = 180.0 / kPi;
+
+TEST(PixelToAngleTest, ZeroOffsetGivesZeroAngle) {
+    PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
+    EXPECT_DOUBLE_EQ(conv.Convert(0.0), 0.0);
+}
+
+TEST(PixelToAngleTest, HalfSizeOffsetGivesHalfFov) {
+    // 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
+    PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
+    EXPECT_NEAR(conv.Convert(500.0), 45.0 * kPi / 180.0, 1e-12);
+    EXPECT_NEAR(conv.Convert(-500.0), -45.0 * kPi / 180.0, 1e-12);
+}
+
+TEST(PixelToAngleTest, OddSymmetry) {
+    PixelToAngle conv(1920.0, 60.0 * kPi / 180.0);
+    for (const double offset : {1.0, 100.0, 730.5}) {
+        EXPECT_NEAR(conv.Convert(-offset), -conv.Convert(offset), 1e-15);
+    }
+}
+
+TEST(PixelToAngleTest, MonotonicIncreasing) {
+    PixelToAngle conv(1080.0, 35.0 * kPi / 180.0);
+    double prev = conv.Convert(-600.0);
+    for (double x = -500.0; x <= 600.0; x += 100.0) {
+        const double cur = conv.Convert(x);
+        EXPECT_GT(cur, prev);
+        prev = cur;
+    }
+}
+
+TEST(PixelToAngleTest, DeviatesFromLinearApproximation) {
+    // C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
+    // 针孔结果必须与之明显偏离（证明未走线性支路）。
+    PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
+    const double pinhole_deg = conv.Convert(250.0) * kRadToDeg;  // atan(250·2·tan45°/1000)
+    const double linear_deg = 250.0 * (90.0 / 1000.0);           // θ·offset/size = 22.5°
+    EXPECT_NEAR(pinhole_deg, std::atan(0.5) * kRadToDeg, 1e-9);  // 26.565°
+    EXPECT_GT(std::fabs(pinhole_deg - linear_deg), 2.0);
+}
+
+TEST(PixelToAngleTest, ConstructorRejectsInvalidArgs) {
+    EXPECT_THROW(PixelToAngle(0.0, 1.0), std::invalid_argument);
+    EXPECT_THROW(PixelToAngle(-10.0, 1.0), std::invalid_argument);
+    EXPECT_THROW(PixelToAngle(1000.0, 0.0), std::invalid_argument);
+    EXPECT_THROW(PixelToAngle(1000.0, kPi), std::invalid_argument);
+    EXPECT_THROW(PixelToAngle(1000.0, kPi + 0.1), std::invalid_argument);
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp

@@ -0,0 +1,181 @@
+// =============================================================================
+// mavlink_command_gateway_test.cpp —— MavlinkCommandGateway 单元测试
+//
+// 覆盖（物理追踪思路 §2.2、§4.2，A2 掩码三分支）：
+//   ID 84 速度模式的 type_mask 选择（角速率非零 → 速率掩码；航向角非零 →
+//   航向掩码；两者均零 → 速率掩码配零角速率，保持航向）、
+//   速度/航向字段编码、刹车消息、坐标基准、传输失败传播。
+//
+// 构建约束：本文件**不得**包含 common/mavlink.h（MAVLINK_HELPER 默认空宏，
+// 多 TU 包含会多重定义），故按 MAVLink v2 线格式手工解码：
+//   帧头 10 字节（STX=0xFD, len, flags×2, seq, sys, comp, msgid×3 LE），
+//   载荷按字段尺寸降序排列（ID 84：time,x,y,z,vx,vy,vz,afx,afy,afz,
+//   yaw,yaw_rate 均 4 字节在前，随后 u16 type_mask，再 target_system、
+//   target_component、coordinate_frame 三个 u8）。
+// =============================================================================
+
+#include <cstdint>
+#include <cstring>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/i_command_transport.h"
+#include "visual_track_control/mavlink_command_gateway.h"
+#include "visual_track_control/vtc_types.h"
+
+namespace {
+
+using drone::vtc::ControlOutput;
+using drone::vtc::ICommandTransport;
+using drone::vtc::MavlinkCommandGateway;
+
+// ---- MAVLink v2 帧布局常量（见文件头说明） ----
+constexpr std::uint8_t kMavlinkV2Stx = 0xFD;
+constexpr std::size_t kHeaderLen = 10;
+constexpr std::size_t kMsgId84PayloadLen = 53;
+
+// ---- ID 84 载荷字段偏移（相对载荷起点） ----
+constexpr std::size_t kOffVx = 16;
+constexpr std::size_t kOffVy = 20;
+constexpr std::size_t kOffVz = 24;
+constexpr std::size_t kOffYaw = 40;
+constexpr std::size_t kOffYawRate = 44;
+constexpr std::size_t kOffTypeMask = 48;
+// u8 三字段线序（生成头 mavlink_msg_set_position_target_local_ned.h 为准）
+constexpr std::size_t kOffTargetSystem = 50;
+constexpr std::size_t kOffTargetComponent = 51;
+constexpr std::size_t kOffCoordFrame = 52;
+
+// ---- 预期掩码值（与实现中常量一致：置位 = 忽略） ----
+// 基础 0x01C7（忽略位置与加速度）| 航向相关位
+constexpr std::uint16_t kExpectMaskYaw = 0x01C7 | 0x0800;      // 忽略 yaw_rate
+constexpr std::uint16_t kExpectMaskYawRate = 0x01C7 | 0x0400;  // 忽略 yaw
+constexpr std::uint16_t kExpectMaskBrake = 0x01C7 | 0x0C00;    // 两者均忽略
+
+/// 捕获式传输：记录最后一次发送的字节流，可注入失败
+class CaptureTransport : public ICommandTransport {
+public:
+    bool Send(const std::uint8_t* data, std::size_t len) override {
+        ++send_count_;
+        last_.assign(data, data + len);
+        return succeed_;
+    }
+
+    void SetSucceed(bool ok) { succeed_ = ok; }
+    int send_count() const { return send_count_; }
+    const std::vector<std::uint8_t>& last() const { return last_; }
+
+private:
+    bool succeed_ = true;
+    int send_count_ = 0;
+    std::vector<std::uint8_t> last_;
+};
+
+float ReadFloat(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
+    float v;
+    std::memcpy(&v, buf.data() + kHeaderLen + payload_off, sizeof(v));
+    return v;
+}
+
+std::uint16_t ReadU16(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
+    return static_cast<std::uint16_t>(buf[kHeaderLen + payload_off]) |
+           (static_cast<std::uint16_t>(buf[kHeaderLen + payload_off + 1]) << 8);
+}
+
+void ExpectId84Header(const std::vector<std::uint8_t>& buf) {
+    ASSERT_GE(buf.size(), kHeaderLen + kMsgId84PayloadLen + 2);  // +CRC
+    EXPECT_EQ(buf[0], kMavlinkV2Stx);
+    EXPECT_EQ(buf[1], kMsgId84PayloadLen);
+    // msgid 24 位小端（字节 7-9）= 84
+    EXPECT_EQ(buf[7], 84);
+    EXPECT_EQ(buf[8], 0);
+    EXPECT_EQ(buf[9], 0);
+}
+
+constexpr double kDegToRad = 3.14159265358979323846 / 180.0;
+
+TEST(MavlinkGatewayTest, YawRateModeSelectsRateMask) {
+    CaptureTransport transport;
+    MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
+
+    ControlOutput out;
+    out.mode = drone::vtc::ControlMode::kVelocityHeading;
+    out.vx_mps = 1.5;
+    out.vy_mps = -0.5;
+    out.vz_mps = 0.25;
+    out.yaw_rate_dps = 5.0;
+    out.yaw_deg = 0.0;
+    ASSERT_TRUE(gateway.SendVelocityHeading(out, 12345));
+
+    const auto& buf = transport.last();
+    ExpectId84Header(buf);
+    EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 1.5F);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), -0.5F);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.25F);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), static_cast<float>(5.0 * kDegToRad));
+    EXPECT_EQ(buf[kHeaderLen + kOffCoordFrame], 1);       // MAV_FRAME_LOCAL_NED
+    EXPECT_EQ(buf[kHeaderLen + kOffTargetSystem], 3);     // target_system
+    EXPECT_EQ(buf[kHeaderLen + kOffTargetComponent], 4);  // target_component
+}
+
+TEST(MavlinkGatewayTest, YawOnlySelectsYawMask) {
+    CaptureTransport transport;
+    MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
+
+    ControlOutput out;
+    out.mode = drone::vtc::ControlMode::kVelocityHeading;
+    out.yaw_deg = 10.0;
+    out.yaw_rate_dps = 0.0;
+    ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
+
+    const auto& buf = transport.last();
+    ExpectId84Header(buf);
+    EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYaw);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYaw), static_cast<float>(10.0 * kDegToRad));
+}
+
+TEST(MavlinkGatewayTest, BothZeroKeepsHeadingViaRateMask) {
+    // A2 回归：旧实现在零角速率时落到航向掩码，等于误发绝对航向 0（正北）。
+    // 正确行为：速率掩码 + 零角速率 = 保持当前航向。
+    CaptureTransport transport;
+    MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
+
+    ControlOutput out;
+    out.mode = drone::vtc::ControlMode::kVelocityHeading;
+    out.vx_mps = 1.0;
+    ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
+
+    const auto& buf = transport.last();
+    ExpectId84Header(buf);
+    EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), 0.0F);
+}
+
+TEST(MavlinkGatewayTest, BrakeHoverSendsZeroVelocityAllIgnored) {
+    CaptureTransport transport;
+    MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
+    ASSERT_TRUE(gateway.SendBrakeHover(777));
+
+    const auto& buf = transport.last();
+    ExpectId84Header(buf);
+    EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskBrake);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 0.0F);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), 0.0F);
+    EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.0F);
+}
+
+TEST(MavlinkGatewayTest, TransportFailurePropagates) {
+    CaptureTransport transport;
+    transport.SetSucceed(false);
+    MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
+
+    ControlOutput out;
+    out.mode = drone::vtc::ControlMode::kVelocityHeading;
+    EXPECT_FALSE(gateway.SendVelocityHeading(out, 1));
+    EXPECT_FALSE(gateway.SendBrakeHover(1));
+    EXPECT_EQ(transport.send_count(), 2);
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/pid_controller_test.cpp

@@ -0,0 +1,123 @@
+// =============================================================================
+// pid_controller_test.cpp —— PidController 单元测试
+//
+// 覆盖（物理追踪思路 §6.2 / §6.4）：
+//   位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、
+//   anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、
+//   ResetIntegral / Reset 行为。
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/pid_controller.h"
+
+namespace {
+
+using drone::vtc::PidController;
+using drone::vtc::PidParams;
+
+PidParams MakeParams(double kp, double ki, double kd, double integral_limit,
+                     double output_limit, double filter_coef = 0.0) {
+    PidParams p;
+    p.kp = kp;
+    p.ki = ki;
+    p.kd = kd;
+    p.integral_limit = integral_limit;
+    p.derivative_filter_coef = filter_coef;
+    p.output_limit = output_limit;
+    return p;
+}
+
+TEST(PidControllerTest, ProportionalOnly) {
+    PidController pid(MakeParams(2.0, 0.0, 0.0, 0.0, 0.0));
+    EXPECT_DOUBLE_EQ(pid.Update(3.0, 0.1), 6.0);
+    EXPECT_DOUBLE_EQ(pid.Update(-1.5, 0.1), -3.0);
+}
+
+TEST(PidControllerTest, IntegralAccumulates) {
+    PidController pid(MakeParams(0.0, 1.0, 0.0, 0.0, 0.0));
+    // 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
+    EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 1.0);
+    EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 2.0);
+}
+
+TEST(PidControllerTest, IntegralClampedToLimit) {
+    PidController pid(MakeParams(0.0, 1.0, 0.0, /*integral_limit=*/1.0, 0.0));
+    for (int i = 0; i < 5; ++i) {
+        pid.Update(10.0, 1.0);
+    }
+    // 积分候选值被钳位在 1.0 → 输出恒为 1.0
+    EXPECT_DOUBLE_EQ(pid.LastOutput(), 1.0);
+}
+
+TEST(PidControllerTest, OutputClampedToLimit) {
+    PidController pid(MakeParams(10.0, 0.0, 0.0, 0.0, /*output_limit=*/3.0));
+    EXPECT_DOUBLE_EQ(pid.Update(1.0, 0.1), 3.0);
+    EXPECT_DOUBLE_EQ(pid.Update(-1.0, 0.1), -3.0);
+}
+
+TEST(PidControllerTest, AntiWindupFreezeWhileSaturatedSameDirection) {
+    // kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
+    // 输出恒钉在 +5（10 拍累计 10 拍也不会让积分继续增长）。
+    PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
+    for (int i = 0; i < 10; ++i) {
+        EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
+    }
+}
+
+TEST(PidControllerTest, AntiWindupUnwindsAfterErrorFlips) {
+    // C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
+    // 冻结回卷、输出一直钉在 +5）。
+    // kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
+    // error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
+    // 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
+    PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
+    }
+    EXPECT_DOUBLE_EQ(pid.Update(-1.0, 1.0), -2.0);  // 首拍立即回卷
+    double out = -2.0;
+    for (int i = 1; i < 12; ++i) {
+        out = pid.Update(-1.0, 1.0);
+    }
+    EXPECT_NEAR(out, -5.0, 1e-9);  // 旧判据此处仍为 +5.0
+}
+
+TEST(PidControllerTest, DerivativeFirstStepIsZero) {
+    PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0));
+    // 首拍无微分历史：微分项为 0
+    EXPECT_DOUBLE_EQ(pid.Update(5.0, 1.0), 0.0);
+}
+
+TEST(PidControllerTest, DerivativeWithLowPassFilter) {
+    PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0, /*filter=*/0.5));
+    pid.Update(0.0, 1.0);  // 建立微分历史（导数 0）
+    // 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
+    EXPECT_DOUBLE_EQ(pid.Update(1.0, 1.0), 0.5);
+}
+
+TEST(PidControllerTest, ZeroOrNegativeDtHoldsLastOutput) {
+    PidController pid(MakeParams(1.0, 0.0, 0.0, 0.0, 0.0));
+    EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.1), 2.0);
+    EXPECT_DOUBLE_EQ(pid.Update(99.0, 0.0), 2.0);
+    EXPECT_DOUBLE_EQ(pid.Update(99.0, -1.0), 2.0);
+}
+
+TEST(PidControllerTest, ResetIntegralClearsOnlyIntegral) {
+    PidController pid(MakeParams(0.0, 1.0, 1.0, 0.0, 0.0));
+    pid.Update(2.0, 1.0);  // 积分 = 2
+    pid.ResetIntegral();
+    // 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
+    EXPECT_DOUBLE_EQ(pid.Update(0.0, 1.0), -2.0);
+}
+
+TEST(PidControllerTest, ResetClearsEverything) {
+    PidController pid(MakeParams(1.0, 1.0, 1.0, 0.0, 0.0));
+    pid.Update(5.0, 1.0);
+    pid.Reset();
+    EXPECT_DOUBLE_EQ(pid.LastOutput(), 0.0);
+    // 全清后等同全新控制器：首拍无微分历史
+    EXPECT_DOUBLE_EQ(pid.Update(3.0, 1.0), 3.0 + 3.0);  // kp·e + ki·∫e
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/rate_limiter_test.cpp

@@ -0,0 +1,76 @@
+// =============================================================================
+// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试
+//
+// 覆盖（物理追踪思路 §8、状态机设计 S6）：
+//   静态限幅、变化率限幅步进、首条指令从零渐变（S6）、
+//   异常节拍保护、Reset 归零。
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/rate_limiter.h"
+
+namespace {
+
+using drone::vtc::Clamp;
+using drone::vtc::SlewRateLimiter;
+
+TEST(ClampTest, ClampsValue) {
+    EXPECT_DOUBLE_EQ(Clamp(5.0, -2.0, 2.0), 2.0);
+    EXPECT_DOUBLE_EQ(Clamp(-5.0, -2.0, 2.0), -2.0);
+    EXPECT_DOUBLE_EQ(Clamp(1.5, -2.0, 2.0), 1.5);
+    // 边界值：落在区间内
+    EXPECT_DOUBLE_EQ(Clamp(2.0, -2.0, 2.0), 2.0);
+    EXPECT_DOUBLE_EQ(Clamp(-2.0, -2.0, 2.0), -2.0);
+}
+
+TEST(SlewRateLimiterTest, FirstCommandRampsFromZero) {
+    // S6：初始输出为 0，首条大指令只能按变化率渐变
+    SlewRateLimiter lim(/*rate_limit=*/2.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 1.0);   // 0 + 2×0.5
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 2.0);   // 继续渐变
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 10.0), 10.0); // 步长足够 → 到达目标
+}
+
+TEST(SlewRateLimiterTest, TracksTargetWithinStep) {
+    SlewRateLimiter lim(10.0);
+    // 步长 10×1=10，目标 3 在步长内 → 直接到达
+    EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0);  // 已在目标：保持
+}
+
+TEST(SlewRateLimiterTest, NegativeDirection) {
+    SlewRateLimiter lim(2.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -1.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -2.0);
+}
+
+TEST(SlewRateLimiterTest, ZeroOrNegativeDtHolds) {
+    SlewRateLimiter lim(2.0);
+    lim.Limit(10.0, 1.0);  // 输出 2.0
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.0), 2.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, -1.0), 2.0);
+}
+
+TEST(SlewRateLimiterTest, ZeroRateLimitFreezesOutput) {
+    SlewRateLimiter lim(0.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(5.0, 1.0), 0.0);
+}
+
+TEST(SlewRateLimiterTest, ResetToValue) {
+    SlewRateLimiter lim(2.0);
+    lim.Limit(10.0, 1.0);
+    lim.Reset();
+    EXPECT_DOUBLE_EQ(lim.LastOutput(), 0.0);
+    lim.Reset(3.5);
+    EXPECT_DOUBLE_EQ(lim.LastOutput(), 3.5);
+    // 复位后从新值继续渐变
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 5.5);  // 3.5 + 2×1
+}
+
+TEST(SlewRateLimiterTest, CustomInitialValue) {
+    SlewRateLimiter lim(1.0, /*initial=*/5.0);
+    EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 6.0);
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/staleness_checker_test.cpp

@@ -0,0 +1,41 @@
+// =============================================================================
+// staleness_checker_test.cpp —— StalenessChecker 单元测试
+//
+// 覆盖（物理追踪思路 §7.1）：
+//   从未取样判超龄、有效期内新鲜、超出有效期超龄、
+//   边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
+// =============================================================================
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/staleness_checker.h"
+
+namespace {
+
+using drone::vtc::StalenessChecker;
+
+TEST(StalenessCheckerTest, NeverSampledIsStale) {
+    EXPECT_TRUE(StalenessChecker::IsStale(0, 1000, 200));
+    EXPECT_TRUE(StalenessChecker::IsStale(-5, 1000, 200));
+}
+
+TEST(StalenessCheckerTest, WithinValidityIsFresh) {
+    EXPECT_FALSE(StalenessChecker::IsStale(900, 1000, 200));
+    EXPECT_FALSE(StalenessChecker::IsStale(1000, 1000, 200));  // 刚取样
+}
+
+TEST(StalenessCheckerTest, BeyondValidityIsStale) {
+    EXPECT_TRUE(StalenessChecker::IsStale(799, 1000, 200));  // 201 > 200
+}
+
+TEST(StalenessCheckerTest, ExactlyAtBoundaryIsFresh) {
+    // 超龄判定为严格大于：恰好等于有效期仍算新鲜
+    EXPECT_FALSE(StalenessChecker::IsStale(800, 1000, 200));
+}
+
+TEST(StalenessCheckerTest, InconsistentTimestampIsStale) {
+    // C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
+    EXPECT_TRUE(StalenessChecker::IsStale(2000, 1000, 200));
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/tracking_control_law_test.cpp

@@ -0,0 +1,349 @@
+// =============================================================================
+// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试
+//
+// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
+//   超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
+//   Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
+//   A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
+//   首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
+// =============================================================================
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/tracking_control_law.h"
+#include "visual_track_control/vtc_config.h"
+#include "visual_track_control/vtc_types.h"
+
+namespace {
+
+using namespace drone::vtc;
+
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kDegToRad = kPi / 180.0;
+constexpr double kRadToDeg = 180.0 / kPi;
+
+/// 测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
+/// 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
+/// 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
+TrackingConfig MakeConfig() {
+    TrackingConfig cfg;
+    cfg.control.frequency_hz = 20.0;
+    cfg.control.visual_stale_ms = 200;
+    cfg.control.radar_stale_ms = 500;
+    cfg.control.attitude_stale_ms = 300;
+    cfg.camera.image_width = 1000;
+    cfg.camera.image_height = 1000;
+    cfg.camera.fov_h_deg = 90.0;
+    cfg.camera.fov_v_deg = 90.0;
+    cfg.heading.mode = HeadingMode::kRate;
+    cfg.heading.gain = 1.0;
+    cfg.heading.yaw_rate_limit_dps = 30.0;
+    cfg.heading.yaw_slew_limit_dps2 = 100000.0;
+    cfg.vertical.vz_gain_mps_per_deg = 0.1;
+    cfg.vertical.vz_limit_mps = 2.0;
+    cfg.distance.d_exp_m = 10.0;
+    cfg.distance.kp = 0.5;
+    cfg.distance.ki = 0.0;
+    cfg.distance.kd = 0.0;
+    cfg.distance.integral_limit = 0.0;
+    cfg.distance.derivative_filter_coef = 0.0;
+    cfg.distance.approach_velocity_limit_mps = 3.0;
+    cfg.distance.retreat_velocity_limit_mps = 1.0;
+    cfg.distance.no_distance_action = NoDistanceAction::kHold;
+    cfg.distance.no_distance_approach_limit_mps = 0.5;
+    cfg.accel_limit.ax_mps2 = 1000.0;
+    cfg.accel_limit.ay_mps2 = 1000.0;
+    cfg.accel_limit.az_mps2 = 1000.0;
+    return cfg;
+}
+
+/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜
+ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
+    ControlSnapshot s;
+    s.track.tracked = true;
+    s.track.is_predicted = false;
+    s.track.center = {500.0, 500.0};
+    s.track_time_ms = now_ms;
+    s.attitude_present = true;
+    s.attitude_time_ms = now_ms;
+    return s;
+}
+
+// ---- 超龄降级（§7.1 / §9.2） ----
+
+TEST(TrackingControlLawTest, AttitudeMissingDegrades) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.attitude_present = false;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
+    EXPECT_FALSE(out.valid);
+}
+
+TEST(TrackingControlLawTest, AttitudeStaleDegrades) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.attitude_time_ms = 600;  // 400 ms 前 > 300 ms 阈值
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
+}
+
+TEST(TrackingControlLawTest, VisionStaleDegrades) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track_time_ms = 700;  // 300 ms 前 > 200 ms 阈值
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
+}
+
+TEST(TrackingControlLawTest, NeverSampledTrackDegrades) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track_time_ms = 0;  // 从未取样
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
+}
+
+TEST(TrackingControlLawTest, AttitudeStaleBeatsVisionStale) {
+    // 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.attitude_present = false;
+    snap.track_time_ms = 0;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
+}
+
+// ---- 判丢刹车（§6.4） ----
+
+TEST(TrackingControlLawTest, LostTargetBrakes) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.tracked = false;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kBrakeHover);
+    EXPECT_TRUE(out.valid);
+    EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
+    EXPECT_DOUBLE_EQ(out.vy_mps, 0.0);
+    EXPECT_DOUBLE_EQ(out.vz_mps, 0.0);
+}
+
+// ---- Coast（§6.4） ----
+
+TEST(TrackingControlLawTest, CoastSetsFlagAndKeepsTracking) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.is_predicted = true;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
+    EXPECT_TRUE(out.coast_active);
+    EXPECT_TRUE(out.valid);
+}
+
+// ---- 水平航向通道（§4.2，A2） ----
+
+TEST(TrackingControlLawTest, RateModeMapsBetaToYawRate) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {600.0, 500.0};  // 目标偏右 100 像素
+    const auto out = law.Update(snap, 1000);
+    const double beta_deg = std::atan(0.2) * kRadToDeg;  // ≈ 11.31°
+    EXPECT_NEAR(out.yaw_rate_dps, beta_deg, 1e-6);       // gain=1，右转正
+    EXPECT_DOUBLE_EQ(out.yaw_deg, 0.0);
+}
+
+TEST(TrackingControlLawTest, RateModeClampsToYawRateLimit) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {950.0, 500.0};  // 大偏移：β≈43.5° > 30°/s 上限
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.yaw_rate_dps, 30.0, 1e-9);
+}
+
+TEST(TrackingControlLawTest, PositionModeComposesAbsoluteHeading) {
+    // A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
+    auto cfg = MakeConfig();
+    cfg.heading.mode = HeadingMode::kPosition;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {600.0, 500.0};
+    snap.yaw_rad = 45.0 * kDegToRad;
+    const auto out = law.Update(snap, 1000);
+    const double beta_deg = std::atan(0.2) * kRadToDeg;
+    EXPECT_NEAR(out.yaw_deg, 45.0 + beta_deg, 1e-6);
+    EXPECT_DOUBLE_EQ(out.yaw_rate_dps, 0.0);
+}
+
+TEST(TrackingControlLawTest, PositionModeWrapsAround180) {
+    auto cfg = MakeConfig();
+    cfg.heading.mode = HeadingMode::kPosition;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {600.0, 500.0};
+    snap.yaw_rad = 175.0 * kDegToRad;
+    const auto out = law.Update(snap, 1000);
+    const double beta_deg = std::atan(0.2) * kRadToDeg;
+    // 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
+    EXPECT_NEAR(out.yaw_deg, 175.0 + beta_deg - 360.0, 1e-6);
+    EXPECT_GT(out.yaw_deg, -180.0);
+    EXPECT_LT(out.yaw_deg, 180.0);
+}
+
+// ---- 垂直统一速度支路（§5，A1） ----
+
+TEST(TrackingControlLawTest, VerticalOffsetDrivesVz) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {500.0, 600.0};  // 目标偏低 100 像素 → α>0 → 下降
+    const auto out = law.Update(snap, 1000);
+    const double alpha_deg = std::atan(0.2) * kRadToDeg;
+    EXPECT_NEAR(out.vz_mps, 0.1 * alpha_deg, 1e-6);
+    EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);  // A1：不切姿态微调模式
+}
+
+TEST(TrackingControlLawTest, VerticalVelocityClamped) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.track.center = {500.0, 990.0};  // α≈44.5° × 0.1 = 4.45 > 2 上限
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.vz_mps, 2.0, 1e-9);
+}
+
+// ---- 距离通道（§6.2 / §6.3） ----
+
+TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.radar_distance_m = 30.0;  // Δd = +20 → kp 输出 10 → 限幅 3
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
+}
+
+TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.radar_distance_m = 5.0;  // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
+}
+
+TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
+    TrackingControlLaw law(MakeConfig());  // 缺省 kHold
+    auto snap = MakeSnapshot(1000);
+    snap.associated_to_target = false;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
+    EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
+}
+
+TEST(TrackingControlLawTest, NoDistanceSlowApproach) {
+    auto cfg = MakeConfig();
+    cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.associated_to_target = false;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
+}
+
+TEST(TrackingControlLawTest, NoDistanceExitDegrades) {
+    auto cfg = MakeConfig();
+    cfg.distance.no_distance_action = NoDistanceAction::kExit;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.associated_to_target = false;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
+    EXPECT_FALSE(out.valid);
+}
+
+TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) {
+    TrackingControlLaw law(MakeConfig());  // kHold
+    auto snap = MakeSnapshot(1000);
+    snap.radar_distance_m = 30.0;
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 400;  // 600 ms 前 > 500 ms 阈值 → 距离不可用
+    const auto out = law.Update(snap, 1000);
+    EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
+    EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
+}
+
+// ---- 首拍/限幅（§8、S6） ----
+
+TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) {
+    // S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
+    auto cfg = MakeConfig();
+    cfg.accel_limit.ax_mps2 = 2.0;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.radar_distance_m = 30.0;  // 通道输出本应为 3.0
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    const auto out = law.Update(snap, 1000);  // 首拍：dt 按 1/20 Hz 兜底
+    EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9);  // 0.1
+}
+
+// ---- 机体系 → NED 旋转（§2.3，A4） ----
+
+TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.yaw_rad = kPi / 2.0;  // 机头朝东
+    snap.radar_distance_m = 30.0;
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    const auto out = law.Update(snap, 1000);
+    // 机体前向 3.0 → 东向 3.0，北向 ≈ 0
+    EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
+    EXPECT_NEAR(out.vy_mps, 3.0, 1e-9);
+}
+
+TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) {
+    TrackingControlLaw law(MakeConfig());
+    auto snap = MakeSnapshot(1000);
+    snap.yaw_rad = 0.0;
+    snap.radar_distance_m = 30.0;
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    const auto out = law.Update(snap, 1000);
+    EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
+    EXPECT_NEAR(out.vy_mps, 0.0, 1e-9);
+}
+
+// ---- Reset（§6.4） ----
+
+TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) {
+    auto cfg = MakeConfig();
+    cfg.accel_limit.ax_mps2 = 2.0;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.radar_distance_m = 30.0;
+    snap.associated_to_target = true;
+    snap.radar_time_ms = 1000;
+    for (std::int64_t t = 1000; t < 3000; t += 50) {
+        snap.track_time_ms = t;
+        snap.radar_time_ms = t;
+        snap.attitude_time_ms = t;
+        law.Update(snap, t);  // 若干拍后 vx 已爬升到限幅 3.0
+    }
+    law.Reset();
+    snap.track_time_ms = 3000;
+    snap.radar_time_ms = 3000;
+    snap.attitude_time_ms = 3000;
+    const auto out = law.Update(snap, 3000);
+    // Reset 后限幅器归零：又从 0.1 开始渐变
+    EXPECT_NEAR(out.vx_mps, 0.1, 1e-9);
+}
+
+}  // namespace

reference-c++/visual_track_control/tests/vtc_config_test.cpp

@@ -0,0 +1,224 @@
+// =============================================================================
+// vtc_config_test.cpp —— TrackingConfig::LoadFromJson 单元测试
+//
+// 覆盖（物理追踪思路 §8、需求分析 NFR-007）：
+//   真实配置文件加载成功与取值正确（含 A1/A2 修改后的字段结构）、
+//   缺字段 / 缺分组 / 非法枚举 / 非法数值 / 文件缺失均抛带字段名的异常。
+//
+// 依赖编译期宏：
+//   VTC_TEST_CONFIG   —— config/track_control.json 绝对路径
+//   VTC_TEST_TMP_DIR  —— 测试临时文件输出目录（构建目录）
+// =============================================================================
+
+#include <cstdio>
+#include <fstream>
+#include <stdexcept>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "visual_track_control/vtc_config.h"
+
+namespace {
+
+using drone::vtc::HeadingMode;
+using drone::vtc::NoDistanceAction;
+using drone::vtc::TrackingConfig;
+
+/// 与真实配置同构的完整合法 JSON（用于派生各非法变体）
+const char* kFullJson = R"({
+    "control": {
+        "frequency_hz": 20.0,
+        "visual_stale_ms": 200,
+        "radar_stale_ms": 500,
+        "attitude_stale_ms": 300
+    },
+    "camera": {
+        "image_width": 1920,
+        "image_height": 1080,
+        "fov_h_deg": 60.0,
+        "fov_v_deg": 35.0
+    },
+    "heading": {
+        "mode": "rate",
+        "gain": 1.0,
+        "yaw_rate_limit_dps": 30.0,
+        "yaw_slew_limit_dps2": 60.0
+    },
+    "vertical": {
+        "vz_gain_mps_per_deg": 0.1,
+        "vz_limit_mps": 2.0
+    },
+    "distance": {
+        "d_exp_m": 10.0,
+        "kp": 0.5,
+        "ki": 0.05,
+        "kd": 0.1,
+        "integral_limit": 2.0,
+        "derivative_filter_coef": 0.2,
+        "approach_velocity_limit_mps": 3.0,
+        "retreat_velocity_limit_mps": 1.0,
+        "no_distance_action": "hold",
+        "no_distance_approach_limit_mps": 0.5
+    },
+    "accel_limit": {
+        "ax_mps2": 2.0,
+        "ay_mps2": 2.0,
+        "az_mps2": 1.5
+    }
+})";
+
+/// 在 json 中把首个 from 替换为 to（未找到即测试失败）
+void ReplaceOnce(std::string& json, const std::string& from, const std::string& to) {
+    const auto pos = json.find(from);
+    ASSERT_NE(pos, std::string::npos) << "未找到待替换片段: " << from;
+    json.replace(pos, from.size(), to);
+}
+
+/// 把 JSON 文本写入临时文件并返回路径（调用方用完后删除）
+std::string WriteTempJson(const std::string& content, const char* name) {
+    const std::string path = std::string(VTC_TEST_TMP_DIR) + "/" + name;
+    std::ofstream ofs(path);
+    EXPECT_TRUE(static_cast<bool>(ofs)) << "无法写入临时配置: " << path;
+    ofs << content;
+    return path;
+}
+
+TEST(VtcConfigTest, LoadRealConfigSucceeds) {
+    const TrackingConfig cfg = TrackingConfig::LoadFromJson(VTC_TEST_CONFIG);
+    EXPECT_DOUBLE_EQ(cfg.control.frequency_hz, 20.0);
+    EXPECT_EQ(cfg.control.visual_stale_ms, 200);
+    EXPECT_EQ(cfg.camera.image_width, 1920);
+    EXPECT_EQ(cfg.camera.image_height, 1080);
+    EXPECT_DOUBLE_EQ(cfg.camera.fov_h_deg, 60.0);
+    EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate);  // A2：默认速率式
+    EXPECT_DOUBLE_EQ(cfg.heading.yaw_rate_limit_dps, 30.0);
+    EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1);
+    EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0);
+    EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0);
+    EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold);
+    EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0);
+}
+
+TEST(VtcConfigTest, FullJsonRoundTripMatchesExpected) {
+    // 内嵌合法 JSON 与真实配置文件行为一致
+    const std::string path = WriteTempJson(kFullJson, "vtc_cfg_full.json");
+    const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
+    EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate);
+    EXPECT_DOUBLE_EQ(cfg.distance.kp, 0.5);
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, PositionModeAccepted) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"position\"");
+    const std::string path = WriteTempJson(json, "vtc_cfg_position.json");
+    const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
+    EXPECT_EQ(cfg.heading.mode, HeadingMode::kPosition);
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, MissingFileThrowsWithPath) {
+    try {
+        TrackingConfig::LoadFromJson("/nonexistent/vtc_no_such_file.json");
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("vtc_no_such_file.json"),
+                  std::string::npos);
+    }
+}
+
+TEST(VtcConfigTest, MalformedJsonThrows) {
+    const std::string path = WriteTempJson("{ this is not json", "vtc_cfg_bad.json");
+    EXPECT_THROW(TrackingConfig::LoadFromJson(path), std::runtime_error);
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, MissingFieldThrowsWithFieldName) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"gain\": 1.0,\n", "");
+    const std::string path = WriteTempJson(json, "vtc_cfg_missing.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("heading.gain"), std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, MissingSectionThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json,
+                ",\n    \"accel_limit\": {\n"
+                "        \"ax_mps2\": 2.0,\n"
+                "        \"ay_mps2\": 2.0,\n"
+                "        \"az_mps2\": 1.5\n"
+                "    }",
+                "");
+    const std::string path = WriteTempJson(json, "vtc_cfg_nosection.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("accel_limit"), std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, InvalidHeadingModeThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"magic\"");
+    const std::string path = WriteTempJson(json, "vtc_cfg_badmode.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("heading.mode"), std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, InvalidNoDistanceActionThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"no_distance_action\": \"hold\"",
+                "\"no_distance_action\": \"zoom\"");
+    const std::string path = WriteTempJson(json, "vtc_cfg_badaction.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("no_distance_action"),
+                  std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, NegativeFrequencyThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"frequency_hz\": 20.0", "\"frequency_hz\": -1.0");
+    const std::string path = WriteTempJson(json, "vtc_cfg_negfreq.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("control.frequency_hz"),
+                  std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+TEST(VtcConfigTest, FovOutOfRangeThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"fov_h_deg\": 60.0", "\"fov_h_deg\": 200.0");
+    const std::string path = WriteTempJson(json, "vtc_cfg_badfov.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("camera.fov_h_deg"), std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
+}  // namespace

reference-c++/visual_track_control/视觉跟踪控制律实现文档.md

@@ -0,0 +1,443 @@
+# 视觉跟踪控制律实现文档（visual_track_control）
+
+> 独立静态库子工程，实现 `../docs/物理追踪思路.md`（下称"规格"）定义的
+> 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、
+> 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级，
+> 输出端随机体航向完成机体系→局部 NED 旋转，经 `MavlinkCommandGateway`
+> 打包为 MAVLink 字节流，交由注入的 `ICommandTransport` 实现送达。
+>
+> 本文档按 `../AGENTS.md`"实现文档规范"六要素组织。三份文档分工明确、
+> 互不重复：
+>
+> - `../docs/物理追踪思路.md`——控制律**物理与行为规格**（唯一权威），
+>   本文档不重复抄录公式推导；
+> - `../docs/视觉跟踪控制律技术路线.md`——**语言无关实现路线**：单周期八步
+>   伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引；
+> - 本文档——**C++ 落地说明**：公共头一览、构建与测试方式、日志行为、
+>   排查与修改要点。
+>
+> 追踪库行为约定见 `../target_tracker/target_tracker.md`，同样只引用不复述。
+
+---
+
+## 一、功能职责与边界
+
+命名空间 `drone::vtc`，头文件保护一律 `#pragma once`。
+
+**做什么**：
+
+- **三路控制律**（`TrackingControlLaw::Update()` 单周期计算）：
+  - 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 →
+    角速度限幅，按 `HeadingMode` 配置输出速率式 `yaw_rate_dps`（默认）
+    或位置式 `yaw_deg`（绝对 NED 航向，见 §3.8）；
+  - 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → `vz_mps`
+    垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
+    原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
+  - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
+    `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
+    （hold / slow_approach / exit）。
+- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
+  `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
+  全部在机体系完成。
+- **坐标系旋转**（规格 §2.3）：输出端用 `RotateBodyToNed()` 把机体系
+  速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的
+  `MAV_FRAME_LOCAL_NED` 一致；仅绕下轴旋转（滚转/俯仰对速度指令为
+  二阶小量，由飞控内环处理）。
+- **Coast 清积分**（规格 §6.4）：`is_predicted == true` 时清空距离 PID
+  积分项（`ResetIntegral()`），防积分饱和。
+- **判丢刹车**（规格 §6.4）：`tracked == false` 时输出 `kBrakeHover`
+  零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。
+- **超龄降级**（规格 §7.1、§9.2）：姿态过期/缺失 → `kDegradedHold`
+  （`kAttitudeStale`，禁止控制）；视觉结果超龄 → `kDegradedHold`
+  （`kVisionStale`）；判定器为无状态 `StalenessChecker::IsStale()`。
+- **MAVLink 打包**（规格 §2.2）：`MavlinkCommandGateway` 把
+  `ControlOutput` 打包为 `SET_POSITION_TARGET_LOCAL_NED`（ID 84）字节流。
+- **配置化**（规格 §8、需求分析 NFR-007）：全部阈值集中在
+  `TrackingConfig`，由 `TrackingConfig::LoadFromJson()` 启动期一次性加载，
+  代码不写死任何参数。
+
+**不做什么**（边界）：
+
+- **不做感知**：不运行检测/光流/雷达驱动，只消费调用方填入的
+  `ControlSnapshot`；
+- **不做状态机仲裁**：S6/S8 切换、目标丢失策略（`ITargetLostStrategy`）、
+  障碍接管均在状态机层，本库只按输入标志产生当拍输出；
+- **不写串口/传输**：库内无任何链路代码，字节流交给 `ICommandTransport`
+  实现方；
+- **不解析 MAVLink 输入**：只有打包发送方向，无 `mavlink_parse_*`；
+- **不做机体补偿**：`ControlSnapshot` 的 `roll_rad` / `pitch_rad` /
+  `yaw_rate_radps` / `velocity_*` 字段为预留输入（规格 §4.1）；当前仅
+  `yaw_rad` 参与计算（NED 旋转与位置式航向合成）；
+- **不做热更新**：配置启动期加载一次，调参重启生效。
+
+---
+
+## 二、接口与数据流
+
+### 2.1 公共头一览
+
+| 头文件 | 内容 |
+|--------|------|
+| `include/visual_track_control/vtc_types.h` | `ControlMode`、`DegradedReason`、`ControlSnapshot`、`ControlOutput` |
+| `include/visual_track_control/vtc_config.h` | `HeadingMode`、`NoDistanceAction`、各分组配置结构、`TrackingConfig` |
+| `include/visual_track_control/tracking_control_law.h` | `TrackingControlLaw`（主控律） |
+| `include/visual_track_control/pid_controller.h` | `PidParams`、`PidController` |
+| `include/visual_track_control/rate_limiter.h` | `Clamp()`、`SlewRateLimiter`（header-only） |
+| `include/visual_track_control/staleness_checker.h` | `StalenessChecker`（header-only） |
+| `include/visual_track_control/line_of_sight.h` | `PixelToAngle`（像素→视线角，全程针孔模型） |
+| `include/visual_track_control/frame_transform.h` | `RotateBodyToNed()`（机体系→局部 NED，header-only） |
+| `include/visual_track_control/i_command_transport.h` | `ICommandTransport`（传输抽象） |
+| `include/visual_track_control/mavlink_command_gateway.h` | `MavlinkCommandGateway`（MAVLink 打包） |
+
+### 2.2 关键字段（以代码为准）
+
+`enum class ControlMode`：`kVelocityHeading`（速度航向主输出）/
+`kBrakeHover`（零速刹车）/ `kDegradedHold`（降级保持，禁止正常控制）。
+（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
+
+`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
+`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
+
+`ControlSnapshot`（输入，纯值类型）：
+
+| 字段 | 含义 |
+|------|------|
+| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
+| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
+| `radar_distance_m` | 机头前向单点距离（米） |
+| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
+| `radar_time_ms` | 雷达取样单调时钟 |
+| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
+| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
+| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
+| `velocity_x_mps` / `velocity_y_mps` / `velocity_z_mps` | 自机速度分量（预留，接入主工程时由 `FlightStateSnapshot` 填充） |
+
+`ControlOutput`（输出，已限幅）：
+
+| 字段 | 含义 |
+|------|------|
+| `mode` / `valid` | 输出模式 / 指令是否可用于发送 |
+| `vx_mps` / `vy_mps` / `vz_mps` | **局部 NED 速度分量（北/东/地，米/秒）**：各通道在机体系计算并限幅后，输出端随 `snapshot.yaw_rad` 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 `vy_mps` 可非零 |
+| `yaw_deg` | 位置式目标**绝对航向**（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值） |
+| `yaw_rate_dps` | 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义） |
+| `degraded_reason` | 降级原因（仅 `kDegradedHold` 有意义） |
+| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
+
+`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
+`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
+`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
+`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
+（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
+`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
+`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
+`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit`
+（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
+`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
+相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
+
+### 2.3 ICommandTransport 契约
+
+```cpp
+class ICommandTransport {
+public:
+    virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
+};
+```
+
+- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层
+  只换实现，不动打包与控制律；
+- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）；
+- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。
+
+### 2.4 数据流向
+
+```text
+推理侧每帧: TargetTracker::Update(detections, frame_shape)
+控制线程(固定周期, 规格 §7.1):
+    TargetTracker::LastResult()          # 线程安全拷贝
+    → 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms
+    → TrackingControlLaw::Update(snapshot, now_ms)   # 单周期计算
+    → 按 ControlOutput.mode 分发:
+        kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading()
+        kBrakeHover      → MavlinkCommandGateway::SendBrakeHover()
+        kDegradedHold    → 不发送（仅告警/降级）
+    → mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流)
+停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4）
+```
+
+---
+
+## 三、关键实现点
+
+### 3.1 传输接口只吃字节流的双重解耦
+
+打包（`MavlinkCommandGateway`）与传输（`ICommandTransport`）分离：
+网关只负责 `ControlOutput` → MAVLink 字节，不感知链路形态；传输实现
+只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的
+映射——控制律不直接写飞控。
+
+### 3.2 TrackResult 无时间戳，由调用方取样打戳
+
+`TrackResult` 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定：
+调用方在读取 `LastResult()` 的**瞬间**用单调时钟打戳填入
+`track_time_ms`；超龄判定一律用"取样时间 + `valid_for_ms`"
+（`StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)`）：
+
+- `sample_time_ms <= 0` 视为从未取样，直接判超龄；
+- `now_ms < sample_time_ms`（时间戳不一致：时钟源错配/打戳错误）同样
+  判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。
+
+### 3.3 PID anti-windup 策略（条件积分）
+
+`PidController::Update()`（位置式）三层防护：
+
+1. **积分钳位**：候选积分值先钳制在 ±`integral_limit`；
+2. **anti-windup（条件积分）**：输出饱和（`|raw| > output_limit`）且
+   **本拍积分增量仍在推深饱和**（增量 `error·dt` 与原始输出 `raw`
+   同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向，
+   积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与
+   输出同向"：后者在误差反号后仍会冻结回卷；也不用 `raw·error` 的
+   变体含混表述，实现即 `(error*dt_s) * raw > 0`；
+3. **输出限幅**：最终输出钳制在 ±`output_limit`。
+
+微分带一阶低通（`derivative_filter_coef ∈ [0,1)`，0 = 不滤波）；
+`dt_s <= 0` 时保持上次输出（异常节拍保护）。`ResetIntegral()` 只清积分，
+保留微分历史与上次输出；`Reset()` 清全部，均 O(1)（Coast 清积分，
+规格 §6.4）。
+
+### 3.4 SlewRateLimiter 兼作首条指令突变抑制
+
+`SlewRateLimiter` 保存上次输出，单步变化不超过 `rate_limit · dt_s`；
+构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅"
+（状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变，
+无需专门的切换分支。判丢进刹车时调用 `Reset()` 归零，恢复跟踪后同样
+从零渐变。主控律共 4 个限幅器：`yaw_deg_limiter_` / `yaw_rate_limiter_`
+（度/秒²）、`vx_limiter_` / `vz_limiter_`（米/秒²）。
+
+### 3.5 像素→角度：全程针孔模型
+
+`PixelToAngle` 构造期预计算针孔系数 `2·tan(θ/2)/size`，热路径仅一次
+乘加 + `atan`：`angle = atan(offset·2·tan(θ/2)/size)`（规格 §4.1/§5.1）。
+原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界
+两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入
+突变；`atan` 代价可忽略，全程采用精确针孔式。配置中 `linear_ratio` 键
+随之移除。水平 β（`PixelToAngle(W, θ_h)`）与垂直 α（`PixelToAngle(H, θ_v)`）
+各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛
+`std::invalid_argument`。
+
+### 3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头
+
+c_library_v2 的 `mavlink_helpers.h` 中 **`MAVLINK_HELPER` 默认为空宏**，
+辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 `common/mavlink.h`
+会产生**多重定义**链接错误。因此：
+
+- `common/mavlink.h` 只被 `src/mavlink_command_gateway.cpp` 包含
+  （该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样**不得**包含
+  该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）；
+- 公共头 `mavlink_command_gateway.h` 不出现任何 `mavlink_*` 类型，
+  打包/发送缓冲用成员固定字节存储承载（`msg_storage_` / `tx_buffer_`，
+  各 512 字节，`.cpp` 中 `static_assert` 校验 ≥ `sizeof(mavlink_message_t)`
+  与 `MAVLINK_MAX_PACKET_LEN`）；
+- 打包用 `mavlink_msg_*_pack`（无通道状态、纯静态内联、可重入），
+  不使用 `_pack_chan` 通道 API；
+- CMake 中 MAVLink include 目录设为 `SYSTEM PRIVATE`：压第三方告警噪音，
+  且不泄漏给下游。
+
+同理，`nlohmann/json` 只被 `src/vtc_config.cpp` 一个 TU 包含。
+
+### 3.7 配置启动期一次加载，不做热更新
+
+`TrackingConfig::LoadFromJson()` 仅在启动期调用一次：解析结果拷贝进
+`TrackingConfig` 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象；
+调参重启即可。若未来需要热更新，扩展点为**原子指针交换**：持有
+`std::atomic<const TrackingConfig*>`，后台解析新配置成功后原子换指针，
+控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。
+
+### 3.8 主控律单周期流程
+
+`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍
+（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底，
+dt 上限 `kMaxDtS = 0.5` 秒）：
+
+① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`）
+→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
+→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
+→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
+→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
+`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
+随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
+
+两个设计决策记录：
+
+- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
+  → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
+  消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型
+  逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经
+  `vz_gain` 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。
+- **位置式航向输出绝对值（A2）**：ID 84 的 `yaw` 字段是**绝对** NED
+  航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
+  位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
+  （默认）输出角速度指令，天然相对量、无此问题。
+
+### 3.9 MAVLink 消息映射（规格 §2.2）
+
+全库仅 ID 84 `SET_POSITION_TARGET_LOCAL_NED` 一种消息（原 `kAttitudeTrim`
+→ ID 82 `SET_ATTITUDE_TARGET` 已随姿态微调支路移除）：
+
+| 模式 | 消息 | 要点 |
+|------|------|------|
+| `kVelocityHeading` | ID 84 | `coordinate_frame = MAV_FRAME_LOCAL_NED`（速度分量已是 NED 语义）。速度模式 `type_mask` 基础位 `kMaskVelocityBase = 0x01C7`（忽略位置与加速度），航向三分支：角速率非零 → `kMaskVelocityYawRate`（再忽略 yaw）；仅航向角非零 → `kMaskVelocityYaw`（再忽略 yaw_rate）；**两者均零 → 速率掩码配零角速率（保持当前航向）**——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北） |
+| `kBrakeHover` | ID 84 | `kMaskBrake = 0x0DC7`：速度全零，位置/加速度/航向/角速率字段全部忽略 |
+
+---
+
+## 四、日志行为
+
+遵循 `../AGENTS.md` 日志纪律：**库内仅状态切换点打日志，热路径零日志、
+零堆分配**。
+
+| 位置 | 日志 | 触发条件 |
+|------|------|----------|
+| `tracking_control_law.cpp`（`Update()` 内 `FinishAndLog`） | `spdlog::info` "[ControlLaw] 状态切换: X -> Y (原因)" | **仅当 `ControlMode` 相对 `last_mode_` 发生变化**时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印 |
+| `tracking_control_law.cpp` | —— | 每拍正常计算（含三路通道数学）**不打任何日志** |
+| `mavlink_command_gateway.cpp` / `pid_controller.cpp` / `line_of_sight.cpp` / `vtc_config.cpp` | —— | 不打日志（配置错误走异常，见第六章） |
+
+示例程序（`examples/track_control_demo.cpp`，非生产代码）另有两处
+**演示观测日志**，均带节流防刷屏：
+
+- `LoggingTransport::Send()`：字节统计与包头十六进制摘要，**秒级节流**
+  （距上次打印 ≥ 1000 ms 才输出）；
+- 主循环状态摘要：**每 10 拍一次**（20 Hz 下即半秒一次）。
+
+---
+
+## 五、构建、测试与运行方式
+
+### 5.1 构建
+
+`../AGENTS.md` 规定构建环境为 WSL2 Ubuntu 24.04。**当前开发机 WSL 仅
+装有 Ubuntu 22.04（发行版名 `Ubuntu`）**，本库的构建与测试验证在
+22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版
+安装后应按其复核一遍（见 §6.4）。
+
+```bash
+cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF
+cmake --build build_vtc -j$(nproc)
+./build_vtc/track_control_demo
+```
+
+- **为什么 `-DBUILD_TESTING=OFF`**：本库以 `add_subdirectory` 零改动复用
+  `target_tracker`，而 `target_tracker/CMakeLists.txt` 中 `include(CTest)`
+  会把 `BUILD_TESTING` 默认置为 `ON`，从而连带构建追踪库的 3 个测试
+  可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的
+  测试方式见 `../target_tracker/target_tracker.md` §五）。
+- **本库自身测试不复用 `BUILD_TESTING`**：由独立选项 `VTC_BUILD_TESTS`
+  （默认 `ON`）控制，与上面的关闭互不干扰。
+- **示例无需 `LD_LIBRARY_PATH`**：`target_tracker` 为 SHARED 库，
+  构建期 `CMAKE_BUILD_RPATH` 已指向构建目录，`track_control_demo`
+  可直接运行。
+- 示例缺省配置路径由编译期宏 `VTC_DEMO_DEFAULT_CONFIG` 内置为
+  `config/track_control.json` 的绝对路径；也可命令行传入
+  `track_control_demo [配置文件路径]`。
+
+### 5.2 示例演示内容
+
+单线程模拟完整链路（合成运动检测框 → `TargetTracker::Update()` →
+`LastResult()` + 打戳 → `TrackingControlLaw::Update()` → Gateway →
+`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
+（对应规格 §6.4）：
+
+1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
+   关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
+   角速度随 β 收敛而减小）；
+2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
+   继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
+   前向清零）；
+3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
+   零速刹车。
+
+示例将追踪库判丢阈值放大为 `kDemoMaxLostFrames = 60` 帧（默认 8 帧在
+20 Hz 下 Coast 仅 0.4 秒不便观察），**仅演示专用**，正式参数以追踪库
+规格为准。程序跑完总拍数后自行调用 `TrackingControlLaw::Reset()` 并
+正常退出（返回 0），无需手动终止。
+
+### 5.3 单元测试（Google Test）
+
+测试源文件位于 `tests/`，覆盖 8 个测试套件：
+
+| 测试文件 | 覆盖要点 |
+|----------|----------|
+| `rate_limiter_test.cpp` | `Clamp` 边界；`SlewRateLimiter` 单步限幅、`dt<=0`/冻结保护、`Reset` |
+| `staleness_checker_test.cpp` | 从未取样、时钟回拨（`now < sample`）、恰在有效期边界（临界判新鲜） |
+| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
+| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
+| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
+| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
+| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
+| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
+
+运行方式（构建后）：
+
+```bash
+./build_vtc/vtc_unit_tests                      # 直接跑
+ctest --test-dir build_vtc --output-on-failure  # 或经 ctest（gtest_discover_tests 注册）
+```
+
+依赖：系统安装 Google Test（`libgtest-dev`，FindGTest 模块模式，无需
+CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。
+`vtc_config_test` 通过编译期宏 `VTC_TEST_CONFIG` 读取真实配置文件
+`config/track_control.json`——**修改该配置文件时须同步核对测试断言**。
+
+### 5.4 部署提示
+
+`target_tracker` 为 SHARED 库（`libtarget_tracker.so.1`）：部署
+`track_control_demo` 或未来接入主工程时，必须将该 so 一并随带
+（目录式部署与 RPATH 约定见 `../target_tracker/target_tracker.md` §7）。
+本库 `visual_track_control` 自身为 STATIC 库，链接进可执行文件，无需随带。
+
+---
+
+## 六、排查 / 修改要点
+
+### 6.1 配置缺字段 / 非法值报错定位
+
+`LoadFromJson()` 缺字段、类型错误、非法取值均抛 `std::runtime_error`，
+**异常消息携带完整字段名**（如 `配置缺失字段或类型错误: heading.gain`、
+`配置非法(必须为正): control.frequency_hz`、
+`配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit`），
+按异常文本直接定位到 `config/track_control.json` 的对应分组与键即可；
+文件打开/解析失败则携带文件路径。`PixelToAngle` 构造期参数非法（尺寸
+非正、视场角不在 (0, π)）抛 `std::invalid_argument`，同样见消息文本。
+
+### 6.2 MAVLink 消息待实测适配项
+
+以下各项均以 **PX4 1.11.3 Offboard 实测为准**（规格 §10.1 待定项），
+全部集中在 `src/mavlink_command_gateway.cpp`，修改时只动这一个文件：
+
+| 适配项 | 位置 | 现状 |
+|--------|------|------|
+| `coordinate_frame` | `SendVelocityHeading()` / `SendBrakeHover()` 的 `mavlink_msg_set_position_target_local_ned_pack` 调用 | 固定取 `MAV_FRAME_LOCAL_NED`；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认 |
+| 速度模式 `type_mask` | 匿名命名空间 `kMaskVelocityBase` / `kMaskVelocityYaw` / `kMaskVelocityYawRate` / `kMaskBrake` | 掩码位组合待实测确认 |
+
+### 6.3 接入主工程时需补齐的事项
+
+1. **机体补偿（可选）**：机体系→NED 旋转已在库内完成（仅绕下轴按
+   `yaw_rad`，规格 §2.3）；`roll_rad` / `pitch_rad` / `yaw_rate_radps` /
+   `velocity_*` 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充；
+2. **`TrackedTarget` 主题接线**：`TrackResult` 尚未进入主工程
+   `include/common/types.h`；需先按 `../target_tracker/target_tracker.md`
+   §7 与规格 §7.2 正式定义 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
+   由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
+3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
+   （`ICommandTransport` 注入），打包与控制律零改动；
+4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
+   规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
+
+### 6.4 红线声明
+
+- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为
+  占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/
+  畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库
+  **仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。
+- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅
+  装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。

utils/README.md

@@ -0,0 +1,58 @@
+# utils —— reference-c++ 两个子项目的 Python 移植
+
+将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
+`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
+（唯一的数据转换点在胶水层 demo）。
+
+## 目录结构
+
+```
+utils/
+├── target_tracker/            # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy）
+│   └── README.md              # 接口清单 + C++ 差异
+├── visual_track_control/      # 包 2：三通道控制律 + MAVSDK offboard 网关
+│   └── README.md              # 接口清单 + C++ 差异
+├── demo_track_control.py      # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
+├── config/track_control.json  # 控制律配置（拷贝自 reference-c++）
+├── tests/                     # pytest 测试（对应 C++ 全部 13 个测试文件）
+│   ├── target_tracker/        # 27 条
+│   └── visual_track_control/  # 78 条
+├── conftest.py                # 保证可从任意工作目录导入两包
+└── pytest.ini                 # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
+```
+
+## 环境
+
+conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
+依赖：numpy、mavsdk、pytest。
+
+## 运行测试
+
+```bash
+cd utils
+"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
+```
+
+105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4
+回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。
+
+## 运行演示
+
+```bash
+# mock 传输（本地记录指令，无实机）
+python utils/demo_track_control.py --speedup 10
+
+# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
+python utils/demo_track_control.py --transport mavsdk
+```
+
+时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
+目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
+
+## 移植约定
+
+- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
+  映射保留 C++ A2 回归语义，详见包 2 README。
+- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
+  `EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
+- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。

utils/config/track_control.json

@@ -0,0 +1,48 @@
+{
+    "_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。",
+
+    "control": {
+        "frequency_hz": 20.0,
+        "visual_stale_ms": 200,
+        "radar_stale_ms": 500,
+        "attitude_stale_ms": 300
+    },
+
+    "camera": {
+        "image_width": 1920,
+        "image_height": 1080,
+        "fov_h_deg": 60.0,
+        "fov_v_deg": 35.0
+    },
+
+    "heading": {
+        "mode": "rate",
+        "gain": 1.0,
+        "yaw_rate_limit_dps": 30.0,
+        "yaw_slew_limit_dps2": 60.0
+    },
+
+    "vertical": {
+        "vz_gain_mps_per_deg": 0.1,
+        "vz_limit_mps": 2.0
+    },
+
+    "distance": {
+        "d_exp_m": 10.0,
+        "kp": 0.5,
+        "ki": 0.05,
+        "kd": 0.1,
+        "integral_limit": 2.0,
+        "derivative_filter_coef": 0.2,
+        "approach_velocity_limit_mps": 3.0,
+        "retreat_velocity_limit_mps": 1.0,
+        "no_distance_action": "hold",
+        "no_distance_approach_limit_mps": 0.5
+    },
+
+    "accel_limit": {
+        "ax_mps2": 2.0,
+        "ay_mps2": 2.0,
+        "az_mps2": 1.5
+    }
+}

utils/conftest.py

@@ -0,0 +1,8 @@
+"""pytest 根配置：保证无论从哪个工作目录启动 pytest，均可导入 utils/ 下的两个包。"""
+
+import sys
+from pathlib import Path
+
+_UTILS_DIR = Path(__file__).resolve().parent
+if str(_UTILS_DIR) not in sys.path:
+    sys.path.insert(0, str(_UTILS_DIR))

utils/demo_track_control.py

@@ -0,0 +1,207 @@
+"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。
+
+串联三个组件（target_tracker 与 visual_track_control 互不 import，
+本脚本是唯一的转换点）：
+  模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪）
+    → TrackResult 转 TrackState（解耦边界）
+    → TrackingControlLaw（三通道控制律 + 限幅 + 状态机）
+    → CommandSender（mock 记录 / MAVSDK offboard 实发）
+
+场景时间线（20 Hz，--speedup 可加速）：
+  0~ 6s  正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近
+  6~10s  目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
+         控制律自动切换零速刹车）
+  10~13s 持续判丢：维持零速刹车
+
+用法：
+  python utils/demo_track_control.py                     # mock 传输
+  python utils/demo_track_control.py --transport mavsdk  # MAVSDK（需飞控/SITL）
+  python utils/demo_track_control.py --speedup 4         # 4 倍速
+"""
+
+import argparse
+import asyncio
+import logging
+from pathlib import Path
+
+from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig
+from visual_track_control.config import load_from_json
+from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
+from visual_track_control.tracking_control_law import TrackingControlLaw
+from visual_track_control.types import (ControlMode, ControlSnapshot,
+                                        ControlOutput, TrackState)
+
+logger = logging.getLogger("demo")
+
+_DEMO_DIR = Path(__file__).resolve().parent
+_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json"
+
+# 模拟场景参数
+_NORMAL_SECS = 6.0    # 正常跟踪阶段
+_COAST_SECS = 4.0     # 目标消失 → Coast（含轨道判亡）
+_LOST_SECS = 3.0      # 判丢后持续刹车阶段
+_DEMO_MAX_LOST_FRAMES = 60  # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
+_STATUS_EVERY_TICKS = 10    # 每 0.5 s 打印一行状态
+
+
+def track_result_to_state(result) -> TrackState:
+    """两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。"""
+    box = result.box
+    raw = result.raw
+    return TrackState(
+        tracked=result.tracked,
+        primary_id=result.primary_id,
+        center=result.center,
+        box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None,
+        confidence=result.confidence,
+        is_predicted=result.is_predicted,
+        lost_frames=result.lost_frames,
+        n_active=result.n_active,
+        raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None,
+    )
+
+
+def simulate_detections(tick: int, normal_ticks: int, period_s: float):
+    """场景检测源：正常阶段目标匀速横移，之后消失。"""
+    if tick >= normal_ticks:
+        return []
+    cx = 960.0 + 3.0 * tick  # 3 px/帧 匀速右移
+    cy = 540.0
+    w, h = 120.0, 160.0
+    return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)]
+
+
+def simulate_radar(tick: int, normal_ticks: int, period_s: float):
+    """场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。"""
+    if tick >= normal_ticks:
+        return 0.0, False
+    distance = max(10.0, 30.0 - tick * period_s * 3.0)
+    return distance, True
+
+
+class MockCommandSender:
+    """CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。"""
+
+    def __init__(self) -> None:
+        self.last_desc = "-"
+
+    async def start(self) -> bool:
+        logger.info("[MockTransport] offboard 启动（模拟）")
+        return True
+
+    async def send_velocity_heading(self, output: ControlOutput,
+                                    time_boot_ms=None) -> bool:
+        self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} "
+                          f"vz={output.vz_mps:+.2f} "
+                          f"yaw_rate={output.yaw_rate_dps:+.2f}°/s "
+                          f"yaw={output.yaw_deg:+.2f}°")
+        return True
+
+    async def send_brake_hover(self, time_boot_ms=None) -> bool:
+        self.last_desc = "零速刹车（全零机体速度设定点）"
+        return True
+
+    async def stop(self) -> None:
+        logger.info("[MockTransport] offboard 停止（模拟）")
+
+
+async def make_sender(args) -> object:
+    if args.transport == "mock":
+        return MockCommandSender()
+    from mavsdk import System
+    drone = System()
+    logger.info("连接飞控 %s ...", args.conn)
+    await drone.connect(system_address=args.conn)
+    async for state in drone.core.connection_state():
+        if state.is_connected:
+            logger.info("飞控已连接")
+            break
+    return MavsdkCommandGateway(drone)
+
+
+async def dispatch(sender, out: ControlOutput, now_ms: int):
+    """按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。"""
+    if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
+        return await sender.send_velocity_heading(out, now_ms)
+    if out.mode is ControlMode.BRAKE_HOVER and out.valid:
+        return await sender.send_brake_hover(now_ms)
+    return None
+
+
+async def run_demo(args) -> None:
+    cfg = load_from_json(args.config)
+    period_s = 1.0 / cfg.control.frequency_hz
+    period_ms = int(round(period_s * 1000.0))
+
+    tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES))
+    law = TrackingControlLaw(cfg)
+    sender = await make_sender(args)
+
+    normal_ticks = int(round(_NORMAL_SECS / period_s))
+    coast_ticks = int(round(_COAST_SECS / period_s))
+    lost_ticks = int(round(_LOST_SECS / period_s))
+    total_ticks = normal_ticks + coast_ticks + lost_ticks
+
+    if not await sender.start():
+        logger.error("offboard 启动失败，退出")
+        return
+
+    logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs",
+                total_ticks, total_ticks * period_s, cfg.control.frequency_hz,
+                _NORMAL_SECS, _COAST_SECS, _LOST_SECS)
+    try:
+        for tick in range(total_ticks):
+            now_ms = 1000 + tick * period_ms
+            detections = simulate_detections(tick, normal_ticks, period_s)
+            radar_m, associated = simulate_radar(tick, normal_ticks, period_s)
+
+            result = tracker.update(detections)
+            snap = ControlSnapshot(
+                track=track_result_to_state(result),
+                track_time_ms=now_ms,          # 每拍取样即打戳（视觉始终新鲜）
+                radar_distance_m=radar_m,
+                associated_to_target=associated,
+                radar_time_ms=now_ms if associated else 0,
+                attitude_present=True,
+                attitude_time_ms=now_ms,       # 模拟姿态源持续可用
+                yaw_rad=0.0,                   # 机头朝北（NED 旋转恒等）
+            )
+            out = law.update(snap, now_ms)
+            await dispatch(sender, out, now_ms)
+
+            if tick % _STATUS_EVERY_TICKS == 0:
+                logger.info(
+                    "t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | "
+                    "[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s",
+                    (tick * period_s), len(detections), result.tracked,
+                    result.is_predicted, result.lost_frames, out.mode.name,
+                    out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps,
+                    out.coast_active, sender.last_desc)
+            await asyncio.sleep(period_s / args.speedup)
+    finally:
+        await sender.stop()
+    logger.info("演示结束")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示")
+    parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock",
+                        help="指令发送方式：mock=本地记录，mavsdk=offboard 实发")
+    parser.add_argument("--conn", default="udp://:14540",
+                        help="MAVSDK 连接串（仅 --transport mavsdk 生效）")
+    parser.add_argument("--speedup", type=float, default=1.0,
+                        help="时间倍速（>1 加速演示）")
+    parser.add_argument("--config", default=str(_DEFAULT_CONFIG),
+                        help="控制律配置文件路径")
+    args = parser.parse_args()
+
+    logging.basicConfig(level=logging.INFO,
+                        format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
+                        datefmt="%H:%M:%S")
+    asyncio.run(run_demo(args))
+
+
+if __name__ == "__main__":
+    main()

utils/pytest.ini

@@ -0,0 +1,3 @@
+[pytest]
+testpaths = tests
+python_files = *_test.py test_*.py

utils/target_tracker/README.md

@@ -0,0 +1,29 @@
+# target_tracker（Python 版）
+
+多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。
+移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。
+
+## 接口清单
+
+| 名称 | 说明 |
+| --- | --- |
+| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 |
+| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） |
+| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw |
+| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 |
+| `TargetTracker(config=None)` | 追踪器主类 |
+| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） |
+| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） |
+| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 |
+| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） |
+| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 |
+
+## 与 C++ 版的差异
+
+- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。
+- **Optional 语义**：C++ `std::optional<T>` → Python `None`；`State()` → `state` 属性（返回拷贝）。
+- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。
+- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。
+- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、
+  ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化）
+  与 C++ 完全一致，由 27 条移植测试锁定。

utils/target_tracker/__init__.py

@@ -0,0 +1,15 @@
+"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。
+
+纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。
+"""
+
+from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter
+from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult
+from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy
+
+__all__ = [
+    "STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE",
+    "BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet",
+    "Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult",
+    "compute_iou", "to_center_size", "to_xyxy",
+]

utils/target_tracker/kalman_box_filter.py

@@ -0,0 +1,111 @@
+"""单目标 8 维恒定速度卡尔曼滤波器。
+
+对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）：
+状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。
+矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。
+"""
+
+from dataclasses import dataclass
+
+import numpy as np
+
+# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优）
+STD_POS = 1.0 / 50.0    # 位置噪声比例系数
+STD_VEL = 1.0 / 200.0   # 速度噪声比例系数
+INI_POS = 2.0           # 初始位置协方差
+INI_VEL = 100.0         # 初始速度协方差
+R_SCALE = 2.0           # R 额外缩放因子（增强平滑）
+_S_REG = 1e-9           # S 求逆前的对角正则化（§9.3）
+
+# 状态转移矩阵 F（§4.1）：I4 + 右上 I4
+_F = np.eye(8, dtype=float)
+_F[0:4, 4:8] = np.eye(4)
+
+# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度
+_H_COLS = slice(0, 4)
+
+
+@dataclass
+class BoxCXCYWH:
+    """轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。"""
+
+    cx: float = 0.0
+    cy: float = 0.0
+    w: float = 0.0
+    h: float = 0.0
+
+
+class KalmanBoxFilter:
+    def __init__(self) -> None:
+        self._x = np.zeros(8, dtype=float)
+        self._p = np.zeros((8, 8), dtype=float)
+        self._initialized = False
+
+    @property
+    def initialized(self) -> bool:
+        return self._initialized
+
+    @property
+    def state(self) -> np.ndarray:
+        """状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。"""
+        return self._x.copy()
+
+    def initiate(self, z: BoxCXCYWH) -> None:
+        # §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100)
+        self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0])
+        self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4)
+        self._initialized = True
+
+    def predict(self) -> BoxCXCYWH | None:
+        # §4.4：未初始化返回 None
+        if not self._initialized:
+            return None
+
+        # §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。
+        # Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。
+        w = self._x[2]
+        h = self._x[3]
+        std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS])
+        std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL])
+        q = np.zeros((8, 8), dtype=float)
+        q[np.arange(4), np.arange(4)] = std_pos * std_pos
+        q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel
+
+        # §4.4：x ← F·x；P ← F·P·Fᵀ + Q
+        self._x = _F @ self._x
+        self._p = _F @ self._p @ _F.T + q
+        return BoxCXCYWH(float(self._x[0]), float(self._x[1]),
+                         float(self._x[2]), float(self._x[3]))
+
+    def update(self, z: BoxCXCYWH) -> None:
+        # §4.5：未初始化直接返回
+        if not self._initialized:
+            return
+
+        # §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE
+        w = abs(self._x[2])
+        h = abs(self._x[3])
+        rw = R_SCALE * (w * STD_POS) * (w * STD_POS)
+        rh = R_SCALE * (h * STD_POS) * (h * STD_POS)
+        r = np.diag([rw, rh, rw, rh])
+
+        # §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块
+        s = self._p[:4, :4] + r
+        try:
+            s_inv = np.linalg.inv(s + _S_REG * np.eye(4))
+        except np.linalg.LinAlgError:
+            return  # 求逆失败：防御性跳过本次更新（C++ 版同样跳过）
+
+        # §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4）
+        k = self._p[:, _H_COLS] @ s_inv
+
+        # 残差 y = z − H·x（H·x 即状态前 4 维）
+        y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4]
+
+        # x ← x + K·y
+        self._x = self._x + k @ y
+
+        # P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上
+        kh = np.zeros((8, 8), dtype=float)
+        kh[:, :4] = k
+        self._p = (np.eye(8) - kh) @ self._p

utils/target_tracker/target_tracker.py

@@ -0,0 +1,269 @@
+"""多目标追踪过滤器（TargetTracker）。
+
+对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp
+（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。
+
+线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result
+缓存加锁，last_result() 返回受保护拷贝。
+"""
+
+import math
+import threading
+from dataclasses import dataclass, field
+from typing import Optional, Sequence
+
+from .kalman_box_filter import BoxCXCYWH
+from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size
+
+
+@dataclass
+class Detection:
+    """单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。"""
+
+    x1: float = 0.0
+    y1: float = 0.0
+    x2: float = 0.0
+    y2: float = 0.0
+    conf: float = 0.0
+    cls_id: int = 0
+
+
+@dataclass
+class FrameShape:
+    """画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。"""
+
+    height: int = 0
+    width: int = 0
+
+
+@dataclass
+class TrackResult:
+    """每帧追踪输出（§2.3 九字段）。"""
+
+    tracked: bool = False
+    primary_id: Optional[int] = None
+    center: tuple[float, float] = (0.0, 0.0)
+    box: Optional[BoxXYXY] = None
+    confidence: float = 0.0
+    is_predicted: bool = False
+    lost_frames: int = 0
+    n_active: int = 0
+    raw: Optional[BoxXYXY] = None
+
+
+@dataclass
+class TrackerConfig:
+    """追踪器配置参数（§8.1 默认值取自规格）。"""
+
+    max_lost_frames: int = 8
+    max_association_dist: float = 200.0
+    min_hits: int = 3
+    dist_weight: float = 0.5
+    iou_weight: float = 0.5
+
+
+@dataclass
+class _AssociationResult:
+    matches: list[tuple[int, int]] = field(default_factory=list)
+    unmatched_track_ids: list[int] = field(default_factory=list)
+    unmatched_det_indices: list[int] = field(default_factory=list)
+
+
+class TargetTracker:
+    def __init__(self, config: Optional[TrackerConfig] = None) -> None:
+        self._config = config if config is not None else TrackerConfig()
+        self._next_id = 0
+        self._tracks: dict[int, Tracklet] = {}
+        self._frame_count = 0
+        self._locked_primary_id: Optional[int] = None
+        self._last_result = TrackResult()
+        self._last_result_mutex = threading.Lock()
+
+    @property
+    def config(self) -> TrackerConfig:
+        return self._config
+
+    def get_config(self) -> TrackerConfig:
+        return self._config
+
+    def update(self, detections: Sequence[Detection],
+               frame_shape: Optional[FrameShape] = None) -> TrackResult:
+        # §7 六步主流程
+        self._frame_count += 1
+
+        # 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入
+        # 卡尔曼状态将永久污染轨道并传导到主目标输出）
+        valid_detections = [
+            det for det in detections
+            if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf))
+        ]
+
+        # 步骤 1：所有存活轨道先行预测（推进一帧）
+        for track in self._tracks.values():
+            track.predict()
+
+        # 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数）
+        if valid_detections:
+            assoc = self._associate_detections(valid_detections)
+        else:
+            assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys()))
+
+        # 步骤 3：用匹配到的检测更新轨道
+        for track_id, det_idx in assoc.matches:
+            det = valid_detections[det_idx]
+            raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2)
+            self._tracks[track_id].update(raw_box, det.conf, det.cls_id)
+
+        # 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一）
+        for track_id in assoc.unmatched_track_ids:
+            track = self._tracks.get(track_id)
+            if track is None:
+                continue
+            track.mark_miss()
+            if track.is_dead:
+                if self._locked_primary_id == track_id:
+                    self._locked_primary_id = None
+                del self._tracks[track_id]
+
+        # 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用）
+        for det_idx in assoc.unmatched_det_indices:
+            det = valid_detections[det_idx]
+            measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2))
+            self._tracks[self._next_id] = Tracklet(
+                self._next_id, measurement, det.conf, det.cls_id,
+                self._config.max_lost_frames)
+            self._next_id += 1
+
+        # 步骤 6：选取/延续主目标，组装输出并缓存
+        result = self._select_primary(frame_shape)
+        with self._last_result_mutex:
+            self._last_result = result
+        return result
+
+    def last_result(self) -> TrackResult:
+        with self._last_result_mutex:
+            return self._last_result
+
+    def reset(self) -> None:
+        # §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零）
+        self._tracks.clear()
+        self._next_id = 0
+        self._frame_count = 0
+        self._locked_primary_id = None
+        with self._last_result_mutex:
+            self._last_result = TrackResult()
+
+    def active_count(self) -> int:
+        return len(self._tracks)
+
+    def active_tracks(self) -> dict[int, Tracklet]:
+        return dict(self._tracks)
+
+    def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult:
+        result = _AssociationResult()
+
+        # §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。
+        # 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。
+        def sort_key(track_id: int) -> tuple[float, int]:
+            raw = self._tracks[track_id].confidence
+            conf = raw if math.isfinite(raw) else -math.inf
+            return (-conf, track_id)
+
+        sorted_ids = sorted(self._tracks.keys(), key=sort_key)
+        det_assigned = [False] * len(detections)
+
+        for track_id in sorted_ids:
+            track = self._tracks[track_id]
+            # §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配）
+            best_cost = 1.0
+            best_det = -1
+            for d, det in enumerate(detections):
+                if det_assigned[d]:
+                    continue
+                det_cx = (det.x1 + det.x2) / 2.0
+                det_cy = (det.y1 + det.y2) / 2.0
+                dist = math.hypot(track.last_center[0] - det_cx,
+                                  track.last_center[1] - det_cy)
+                norm_dist = dist / max(self._config.max_association_dist, 1e-3)
+                iou = (compute_iou(track.last_box,
+                                   BoxXYXY(det.x1, det.y1, det.x2, det.y2))
+                       if track.last_box is not None else 0.0)
+                cost = (self._config.dist_weight * norm_dist +
+                        self._config.iou_weight * (1.0 - iou))
+                if cost < best_cost:
+                    best_cost = cost
+                    best_det = d
+            if best_det >= 0:
+                result.matches.append((track_id, best_det))
+                det_assigned[best_det] = True
+            else:
+                result.unmatched_track_ids.append(track_id)
+
+        result.unmatched_det_indices = [
+            d for d, assigned in enumerate(det_assigned) if not assigned
+        ]
+        return result
+
+    def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult:
+        result = TrackResult(n_active=len(self._tracks))
+
+        # §6.3：轨道表为空 → 清除锁定，返回 tracked=false
+        if not self._tracks:
+            self._locked_primary_id = None
+            return result
+
+        # §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出，
+        # 不被高置信度目标抢占）
+        chosen: Optional[Tracklet] = None
+        is_predicted = False
+        if self._locked_primary_id is not None:
+            track = self._tracks.get(self._locked_primary_id)
+            if track is not None:
+                chosen = track
+                is_predicted = track.is_coasting
+
+        # §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
+        if chosen is None:
+            measured = [t for t in self._tracks.values() if not t.is_coasting]
+            coasting = [t for t in self._tracks.values() if t.is_coasting]
+
+            if measured:
+                # confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
+                # （min_hits 只影响候选池优先级，不阻止输出）
+                pool = [t for t in measured if t.hits >= self._config.min_hits]
+                if not pool:
+                    pool = measured
+                chosen = pool[0]
+                for t in pool:
+                    if t.confidence > chosen.confidence:
+                        chosen = t
+                is_predicted = False
+            elif coasting:
+                # 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)）
+                center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0
+                center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0
+                best_dist_sq = math.inf
+                for t in coasting:
+                    dx = t.last_center[0] - center_x
+                    dy = t.last_center[1] - center_y
+                    dist_sq = dx * dx + dy * dy
+                    if dist_sq < best_dist_sq:
+                        best_dist_sq = dist_sq
+                        chosen = t
+                is_predicted = True
+            else:
+                # 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
+                self._locked_primary_id = None
+                return result
+            self._locked_primary_id = chosen.track_id  # 选举后锁定
+
+        # 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空
+        result.tracked = True
+        result.primary_id = chosen.track_id
+        result.center = chosen.last_center
+        result.box = chosen.last_box
+        result.confidence = chosen.confidence
+        result.is_predicted = is_predicted
+        result.lost_frames = chosen.lost_count
+        result.raw = None if is_predicted else chosen.last_raw_box
+        return result

utils/target_tracker/tracklet.py

@@ -0,0 +1,110 @@
+"""轨道（Tracklet）与基础几何工具。
+
+对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h
+（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。
+"""
+
+from dataclasses import dataclass
+from typing import Optional
+
+from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
+
+
+@dataclass
+class BoxXYXY:
+    """轴对齐边界框（像素坐标，左上/右下角表示）。"""
+
+    x1: float = 0.0
+    y1: float = 0.0
+    x2: float = 0.0
+    y2: float = 0.0
+
+
+def to_center_size(box: BoxXYXY) -> BoxCXCYWH:
+    # §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
+    return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
+                     box.x2 - box.x1, box.y2 - box.y1)
+
+
+def to_xyxy(box: BoxCXCYWH) -> BoxXYXY:
+    # §7 TRACKLET_PREDICT 的 center_size 逆变换
+    return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0,
+                   box.cx + box.w / 2.0, box.cy + box.h / 2.0)
+
+
+def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float:
+    """标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。"""
+    area_a = (a.x2 - a.x1) * (a.y2 - a.y1)
+    area_b = (b.x2 - b.x1) * (b.y2 - b.y1)
+    if area_a <= 0.0 or area_b <= 0.0:
+        return 0.0
+
+    inter_w = min(a.x2, b.x2) - max(a.x1, b.x1)
+    inter_h = min(a.y2, b.y2) - max(a.y1, b.y1)
+    if inter_w <= 0.0 or inter_h <= 0.0:
+        return 0.0
+
+    inter_area = inter_w * inter_h
+    union_area = area_a + area_b - inter_area
+    if union_area <= 0.0:
+        return 0.0
+    return inter_area / union_area
+
+
+class Tracklet:
+    """单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。
+
+    构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、
+    lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。
+    """
+
+    def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float,
+                 cls_id: int, max_lost_frames: int) -> None:
+        self.track_id = track_id
+        self.kf = KalmanBoxFilter()
+        self.lost_count = 0
+        self.max_lost = max_lost_frames
+        self.confidence = conf
+        self.cls_id = cls_id
+        self.age = 1
+        self.hits = 1
+        self.last_center: tuple[float, float] = (measurement.cx, measurement.cy)
+        self.last_box: Optional[BoxXYXY] = to_xyxy(measurement)
+        # 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新）
+        self.last_raw_box: Optional[BoxXYXY] = self.last_box
+        self.kf.initiate(measurement)
+
+    @property
+    def is_dead(self) -> bool:
+        # §6.1 注记 2：lost_count > max_lost（严格大于）
+        return self.lost_count > self.max_lost
+
+    @property
+    def is_coasting(self) -> bool:
+        # §3.2：lost_count > 0 即处于纯预测状态
+        return self.lost_count > 0
+
+    def predict(self) -> None:
+        # §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1
+        pred = self.kf.predict()
+        if pred is not None:
+            self.last_center = (pred.cx, pred.cy)
+            self.last_box = to_xyxy(pred)
+            self.age += 1
+
+    def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None:
+        # §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
+        self.kf.update(to_center_size(box))
+        self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0)
+        self.last_box = box
+        self.last_raw_box = box
+        self.confidence = conf
+        self.cls_id = cls_id
+        self.lost_count = 0
+        self.hits += 1
+        self.age += 1
+
+    def mark_miss(self) -> None:
+        # §7 步骤 4：未匹配计丢失
+        self.lost_count += 1
+        self.age += 1

utils/tests/target_tracker/kalman_box_filter_test.py

@@ -0,0 +1,107 @@
+"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。
+
+移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。
+覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛
+且预测超前一个 dt、未初始化 predict 返回 None。
+"""
+
+import math
+
+import pytest
+
+from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
+
+
+def test_uninitialized_predict_returns_none():
+    kf = KalmanBoxFilter()
+    assert not kf.initialized
+    assert kf.predict() is None
+
+    # 未初始化时 update 无副作用
+    kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0))
+    assert not kf.initialized
+    assert kf.predict() is None
+    for v in kf.state:
+        assert v == 0.0
+
+
+def test_initiate_sets_state_correctly():
+    kf = KalmanBoxFilter()
+    kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0))
+    assert kf.initialized
+
+    expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0]
+    state = kf.state
+    for i, want in enumerate(expected):
+        assert state[i] == want, f"index={i}"
+
+    # 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4）
+    pred = kf.predict()
+    assert pred is not None
+    assert pred.cx == pytest.approx(320.0, abs=1e-9)
+    assert pred.cy == pytest.approx(240.0, abs=1e-9)
+    assert pred.w == pytest.approx(40.0, abs=1e-9)
+    assert pred.h == pytest.approx(30.0, abs=1e-9)
+
+
+def test_static_target_converges_to_observation():
+    kf = KalmanBoxFilter()
+    z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0)
+    kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0))  # 带初始偏差，验证收敛能力
+
+    for _ in range(50):
+        kf.predict()
+        kf.update(z)
+
+    state = kf.state
+    assert state[0] == pytest.approx(z.cx, abs=0.5)   # cx
+    assert state[1] == pytest.approx(z.cy, abs=0.5)   # cy
+    assert state[2] == pytest.approx(z.w, abs=0.5)    # w
+    assert state[3] == pytest.approx(z.h, abs=0.5)    # h
+    # 静止目标速度应收敛到约 0
+    assert state[4] == pytest.approx(0.0, abs=0.1)
+    assert state[5] == pytest.approx(0.0, abs=0.1)
+
+
+def test_constant_velocity_converges_and_predict_leads_one_dt():
+    kf = KalmanBoxFilter()
+    vx = 3.0   # 像素/帧
+    vy = 2.0   # 像素/帧
+
+    # 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
+    z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0)
+    kf.initiate(z)
+    for _ in range(60):
+        kf.predict()
+        z.cx += vx
+        z.cy += vy
+        kf.update(z)
+
+    state = kf.state
+    # 速度收敛到真实速度（像素/帧）
+    assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛"
+    assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛"
+    # 位置贴合当前观测
+    assert state[0] == pytest.approx(z.cx, abs=1.5)
+    assert state[1] == pytest.approx(z.cy, abs=1.5)
+
+    # 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
+    pred = kf.predict()
+    assert pred is not None
+    assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt"
+    assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt"
+
+
+def test_small_target_noise_scaling_stable():
+    # 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
+    kf = KalmanBoxFilter()
+    z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0)  # 小目标（4×4 像素）
+    kf.initiate(z)
+    for _ in range(30):
+        kf.predict()
+        kf.update(z)
+    state = kf.state
+    assert state[0] == pytest.approx(z.cx, abs=0.5)
+    assert state[1] == pytest.approx(z.cy, abs=0.5)
+    assert math.isfinite(state[2])
+    assert math.isfinite(state[3])

utils/tests/target_tracker/target_tracker_test.py

@@ -0,0 +1,364 @@
+"""target_tracker_test.py —— TargetTracker 单元测试。
+
+移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。
+逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
+  1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
+  2. ID 严格递增不复用（含 Reset 后归零）
+  3. 锁定不被抢占；锁定目标死亡后切换到新目标
+  4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
+  5. cost=1.0 恰好不构成匹配（严格小于）
+  6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
+另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。
+"""
+
+import math
+
+import pytest
+
+from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker,
+                                           TrackerConfig)
+
+
+def make_det(x1, y1, x2, y2, conf, cls=0):
+    return Detection(x1, y1, x2, y2, conf, cls)
+
+
+# ============================================================================
+# 验收用例 1（§9.4-1）：空检测帧序列
+# ============================================================================
+def test_empty_frames_coast_then_lost():
+    tracker = TargetTracker()  # 默认配置 max_lost_frames = 8
+    assert tracker.get_config().max_lost_frames == 8
+
+    # 第 1 帧：给出一个检测，建立轨道并锁定为主目标
+    r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
+    assert r.tracked
+    assert r.primary_id is not None
+    assert not r.is_predicted      # 当帧有实测
+    assert r.lost_frames == 0
+    assert r.n_active == 1
+    assert r.box is not None
+    assert r.raw is not None       # 当帧关联到原始检测框
+
+    # 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True）
+    for i in range(1, 9):
+        r = tracker.update([])
+        assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢"
+        assert r.is_predicted, f"第 {i} 帧应为纯预测输出"
+        assert r.lost_frames == i
+        assert r.primary_id is not None
+        assert r.box is not None      # Coast 期间继续输出预测框
+        assert r.raw is None          # Coast 时 raw 为空（§2.3）
+        assert r.confidence == pytest.approx(0.9)  # 继承最近实测置信度（§2.3）
+
+    # 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False
+    r = tracker.update([])
+    assert not r.tracked
+    assert r.primary_id is None
+    assert r.box is None
+    assert r.lost_frames == 0  # 丢失时 lost_frames=0（§2.3）
+    assert r.n_active == 0
+
+
+# ============================================================================
+# 验收用例 2（§9.4-2）：ID 单调性
+# ============================================================================
+def test_id_strictly_increasing_no_reuse():
+    cfg = TrackerConfig()
+    cfg.max_lost_frames = 1  # 缩短 Coast 窗口，加速轨道死亡
+    tracker = TargetTracker(cfg)
+
+    # 目标 A 出现 → id 0
+    r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
+    assert r.primary_id is not None
+    id_a = r.primary_id
+    assert id_a == 0
+
+    # 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
+    tracker.update([])
+    r = tracker.update([])
+    assert not r.tracked
+    assert tracker.active_count() == 0
+
+    # 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
+    r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
+    assert r.primary_id is not None
+    id_b = r.primary_id
+    assert id_b == 1
+    assert id_b > id_a
+
+    # 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
+    r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9),
+                        make_det(400.0, 300.0, 430.0, 330.0, 0.8)])
+    assert tracker.active_count() == 2
+    max_id = max(tracker.active_tracks().keys())
+    assert max_id == 2
+
+    # Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
+    tracker.reset()
+    assert tracker.active_count() == 0
+    assert not tracker.last_result().tracked
+    r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)])
+    assert r.primary_id is not None
+    assert r.primary_id == 0
+
+
+# ============================================================================
+# 验收用例 3（§9.4-3）：锁定不被抢占
+# ============================================================================
+def test_lock_not_stolen_and_switch_after_death():
+    cfg = TrackerConfig()
+    cfg.max_lost_frames = 3
+    tracker = TargetTracker(cfg)
+
+    # 低置信度目标 A 建立并锁定
+    r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)])
+    assert r.primary_id is not None
+    id_a = r.primary_id
+
+    # A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
+    r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
+    assert r.primary_id is not None
+    assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占"
+    assert r.is_predicted
+    assert tracker.active_count() == 2  # A（Coast）与 B 并存
+
+    # 继续 Coast，锁定依旧延续
+    r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
+    assert r.primary_id == id_a
+    assert r.is_predicted
+
+    # A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
+    for _ in range(2):
+        r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
+    # A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
+    assert r.tracked
+    assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标"
+    assert not r.is_predicted
+
+    # 后续 B 持续作为主目标
+    r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
+    assert r.primary_id is not None
+    assert r.primary_id != id_a
+
+
+# ============================================================================
+# 验收用例 4（§9.4-4）：min_hits 回退
+# ============================================================================
+def test_min_hits_fallback_allows_new_track():
+    cfg = TrackerConfig()
+    cfg.min_hits = 5  # 抬高确认门槛，保证测试期间轨道始终未确认
+    tracker = TargetTracker(cfg)
+
+    # 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
+    r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
+    assert r.tracked, "无已确认轨道时新轨道应能当选主目标"
+    assert r.primary_id is not None
+    assert r.primary_id == 0
+    assert not r.is_predicted
+
+    # 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
+    for _ in range(3):
+        r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
+        assert r.tracked
+        assert r.primary_id == 0
+
+
+# ============================================================================
+# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配）
+# ============================================================================
+def test_cost_exactly_one_does_not_match():
+    cfg = TrackerConfig()
+    cfg.max_lost_frames = 1
+    cfg.max_association_dist = 200.0
+    cfg.dist_weight = 1.0  # 纯距离项，IoU 项权重为 0 且两框零交集
+    cfg.iou_weight = 0.0
+    tracker = TargetTracker(cfg)
+
+    # 建立轨道：中心 (100,100)
+    r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
+    assert r.primary_id is not None
+    id_a = r.primary_id
+
+    # 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
+    r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)])
+    # 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
+    assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道"
+    # 原轨道（锁定）进入 Coast：lost_frames=1
+    assert r.tracked
+    assert r.is_predicted
+    assert r.primary_id == id_a
+    assert r.lost_frames == 1
+
+    # 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
+    tracker2 = TargetTracker(cfg)
+    r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
+    id_b = r.primary_id
+    r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)])
+    assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道"
+    assert r.primary_id is not None
+    assert r.primary_id == id_b
+    assert not r.is_predicted  # 实测命中
+
+
+# ============================================================================
+# 验收用例 6（§9.4-6）：置信度优先
+# ============================================================================
+def test_higher_confidence_track_wins_detection():
+    tracker = TargetTracker()
+
+    # 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
+    r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5),
+                        make_det(180.0, 100.0, 200.0, 120.0, 0.9)])
+    assert tracker.active_count() == 2
+
+    # 找出两条轨道 ID（按置信度区分）
+    id_low = None
+    id_high = None
+    for track_id, track in tracker.active_tracks().items():
+        if track.confidence > 0.8:
+            id_high = track_id
+        else:
+            id_low = track_id
+    assert id_low is not None
+    assert id_high is not None
+
+    # 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
+    # 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
+    r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)])
+
+    tracks = tracker.active_tracks()
+    assert len(tracks) == 2
+    # 获胜者：命中（lost=0、hits=2、置信度更新）
+    assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜"
+    assert tracks[id_high].hits == 2
+    assert tracks[id_high].confidence == pytest.approx(0.7)
+    # 失败者：未匹配进入 Coast（lost=1、hits 不变）
+    assert tracks[id_low].lost_count == 1, "低置信度轨道应落选"
+    assert tracks[id_low].hits == 1
+    assert tracks[id_low].confidence == pytest.approx(0.5)
+
+
+# ============================================================================
+# NaN/Inf 防御（评审修复项）
+# ============================================================================
+def test_non_finite_detections_are_discarded():
+    tracker = TargetTracker()
+    nan = math.nan
+    inf = math.inf
+
+    # 第 1 帧：建立正常轨道
+    r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
+    assert r.tracked
+
+    # 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
+    # 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
+    r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9),
+                        make_det(100.0, -inf, 140.0, 140.0, 0.9),
+                        make_det(100.0, 100.0, nan, 140.0, 0.9),
+                        make_det(102.0, 102.0, 142.0, 142.0, nan),
+                        make_det(101.0, 101.0, 141.0, 141.0, 0.9)])
+    assert tracker.active_count() == 1, "非有限检测不应新建轨道"
+    assert r.tracked
+    assert not r.is_predicted  # 正常检测命中，实测输出
+    # 主目标输出未被 NaN 污染
+    assert math.isfinite(r.center[0])
+    assert math.isfinite(r.center[1])
+    assert r.box is not None
+    assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1)
+    assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2)
+    assert math.isfinite(r.confidence)
+    assert r.raw is not None  # 实测帧 raw 有值
+
+    # 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
+    r = tracker.update([make_det(nan, nan, nan, nan, nan)])
+    assert r.tracked
+    assert r.is_predicted
+    assert r.lost_frames == 1
+    assert r.raw is None  # Coast 时 raw 为空（§2.3）
+    assert math.isfinite(r.center[0])
+
+    # 持续 NaN 检测直至轨道死亡回收，全程不崩溃
+    for _ in range(10):
+        r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)])
+    assert not r.tracked
+    assert tracker.active_count() == 0
+
+
+# ============================================================================
+# 补充行为测试（§2.3 / §6.3 / §9.5）
+# ============================================================================
+
+def test_lost_result_fields_are_empty():
+    tracker = TargetTracker()
+    r = tracker.update([])  # 空帧序列起点：无任何轨道
+    assert not r.tracked
+    assert r.primary_id is None
+    assert r.box is None
+    assert r.raw is None
+    assert r.lost_frames == 0
+    assert r.n_active == 0
+    assert r.confidence == 0.0
+
+    # LastResult 返回同一快照（§9.5 跨线程读取入口）
+    cached = tracker.last_result()
+    assert not cached.tracked
+
+
+def test_coasting_fallback_chooses_nearest_to_frame_center():
+    # Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
+    # 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。
+    cfg = TrackerConfig()
+    cfg.max_lost_frames = 2
+    tracker = TargetTracker(cfg)
+
+    # F1：角落目标（高置信度）先建立并被锁定
+    tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)])
+    # F2：画面中心附近目标建立；角落目标丢失(1)
+    tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)])
+    # F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
+    tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)])
+    # F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
+    # 其余两条 Coast 存活 → 重新选举选离画面中心最近者
+    r = tracker.update([], FrameShape(480, 640))
+    assert r.tracked
+    assert r.is_predicted
+    assert r.center[0] == pytest.approx(320.0, abs=25.0)   # 靠近画面中心 (320,240)
+    assert r.center[1] == pytest.approx(240.0, abs=25.0)
+    assert r.n_active == 2                                 # 角落轨道已回收
+
+
+def test_locked_primary_kept_while_measured():
+    # 锁定延续优先于置信度选举（§6.3 ①）
+    tracker = TargetTracker()
+    r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)])
+    assert r.primary_id is not None
+    locked_id = r.primary_id
+
+    # 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
+    for i in range(5):
+        r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
+                            make_det(400.0, 300.0, 430.0, 330.0, 0.95)])
+        assert r.primary_id is not None
+        assert r.primary_id == locked_id
+        assert not r.is_predicted
+
+
+def test_default_frame_center_is_320x240():
+    # frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）
+    cfg = TrackerConfig()
+    cfg.max_lost_frames = 2
+    tracker = TargetTracker(cfg)
+
+    # F1：角落目标先建立并被锁定（高置信度）
+    tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)])
+    # F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
+    tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)])
+    # F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
+    tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)])
+    # F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
+    r = tracker.update([])
+    assert r.tracked
+    assert r.is_predicted
+    assert r.center[0] == pytest.approx(280.0, abs=30.0)   # 左侧更近，当选
+    assert r.center[1] == pytest.approx(240.0, abs=30.0)

utils/tests/target_tracker/tracklet_test.py

@@ -0,0 +1,159 @@
+"""tracklet_test.py —— Tracklet 与几何工具单元测试。
+
+移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。
+覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、
+IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。
+"""
+
+import pytest
+
+from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou,
+                                     to_center_size, to_xyxy)
+
+
+# ---- 框表示互转（§7） ----
+
+def test_round_trip_preserves_values():
+    box = BoxXYXY(10.0, 20.0, 50.0, 80.0)
+    c = to_center_size(box)
+    assert c.cx == 30.0   # (10+50)/2
+    assert c.cy == 50.0   # (20+80)/2
+    assert c.w == 40.0    # 50−10
+    assert c.h == 60.0    # 80−20
+
+    back = to_xyxy(c)
+    assert back.x1 == box.x1
+    assert back.y1 == box.y1
+    assert back.x2 == box.x2
+    assert back.y2 == box.y2
+
+
+# ---- IoU 边界（§5.2） ----
+
+def test_identical_boxes_return_one():
+    a = BoxXYXY(10.0, 10.0, 30.0, 30.0)
+    assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12)  # 完全重合 = 1
+
+
+def test_disjoint_boxes_return_zero():
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(20.0, 20.0, 30.0, 30.0)
+    assert compute_iou(a, b) == 0.0  # 零交集
+
+
+def test_touching_edges_return_zero():
+    # 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(10.0, 0.0, 20.0, 10.0)   # 右边缘相接
+    assert compute_iou(a, b) == 0.0
+    c = BoxXYXY(10.0, 10.0, 20.0, 20.0)  # 角点相接
+    assert compute_iou(a, c) == 0.0
+
+
+def test_empty_box_returns_zero():
+    normal = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0)    # 零面积
+    inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0)   # 反向（负宽高）
+    assert compute_iou(normal, zero_area) == 0.0
+    assert compute_iou(zero_area, normal) == 0.0
+    assert compute_iou(normal, inverted) == 0.0
+    assert compute_iou(zero_area, zero_area) == 0.0
+
+
+def test_partial_overlap_is_correct():
+    # A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(5.0, 5.0, 15.0, 15.0)
+    assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12)
+    assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12)  # 对称性
+
+
+# ---- 构造与派生状态（§3.2 / §6.1） ----
+
+def test_construction_initial_fields():
+    t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8)
+
+    assert t.track_id == 7
+    assert t.lost_count == 0
+    assert t.max_lost == 8
+    assert t.confidence == 0.9
+    assert t.cls_id == 1
+    assert t.age == 1   # 创建时为 1（§3.2）
+    assert t.hits == 1  # 创建时为 1（§3.2）
+    assert t.last_center == (100.0, 100.0)
+    assert t.last_box is not None  # 创建当帧即赋 last_box（§7 步骤 5）
+    assert t.last_box.x1 == 90.0
+    assert t.last_box.y1 == 90.0
+    assert t.last_box.x2 == 110.0
+    assert t.last_box.y2 == 110.0
+    assert not t.is_dead
+    assert not t.is_coasting
+
+
+def test_is_dead_uses_strictly_greater():
+    # IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    for _ in range(8):
+        t.mark_miss()
+        assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死"
+    assert t.lost_count == 8
+    t.mark_miss()  # lost_count = 9 > 8
+    assert t.is_dead
+
+
+def test_is_coasting_when_lost_positive():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    assert not t.is_coasting
+    t.mark_miss()
+    assert t.is_coasting  # lost_count > 0 即 Coast（§3.2）
+
+
+# ---- 生命周期方法计数语义（§7） ----
+
+def test_predict_advances_age_and_updates_last_state():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    age_before = t.age
+    t.predict()  # 速度为零：位置不变，但 age+1、last_box 更新为预测框
+    assert t.age == age_before + 1
+    assert t.last_box is not None
+    assert t.last_center[0] == pytest.approx(100.0, abs=1e-9)
+    assert t.last_center[1] == pytest.approx(100.0, abs=1e-9)
+    # hits / lost_count / confidence 不受预测影响
+    assert t.hits == 1
+    assert t.lost_count == 0
+    assert t.confidence == 0.9
+
+
+def test_update_resets_lost_and_increments_hits():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    t.mark_miss()
+    t.mark_miss()
+    assert t.lost_count == 2
+    assert t.is_coasting
+
+    age_before = t.age
+    box = BoxXYXY(150.0, 160.0, 170.0, 180.0)
+    t.update(box, 0.75, 2)
+
+    assert t.lost_count == 0   # 匹配成功清零（§7）
+    assert t.hits == 2         # hits+1
+    assert t.age == age_before + 1  # age+1
+    assert t.confidence == 0.75
+    assert t.cls_id == 2
+    assert t.last_center == (160.0, 170.0)  # ((150+170)/2, (160+180)/2)
+    assert t.last_box is not None
+    assert t.last_box.x1 == box.x1
+    assert t.last_raw_box is not None  # 实测框同步记录
+    assert t.last_raw_box.x2 == box.x2
+    assert not t.is_coasting
+
+
+def test_mark_miss_increments_lost_and_age():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    age_before = t.age
+    t.mark_miss()
+    assert t.lost_count == 1
+    assert t.age == age_before + 1
+    # MarkMiss 不影响 hits / confidence
+    assert t.hits == 1
+    assert t.confidence == 0.9

utils/tests/visual_track_control/frame_transform_test.py

@@ -0,0 +1,43 @@
+"""frame_transform_test.py —— rotate_body_to_ned 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
+覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
+垂直分量不变、模长守恒。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.frame_transform import rotate_body_to_ned
+
+
+def test_zero_yaw_is_identity():
+    north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
+    assert north == pytest.approx(1.0, abs=1e-12)   # 北 = 前
+    assert east == pytest.approx(0.5, abs=1e-12)    # 东 = 右
+    assert down == pytest.approx(-2.0, abs=1e-12)
+
+
+def test_yaw_east_90_maps_forward_to_east():
+    north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0)  # 机头朝东
+    assert north == pytest.approx(0.0, abs=1e-9)  # 北向 ≈ 0
+    assert east == pytest.approx(2.0, abs=1e-9)   # 东向 = 前向速度
+
+
+def test_right_is_east_when_heading_north():
+    north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
+    assert north == pytest.approx(0.0, abs=1e-12)
+    assert east == pytest.approx(1.0, abs=1e-12)
+
+
+def test_down_component_unchanged():
+    _, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
+    assert down == 3.5
+
+
+def test_preserves_horizontal_norm():
+    # 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
+    north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
+    assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
+                                                    abs=1e-9)

utils/tests/visual_track_control/line_of_sight_test.py

@@ -0,0 +1,63 @@
+"""line_of_sight_test.py —— PixelToAngle 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/line_of_sight_test.cpp。
+覆盖（物理追踪思路 §4.1/§5.1，C2）：零偏移、画面边缘偏移 = 半视场角、
+奇对称、单调性、与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.line_of_sight import PixelToAngle
+
+
+def test_zero_offset_gives_zero_angle():
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    assert conv.convert(0.0) == 0.0
+
+
+def test_half_size_offset_gives_half_fov():
+    # 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    assert conv.convert(500.0) == pytest.approx(math.radians(45.0), abs=1e-12)
+    assert conv.convert(-500.0) == pytest.approx(-math.radians(45.0), abs=1e-12)
+
+
+def test_odd_symmetry():
+    conv = PixelToAngle(1920.0, math.radians(60.0))
+    for offset in (1.0, 100.0, 730.5):
+        assert conv.convert(-offset) == pytest.approx(-conv.convert(offset),
+                                                      abs=1e-15)
+
+
+def test_monotonic_increasing():
+    conv = PixelToAngle(1080.0, math.radians(35.0))
+    prev = conv.convert(-600.0)
+    for x in range(-500, 601, 100):
+        cur = conv.convert(float(x))
+        assert cur > prev
+        prev = cur
+
+
+def test_deviates_from_linear_approximation():
+    # C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
+    # 针孔结果必须与之明显偏离（证明未走线性支路）。
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    pinhole_deg = math.degrees(conv.convert(250.0))  # atan(250·2·tan45°/1000)
+    linear_deg = 250.0 * (90.0 / 1000.0)             # θ·offset/size = 22.5°
+    assert pinhole_deg == pytest.approx(math.degrees(math.atan(0.5)), abs=1e-9)
+    assert abs(pinhole_deg - linear_deg) > 2.0
+
+
+def test_constructor_rejects_invalid_args():
+    with pytest.raises(ValueError):
+        PixelToAngle(0.0, 1.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(-10.0, 1.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, 0.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, math.pi)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, math.pi + 0.1)

utils/tests/visual_track_control/mavsdk_gateway_test.py

@@ -0,0 +1,176 @@
+"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。
+
+移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。
+C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射，
+改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、
+A2 掩码三分支的语义等价）：
+
+  角速率非零 → 机体速度设定点 + 非零 yawspeed；
+  航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw；
+  两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）；
+  刹车 → 全零机体速度设定点。
+  传输失败（异常）→ 返回 False 且不向外抛。
+"""
+
+import asyncio
+
+import pytest
+
+from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
+from visual_track_control.types import ControlOutput
+
+
+class FakeOffboard:
+    """捕获式 offboard mock：记录每次调用的设定点，可注入失败。"""
+
+    def __init__(self):
+        self.body_setpoints = []
+        self.ned_setpoints = []
+        self.attempts = 0
+        self.start_count = 0
+        self.stop_count = 0
+        self.succeed = True
+
+    async def set_velocity_body(self, setpoint):
+        self.attempts += 1  # 与 C++ CaptureTransport 一致：失败也计入发送次数
+        if not self.succeed:
+            raise RuntimeError("mock transport failure")
+        self.body_setpoints.append(setpoint)
+
+    async def set_velocity_ned(self, setpoint):
+        self.attempts += 1
+        if not self.succeed:
+            raise RuntimeError("mock transport failure")
+        self.ned_setpoints.append(setpoint)
+
+    async def start(self):
+        if not self.succeed:
+            raise RuntimeError("mock start failure")
+        self.start_count += 1
+
+    async def stop(self):
+        self.stop_count += 1
+
+    @property
+    def send_count(self):
+        return self.attempts
+
+
+class FakeDrone:
+    def __init__(self):
+        self.offboard = FakeOffboard()
+
+
+def run(coro):
+    return asyncio.run(coro)
+
+
+def make_gateway():
+    drone = FakeDrone()
+    return MavsdkCommandGateway(drone), drone.offboard
+
+
+def test_start_sends_initial_setpoint_then_starts():
+    gateway, offboard = make_gateway()
+    assert run(gateway.start())
+    assert offboard.start_count == 1
+    # PX4 要求先流式发送首条设定点（全零）再请求启动
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == 0.0
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == 0.0
+    assert sp.yawspeed_deg_s == 0.0
+
+
+def test_start_failure_returns_false():
+    gateway, offboard = make_gateway()
+    offboard.succeed = False
+    assert not run(gateway.start())
+
+
+def _make_velocity_heading_output():
+    from visual_track_control.types import ControlMode
+    out = ControlOutput()
+    out.mode = ControlMode.VELOCITY_HEADING
+    return out
+
+
+def test_yaw_rate_branch_uses_body_frame_with_yawspeed():
+    from visual_track_control.types import ControlMode
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_body_mps = 1.5
+    out.vz_mps = 0.25
+    out.yaw_rate_dps = 5.0
+    assert run(gateway.send_velocity_heading(out, 12345))
+    assert offboard.send_count == 1
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == pytest.approx(1.5)
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == pytest.approx(0.25)
+    assert sp.yawspeed_deg_s == pytest.approx(5.0)
+    assert offboard.ned_setpoints == []
+
+
+def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw():
+    # 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_mps = 2.0
+    out.vy_mps = -1.0
+    out.vz_mps = 0.5
+    out.yaw_deg = 10.0
+    out.yaw_rate_dps = 0.0
+    assert run(gateway.send_velocity_heading(out, 1))
+    assert offboard.send_count == 1
+    assert len(offboard.ned_setpoints) == 1
+    sp = offboard.ned_setpoints[0]
+    assert sp.north_m_s == pytest.approx(2.0)
+    assert sp.east_m_s == pytest.approx(-1.0)
+    assert sp.down_m_s == pytest.approx(0.5)
+    assert sp.yaw_deg == pytest.approx(10.0)
+    assert offboard.body_setpoints == []
+
+
+def test_both_zero_keeps_heading_via_body_zero_yawspeed():
+    # A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向，
+    # 不得误发绝对航向 0（正北）。
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_body_mps = 1.0
+    out.yaw_rate_dps = 0.0
+    out.yaw_deg = 0.0
+    assert run(gateway.send_velocity_heading(out, 1))
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == pytest.approx(1.0)
+    assert sp.yawspeed_deg_s == 0.0
+    assert offboard.ned_setpoints == []  # 不得走绝对航向支路
+
+
+def test_brake_hover_sends_zero_velocity_body():
+    gateway, offboard = make_gateway()
+    assert run(gateway.send_brake_hover(777))
+    assert offboard.send_count == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == 0.0
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == 0.0
+    assert sp.yawspeed_deg_s == 0.0
+
+
+def test_transport_failure_propagates_as_false():
+    gateway, offboard = make_gateway()
+    offboard.succeed = False
+    out = _make_velocity_heading_output()
+    assert not run(gateway.send_velocity_heading(out, 1))
+    assert not run(gateway.send_brake_hover(1))
+    assert offboard.send_count == 2
+
+
+def test_stop_calls_offboard_stop():
+    gateway, offboard = make_gateway()
+    run(gateway.stop())
+    assert offboard.stop_count == 1

utils/tests/visual_track_control/pid_controller_test.py

@@ -0,0 +1,103 @@
+"""pid_controller_test.py —— PidController 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
+覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
+异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
+"""
+
+import pytest
+
+from visual_track_control.pid_controller import PidController, PidParams
+
+
+def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
+    return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
+                     derivative_filter_coef=filter_coef, output_limit=output_limit)
+
+
+def test_proportional_only():
+    pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(3.0, 0.1) == 6.0
+    assert pid.update(-1.5, 0.1) == -3.0
+
+
+def test_integral_accumulates():
+    pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
+    # 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
+    assert pid.update(2.0, 0.5) == 1.0
+    assert pid.update(2.0, 0.5) == 2.0
+
+
+def test_integral_clamped_to_limit():
+    pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
+    for _ in range(5):
+        pid.update(10.0, 1.0)
+    # 积分候选值被钳位在 1.0 → 输出恒为 1.0
+    assert pid.last_output == 1.0
+
+
+def test_output_clamped_to_limit():
+    pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
+    assert pid.update(1.0, 0.1) == 3.0
+    assert pid.update(-1.0, 0.1) == -3.0
+
+
+def test_anti_windup_freeze_while_saturated_same_direction():
+    # kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
+    # 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(10):
+        assert pid.update(10.0, 1.0) == 5.0
+
+
+def test_anti_windup_unwinds_after_error_flips():
+    # C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
+    # 冻结回卷、输出一直钉在 +5）。
+    # kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
+    # error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
+    # 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(3):
+        assert pid.update(10.0, 1.0) == 5.0
+    assert pid.update(-1.0, 1.0) == -2.0  # 首拍立即回卷
+    out = -2.0
+    for _ in range(11):
+        out = pid.update(-1.0, 1.0)
+    assert out == pytest.approx(-5.0, abs=1e-9)  # 旧判据此处仍为 +5.0
+
+
+def test_derivative_first_step_is_zero():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
+    # 首拍无微分历史：微分项为 0
+    assert pid.update(5.0, 1.0) == 0.0
+
+
+def test_derivative_with_low_pass_filter():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
+    pid.update(0.0, 1.0)  # 建立微分历史（导数 0）
+    # 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
+    assert pid.update(1.0, 1.0) == 0.5
+
+
+def test_zero_or_negative_dt_holds_last_output():
+    pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(2.0, 0.1) == 2.0
+    assert pid.update(99.0, 0.0) == 2.0
+    assert pid.update(99.0, -1.0) == 2.0
+
+
+def test_reset_integral_clears_only_integral():
+    pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(2.0, 1.0)  # 积分 = 2
+    pid.reset_integral()
+    # 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
+    assert pid.update(0.0, 1.0) == -2.0
+
+
+def test_reset_clears_everything():
+    pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(5.0, 1.0)
+    pid.reset()
+    assert pid.last_output == 0.0
+    # 全清后等同全新控制器：首拍无微分历史
+    assert pid.update(3.0, 1.0) == 3.0 + 3.0  # kp·e + ki·∫e

utils/tests/visual_track_control/rate_limiter_test.py

@@ -0,0 +1,68 @@
+"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。
+覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、
+首条指令从零渐变、异常节拍保护、Reset 归零。
+"""
+
+import pytest
+
+from visual_track_control.rate_limiter import SlewRateLimiter, clamp
+
+
+def test_clamp_clamps_value():
+    assert clamp(5.0, -2.0, 2.0) == 2.0
+    assert clamp(-5.0, -2.0, 2.0) == -2.0
+    assert clamp(1.5, -2.0, 2.0) == 1.5
+    # 边界值：落在区间内
+    assert clamp(2.0, -2.0, 2.0) == 2.0
+    assert clamp(-2.0, -2.0, 2.0) == -2.0
+
+
+def test_first_command_ramps_from_zero():
+    # S6：初始输出为 0，首条大指令只能按变化率渐变
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(10.0, 0.5) == 1.0    # 0 + 2×0.5
+    assert lim.limit(10.0, 0.5) == 2.0    # 继续渐变
+    assert lim.limit(10.0, 10.0) == 10.0  # 步长足够 → 到达目标
+
+
+def test_tracks_target_within_step():
+    lim = SlewRateLimiter(10.0)
+    # 步长 10×1=10，目标 3 在步长内 → 直接到达
+    assert lim.limit(3.0, 1.0) == 3.0
+    assert lim.limit(3.0, 1.0) == 3.0  # 已在目标：保持
+
+
+def test_negative_direction():
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(-10.0, 0.5) == -1.0
+    assert lim.limit(-10.0, 0.5) == -2.0
+
+
+def test_zero_or_negative_dt_holds():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)  # 输出 2.0
+    assert lim.limit(10.0, 0.0) == 2.0
+    assert lim.limit(10.0, -1.0) == 2.0
+
+
+def test_zero_rate_limit_freezes_output():
+    lim = SlewRateLimiter(0.0)
+    assert lim.limit(5.0, 1.0) == 0.0
+
+
+def test_reset_to_value():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)
+    lim.reset()
+    assert lim.last_output == 0.0
+    lim.reset(3.5)
+    assert lim.last_output == 3.5
+    # 复位后从新值继续渐变
+    assert lim.limit(10.0, 1.0) == 5.5  # 3.5 + 2×1
+
+
+def test_custom_initial_value():
+    lim = SlewRateLimiter(1.0, initial=5.0)
+    assert lim.limit(10.0, 1.0) == 6.0

utils/tests/visual_track_control/staleness_checker_test.py

@@ -0,0 +1,32 @@
+"""staleness_checker_test.py —— is_stale 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/staleness_checker_test.cpp。
+覆盖（物理追踪思路 §7.1）：从未取样判超龄、有效期内新鲜、超出有效期超龄、
+边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
+"""
+
+from visual_track_control.staleness_checker import is_stale
+
+
+def test_never_sampled_is_stale():
+    assert is_stale(0, 1000, 200)
+    assert is_stale(-5, 1000, 200)
+
+
+def test_within_validity_is_fresh():
+    assert not is_stale(900, 1000, 200)
+    assert not is_stale(1000, 1000, 200)  # 刚取样
+
+
+def test_beyond_validity_is_stale():
+    assert is_stale(799, 1000, 200)  # 201 > 200
+
+
+def test_exactly_at_boundary_is_fresh():
+    # 超龄判定为严格大于：恰好等于有效期仍算新鲜
+    assert not is_stale(800, 1000, 200)
+
+
+def test_inconsistent_timestamp_is_stale():
+    # C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
+    assert is_stale(2000, 1000, 200)

utils/tests/visual_track_control/tracking_control_law_test.py

@@ -0,0 +1,338 @@
+"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。
+
+移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
+覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
+超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
+通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
+无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.config import (HeadingMode, NoDistanceAction,
+                                         TrackingConfig)
+from visual_track_control.tracking_control_law import TrackingControlLaw
+from visual_track_control.types import (ControlMode, ControlSnapshot,
+                                        DegradedReason, TrackState)
+
+
+def make_config() -> TrackingConfig:
+    """测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
+    画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
+    偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
+    """
+    cfg = TrackingConfig()
+    cfg.control.frequency_hz = 20.0
+    cfg.control.visual_stale_ms = 200
+    cfg.control.radar_stale_ms = 500
+    cfg.control.attitude_stale_ms = 300
+    cfg.camera.image_width = 1000
+    cfg.camera.image_height = 1000
+    cfg.camera.fov_h_deg = 90.0
+    cfg.camera.fov_v_deg = 90.0
+    cfg.heading.mode = HeadingMode.RATE
+    cfg.heading.gain = 1.0
+    cfg.heading.yaw_rate_limit_dps = 30.0
+    cfg.heading.yaw_slew_limit_dps2 = 100000.0
+    cfg.vertical.vz_gain_mps_per_deg = 0.1
+    cfg.vertical.vz_limit_mps = 2.0
+    cfg.distance.d_exp_m = 10.0
+    cfg.distance.kp = 0.5
+    cfg.distance.ki = 0.0
+    cfg.distance.kd = 0.0
+    cfg.distance.integral_limit = 0.0
+    cfg.distance.derivative_filter_coef = 0.0
+    cfg.distance.approach_velocity_limit_mps = 3.0
+    cfg.distance.retreat_velocity_limit_mps = 1.0
+    cfg.distance.no_distance_action = NoDistanceAction.HOLD
+    cfg.distance.no_distance_approach_limit_mps = 0.5
+    cfg.accel_limit.ax_mps2 = 1000.0
+    cfg.accel_limit.ay_mps2 = 1000.0
+    cfg.accel_limit.az_mps2 = 1000.0
+    return cfg
+
+
+def make_snapshot(now_ms: int) -> ControlSnapshot:
+    """新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
+    return ControlSnapshot(
+        track=TrackState(tracked=True, is_predicted=False,
+                         center=(500.0, 500.0)),
+        track_time_ms=now_ms,
+        attitude_present=True,
+        attitude_time_ms=now_ms,
+    )
+
+
+# ---- 超龄降级（§7.1 / §9.2） ----
+
+def test_attitude_missing_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_present = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+    assert not out.valid
+
+
+def test_attitude_stale_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_time_ms = 600  # 400 ms 前 > 300 ms 阈值
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+
+
+def test_vision_stale_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track_time_ms = 700  # 300 ms 前 > 200 ms 阈值
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.VISION_STALE
+
+
+def test_never_sampled_track_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track_time_ms = 0  # 从未取样
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.VISION_STALE
+
+
+def test_attitude_stale_beats_vision_stale():
+    # 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_present = False
+    snap.track_time_ms = 0
+    out = law.update(snap, 1000)
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+
+
+# ---- 判丢刹车（§6.4） ----
+
+def test_lost_target_brakes():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.tracked = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+    assert out.valid
+    assert out.vx_mps == 0.0
+    assert out.vy_mps == 0.0
+    assert out.vz_mps == 0.0
+
+
+# ---- Coast（§6.4） ----
+
+def test_coast_sets_flag_and_keeps_tracking():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.is_predicted = True
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.coast_active
+    assert out.valid
+
+
+# ---- 水平航向通道（§4.2，A2） ----
+
+def test_rate_mode_maps_beta_to_yaw_rate():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)  # 目标偏右 100 像素
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))  # ≈ 11.31°
+    assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6)  # gain=1，右转正
+    assert out.yaw_deg == 0.0
+
+
+def test_rate_mode_clamps_to_yaw_rate_limit():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (950.0, 500.0)  # 大偏移：β≈43.5° > 30°/s 上限
+    out = law.update(snap, 1000)
+    assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9)
+
+
+def test_position_mode_composes_absolute_heading():
+    # A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
+    cfg = make_config()
+    cfg.heading.mode = HeadingMode.POSITION
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)
+    snap.yaw_rad = math.radians(45.0)
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))
+    assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6)
+    assert out.yaw_rate_dps == 0.0
+
+
+def test_position_mode_wraps_around_180():
+    cfg = make_config()
+    cfg.heading.mode = HeadingMode.POSITION
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)
+    snap.yaw_rad = math.radians(175.0)
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))
+    # 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
+    assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6)
+    assert -180.0 < out.yaw_deg < 180.0
+
+
+# ---- 垂直统一速度支路（§5，A1） ----
+
+def test_vertical_offset_drives_vz():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (500.0, 600.0)  # 目标偏低 100 像素 → α>0 → 下降
+    out = law.update(snap, 1000)
+    alpha_deg = math.degrees(math.atan(0.2))
+    assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6)
+    assert out.mode is ControlMode.VELOCITY_HEADING  # A1：不切姿态微调模式
+
+
+def test_vertical_velocity_clamped():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (500.0, 990.0)  # α≈44.5° × 0.1 = 4.45 > 2 上限
+    out = law.update(snap, 1000)
+    assert out.vz_mps == pytest.approx(2.0, abs=1e-9)
+
+
+# ---- 距离通道（§6.2 / §6.3） ----
+
+def test_distance_approach_when_far():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0  # Δd = +20 → kp 输出 10 → 限幅 3
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
+
+
+def test_distance_retreat_clamped_when_too_close():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 5.0  # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
+
+
+def test_no_distance_hold_zeros_vx():
+    law = TrackingControlLaw(make_config())  # 缺省 kHold
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_mps == 0.0
+
+
+def test_no_distance_slow_approach():
+    cfg = make_config()
+    cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
+
+
+def test_no_distance_exit_degrades():
+    cfg = make_config()
+    cfg.distance.no_distance_action = NoDistanceAction.EXIT
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
+    assert not out.valid
+
+
+def test_stale_radar_falls_back_to_no_distance():
+    law = TrackingControlLaw(make_config())  # kHold
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 400  # 600 ms 前 > 500 ms 阈值 → 距离不可用
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_mps == 0.0
+
+
+# ---- 首拍/限幅（§8、S6） ----
+
+def test_first_tick_vx_slews_from_zero():
+    # S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
+    cfg = make_config()
+    cfg.accel_limit.ax_mps2 = 2.0
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0  # 通道输出本应为 3.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)  # 首拍：dt 按 1/20 Hz 兜底
+    assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9)  # 0.1
+
+
+# ---- 机体系 → NED 旋转（§2.3，A4） ----
+
+def test_output_rotated_to_ned_by_yaw():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.yaw_rad = math.pi / 2.0  # 机头朝东
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    # 机体前向 3.0 → 东向 3.0，北向 ≈ 0
+    assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
+    assert out.vy_mps == pytest.approx(3.0, abs=1e-9)
+
+
+def test_zero_yaw_keeps_forward_as_north():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.yaw_rad = 0.0
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
+    assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
+
+
+# ---- Reset（§6.4） ----
+
+def test_reset_restores_slew_from_zero():
+    cfg = make_config()
+    cfg.accel_limit.ax_mps2 = 2.0
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    for t in range(1000, 3000, 50):
+        snap.track_time_ms = t
+        snap.radar_time_ms = t
+        snap.attitude_time_ms = t
+        law.update(snap, t)  # 若干拍后 vx 已爬升到限幅 3.0
+    law.reset()
+    snap.track_time_ms = 3000
+    snap.radar_time_ms = 3000
+    snap.attitude_time_ms = 3000
+    out = law.update(snap, 3000)
+    # Reset 后限幅器归零：又从 0.1 开始渐变
+    assert out.vx_mps == pytest.approx(0.1, abs=1e-9)

utils/tests/visual_track_control/vtc_config_test.py

@@ -0,0 +1,171 @@
+"""vtc_config_test.py —— TrackingConfig::load_from_json 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/vtc_config_test.cpp。
+覆盖（物理追踪思路 §8、需求分析 NFR-007）：真实配置文件加载成功与取值
+正确（含 A1/A2 修改后的字段结构）、缺字段 / 缺分组 / 非法枚举 / 非法数值 /
+文件缺失均抛带字段名的 ValueError。
+
+C++ 版用字符串替换构造非法变体；Python 版改为 dict 变异（更稳健），
+临时文件经 pytest 的 tmp_path 提供。
+"""
+
+import json
+from pathlib import Path
+
+import pytest
+
+from visual_track_control.config import (HeadingMode, NoDistanceAction,
+                                         load_from_json)
+
+_REPO_ROOT = Path(__file__).resolve().parents[3]
+REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
+
+
+def full_config() -> dict:
+    """与真实配置同构的完整合法配置（用于派生各非法变体）。"""
+    return {
+        "control": {
+            "frequency_hz": 20.0,
+            "visual_stale_ms": 200,
+            "radar_stale_ms": 500,
+            "attitude_stale_ms": 300,
+        },
+        "camera": {
+            "image_width": 1920,
+            "image_height": 1080,
+            "fov_h_deg": 60.0,
+            "fov_v_deg": 35.0,
+        },
+        "heading": {
+            "mode": "rate",
+            "gain": 1.0,
+            "yaw_rate_limit_dps": 30.0,
+            "yaw_slew_limit_dps2": 60.0,
+        },
+        "vertical": {
+            "vz_gain_mps_per_deg": 0.1,
+            "vz_limit_mps": 2.0,
+        },
+        "distance": {
+            "d_exp_m": 10.0,
+            "kp": 0.5,
+            "ki": 0.05,
+            "kd": 0.1,
+            "integral_limit": 2.0,
+            "derivative_filter_coef": 0.2,
+            "approach_velocity_limit_mps": 3.0,
+            "retreat_velocity_limit_mps": 1.0,
+            "no_distance_action": "hold",
+            "no_distance_approach_limit_mps": 0.5,
+        },
+        "accel_limit": {
+            "ax_mps2": 2.0,
+            "ay_mps2": 2.0,
+            "az_mps2": 1.5,
+        },
+    }
+
+
+def write_temp_json(tmp_path: Path, config: dict, name: str) -> str:
+    path = tmp_path / name
+    path.write_text(json.dumps(config), encoding="utf-8")
+    return str(path)
+
+
+def test_load_real_config_succeeds():
+    cfg = load_from_json(str(REAL_CONFIG))
+    assert cfg.control.frequency_hz == 20.0
+    assert cfg.control.visual_stale_ms == 200
+    assert cfg.camera.image_width == 1920
+    assert cfg.camera.image_height == 1080
+    assert cfg.camera.fov_h_deg == 60.0
+    assert cfg.heading.mode is HeadingMode.RATE  # A2：默认速率式
+    assert cfg.heading.yaw_rate_limit_dps == 30.0
+    assert cfg.vertical.vz_gain_mps_per_deg == 0.1
+    assert cfg.vertical.vz_limit_mps == 2.0
+    assert cfg.distance.d_exp_m == 10.0
+    assert cfg.distance.no_distance_action is NoDistanceAction.HOLD
+    assert cfg.accel_limit.ax_mps2 == 2.0
+
+
+def test_full_json_round_trip_matches_expected(tmp_path):
+    # 内嵌合法配置与真实配置文件行为一致
+    path = write_temp_json(tmp_path, full_config(), "vtc_cfg_full.json")
+    cfg = load_from_json(path)
+    assert cfg.heading.mode is HeadingMode.RATE
+    assert cfg.distance.kp == 0.5
+
+
+def test_position_mode_accepted(tmp_path):
+    config = full_config()
+    config["heading"]["mode"] = "position"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_position.json")
+    cfg = load_from_json(path)
+    assert cfg.heading.mode is HeadingMode.POSITION
+
+
+def test_missing_file_throws_with_path():
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json("/nonexistent/vtc_no_such_file.json")
+    assert "vtc_no_such_file.json" in str(exc_info.value)
+
+
+def test_malformed_json_throws(tmp_path):
+    path = tmp_path / "vtc_cfg_bad.json"
+    path.write_text("{ this is not json", encoding="utf-8")
+    with pytest.raises(ValueError):
+        load_from_json(str(path))
+
+
+def test_missing_field_throws_with_field_name(tmp_path):
+    config = full_config()
+    del config["heading"]["gain"]
+    path = write_temp_json(tmp_path, config, "vtc_cfg_missing.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "heading.gain" in str(exc_info.value)
+
+
+def test_missing_section_throws(tmp_path):
+    config = full_config()
+    del config["accel_limit"]
+    path = write_temp_json(tmp_path, config, "vtc_cfg_nosection.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "accel_limit" in str(exc_info.value)
+
+
+def test_invalid_heading_mode_throws(tmp_path):
+    config = full_config()
+    config["heading"]["mode"] = "magic"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badmode.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "heading.mode" in str(exc_info.value)
+
+
+def test_invalid_no_distance_action_throws(tmp_path):
+    config = full_config()
+    config["distance"]["no_distance_action"] = "zoom"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badaction.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "no_distance_action" in str(exc_info.value)
+
+
+def test_negative_frequency_throws(tmp_path):
+    config = full_config()
+    config["control"]["frequency_hz"] = -1.0
+    path = write_temp_json(tmp_path, config, "vtc_cfg_negfreq.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "control.frequency_hz" in str(exc_info.value)
+
+
+def test_fov_out_of_range_throws(tmp_path):
+    config = full_config()
+    config["camera"]["fov_h_deg"] = 200.0
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badfov.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "camera.fov_h_deg" in str(exc_info.value)

utils/visual_track_control/README.md

@@ -0,0 +1,41 @@
+# visual_track_control（Python 版）
+
+视觉跟踪控制律：像素视线角 → 水平航向 / 垂直速度 / 距离 PID 三通道 + 统一限幅 +
+状态机（正常 / Coast / 判丢刹车 / 降级保持）+ MAVSDK offboard 指令输出。
+移植自 `reference-c++/visual_track_control/`（规格：docs/物理追踪思路.md）。
+
+## 接口清单
+
+| 名称 | 说明 |
+| --- | --- |
+| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
+| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） |
+| `TrackingControlLaw(cfg)` | 主控律 |
+| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
+| `.reset()` | 停止闭环时清全部内部状态 |
+| `ControlSnapshot` | 输入快照（自带 `TrackState`，track_time_ms 由调用方打戳） |
+| `ControlOutput` | 输出：mode / valid / vx·vy·vz（NED）/ vx_body_mps（机体系前向）/ yaw_deg / yaw_rate_dps / degraded_reason / coast_active |
+| `PidController` / `PidParams` | 距离 PID（anti-windup 条件积分） |
+| `SlewRateLimiter` / `clamp` | 变化率限幅（初值 0 → 首条指令渐变，S6） |
+| `PixelToAngle` / `is_stale` / `rotate_body_to_ned` | 针孔视线角 / 超龄判定 / 机体→NED 旋转 |
+| `CommandSender`（Protocol） | 指令输出抽象（供注入 mock） |
+| `MavsdkCommandGateway(drone)` | MAVSDK offboard 网关（async：start / send_velocity_heading / send_brake_hover / stop） |
+
+## 与 C++ 版的差异
+
+- **MAVLink → MAVSDK 语义映射**（替代 C++ 的 ID 84 手工编码）：
+  - 角速率非零 → `set_velocity_body(forward, 0, down, yawspeed)`；
+  - 航向角非零（角速率为零）→ `set_velocity_ned(north, east, down, yaw)`；
+  - 两者均零 → `set_velocity_body` 配零角速率（保持航向，A2 回归语义）；
+  - 刹车 → `set_velocity_body(0,0,0,0)`（MAVSDK 无刹车掩码的行为等效适配）；
+  - 降级保持不发送（由调用方判断模式）。
+- **异步**：MAVSDK Python 为 asyncio 接口，网关方法均为协程；传输异常捕获后返回
+  `False`（与 C++ 传输失败传播一致）。
+- **解耦**：C++ 的 ControlSnapshot 内嵌 `target_tracker::TrackResult` 是唯一耦合点；
+  Python 版自带字段对齐的 `TrackState`（box/raw 为 plain tuple），两包互不 import，
+  转换在胶水层（`utils/demo_track_control.py` 的 `track_result_to_state`）。
+- **新增字段** `ControlOutput.vx_body_mps`：旋转前的机体系前向速度，供速率式
+  机体设定点直接使用（免反旋）。
+- **日志**：spdlog → 标准 `logging`，仅状态切换点打印（热路径零日志）不变。
+- 行为语义（姿态超龄优先于视觉、判丢进刹车清限幅器、Coast 清距离积分、
+  dt 兜底 1/frequency_hz 上限 0.5s、WrapDeg180 回绕）与 C++ 一致，由移植测试锁定。

utils/visual_track_control/__init__.py

@@ -0,0 +1,55 @@
+"""视觉跟踪控制包（visual_track_control Python 版）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/。本包与 target_tracker
+包互不依赖（TrackState 为字段对齐的独立值类型，胶水层负责转换）。
+
+分层：
+  纯算法构件   rate_limiter / staleness_checker / line_of_sight /
+               frame_transform / pid_controller
+  配置         config（TrackingConfig + load_from_json）
+  主控律       tracking_control_law（TrackingControlLaw）
+  指令输出     command_sender（抽象 Protocol）+ mavsdk_gateway（MAVSDK offboard）
+"""
+
+from .command_sender import CommandSender
+from .config import (AccelLimitConfig, CameraGroupConfig, ControlGroupConfig,
+                     DistanceGroupConfig, HeadingGroupConfig, HeadingMode,
+                     NoDistanceAction, TrackingConfig, VerticalGroupConfig,
+                     load_from_json)
+from .frame_transform import rotate_body_to_ned
+from .line_of_sight import PixelToAngle
+from .mavsdk_gateway import MavsdkCommandGateway
+from .pid_controller import PidController, PidParams
+from .rate_limiter import SlewRateLimiter, clamp
+from .staleness_checker import is_stale
+from .tracking_control_law import TrackingControlLaw
+from .types import (ControlMode, ControlOutput, ControlSnapshot, DegradedReason,
+                    TrackState)
+
+__all__ = [
+    "CommandSender",
+    "MavsdkCommandGateway",
+    "TrackingControlLaw",
+    "TrackingConfig",
+    "load_from_json",
+    "HeadingMode",
+    "NoDistanceAction",
+    "ControlGroupConfig",
+    "CameraGroupConfig",
+    "HeadingGroupConfig",
+    "VerticalGroupConfig",
+    "DistanceGroupConfig",
+    "AccelLimitConfig",
+    "ControlMode",
+    "DegradedReason",
+    "TrackState",
+    "ControlSnapshot",
+    "ControlOutput",
+    "PidController",
+    "PidParams",
+    "SlewRateLimiter",
+    "clamp",
+    "is_stale",
+    "PixelToAngle",
+    "rotate_body_to_ned",
+]

utils/visual_track_control/command_sender.py

@@ -0,0 +1,32 @@
+"""控制指令发送抽象（语义接口，对应 C++ ICommandTransport + 网关门面）。
+
+C++ 版的 ICommandTransport 是字节流传输抽象（Send(bytes)），MAVLink 编码
+在网关内完成；Python 版采用 MAVSDK 语义映射后，"传输"被抽象为设定点级
+指令发送。本 Protocol 仅声明门面接口，供胶水层依赖注入与 mock 测试。
+
+异常约定：实现不得向外抛传输层异常，失败一律返回 False（网关已捕获）。
+"""
+
+from typing import Optional, Protocol, runtime_checkable
+
+from .types import ControlOutput
+
+
+@runtime_checkable
+class CommandSender(Protocol):
+    async def start(self) -> bool:
+        """进入 offboard 模式。失败返回 False。"""
+        ...
+
+    async def send_velocity_heading(self, output: ControlOutput,
+                                    time_boot_ms: Optional[int] = None) -> bool:
+        """发送速度+航向意图（ControlMode.VELOCITY_HEADING）。失败返回 False。"""
+        ...
+
+    async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
+        """发送零速刹车/悬停意图（ControlMode.BRAKE_HOVER）。失败返回 False。"""
+        ...
+
+    async def stop(self) -> None:
+        """退出 offboard 模式（结束时调用）。"""
+        ...

utils/visual_track_control/config.py

@@ -0,0 +1,278 @@
+"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp
+（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。
+
+启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整
+字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。
+"""
+
+import json
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Any
+
+from .types import ControlMode  # noqa: F401  (re-export 便利性)
+
+
+class HeadingMode(Enum):
+    """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。"""
+
+    RATE = "rate"          # β → 偏航角速度，输出 yaw_rate_dps
+    POSITION = "position"  # β → 修正量，与当前航向合成绝对航向输出 yaw_deg
+
+
+class NoDistanceAction(Enum):
+    """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
+
+    HOLD = "hold"                  # 保持：前向速度指令清零
+    SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
+    EXIT = "exit"                  # 退出近距操作：输出降级保持
+
+
+@dataclass
+class ControlGroupConfig:
+    frequency_hz: float = 20.0
+    visual_stale_ms: int = 200
+    radar_stale_ms: int = 500
+    attitude_stale_ms: int = 300
+
+
+@dataclass
+class CameraGroupConfig:
+    image_width: int = 1920
+    image_height: int = 1080
+    fov_h_deg: float = 60.0
+    fov_v_deg: float = 35.0
+
+
+@dataclass
+class HeadingGroupConfig:
+    mode: HeadingMode = HeadingMode.RATE
+    gain: float = 1.0
+    yaw_rate_limit_dps: float = 30.0
+    yaw_slew_limit_dps2: float = 60.0
+
+
+@dataclass
+class VerticalGroupConfig:
+    vz_gain_mps_per_deg: float = 0.1
+    vz_limit_mps: float = 2.0
+
+
+@dataclass
+class DistanceGroupConfig:
+    d_exp_m: float = 10.0
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+    integral_limit: float = 0.0
+    derivative_filter_coef: float = 0.0
+    approach_velocity_limit_mps: float = 3.0
+    retreat_velocity_limit_mps: float = 1.0
+    no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
+    no_distance_approach_limit_mps: float = 0.5
+
+
+@dataclass
+class AccelLimitConfig:
+    ax_mps2: float = 2.0
+    ay_mps2: float = 2.0
+    az_mps2: float = 1.5
+
+
+@dataclass
+class TrackingConfig:
+    control: ControlGroupConfig = field(default_factory=ControlGroupConfig)
+    camera: CameraGroupConfig = field(default_factory=CameraGroupConfig)
+    heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig)
+    vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig)
+    distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig)
+    accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig)
+
+
+# ---------------------------------------------------------------------------
+# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致）
+# ---------------------------------------------------------------------------
+
+def _require_section(root: dict, section: str) -> dict:
+    value = root.get(section)
+    if not isinstance(value, dict):
+        raise ValueError(f"配置缺失分组或类型错误: {section}")
+    return value
+
+
+def _is_number(value: Any) -> bool:
+    return isinstance(value, (int, float)) and not isinstance(value, bool)
+
+
+def _is_integer(value: Any) -> bool:
+    return isinstance(value, int) and not isinstance(value, bool)
+
+
+def _require_number(section: dict, section_name: str, key: str) -> float:
+    value = section.get(key)
+    if not _is_number(value):
+        raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}")
+    return float(value)
+
+
+def _require_integer(section: dict, section_name: str, key: str) -> int:
+    value = section.get(key)
+    if not _is_integer(value):
+        raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}")
+    return int(value)
+
+
+def _require_string(section: dict, section_name: str, key: str) -> str:
+    value = section.get(key)
+    if not isinstance(value, str):
+        raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}")
+    return value
+
+
+def _require_positive(value: float, field_name: str) -> float:
+    if not (value > 0.0):
+        raise ValueError(f"配置非法(必须为正): {field_name}")
+    return value
+
+
+def _require_non_negative(value: float, field_name: str) -> float:
+    if value < 0.0:
+        raise ValueError(f"配置非法(不得为负): {field_name}")
+    return value
+
+
+def _require_range(value: float, lo: float, hi: float, field_name: str) -> float:
+    if not (lo < value < hi):
+        raise ValueError(f"配置非法(超出范围): {field_name}")
+    return value
+
+
+def _load_control(root: dict) -> ControlGroupConfig:
+    s = _require_section(root, "control")
+    c = ControlGroupConfig()
+    c.frequency_hz = _require_positive(
+        _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
+    c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
+    c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
+    c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
+    if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
+        raise ValueError("配置非法: control.*_stale_ms 必须为正")
+    return c
+
+
+def _load_camera(root: dict) -> CameraGroupConfig:
+    s = _require_section(root, "camera")
+    c = CameraGroupConfig()
+    c.image_width = int(_require_positive(
+        float(_require_integer(s, "camera", "image_width")), "camera.image_width"))
+    c.image_height = int(_require_positive(
+        float(_require_integer(s, "camera", "image_height")), "camera.image_height"))
+    c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"),
+                                 0.0, 180.0, "camera.fov_h_deg")
+    c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"),
+                                 0.0, 180.0, "camera.fov_v_deg")
+    return c
+
+
+def _load_heading(root: dict) -> HeadingGroupConfig:
+    s = _require_section(root, "heading")
+    h = HeadingGroupConfig()
+    mode = _require_string(s, "heading", "mode")
+    if mode == "position":
+        h.mode = HeadingMode.POSITION
+    elif mode == "rate":
+        h.mode = HeadingMode.RATE
+    else:
+        raise ValueError("配置非法: heading.mode 仅支持 position / rate")
+    h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain")
+    h.yaw_rate_limit_dps = _require_positive(
+        _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps")
+    h.yaw_slew_limit_dps2 = _require_positive(
+        _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2")
+    return h
+
+
+def _load_vertical(root: dict) -> VerticalGroupConfig:
+    s = _require_section(root, "vertical")
+    v = VerticalGroupConfig()
+    v.vz_gain_mps_per_deg = _require_non_negative(
+        _require_number(s, "vertical", "vz_gain_mps_per_deg"),
+        "vertical.vz_gain_mps_per_deg")
+    v.vz_limit_mps = _require_positive(
+        _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps")
+    return v
+
+
+def _load_distance(root: dict) -> DistanceGroupConfig:
+    s = _require_section(root, "distance")
+    d = DistanceGroupConfig()
+    d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"),
+                                  "distance.d_exp_m")
+    d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp")
+    d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki")
+    d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd")
+    d.integral_limit = _require_non_negative(
+        _require_number(s, "distance", "integral_limit"), "distance.integral_limit")
+    # 容许 -0.0：范围下界略低于 0，读入后归一化为 0
+    d.derivative_filter_coef = _require_range(
+        _require_number(s, "distance", "derivative_filter_coef"),
+        -0.000001, 1.0, "distance.derivative_filter_coef")
+    if d.derivative_filter_coef < 0.0:
+        d.derivative_filter_coef = 0.0
+    d.approach_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "approach_velocity_limit_mps"),
+        "distance.approach_velocity_limit_mps")
+    d.retreat_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "retreat_velocity_limit_mps"),
+        "distance.retreat_velocity_limit_mps")
+    action = _require_string(s, "distance", "no_distance_action")
+    if action == "hold":
+        d.no_distance_action = NoDistanceAction.HOLD
+    elif action == "slow_approach":
+        d.no_distance_action = NoDistanceAction.SLOW_APPROACH
+    elif action == "exit":
+        d.no_distance_action = NoDistanceAction.EXIT
+    else:
+        raise ValueError(
+            "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit")
+    d.no_distance_approach_limit_mps = _require_non_negative(
+        _require_number(s, "distance", "no_distance_approach_limit_mps"),
+        "distance.no_distance_approach_limit_mps")
+    return d
+
+
+def _load_accel_limit(root: dict) -> AccelLimitConfig:
+    s = _require_section(root, "accel_limit")
+    a = AccelLimitConfig()
+    a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"),
+                                  "accel_limit.ax_mps2")
+    a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"),
+                                  "accel_limit.ay_mps2")
+    a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"),
+                                  "accel_limit.az_mps2")
+    return a
+
+
+def load_from_json(path: str) -> TrackingConfig:
+    """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。"""
+    try:
+        with open(path, "r", encoding="utf-8") as fp:
+            root = json.load(fp)
+    except OSError:
+        raise ValueError(f"配置文件打开失败: {path}") from None
+    except json.JSONDecodeError as exc:
+        raise ValueError(f"配置文件解析失败: {path} ({exc})") from None
+
+    if not isinstance(root, dict):
+        raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")
+
+    cfg = TrackingConfig()
+    cfg.control = _load_control(root)
+    cfg.camera = _load_camera(root)
+    cfg.heading = _load_heading(root)
+    cfg.vertical = _load_vertical(root)
+    cfg.distance = _load_distance(root)
+    cfg.accel_limit = _load_accel_limit(root)
+    return cfg

utils/visual_track_control/frame_transform.py

@@ -0,0 +1,25 @@
+"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
+（规格：docs/物理追踪思路.md §2.3）。
+
+速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶
+小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机
+体系完成、旋转在输出端执行"不破坏 §8 限幅约束。
+
+正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
+  机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东；
+  机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。
+"""
+
+import math
+
+
+def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float,
+                       yaw_rad: float) -> tuple[float, float, float]:
+    """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。"""
+    c = math.cos(yaw_rad)
+    s = math.sin(yaw_rad)
+    north = v_forward * c - v_right * s
+    east = v_forward * s + v_right * c
+    return north, east, v_down