feat: 能量机关 tracker/aimer + 共享不定长 EKF 抽取

app/auto_aim_async/src/rbt_threads.rs

@@ -1197,7 +1197,14 @@ pub fn energy_mechanism_estimate_process(
 ) -> JoinHandle<()> {
     tokio::spawn(async move {
         let mut ticker = tokio::time::interval(Duration::from_millis(2));
-        let mut tracker = EnergyMechanismTracker::new(EnergyMechanismMode::Small);
+        let tracker_cfg = GENERIC_RBT_CFG
+            .read()
+            .unwrap()
+            .energy_mechanism_cfg
+            .tracker
+            .clone();
+        let mut tracker =
+            EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Small, &tracker_cfg);
         let mut snapshot_seq = 0_u64;
         let mut last_transition_seq = runtime_router.state().transition_seq;
         loop {
@@ -1321,7 +1328,10 @@ pub fn control_loop_250hz(
                 return;
             }
         };
-        let mut energy_mechanism_control = EnergyMechanismController::new();
+        let mut energy_mechanism_control = EnergyMechanismController::from_aimer_cfg(
+            &cfg.energy_mechanism_cfg.aimer,
+            &cfg.energy_mechanism_cfg.mpc,
+        );
         let mut latest_snapshot: Option<PlannerTrackSnapshot> = None;
         let mut latest_energy_snapshot: Option<EnergyMechanismTrackPacket> = None;
         let mut latest_feedback: Option<(SensData, Instant)> = None;

cfg/rbt_cfg.toml

@@ -41,6 +41,42 @@ engine_path = "./model/engine_mechanism"
 confidence_threshold = 0.8
 nms_iou_threshold = 0.4
 
+# 能量机关 tracker / aimer / mpc 配置。检测参数继续放在上面的 detector_cfg.energy_mechanism。
+[energy_mechanism_cfg.tracker]
+lost_timeout_s = 0.35
+big_lost_timeout_s = 0.08
+big_model_reset_timeout_s = 0.35
+big_curve_ekf_fit_enabled = true
+big_phase_process_noise = 0.02
+big_a_process_noise = 1e-6
+big_w_process_noise = 3e-6
+big_measurement_noise_scale = 4.0
+big_speed_measurement_enabled = true
+big_speed_measurement_noise = 1.50
+big_speed_measurement_gate = 1.2
+big_curve_speed_slew_limit = 3.0
+big_speed_measurement_window_samples = 16
+big_speed_measurement_window_s = 0.30
+big_speed_measurement_min_history = 20
+big_curve_phi_correction_limit = 0.0
+big_phi_seed_frames = 15
+
+[energy_mechanism_cfg.aimer]
+predict_time_s = 0.0
+fire_gap_s = 0.2
+yaw_offset_deg = 0.0
+pitch_offset_deg = 0.0
+pitch_velocity_lead_time_s = 0.0
+snapshot_stale_ms = 180.0
+
+[energy_mechanism_cfg.mpc]
+model_dt_s = 0.004
+horizon = 50
+track_q = 3198.0
+rate_q = 0.0
+command_q = 1000.0
+delta_r = 48343.0
+
 [cam_cfg]
 cam_k = [1600.0, 0.0, 320.0, 0.0, 1705.7, 192.0, 0.0, 0.0, 1.0]
 

docs/vivsionn-gap-checklist.md

@@ -2,10 +2,23 @@
 
 This checklist tracks the functional gaps found while comparing this Rust workspace with `/Users/flamingo/Projects/robomaster/vivsionn`.
 
-| feature | target repository | current repository | notes |
+| feature | 目标仓库 | 当前仓库 | 备注 |
 |---|---|---|---|
-| [x] P0 true CAN TX/RX loop | `Serial + SocketCAN` reads `0x203/0x204` feedback and sends `0x100` control frames | SocketCAN runtime task opens `can0`, pairs feedback frames, feeds `SensData`, and sends serialized `CtrlData` | Implemented in `rbt_comm_device` and wired into `auto_aim_async` |
-| [x] P0 armor pitch ballistic control | Armor fire control computes gravity-compensated pitch and sends it with yaw | Armor route now computes ballistic pitch from planner target position and sends it with yaw | Implemented in armor fire-control controller |
-| [x] P0 YPD geometry recovery | Tracker recovers after armor jump/mismatch by gating windows and inflating geometry covariance | Rust tracker now opens a recovery window after multi-armor observations and inflates `dr/h` covariance after consecutive geometry mismatches | Implemented in YPD tracker with configurable thresholds |
-| [x] P0 outpost specialization | Outpost path has height phase lock, radius prior, and outpost yaw recovery | Rust tracker now converts outpost observed/radial yaw, locks height phase, freezes locked height offsets, applies radius prior, and gates rejected updates | Implemented in YPD tracker with outpost-specific tests |
-| [x] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust solve stage now corrects inconsistent R center geometry and tracker defers/rebinds large-buff target switches with phase state | Implemented minimal R-center correction and switch gate without adding image ROI/template dependencies |
+| [x] P0 真 CAN 收发闭环 | `Serial + SocketCAN` 真读 `0x203/0x204`、真发 `0x100` | SocketCAN runtime task 打开 `can0`，配对 feedback 帧，生产 `SensData`，并发送序列化后的 `CtrlData` | 已补齐主线闭环，协议格式仍由 `rbt_comm_frame.rs` 锁定 |
+| [ ] P0 相机/视频源 | 海康相机、模式切曝光、离线 `.avi + .csv` 回放 | 固定 ffmpeg 读离线视频 | 必补。不上这个很难常驻上车 |
+| [ ] P0 常驻机器人入口 | `supervisor + YoloDetect()` 常驻运行 | 视频结束进程退出 | 实车稳定性缺口 |
+| [x] P0 装甲板 pitch 弹道控制 | 发控会算重力补偿并下发 pitch | 装甲板路线已根据 planner 目标位置计算弹道 pitch，并随 yaw 一起下发 | 已补齐装甲板 3D 发控输出 |
+| [x] P0 YPD geometry recovery | 有 armor jump 后几何恢复、协方差膨胀 | tracker 在多装甲板观测后打开 recovery window，并在连续几何 mismatch 后膨胀 `dr/h` covariance | 已补齐跳板、错配、重获相关基础恢复逻辑 |
+| [x] P0 前哨站特化 | outpost 高度相位锁定、半径先验、yaw 恢复 | tracker 已做 outpost observed/radial yaw 转换、高度相位锁定、锁定高度冻结、半径先验和 rejected update 门控 | 已补齐前哨站专用 tracker 路径 |
+| [x] P0 能量机关 R 圆心/切换门控 | `Buff_Detector` 有 R 圆心修正、模板/轮廓 fallback、锁定门控 | solve stage 已修正不一致 R 圆心几何，tracker 已对大符目标切换做 defer/rebind phase gate | 本轮补了 R 圆心和切换门控；模板/轮廓 fallback 仍未纳入 |
+| [x] P0 能量机关 tracker/aimer | 相位 EKF、大小符曲线模型、相位化预瞄、pitch lead | 大符曲线 EKF（基于共享不定长 EKF）+ 两轮飞行时间迭代 + yaw preview horizon + pitch lead + 配置化偏置 | 大符走 `BigBuffCurveEskf` 曲线预测（`speed=a·sin(phase)+base-a`），小符保留常速；aimer 两轮弹道迭代、yaw MPC horizon 由 tracker 预瞄生成 |
+| [ ] P1 主线热更新调参 | `param.yaml` 每秒 reload，曝光/发控/MPC 可调 | 只有实验入口，主线没接 watcher | 上车调参效率会差。本轮不做热更新，配置仅启动加载 |
+| [x] P1 配置面补齐 | 大量曝光、门控、MPC、buff 参数 | `rbt_cfg.toml` 主要是 detector/cam/estimator | 新增顶层 `energy_mechanism_cfg`（tracker/aimer/mpc），补齐大符曲线 EKF 全部 knob，serde 默认值保证旧配置兼容 |
+| [ ] P1 PnP 稳态保护 | 角点细化、位姿 sanity gate | 直接网络角点 + IPPE | 建议补，降低跳点。本轮延期 |
+| [ ] P1 离线录制/回放 | 可录 `.avi + .csv`，强制 task mode 回放 | 缺主链路复盘工具 | 调现场问题很关键 |
+| [ ] P1 通信/MPC smoke 工具 | `testSerial`、`can_mpc_yaw_test` 工具链完整 | `comm_test` 基本空 | 接 CAN 后应尽快补 |
+| [ ] P2 显示/HUD/录制旁路 | MJPEG/Rerun/HUD/CSV/plot 脚本多 | 有 Rerun 和日志，但观测面较薄 | 影响调试效率 |
+| [ ] P2 TRT/ROI/CUDA 性能路径 | TensorRT/CUDA 预处理更贴 Jetson | ORT + ONNX EP | 不是功能缺口，先看实测延迟 |
+| [x] 已基本对齐：模式路由 | `AutoShot`/`Outpost`/`Buff` 路由切换 | `RuntimeRouter`/`ModeContext` 已有 | 这块方向对 |
+| [x] 已基本对齐：yaw 发控主链 | yaw planner、二阶 MPC、shot phase | Rust 已迁主干 | 主要剩验证/调参 |
+| [x] 已基本对齐：CAN 协议格式 | `0x100`/`0x203`/`0x204` 协议 | `rbt_comm_frame.rs` 有实现和单测 | 协议定义不是短板 |

lib/src/rbt_base/rbt_algorithm.rs

@@ -1,3 +1,4 @@
+pub mod rbt_ekf;
 pub mod rbt_eskf;
 // 几何模块
 pub mod rbt_antigravity;

lib/src/rbt_base/rbt_algorithm/rbt_ekf.rs

@@ -0,0 +1,220 @@
+//! 动态大小扩展卡尔曼滤波器 (EKF)。
+//!
+//! 这是一个通用的、不定长 (dynamic-size) EKF 滤波核，直接维护名义状态 `x` 和协方差 `P`，
+//! 提供 predict / update 标准步骤。
+//!
+//! 设计目标：让 armor (`YpdAngleTracker`) 和能量机关大符 (`BigBuffCurveEskf`) 复用同一份
+//! 滤波数学。业务模块负责构造 `F`/`Q`/`H`/`R` 以及残差与非线性传播函数；本模块只承担
+//! `x = F·x`、`P = F·P·Fᵀ + Q`、Joseph 形式协方差更新和 NIS 门控这些通用步骤。
+//!
+//! 与同目录 `rbt_eskf.rs` 的关系：那是误差状态 (ESKF) 形式，目前是占位代码；本模块是直接形式
+//! EKF，与 vivsionn `BuffTracker` / `TongjiTracker` 的实现风格一致。两者并存，互不依赖。
+
+/// 动态大小直接形式 EKF。
+#[derive(Debug, Clone)]
+pub struct ExtendedKalmanFilter {
+    /// 名义状态 (nominal state)。
+    pub x: na::DVector<f64>,
+    /// 状态协方差矩阵。
+    pub p: na::DMatrix<f64>,
+    initialized: bool,
+}
+
+impl ExtendedKalmanFilter {
+    /// 构造一个未初始化、维度为 `n` 的滤波器，状态与协方差归零。
+    pub fn new(n: usize) -> Self {
+        Self {
+            x: na::DVector::zeros(n),
+            p: na::DMatrix::zeros(n, n),
+            initialized: false,
+        }
+    }
+
+    /// 用初始状态与协方差初始化（或重置）滤波器。
+    pub fn init(&mut self, x0: na::DVector<f64>, p0: na::DMatrix<f64>) {
+        self.x = x0;
+        self.p = p0;
+        self.initialized = true;
+    }
+
+    /// 用初始状态与协方差构造并初始化滤波器。
+    pub fn with_initial(x0: na::DVector<f64>, p0: na::DMatrix<f64>) -> Self {
+        Self {
+            x: x0,
+            p: p0,
+            initialized: true,
+        }
+    }
+
+    /// 是否已初始化。
+    pub fn initialized(&self) -> bool {
+        self.initialized
+    }
+
+    /// 线性 predict：`x = F·x`，`P = F·P·Fᵀ + Q`。
+    pub fn predict(&mut self, f: &na::DMatrix<f64>, q: &na::DMatrix<f64>) {
+        self.x = f * &self.x;
+        self.p = symmetrize(&(f * &self.p * f.transpose() + q));
+    }
+
+    /// 非线性名义传播 predict：先调用 `f` 把名义状态非线性推进一步，再用线性化的 `F` 传播协方差。
+    /// `f` 接收当前 `x` 并返回推进后的 `x`；`F` 是该步对应的状态转移雅可比矩阵。
+    pub fn predict_nonlinear(
+        &mut self,
+        f: &na::DMatrix<f64>,
+        q: &na::DMatrix<f64>,
+        state_step: impl Fn(&na::DVector<f64>) -> na::DVector<f64>,
+    ) {
+        self.x = state_step(&self.x);
+        self.p = symmetrize(&(f * &self.p * f.transpose() + q));
+    }
+
+    /// 标准 EKF 更新。返回 `(是否接受, 残差 NIS)`。
+    ///
+    /// - `z`: 测量向量
+    /// - `h`: 当前线性化点处的测量雅可比
+    /// - `r`: 测量噪声协方差
+    /// - `z_pred`: 当前状态下预测的测量 `h(x)`
+    /// - `residual_fn`: 计算残差 `z - z_pred`，用于处理角度等需要归一化的分量
+    ///
+    /// `S` 奇异时返回 `(false, +inf)` 且不修改状态。协方差采用 Joseph 形式
+    /// `P = (I−KH)·P·(I−KH)ᵀ + K·R·Kᵀ` 以保证对称正定。
+    pub fn update(
+        &mut self,
+        z: &na::DVector<f64>,
+        h: &na::DMatrix<f64>,
+        r: &na::DMatrix<f64>,
+        z_pred: &na::DVector<f64>,
+        residual_fn: impl Fn(&na::DVector<f64>, &na::DVector<f64>) -> na::DVector<f64>,
+    ) -> (bool, f64) {
+        let residual = residual_fn(z, z_pred);
+        let s = h * &self.p * h.transpose() + r;
+        let Some(s_inv) = s.clone().try_inverse() else {
+            return (false, f64::INFINITY);
+        };
+        let nis = (residual.transpose() * &s_inv * &residual)[(0, 0)];
+
+        let k = &self.p * h.transpose() * &s_inv;
+        let i = na::DMatrix::<f64>::identity(self.p.nrows(), self.p.ncols());
+        self.x += &k * &residual;
+        let i_kh = &i - &k * h;
+        self.p = symmetrize(&(&i_kh * &self.p * i_kh.transpose() + &k * r * k.transpose()));
+        (true, nis)
+    }
+
+    /// 仅计算更新会产生的 NIS，但不修改状态。用于门控判断。
+    /// `S` 奇异时返回 `+inf`。
+    pub fn nis(
+        &self,
+        z: &na::DVector<f64>,
+        h: &na::DMatrix<f64>,
+        r: &na::DMatrix<f64>,
+        z_pred: &na::DVector<f64>,
+        residual_fn: impl Fn(&na::DVector<f64>, &na::DVector<f64>) -> na::DVector<f64>,
+    ) -> f64 {
+        let residual = residual_fn(z, z_pred);
+        let s = h * &self.p * h.transpose() + r;
+        let Some(s_inv) = s.try_inverse() else {
+            return f64::INFINITY;
+        };
+        (residual.transpose() * s_inv * &residual)[(0, 0)]
+    }
+}
+
+/// 对称化协方差矩阵：`(M + Mᵀ) / 2`。
+fn symmetrize(matrix: &na::DMatrix<f64>) -> na::DMatrix<f64> {
+    (matrix + matrix.transpose()) * 0.5
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    /// 1D 常速模型：状态 `[pos, vel]`，恒定速度观测，滤波器应收敛到真实速度与位置。
+    #[test]
+    fn constant_velocity_model_converges() {
+        let mut ekf = ExtendedKalmanFilter::with_initial(
+            na::DVector::from_vec(vec![0.0, 0.0]),
+            na::DMatrix::identity(2, 2) * 10.0,
+        );
+
+        let f = na::DMatrix::from_row_slice(2, 2, &[1.0, 0.1, 0.0, 1.0]);
+        let q = na::DMatrix::identity(2, 2) * 1e-3;
+        // 只观测位置
+        let h = na::DMatrix::from_row_slice(1, 2, &[1.0, 0.0]);
+        let r = na::DMatrix::from_row_slice(1, 1, &[0.1]);
+
+        let true_vel = 2.0;
+        for step in 0..40 {
+            ekf.predict(&f, &q);
+            let true_pos = (step as f64 + 1.0) * 0.1 * true_vel;
+            let z = na::DVector::from_vec(vec![true_pos]);
+            let z_pred = na::DVector::from_vec(vec![ekf.x[0]]);
+            ekf.update(&z, &h, &r, &z_pred, |a, b| a - b);
+        }
+
+        assert!(
+            (ekf.x[0] - 40.0 * 0.1 * true_vel).abs() < 1.0,
+            "pos converged"
+        );
+        assert!((ekf.x[1] - true_vel).abs() < 0.5, "vel converged");
+    }
+
+    /// `S` 奇异（零测量噪声且 `H` 行退化）时 update 不修改状态并返回拒绝。
+    #[test]
+    fn update_rejects_when_s_is_singular() {
+        let mut ekf = ExtendedKalmanFilter::with_initial(
+            na::DVector::from_vec(vec![1.0]),
+            na::DMatrix::identity(1, 1),
+        );
+        // H = 0 行导致 S = 0，奇异不可逆
+        let h = na::DMatrix::from_row_slice(1, 1, &[0.0]);
+        let r = na::DMatrix::from_row_slice(1, 1, &[0.0]);
+        let z = na::DVector::from_vec(vec![5.0]);
+        let z_pred = na::DVector::from_vec(vec![1.0]);
+
+        let x_before = ekf.x[0];
+        let (accepted, nis) = ekf.update(&z, &h, &r, &z_pred, |a, _b| a.clone());
+        assert!(!accepted);
+        assert!(nis.is_infinite());
+        assert_eq!(ekf.x[0], x_before);
+    }
+
+    /// Joseph 形式协方差更新后 P 保持对称。
+    #[test]
+    fn covariance_stays_symmetric_after_update() {
+        let mut ekf = ExtendedKalmanFilter::with_initial(
+            na::DVector::from_vec(vec![0.0, 0.0]),
+            na::DMatrix::identity(2, 2) * 5.0,
+        );
+        let h = na::DMatrix::from_row_slice(1, 2, &[1.0, 0.0]);
+        let r = na::DMatrix::from_row_slice(1, 1, &[0.5]);
+        let z = na::DVector::from_vec(vec![1.0]);
+        let z_pred = na::DVector::from_vec(vec![0.0]);
+        ekf.update(&z, &h, &r, &z_pred, |a, b| a - b);
+
+        let diff = &ekf.p - ekf.p.transpose();
+        let max_asym = diff.abs().max();
+        assert!(max_asym < 1e-12, "P symmetric, max asym = {max_asym}");
+    }
+
+    /// NIS 门控：离群测量产生远大于正常测量的 NIS，可用 `nis()` 预判再决定是否 update。
+    #[test]
+    fn nis_distinguishes_inlier_from_outlier() {
+        let ekf = ExtendedKalmanFilter::with_initial(
+            na::DVector::from_vec(vec![0.0]),
+            na::DMatrix::identity(1, 1),
+        );
+        let h = na::DMatrix::from_row_slice(1, 1, &[1.0]);
+        let r = na::DMatrix::from_row_slice(1, 1, &[0.1]);
+        let z_pred = na::DVector::from_vec(vec![0.0]);
+
+        let inlier = na::DVector::from_vec(vec![0.05]);
+        let outlier = na::DVector::from_vec(vec![50.0]);
+        let nis_in = ekf.nis(&inlier, &h, &r, &z_pred, |a, b| a - b);
+        let nis_out = ekf.nis(&outlier, &h, &r, &z_pred, |a, b| a - b);
+
+        assert!(nis_in < 1.0, "inlier nis small: {nis_in}");
+        assert!(nis_out > 100.0, "outlier nis large: {nis_out}");
+    }
+}