Conversation
L3 估计层重构完成的快照：ekf_tracker/reprojection_error 拆分为 ekf_ + target_estimator + tracker，PnP 求解器与观测噪声调整， 新增 sp_auto_aim 离线回放数据和 auto_aim_test harness。 未跟踪的 include/l3_estimation/eskf.hpp 是外部 eststack 库的孤儿 文件，缺依赖且未接入构建，故意不纳入本次提交。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
新增 L3Estimation::IteratedKalmanFilter (ieskf.hpp/cpp)，继承 ExtendedKalmanFilter，增加一个带 Gauss-Newton 迭代的 update 重载。观测模型 经 xyz2ypd 后非线性很强，单次线性化在先验点的 Jacobian 失真明显。 相对 SHtech_auto_aim 的 IESEKF.hpp 修正了迭代式末项符号：正确形式是减去 H_i (x_pri ⊟ x_i)，参考实现写成加号，从第二次迭代起收敛到有偏的不动点。 NIS/NEES 统计抽成 ExtendedKalmanFilter::recordConsistency，两条更新路径共用， 且固定取先验线性化点的创新量——迭代后的残差不再服从卡方分布，否则 Tracker::badRecentNis 的门限会失配。 回放实测发现只上迭代会让复位从 14 次增至 20 次：迭代更紧地拟合观测，把 r1 推成负数。根因是半径先验过松 (P0 = 1.0 m²，物理范围仅 0.05~0.5 m) 且过程噪声 为零。因此 TrackedTarget 的 x_add 增加半径投影，diverged() 相应从"越界"改判 "连续贴边 10 次"，保留发散保护。 demo.avi 620 帧：tracking 444 → 467 (+5.2%)，复位 14 → 9 (−36%)。迭代在第 2 次 即收敛，tracker 耗时 +4% (0.289 → 0.300 ms)。 TrackerConfig::ekf_max_iterations 默认 5，设为 1 时与改动前逐位等价； auto_aim_test 新增 --ekf-iterations 便于同一段回放做 A/B。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
TrackerConfig::ekf_max_iterations 默认从 5 改为 1，runtime 主干路恢复成单次 线性化的普通 EKF，与引入迭代前逐位一致。IteratedKalmanFilter 及其测试全部保留， 需要时调配置或用 auto_aim_test --ekf-iterations 开启。 迭代在回放上是净收益，但只在一段离线数据上验证过，样本量不足以支撑改默认； 实车验收前不作为默认路径。 半径投影和 diverged() 的贴边判据不受这个开关影响，保持默认生效——它们是独立 于迭代的修正，单独相对改动前基线就有收益 (tracking 444 -> 462，复位 14 -> 10)。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
TrackedTarget 改回持有 ExtendedKalmanFilter，update_ypda 在先验点算好矩阵 H 后 调用基类的五参数 update()。主干路完全不经过 ieskf.cpp——符号表可验证， target_estimator.cpp.o 只引用基类的构造、predict、update 三个符号。 同时移除随之失效的 TrackerConfig::ekf_max_iterations / ekf_step_threshold 和 auto_aim_test 的 --ekf-iterations：留着会是静默无效的开关，比没有更糟。接回迭代 的两处改动写在 docs/iterated_ekf.md。 IteratedKalmanFilter、ieskf_smoke 和文档全部保留。ieskf_smoke 不依赖 TrackedTarget， 直接测滤波器本身，主干路不接也能守住这份代码。 半径投影和 diverged() 的贴边判据与迭代无关，保持生效。回放结果与改动前的 IESKF@iter=1 完全一致 (tracking 462, lost 15, resets 10)，13 个免硬件冒烟测试全过。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
L4 的本质是解不动点：命中时刻取决于飞行时间，飞行时间又取决于命中点位置。 Planner::plan() 以 fly_time = 0 起步反复代入，直到相邻两次之差小于 fly_time_tolerance，未收敛按 BallisticFailed 拒绝。结构对标 sp_vision 的 Aimer，选板内聚为私有方法而不单独立类。 Predictor 修了一个实质缺陷：原实现只有 position += velocity * dt，没有推进 yaw。装甲板位置由 (中心, yaw, 半径) 共同决定，小陀螺目标因此被算成原地不动 ——v_yaw 取 10 rad/s 时 100 ms 延迟对应 57 度偏差。为此 TargetState 新增 armor_num / second_radius / height_diff，让 L4 自行展开装甲板并外推，保持 "跨层只暴露不可变快照"的约定，Planner 也因此可脱离硬件单测。 BallisticSolver 实现真空闭式解，取飞行时间短的低弧；构造函数已接受 drag_coefficient，k = 0 时严格退化，后续升级线性空气阻力不必改接口。 选板修正了 sp 的一个真 bug：aimer.cpp 的档位判据写的是 ekf_x()[8]，那是半径 而不是 v_yaw（索引 7）。半径恒小于阈值 2，导致它的反陀螺分支对非前哨站目标 是死代码。同一函数后面用 [7] 判旋转方向，说明 [8] 是笔误。 与 sp 的另一处刻意差异：plan_time 由调用方传入，不在内部取 steady_clock::now()。 sp 的 Aimer::aim 在 to_now 分支直接读当前时钟，同一段回放跑两次结果不同； newvision 有离线回放 harness，plan() 必须是纯函数。 IPlanner 接口留给 MPC 和五次多项式，两者均未实现。PlanInput 用结构体承载输入， 已预留云台角速度边界条件；PlanConfig 预留角加速度上限和 trajectoryLimitsReady()。 Plan 里 yaw_vel/pitch_vel/yaw_acc/pitch_acc 本就是给轨迹类规划器的，定点实现保持 0。 延迟仍按五段拆分，未标定段保持 0 并由 fireDelayReady() 拦住开火。 planner_smoke 9 个用例全过；L3 回放结果不变 (462/15/10)。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Per-armor fixed point (talos aim_generic): the fly-time iteration now runs once per armor plate with the plate held fixed, and selection happens over the converged candidates. Selecting inside the loop -- what sp_vision and Climber_Vision do -- can oscillate near a window boundary, exactly when a solution matters most. Aim/fire split (talos ControlIntent): Plan::valid now means "the gimbal should follow this angle" and Plan::fire_admissible means "firing is allowed". Previously the coming/leaving anti-spin window was a selection criterion, so the gimbal stopped tracking during the normal firing gaps of a fast-spinning target. It is now a fire criterion, which is where it belongs. Aim phase ladder (talos ArmorAimPhase): AimPhaseTracker replaces the single spin_threshold with SingleArmor / WholeCarArmor / WholeCarCenter, gated by Schmitt thresholds plus an N-frame confirmation count. talos's WholeCarPair rung is collapsed -- its predict_aim_point and select_armor_id only branch on WholeCarCenter, so Pair aims identically to WholeCarArmor. The center phase aims at the point on the rotation circle nearest the muzzle; the fire gate still judges a physical plate. Observability gate (Climber Target::jumped, talos target_jumped): with only one plate ever observed, vehicle yaw, r2-r1 and z2-z1 are barely observed, so aiming at another plate uses fabricated geometry. TrackedTarget gains a sticky multi_armor_observed alongside the existing per-frame jumped flag, exported to TargetState; the planner pins the aim to plate 0 until it is set. Write-only within L3 -- replay Tracking frames stay at 462. Ballistic model/solver split (talos core/trajectory): IBallisticModel does the forward map, BallisticSolver the inverse. LinearDragModel applies drag horizontally only, which keeps the forward solution closed-form and avoids the Ceres dependency Climber_Vision's AirResistTrajectory needs. k=0 degrades bit-for-bit to vacuum. Bullet speed threshold lives only in PlanConfig::min_valid_bullet_speed (21.0, matching the value edited into the solver). Two thresholds that disagree make speeds in the gap report BallisticFailed instead of BadBulletSpeed, hiding the real cause. planner_smoke: 9 -> 15 cases, all passing. All 14 hardware-free smoke tests pass; auto_aim_test replay unchanged at 462 Tracking frames. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
这是一次快照式提交，把当前分支上已有的在制品一次性存档，作为接下来 改 PnpSolver::optimize_yaw 之前的还原点。 包含： - L2 灯条精修与筛选（armor_refiner 及其 smoke / 视频回放测试）； ArmorDetector 里的调用当前是注释掉的，保持原样入库。 - OpenVINO 后端的 LATENCY / 线程数 / 大小核调度属性，按设备过滤， GPU 插件收到 CPU 专有项会抛异常而不是忽略。 - L3/L4/L5 在制品：pnp_solver、tracker、ballistic_solver、planner、 fire_decision 等。 - tests/track_diag.cpp：整车跟踪链路的离线诊断，逐帧输出识别、PnP、 EKF 内部量和开环预测误差三份 CSV。 - config/carmera_config.yaml 改名为 config/camera_config.yaml。 - records/（3 GB 回放录像）和 kernel.errors.txt 加入 .gitignore。 用 track_diag 在 records/3m_run_mid 上量到的现状，留档备查： optimize_yaw 的三分搜索把 19.9% 的观测推到错误的重投影代价盆地， 偏移约 100 度；由此导致 66% 的双板帧把两块板关联到同一个物理面， EKF 半径从 0.200 被压到中位数 0.138 并 34 帧顶在 0.05 下界。 Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
平面四点 PnP 有二义性，重投影代价在整周有两个极小值，实测间隔 100 度以上。
原来的三分搜索假设代价在 [-pi/2, pi/2] 上单峰，这个前提不成立：第二个坑经常
整个落在这个 180 度宽的窗口里，搜索随初值掉进任意一个。在 records/3m_run_mid
上量到 19.9% 的观测落进错误的坑，yaw 偏移约 100 度，重投影代价放大 13~35 倍。
后果不止于 PnP。装甲面关联代价里板 yaw 占一半权重，yaw 错 100 度之后 66% 的
双板帧把两块可见板关联到同一个物理面，同一个面被两组互斥观测各更新一次，EKF
只能靠压半径调和，r1 从 0.200 掉到中位数 0.138 并 34 帧顶在 0.05 下界触发
diverged()。NIS 拦不住是因为 armor_yaw_variance 手调成 sigma=17.8 度，比实测
大一个数量级。
改动：
- 代价函数从"四角点距离之和"改成八维像素残差平方和。前者在残差为零处不可导，
高斯牛顿的正规方程在那里没有定义。
- 粗扫覆盖整周 36 格而不是以枪管 yaw 为中心开窗。开窗要求"真值一定在窗内"，
这个前提被云台大角度转动或外参偏差破坏时是静默错误。
- 粗扫后用三点抛物线插值把初值压到亚度级，再跑带 LM 阻尼的高斯牛顿，
Jacobian 用中心差分（只有一个待优化参数，两次重投影就够）。
- 新增 project_armor_points：不分配堆内存的重投影核心，reproject_armor 和
yaw 搜索共用。常规标定（零斜切、4 或 5 个畸变系数）走展开的 Brown-Conrady，
其余回退 cv::projectPoints。pnp_solver_smoke 里新增整周 52 个角度的一致性
对比，容差按 Point2f 的 float 台阶取 1e-4 像素。
- 收敛点的曲率即 Fisher 信息，据此给出 Armor::yaw_sigma
（sigma_yaw^2 = corner_noise_px^2 / J^T J）。实测正对装甲板 sigma 约 4 度、
侧对约 0.9 度，这个结构是固定方差表达不出来的。L3 的 EKF 暂未消费该字段，
接过去需要先标定 ArmorConfig::corner_noise_px。
- 恢复 single_pnp 里丢失的 quality 标志位和 reprojection_error。此前它们从头
到尾没被赋值，quality.valid() 恒为 false，实机默认的 require_quality=true
会让 Tracker 一帧都起不来；pnp_solver_smoke 的 21 个断言和 tracker_smoke 的
11 个断言就是被这个打挂的，现在全部恢复通过。
- auto_aim_test 的代价曲线同步改成平方和并画整周，否则图上的最小点不是求解器
真正在找的那个。
records/3m_run_mid 实测（4937 个观测）：
改前 改后
落错盆地 19.9% 0.99%
两板关联到同一面 710 帧 0 帧
r1 中位数（真值 0.2） 0.138 0.206
r1 顶在 0.05 下界 34 帧 0 帧
跟踪重置 3 次 2 次（余下两次是 20 帧连续漏检）
通过 ArmorQuality 门限 0 4937
NIS 均值 0.486 0.109
方位角单步创新 1.657 度 0.191 度
100ms 开环预测误差 0.0955 m 0.0629 m
optimize_yaw 耗时 65.4 us 5.02 us
records/3m_low 回归：0 次重置，r1 中位数 0.2026，两板同面 0 帧，
100ms 预测误差 0.0456 m。
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Controller::makeCommand 此前是 `SerialCommand{plan.yaw, plan.pitch, plan.valid}`，
把开火位直接接到了"规划是否成功"上。这绕开 FireConfig::shoot_enable、命中角度
判据和全部 RejectReason——只要规划出解就会发 shoot=1，验收前的安全闸门等于不
存在。现在改成从 FireDecision::shoot 取。
同时把返回类型改成 optional：规划无效时返回 nullopt 而不是一条填零的命令。
yaw = pitch = 0 在 world 系里是一个具体方向，下位机会当正常指令去跟，丢目标时
云台会猛甩。不发、让下位机保持原状才是安全行为。
Controller 和 shouldFire 此前全仓库无调用点，改动不影响既有行为。
auto_aim_test 现在跑完整的 Tracker -> Planner -> FireDecider -> Controller：
- 合成 RobotState（弹速由 --bullet-speed 给，姿态取录像四元数），plan_time 用
本帧实测的检测+跟踪耗时，而不是曝光时刻——填曝光时刻等于白送一段延迟补偿。
- FireInput::actual_yaw/pitch 取录像里的云台实际指向，不是上一帧命令值。
- armor_switching 和 command_jump 由相邻帧的 Plan 推出，门限见 --command-jump。
- 新增 --fire-limits：填一组占位火控参数，让 fire_feasible 的时序可观测。默认
关闭，即实机未标定时的行为（parametersReady() 恒假）。这组值不是标定结果，
不要搬进实机配置；shoot_enable 始终 false。
- 画面叠加瞄准点十字与 fire_armor_point，加 CMD / FIRE 两行文本（含拒绝原因）。
- PlotJuggler 新增 cmd_yaw/cmd_pitch、cmd_yaw_error/cmd_pitch_error、
plan_armor_id、fire_armor_id、aim_phase、fire_feasible、tol_yaw/tol_pitch、
armor_switching、command_jump 等。cmd_yaw 与 gimbal_yaw 同为 world 系绝对角，
可直接相减。
- 结束时打印拒绝原因直方图。fire_feasible 为 0 时，唯一有用的信息是被哪条拦住。
代价曲线同步：yawCost 早先已改成平方和以匹配求解器，本次不再改动。
records/3m_run_mid 全程 3977 帧实测：
L4 规划成功 3967，实际下发命令 3967，命令 yaw 跳变 >10 度 0 次
L4 选板切换 129 次，而 TargetState::armor_id 是 159 次（含 30 次来回抖动）
——Planner 的 switch_hysteresis 正好消掉了那 30 次伪切换
outside_hit_window 1351 帧（34%），是小陀螺的正常间歇
aim_error 全部 3977 帧：回放不是闭环，录像里的云台是操作手手动打的，
命令与录像云台的 yaw 偏差中位数 9.55 度。这条在实机上才有意义。
另给 TargetState::armor_id 补上注释，说明它等价于 sp_vision 标了 "debug only"
的 last_id，不能用来选板或画曲线，该看的是 Plan::armor_id。
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
No description provided.