diff --git a/app/auto_aim_async/src/rbt_threads.rs b/app/auto_aim_async/src/rbt_threads.rs index f08ff6a..a51b215 100644 --- a/app/auto_aim_async/src/rbt_threads.rs +++ b/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 = None; let mut latest_energy_snapshot: Option = None; let mut latest_feedback: Option<(SensData, Instant)> = None; diff --git a/cfg/rbt_cfg.toml b/cfg/rbt_cfg.toml index 70d684b..580f977 100644 --- a/cfg/rbt_cfg.toml +++ b/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] diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index daf0ee3..73a933a 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/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` 有实现和单测 | 协议定义不是短板 | diff --git a/lib/src/rbt_base/rbt_algorithm.rs b/lib/src/rbt_base/rbt_algorithm.rs index cc798d9..5a5aecd 100644 --- a/lib/src/rbt_base/rbt_algorithm.rs +++ b/lib/src/rbt_base/rbt_algorithm.rs @@ -1,3 +1,4 @@ +pub mod rbt_ekf; pub mod rbt_eskf; // 几何模块 pub mod rbt_antigravity; diff --git a/lib/src/rbt_base/rbt_algorithm/rbt_ekf.rs b/lib/src/rbt_base/rbt_algorithm/rbt_ekf.rs new file mode 100644 index 0000000..8276d64 --- /dev/null +++ b/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, + /// 状态协方差矩阵。 + pub p: na::DMatrix, + 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, p0: na::DMatrix) { + self.x = x0; + self.p = p0; + self.initialized = true; + } + + /// 用初始状态与协方差构造并初始化滤波器。 + pub fn with_initial(x0: na::DVector, p0: na::DMatrix) -> 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, q: &na::DMatrix) { + 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, + q: &na::DMatrix, + state_step: impl Fn(&na::DVector) -> na::DVector, + ) { + 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, + h: &na::DMatrix, + r: &na::DMatrix, + z_pred: &na::DVector, + residual_fn: impl Fn(&na::DVector, &na::DVector) -> na::DVector, + ) -> (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::::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, + h: &na::DMatrix, + r: &na::DMatrix, + z_pred: &na::DVector, + residual_fn: impl Fn(&na::DVector, &na::DVector) -> na::DVector, + ) -> 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) -> na::DMatrix { + (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}"); + } +} diff --git a/lib/src/rbt_infra/rbt_cfg.rs b/lib/src/rbt_infra/rbt_cfg.rs index ff9dbb9..33ca29d 100644 --- a/lib/src/rbt_infra/rbt_cfg.rs +++ b/lib/src/rbt_infra/rbt_cfg.rs @@ -119,6 +119,221 @@ fn default_energy_fire_gap_s() -> f64 { 0.2 } +/// 能量机关顶层配置：tracker / aimer / mpc 三段。 +/// +/// 检测相关参数继续放在 `EnergyMechanismDetectorCfg`；这里只覆盖跟踪、瞄准与控制。 +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Default)] +pub struct EnergyMechanismCfg { + #[serde(default)] + pub tracker: EnergyMechanismTrackerCfg, + #[serde(default)] + pub aimer: EnergyMechanismAimerCfg, + #[serde(default)] + pub mpc: EnergyMechanismMpcCfg, +} + +/// 能量机关 tracker 配置（小符常速 + 大符曲线 EKF）。 +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +pub struct EnergyMechanismTrackerCfg { + #[serde(default = "default_tracker_lost_timeout_s")] + pub lost_timeout_s: f64, + #[serde(default = "default_tracker_big_lost_timeout_s")] + pub big_lost_timeout_s: f64, + #[serde(default = "default_tracker_big_model_reset_timeout_s")] + pub big_model_reset_timeout_s: f64, + #[serde(default = "default_tracker_big_curve_ekf_fit_enabled")] + pub big_curve_ekf_fit_enabled: bool, + #[serde(default = "default_tracker_big_phase_process_noise")] + pub big_phase_process_noise: f64, + #[serde(default = "default_tracker_big_a_process_noise")] + pub big_a_process_noise: f64, + #[serde(default = "default_tracker_big_w_process_noise")] + pub big_w_process_noise: f64, + #[serde(default = "default_tracker_big_measurement_noise_scale")] + pub big_measurement_noise_scale: f64, + #[serde(default = "default_tracker_big_speed_measurement_enabled")] + pub big_speed_measurement_enabled: bool, + #[serde(default = "default_tracker_big_speed_measurement_noise")] + pub big_speed_measurement_noise: f64, + #[serde(default = "default_tracker_big_speed_measurement_gate")] + pub big_speed_measurement_gate: f64, + #[serde(default = "default_tracker_big_curve_speed_slew_limit")] + pub big_curve_speed_slew_limit: f64, + #[serde(default = "default_tracker_big_speed_measurement_window_samples")] + pub big_speed_measurement_window_samples: usize, + #[serde(default = "default_tracker_big_speed_measurement_window_s")] + pub big_speed_measurement_window_s: f64, + #[serde(default = "default_tracker_big_speed_measurement_min_history")] + pub big_speed_measurement_min_history: usize, + #[serde(default = "default_tracker_big_curve_phi_correction_limit")] + pub big_curve_phi_correction_limit: f64, + #[serde(default = "default_tracker_big_phi_seed_frames")] + pub big_phi_seed_frames: usize, +} + +impl Default for EnergyMechanismTrackerCfg { + fn default() -> Self { + Self { + lost_timeout_s: default_tracker_lost_timeout_s(), + big_lost_timeout_s: default_tracker_big_lost_timeout_s(), + big_model_reset_timeout_s: default_tracker_big_model_reset_timeout_s(), + big_curve_ekf_fit_enabled: default_tracker_big_curve_ekf_fit_enabled(), + big_phase_process_noise: default_tracker_big_phase_process_noise(), + big_a_process_noise: default_tracker_big_a_process_noise(), + big_w_process_noise: default_tracker_big_w_process_noise(), + big_measurement_noise_scale: default_tracker_big_measurement_noise_scale(), + big_speed_measurement_enabled: default_tracker_big_speed_measurement_enabled(), + big_speed_measurement_noise: default_tracker_big_speed_measurement_noise(), + big_speed_measurement_gate: default_tracker_big_speed_measurement_gate(), + big_curve_speed_slew_limit: default_tracker_big_curve_speed_slew_limit(), + big_speed_measurement_window_samples: + default_tracker_big_speed_measurement_window_samples(), + big_speed_measurement_window_s: default_tracker_big_speed_measurement_window_s(), + big_speed_measurement_min_history: default_tracker_big_speed_measurement_min_history(), + big_curve_phi_correction_limit: default_tracker_big_curve_phi_correction_limit(), + big_phi_seed_frames: default_tracker_big_phi_seed_frames(), + } + } +} + +fn default_tracker_lost_timeout_s() -> f64 { + 0.35 +} +fn default_tracker_big_lost_timeout_s() -> f64 { + 0.08 +} +fn default_tracker_big_model_reset_timeout_s() -> f64 { + 0.35 +} +fn default_tracker_big_curve_ekf_fit_enabled() -> bool { + true +} +fn default_tracker_big_phase_process_noise() -> f64 { + 0.02 +} +fn default_tracker_big_a_process_noise() -> f64 { + 1e-6 +} +fn default_tracker_big_w_process_noise() -> f64 { + 3e-6 +} +fn default_tracker_big_measurement_noise_scale() -> f64 { + 4.0 +} +fn default_tracker_big_speed_measurement_enabled() -> bool { + true +} +fn default_tracker_big_speed_measurement_noise() -> f64 { + 1.50 +} +fn default_tracker_big_speed_measurement_gate() -> f64 { + 1.2 +} +fn default_tracker_big_curve_speed_slew_limit() -> f64 { + 3.0 +} +fn default_tracker_big_speed_measurement_window_samples() -> usize { + 16 +} +fn default_tracker_big_speed_measurement_window_s() -> f64 { + 0.30 +} +fn default_tracker_big_speed_measurement_min_history() -> usize { + 20 +} +fn default_tracker_big_curve_phi_correction_limit() -> f64 { + 0.0 +} +fn default_tracker_big_phi_seed_frames() -> usize { + 15 +} + +/// 能量机关 aimer 配置。 +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +pub struct EnergyMechanismAimerCfg { + #[serde(default)] + pub predict_time_s: f64, + #[serde(default = "default_aimer_fire_gap_s")] + pub fire_gap_s: f64, + #[serde(default)] + pub yaw_offset_deg: f64, + #[serde(default)] + pub pitch_offset_deg: f64, + #[serde(default)] + pub pitch_velocity_lead_time_s: f64, + #[serde(default = "default_aimer_snapshot_stale_ms")] + pub snapshot_stale_ms: f64, +} + +impl Default for EnergyMechanismAimerCfg { + fn default() -> Self { + Self { + predict_time_s: 0.0, + fire_gap_s: default_aimer_fire_gap_s(), + yaw_offset_deg: 0.0, + pitch_offset_deg: 0.0, + pitch_velocity_lead_time_s: 0.0, + snapshot_stale_ms: default_aimer_snapshot_stale_ms(), + } + } +} + +fn default_aimer_fire_gap_s() -> f64 { + 0.2 +} +fn default_aimer_snapshot_stale_ms() -> f64 { + 180.0 +} + +/// 能量机关 MPC 配置（直接映射 `SecondOrderPositionMpcConfig` 关键字段）。 +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +pub struct EnergyMechanismMpcCfg { + #[serde(default = "default_mpc_model_dt_s")] + pub model_dt_s: f64, + #[serde(default = "default_mpc_horizon")] + pub horizon: usize, + #[serde(default = "default_mpc_track_q")] + pub track_q: f64, + #[serde(default = "default_mpc_rate_q")] + pub rate_q: f64, + #[serde(default = "default_mpc_command_q")] + pub command_q: f64, + #[serde(default = "default_mpc_delta_r")] + pub delta_r: f64, +} + +impl Default for EnergyMechanismMpcCfg { + fn default() -> Self { + Self { + model_dt_s: default_mpc_model_dt_s(), + horizon: default_mpc_horizon(), + track_q: default_mpc_track_q(), + rate_q: default_mpc_rate_q(), + command_q: default_mpc_command_q(), + delta_r: default_mpc_delta_r(), + } + } +} + +fn default_mpc_model_dt_s() -> f64 { + 0.004 +} +fn default_mpc_horizon() -> usize { + 50 +} +fn default_mpc_track_q() -> f64 { + 3_198.0 +} +fn default_mpc_rate_q() -> f64 { + 0.0 +} +fn default_mpc_command_q() -> f64 { + 1_000.0 +} +fn default_mpc_delta_r() -> f64 { + 48_343.0 +} + /// 相机相关配置 #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] pub struct CamCfg { @@ -215,6 +430,8 @@ pub struct RbtCfg { pub cam_cfg: CamCfg, pub logger_cfg: LoggerCfg, pub estimator_cfg: EstimatorCfg, + #[serde(default)] + pub energy_mechanism_cfg: EnergyMechanismCfg, } impl RbtCfg { diff --git a/lib/src/rbt_mod/rbt_energy_mechanism.rs b/lib/src/rbt_mod/rbt_energy_mechanism.rs index 8426d1d..af43752 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism.rs @@ -4,11 +4,15 @@ //! Rust-native. Protocol names such as `HitBigBuff` are handled only at the //! route/communication boundary. +pub mod big_buff_curve_ekf; pub mod detected; pub mod fire_control; pub mod solved; pub mod tracker; +pub use big_buff_curve_ekf::{ + BIG_BUFF_BASE_SPEED, BigBuffCurveEskf, big_buff_angle_delta, big_buff_speed, +}; pub use detected::{ ENERGY_MECHANISM_INPUT_HEIGHT, ENERGY_MECHANISM_INPUT_WIDTH, ENERGY_MECHANISM_KEYPOINTS, ENERGY_MECHANISM_OUTPUT_MAX_CHANNELS, EnergyMechanismClass, EnergyMechanismFrame, @@ -23,4 +27,4 @@ pub use solved::{ EnergyMechanismPose, EnergyMechanismSolvedFrame, EnergyMechanismSolvedTarget, solve_energy_mechanism, }; -pub use tracker::{EnergyMechanismTrackSnapshot, EnergyMechanismTracker}; +pub use tracker::{CurveSnapshot, EnergyMechanismTrackSnapshot, EnergyMechanismTracker}; diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/big_buff_curve_ekf.rs b/lib/src/rbt_mod/rbt_energy_mechanism/big_buff_curve_ekf.rs new file mode 100644 index 0000000..97bc5a5 --- /dev/null +++ b/lib/src/rbt_mod/rbt_energy_mechanism/big_buff_curve_ekf.rs @@ -0,0 +1,753 @@ +//! 大符曲线 EKF。 +//! +//! 迁移自 vivsionn `BuffTracker` 的 `big_runev2` 曲线模型，内部基于 `rbt_base` 的共享不定长 +//! EKF（`ExtendedKalmanFilter`）。这里只承担大符相位/曲线参数的滤波与预测；rune 圆心、目标 +//! 几何、装甲板 pitch/yaw 仍由 `solved.rs` 的 PnP 提供，由 `tracker.rs` 持有。 +//! +//! 曲线模型（与 vivsionn 一致）： +//! - `speed(phase) = a·sin(phase) + (base - a)`，`base = 2.090` +//! - `angle_delta(a, w, phase, dt) = (base - a)·dt + a/w·(cos(phase) - cos(phase + w·dt))` +//! +//! 状态向量 `[phase, a, w]`（3 维）。phase 由观察 roll 经方向系数积分推进，a/w 为曲线参数。 +//! `a` 与 `w` 各自有过程噪声与合法范围，超出范围时夹紧到初值。 + +use crate::rbt_base::rbt_algorithm::rbt_ekf::ExtendedKalmanFilter; +use crate::rbt_infra::rbt_cfg::EnergyMechanismTrackerCfg; + +/// 大符基础速度常数（rad/s），与 vivsionn `kBigBuffBaseSpeed` 一致。 +pub const BIG_BUFF_BASE_SPEED: f64 = 2.090; +const BIG_BUFF_A_INIT: f64 = 0.9125; +const BIG_BUFF_W_INIT: f64 = 1.942; +const BIG_BUFF_A_MIN: f64 = 0.780; +const BIG_BUFF_A_MAX: f64 = 1.045; +const BIG_BUFF_W_MIN: f64 = 1.884; +const BIG_BUFF_W_MAX: f64 = 2.000; +const BIG_BUFF_SPEED_HARD_MAX: f64 = 2.35; +const STATE_DIM: usize = 3; +const PHASE: usize = 0; +const A: usize = 1; +const W: usize = 2; + +/// 大符曲线 EKF 配置（从 `EnergyMechanismTrackerCfg` 提取滤波相关字段）。 +#[derive(Debug, Clone, Copy)] +struct CurveFilterConfig { + phase_process_noise: f64, + a_process_noise: f64, + w_process_noise: f64, + speed_measurement_noise: f64, + speed_measurement_gate: f64, + speed_slew_limit: f64, + phi_correction_limit: f64, + phi_seed_frames: usize, + window_samples: usize, + window_s: f64, + min_history: usize, + /// 是否启用曲线 EKF 拟合；关闭后退化为常速预测。 + curve_ekf_fit_enabled: bool, + /// 是否启用速度测量更新。 + speed_measurement_enabled: bool, + /// 测量噪声 R 的整体缩放（>=1 放大噪声，降低测量信任）。 + measurement_noise_scale: f64, +} + +impl CurveFilterConfig { + fn from_tracker_cfg(cfg: &EnergyMechanismTrackerCfg) -> Self { + Self { + phase_process_noise: cfg.big_phase_process_noise.max(0.0), + a_process_noise: cfg.big_a_process_noise.max(0.0), + w_process_noise: cfg.big_w_process_noise.max(0.0), + speed_measurement_noise: cfg.big_speed_measurement_noise.max(1e-6), + speed_measurement_gate: cfg.big_speed_measurement_gate.max(1e-6), + speed_slew_limit: cfg.big_curve_speed_slew_limit.max(0.0), + phi_correction_limit: cfg.big_curve_phi_correction_limit.max(0.0), + phi_seed_frames: cfg.big_phi_seed_frames, + window_samples: cfg.big_speed_measurement_window_samples.clamp(2, 16), + window_s: cfg.big_speed_measurement_window_s.max(1e-3), + min_history: cfg.big_speed_measurement_min_history, + curve_ekf_fit_enabled: cfg.big_curve_ekf_fit_enabled, + speed_measurement_enabled: cfg.big_speed_measurement_enabled, + measurement_noise_scale: cfg.big_measurement_noise_scale.max(1.0), + } + } +} + +/// 大符曲线 EKF。 +#[derive(Debug, Clone)] +pub struct BigBuffCurveEskf { + ekf: ExtendedKalmanFilter, + cfg: CurveFilterConfig, + smoothed_curve_speed: f64, + direction: i32, + history: std::collections::VecDeque, + phi_seeded: bool, +} + +#[derive(Debug, Clone, Copy)] +struct RollSample { + time_s: f64, + roll_rad: f64, +} + +/// 大符曲线模型纯函数：给定 `a/w/phase` 返回当前角速度（rad/s，带符号）。 +pub fn big_buff_speed(a: f64, phase: f64) -> f64 { + let a = clamp_curve_a(a); + let phase = if phase.is_finite() { phase } else { 0.0 }; + a * phase.sin() + (BIG_BUFF_BASE_SPEED - a) +} + +/// 大符曲线模型纯函数：给定 `a/w/phase` 与时间间隔，返回角度增量（rad）。 +pub fn big_buff_angle_delta(a: f64, w: f64, phase: f64, dt: f64) -> f64 { + let a = clamp_curve_a(a); + let w = clamp_curve_w(w); + let phase = if phase.is_finite() { phase } else { 0.0 }; + let dt = dt.max(0.0); + (BIG_BUFF_BASE_SPEED - a) * dt + a / w * (phase.cos() - (phase + w * dt).cos()) +} + +fn clamp_curve_a(a: f64) -> f64 { + if !a.is_finite() { + BIG_BUFF_A_INIT + } else { + a.clamp(BIG_BUFF_A_MIN, BIG_BUFF_A_MAX) + } +} + +fn clamp_curve_w(w: f64) -> f64 { + if !w.is_finite() { + BIG_BUFF_W_INIT + } else { + w.clamp(BIG_BUFF_W_MIN, BIG_BUFF_W_MAX) + } +} + +fn clamp_speed_magnitude(speed: f64) -> f64 { + if !speed.is_finite() { + return speed; + } + speed.abs().min(BIG_BUFF_SPEED_HARD_MAX) +} + +fn normalize_angle(angle: f64) -> f64 { + let mut normalized = (angle + std::f64::consts::PI) % std::f64::consts::TAU; + if normalized < 0.0 { + normalized += std::f64::consts::TAU; + } + normalized - std::f64::consts::PI +} + +/// 把 `angle` 加上若干个 2π，使其与 `reference` 的差落在 (-π, π] 内。 +/// 用于速度窗口回归前展开跨 ±π 的 roll 角，避免正常旋转被当成大跳变。 +fn unwrap_relative_to(angle: f64, reference: f64) -> f64 { + if !angle.is_finite() || !reference.is_finite() { + return angle; + } + let mut delta = (angle - reference) % std::f64::consts::TAU; + if delta < -std::f64::consts::PI { + delta += std::f64::consts::TAU; + } else if delta > std::f64::consts::PI { + delta -= std::f64::consts::TAU; + } + reference + delta +} + +impl BigBuffCurveEskf { + /// 用默认曲线初值构造（`a = 0.9125`、`w = 1.942`、`phase = 0`）。 + pub fn from_tracker_cfg(cfg: &EnergyMechanismTrackerCfg) -> Self { + let curve_cfg = CurveFilterConfig::from_tracker_cfg(cfg); + let x0 = na::DVector::from_vec(vec![0.0, BIG_BUFF_A_INIT, BIG_BUFF_W_INIT]); + let p0 = na::DMatrix::from_diagonal(&na::DVector::from_vec(vec![1.0, 0.1, 0.1])); + Self { + ekf: ExtendedKalmanFilter::with_initial(x0, p0), + cfg: curve_cfg, + smoothed_curve_speed: f64::NAN, + direction: 0, + history: std::collections::VecDeque::with_capacity(64), + phi_seeded: false, + } + } + + /// 当前相位（曲线 φ）。 + pub fn phase(&self) -> f64 { + self.ekf.x[PHASE] + } + + /// 当前曲线参数 `a`。 + pub fn a(&self) -> f64 { + self.ekf.x[A] + } + + /// 当前曲线参数 `w`。 + pub fn w(&self) -> f64 { + self.ekf.x[W] + } + + /// 当前曲线角速度（|speed|，rad/s）。 + pub fn curve_speed(&self) -> f64 { + clamp_speed_magnitude(big_buff_speed(self.ekf.x[A], self.ekf.x[PHASE])) + } + + /// 当前转动方向（+1 / -1 / 0）。 + pub fn direction(&self) -> i32 { + self.direction + } + + /// 重置滤波器（保留配置，清空状态与历史）。 + pub fn reset(&mut self) { + let x0 = na::DVector::from_vec(vec![0.0, BIG_BUFF_A_INIT, BIG_BUFF_W_INIT]); + let p0 = na::DMatrix::from_diagonal(&na::DVector::from_vec(vec![1.0, 0.1, 0.1])); + self.ekf.init(x0, p0); + self.smoothed_curve_speed = f64::NAN; + self.history.clear(); + self.phi_seeded = false; + } + + /// 注入观察到的转动方向（由 tracker 的方向投票给出）。 + pub fn set_direction(&mut self, direction: i32) { + self.direction = direction; + } + + /// 记录一个 roll 观察样本（时间秒 + roll 弧度），用于 rolling-window 估速与 φ seed。 + pub fn record_roll(&mut self, time_s: f64, roll_rad: f64) { + self.history.push_back(RollSample { time_s, roll_rad }); + while self.history.len() > 250 { + self.history.pop_front(); + } + } + + /// 用滚动窗口最小二乘估计当前角速度（rad/s，绝对值）。样本不足时返回 `None`。 + pub fn estimate_observed_speed(&self) -> Option { + estimate_linear_rate_from_history(&self.history, self.cfg.window_samples, self.cfg.window_s) + } + + /// 执行一步 predict：把曲线相位 φ 推进 `dt`，并传播协方差。 + /// + /// 注意：phase（曲线内部相位 φ）的推进由 `w` 决定，**不带方向系数**——方向只影响 + /// roll（外层目标角度）的累积，phase 是驱动 speed = a·sin(φ)+(base-a) 的内部状态。 + /// 这与 vivsionn `BuffTracker::update_ekf` 的 f lambda 一致（phase += w·dt，roll 才带 dir）。 + pub fn predict(&mut self, dt: f64) { + // 关闭曲线拟合时不推进 phase/speed，退化为常速预测（由外层 roll_rate 线性外推）。 + if !self.cfg.curve_ekf_fit_enabled { + return; + } + let dt = dt.clamp(1e-3, 0.1); + + // 名义状态非线性传播：phase += w·dt，a/w 不变。 + let state_step = |x: &na::DVector| { + let phase = if x[PHASE].is_finite() { x[PHASE] } else { 0.0 }; + let w = clamp_curve_w(x[W]); + let mut next = x.clone_owned(); + next[PHASE] = normalize_angle(phase + w * dt); + next[A] = clamp_curve_a(x[A]); + next[W] = w; + next + }; + + // 状态转移雅可比 F。phase 对 w 的偏导 dφ/dw = dt（vivsionn F(9,8)=dt）， + // 必须保留这个耦合，否则速度测量更新对 w 的修正无法在 predict 中保持，w 学不动。 + let mut f = na::DMatrix::::identity(STATE_DIM, STATE_DIM); + f[(PHASE, W)] = dt; + + let q = self.process_noise(dt); + self.ekf.predict_nonlinear(&f, &q, state_step); + self.maybe_seed_phi(); + } + + /// 执行速度测量更新（rolling-window 估速 → 速度残差 → phase/a 修正）。 + /// 返回速度测量的处理状态：`Accepted`、`Gated`（超门控拒绝）、`Skipped`（无有效样本）。 + pub fn update_with_speed(&mut self, dt: f64) -> SpeedUpdateResult { + // 配置关闭速度测量更新时直接跳过。 + if !self.cfg.speed_measurement_enabled { + return SpeedUpdateResult::Skipped; + } + if self.history.len() < self.cfg.min_history.max(2) { + return SpeedUpdateResult::Skipped; + } + let Some(observed_speed) = self.estimate_observed_speed() else { + return SpeedUpdateResult::Skipped; + }; + if !observed_speed.is_finite() || observed_speed >= BIG_BUFF_SPEED_HARD_MAX * 1.5 { + return SpeedUpdateResult::Skipped; + } + let observed_speed = clamp_speed_magnitude(observed_speed); + + let a = clamp_curve_a(self.ekf.x[A]); + let phase = if self.ekf.x[PHASE].is_finite() { + self.ekf.x[PHASE] + } else { + 0.0 + }; + let signed_predicted = big_buff_speed(a, phase); + let predicted_speed = signed_predicted.abs(); + let innovation = observed_speed - predicted_speed; + + if innovation.abs() > self.cfg.speed_measurement_gate { + return SpeedUpdateResult::Gated; + } + + // 测量雅可比 H = d|speed|/dphase, d|speed|/da, d|speed|/dw。 + let speed_sign = if signed_predicted < 0.0 { -1.0 } else { 1.0 }; + let mut h = na::DMatrix::::zeros(1, STATE_DIM); + h[(0, PHASE)] = speed_sign * a * phase.cos(); + h[(0, A)] = speed_sign * (phase.sin() - 1.0); + // w 不直接出现在 speed 公式中，对 speed 导数为 0。 + let r = na::DMatrix::from_row_slice( + 1, + 1, + &[self.cfg.speed_measurement_noise * self.cfg.measurement_noise_scale], + ); + let z = na::DVector::from_vec(vec![observed_speed]); + let z_pred = na::DVector::from_vec(vec![predicted_speed]); + let phase_before = self.ekf.x[PHASE]; + + self.ekf.update(&z, &h, &r, &z_pred, |a_v, b_v| a_v - b_v); + + self.apply_phi_correction_limit(phase_before); + self.clamp_state(); + self.update_smoothed_speed(dt); + SpeedUpdateResult::Accepted + } + + /// 不做测量更新时也要刷新平滑速度（用于 predict 后保持 slew 平滑）。 + pub fn refresh_smoothed_speed(&mut self, dt: f64) { + self.update_smoothed_speed(dt); + } + + /// 预测从当前状态出发 `dt` 秒后的相位与曲线速度，返回 `(phase, speed_abs)`。 + pub fn predict_from_state(&self, dt: f64) -> (f64, f64) { + let dt = dt.max(0.0); + let a = clamp_curve_a(self.ekf.x[A]); + let w = clamp_curve_w(self.ekf.x[W]); + let phase = if self.ekf.x[PHASE].is_finite() { + self.ekf.x[PHASE] + } else { + 0.0 + }; + let future_phase = normalize_angle(phase + w * dt); + let future_speed = clamp_speed_magnitude(big_buff_speed(a, future_phase)); + (future_phase, future_speed) + } + + /// 预测从当前状态出发 `dt` 秒内的角度增量（带方向系数）。 + pub fn predict_angle_delta(&self, dt: f64) -> f64 { + let active_dir = if self.direction != 0 { + self.direction as f64 + } else { + 1.0 + }; + let a = clamp_curve_a(self.ekf.x[A]); + let w = clamp_curve_w(self.ekf.x[W]); + let phase = if self.ekf.x[PHASE].is_finite() { + self.ekf.x[PHASE] + } else { + 0.0 + }; + let raw_delta = big_buff_angle_delta(a, w, phase, dt); + let mut delta = active_dir * raw_delta; + if self.cfg.speed_slew_limit > 0.0 + && self.smoothed_curve_speed.is_finite() + && delta.is_finite() + { + let max_delta = + 0.5 * (self.smoothed_curve_speed + self.curve_speed()) * dt.max(0.0) + 0.02; + if delta.abs() > max_delta { + delta = max_delta.copysign(delta); + } + } + delta + } + + fn process_noise(&self, dt: f64) -> na::DMatrix { + let mut q = na::DMatrix::::zeros(STATE_DIM, STATE_DIM); + q[(PHASE, PHASE)] = self.cfg.phase_process_noise * dt; + q[(A, A)] = self.cfg.a_process_noise * dt; + q[(W, W)] = self.cfg.w_process_noise * dt; + q + } + + fn maybe_seed_phi(&mut self) { + if self.phi_seeded || self.cfg.phi_seed_frames == 0 { + return; + } + if self.history.len() != self.cfg.phi_seed_frames { + return; + } + let Some(observed_speed) = self.estimate_observed_speed() else { + return; + }; + if !observed_speed.is_finite() || observed_speed <= 0.1 { + return; + } + let a = clamp_curve_a(self.ekf.x[A]); + let base = BIG_BUFF_BASE_SPEED - a; + let sin_arg = (observed_speed - base) / a; + if !sin_arg.is_finite() || sin_arg.abs() > 1.0 { + return; + } + let phi_asc = sin_arg.asin(); + let phi_desc = normalize_angle(std::f64::consts::PI - phi_asc); + let phi_seed = self.select_phi_branch(phi_asc, phi_desc); + self.ekf.x[PHASE] = phi_seed; + if self.ekf.p[(PHASE, PHASE)] > 0.05 { + self.ekf.p[(PHASE, PHASE)] = 0.05; + } + self.phi_seeded = true; + } + + fn select_phi_branch(&self, phi_asc: f64, phi_desc: f64) -> f64 { + if self.history.len() < 4 { + return phi_asc; + } + let mut iter = self.history.iter().rev(); + let it0 = iter.next(); + let it1 = iter.next(); + let it3 = iter.nth(1); + match (it0, it1, it3) { + (Some(s0), Some(s1), Some(s3)) => { + let dt_recent = s0.time_s - s1.time_s; + let dt_early = s1.time_s - s3.time_s; + if dt_recent > 1e-4 && dt_early > 1e-4 { + let spd_recent = (s0.roll_rad - s1.roll_rad).abs() / dt_recent; + let spd_early = (s1.roll_rad - s3.roll_rad).abs() / dt_early; + if spd_recent < spd_early - 0.05 { + return phi_desc; + } + } + phi_asc + } + _ => phi_asc, + } + } + + fn apply_phi_correction_limit(&mut self, phase_before: f64) { + if self.cfg.phi_correction_limit <= 0.0 || !phase_before.is_finite() { + return; + } + let correction = normalize_angle(self.ekf.x[PHASE] - phase_before); + if correction.abs() > self.cfg.phi_correction_limit { + self.ekf.x[PHASE] = + normalize_angle(phase_before + self.cfg.phi_correction_limit.copysign(correction)); + } + } + + fn clamp_state(&mut self) { + self.ekf.x[A] = clamp_curve_a(self.ekf.x[A]); + self.ekf.x[W] = clamp_curve_w(self.ekf.x[W]); + self.ekf.x[PHASE] = normalize_angle(self.ekf.x[PHASE]); + } + + fn update_smoothed_speed(&mut self, dt: f64) { + let raw = self.curve_speed(); + if self.cfg.speed_slew_limit > 0.0 + && self.smoothed_curve_speed.is_finite() + && raw.is_finite() + { + let max_delta = self.cfg.speed_slew_limit * dt.max(0.0); + let delta = (raw - self.smoothed_curve_speed).clamp(-max_delta, max_delta); + self.smoothed_curve_speed += delta; + } else { + self.smoothed_curve_speed = raw; + } + } +} + +/// 速度测量更新的处理结果。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SpeedUpdateResult { + /// 测量被接受并写入滤波器。 + Accepted, + /// 测量超出门控，被拒绝。 + Gated, + /// 无有效样本或历史不足，跳过。 + Skipped, +} + +/// 用滚动窗口最小二乘估计角速度（绝对值，rad/s）。 +fn estimate_linear_rate_from_history( + history: &std::collections::VecDeque, + max_samples: usize, + max_window_s: f64, +) -> Option { + if history.len() < 2 || max_samples < 2 || max_window_s <= 0.0 { + return None; + } + let latest_t = history.back()?.time_s; + if !latest_t.is_finite() { + return None; + } + + let mut times = [0.0_f64; 16]; + let mut angles = [0.0_f64; 16]; + let max_n = max_samples.min(16); + let mut count = 0_usize; + for sample in history.iter().rev() { + if !sample.time_s.is_finite() || !sample.roll_rad.is_finite() { + break; + } + if latest_t - sample.time_s > max_window_s && count >= 2 { + break; + } + times[count] = sample.time_s; + angles[count] = sample.roll_rad; + count += 1; + if count >= max_n { + break; + } + } + if count < 2 { + return None; + } + + // 样本是倒序收集的（times[0] 最新）。roll 经 normalize_angle 后落在 (-π, π]， + // 跨越 ±π 时（如 3.1 → -3.1）会被线性回归当成大跳变，估出错误速度。 + // 以最新样本 angles[0] 为基准逐个 unwrap，保证窗口内角度连续。 + let mut unwrapped = [0.0_f64; 16]; + unwrapped[0] = angles[0]; + for i in 1..count { + unwrapped[i] = unwrap_relative_to(angles[i], unwrapped[i - 1]); + } + + let mean_t: f64 = times[..count].iter().sum(); + let mean_a: f64 = unwrapped[..count].iter().sum(); + let mean_t = mean_t / count as f64; + let mean_a = mean_a / count as f64; + + let mut numerator = 0.0; + let mut denominator = 0.0; + for i in 0..count { + let dt = times[i] - mean_t; + numerator += dt * (unwrapped[i] - mean_a); + denominator += dt * dt; + } + if !numerator.is_finite() || !denominator.is_finite() || denominator < 1e-6 { + return None; + } + Some((numerator / denominator).abs()) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn default_cfg() -> EnergyMechanismTrackerCfg { + EnergyMechanismTrackerCfg::default() + } + + #[test] + fn speed_formula_matches_vivsionn_curve_model() { + // phase = π/2 → sin = 1 → speed = a + (base - a) = base + let speed_peak = big_buff_speed(BIG_BUFF_A_INIT, std::f64::consts::FRAC_PI_2); + assert!((speed_peak - BIG_BUFF_BASE_SPEED).abs() < 1e-9); + // phase = -π/2 → sin = -1 → speed = -a + (base - a) = base - 2a + let speed_trough = big_buff_speed(BIG_BUFF_A_INIT, -std::f64::consts::FRAC_PI_2); + assert!((speed_trough - (BIG_BUFF_BASE_SPEED - 2.0 * BIG_BUFF_A_INIT)).abs() < 1e-9); + } + + #[test] + fn angle_delta_at_zero_phase_matches_curve_integral() { + // angle_delta 是曲线积分，phase=0 时含二阶曲率项 a/w·(1 - cos(w·dt))，不能退化为 speed·dt。 + // 用 vivsionn 原始积分公式直接验证：(base-a)·dt + a/w·(cos(0) - cos(w·dt))。 + let dt = 0.05; + let a = BIG_BUFF_A_INIT; + let w = BIG_BUFF_W_INIT; + let phase: f64 = 0.0; + let expected = + (BIG_BUFF_BASE_SPEED - a) * dt + a / w * (phase.cos() - (phase + w * dt).cos()); + let delta = big_buff_angle_delta(a, w, phase, dt); + assert!( + (delta - expected).abs() < 1e-12, + "delta={delta} expected={expected}" + ); + // 且方向系数 ±1 对应的 predict_angle_delta 关于零点对称。 + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + let pos = eskf.predict_angle_delta(dt); + eskf.set_direction(-1); + let neg = eskf.predict_angle_delta(dt); + assert!((pos + neg).abs() < 1e-9); + } + + #[test] + fn clamp_a_w_keeps_params_in_range() { + assert_eq!(clamp_curve_a(0.5), BIG_BUFF_A_MIN); + assert_eq!(clamp_curve_a(2.0), BIG_BUFF_A_MAX); + assert_eq!(clamp_curve_w(1.0), BIG_BUFF_W_MIN); + assert_eq!(clamp_curve_w(3.0), BIG_BUFF_W_MAX); + assert!(!clamp_curve_a(f64::NAN).is_nan()); + } + + #[test] + fn predict_advances_phase_and_speed() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + eskf.record_roll(0.0, 0.0); + eskf.record_roll(0.02, 0.05); + + eskf.predict(0.02); + let phase_after = eskf.phase(); + let speed_after = eskf.curve_speed(); + assert!(phase_after.is_finite()); + assert!(speed_after > 0.0 && speed_after < BIG_BUFF_SPEED_HARD_MAX + 1e-6); + } + + #[test] + fn predict_angle_delta_carries_direction_sign() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + let pos = eskf.predict_angle_delta(0.05); + eskf.set_direction(-1); + let neg = eskf.predict_angle_delta(0.05); + assert!(pos > 0.0); + assert!(neg < 0.0); + assert!((pos + neg).abs() < 1e-9); + } + + #[test] + fn update_with_speed_returns_skipped_without_history() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + assert_eq!(eskf.update_with_speed(0.02), SpeedUpdateResult::Skipped); + } + + #[test] + fn update_with_speed_gates_outlier_measurement() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + // 喂入一个明显与曲线模型不符的高速历史（>gate），应被门控拒绝。 + for i in 0..30 { + eskf.record_roll(i as f64 * 0.02, i as f64 * 0.5); + } + let result = eskf.update_with_speed(0.02); + assert!( + matches!( + result, + SpeedUpdateResult::Gated | SpeedUpdateResult::Skipped + ), + "result={result:?}" + ); + } + + #[test] + fn estimate_observed_speed_uses_least_squares_over_window() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + // 线性 roll：rate = 2.0 rad/s + for i in 0..20 { + let t = i as f64 * 0.02; + eskf.record_roll(t, 2.0 * t); + } + let speed = eskf.estimate_observed_speed().unwrap(); + assert!((speed - 2.0).abs() < 1e-6, "speed={speed}"); + } + + #[test] + fn estimate_observed_speed_unwraps_roll_across_pi_boundary() { + // roll 以 3.0 rad/s 递增，跨越 ±π 边界。若不 unwrap，回归会把 + // π → -π 的跳变当成速度反转，估出错误值。 + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + let rate = 3.0; + for i in 0..20 { + let t = i as f64 * 0.02; + // record_roll 内部存原始角度，但 normalize 在 tracker 侧； + // 这里直接喂 normalize 后的角度模拟实车 tracker 输出。 + let raw = rate * t; + let normalized = + ((raw + std::f64::consts::PI) % std::f64::consts::TAU) - std::f64::consts::PI; + eskf.record_roll(t, normalized); + } + let speed = eskf + .estimate_observed_speed() + .expect("speed estimate present"); + // unwrap 后应接近真实 rate，而不是被边界跳变污染。 + assert!((speed - rate).abs() < 0.1, "speed={speed} expected~{rate}"); + } + + #[test] + fn unwrap_relative_to_handles_pi_boundary() { + use super::unwrap_relative_to; + // -3.13 相对 3.13：差值跨过 -π，应展开为 3.13 附近（≈ 3.153，差 0.023）。 + let r1 = unwrap_relative_to(-3.13, 3.13); + assert!((r1 - 3.13).abs() < 0.05, "r1={r1}"); + // 3.13 相对 -3.13：差值跨过 +π，应展开为 -3.13 附近（≈ -3.153）。 + let r2 = unwrap_relative_to(3.13, -3.13); + assert!((r2 - (-3.13)).abs() < 0.05, "r2={r2}"); + // 不跨边界时基本不变。 + assert!((unwrap_relative_to(1.0, 0.5) - 1.0).abs() < 1e-9); + // 关键性质：跨 ±π 后两个样本的差应等于它们的连续角差，而非 2π 跳变。 + let diff = unwrap_relative_to(-3.13, 3.13) - 3.13; + assert!(diff.abs() < 0.05, "跨边界差应为小量，实际 diff={diff}"); + } + + #[test] + fn reset_clears_state_and_history() { + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&default_cfg()); + eskf.set_direction(1); + eskf.record_roll(0.0, 0.0); + eskf.predict(0.02); + assert!(eskf.phase().abs() < 1.0); + + eskf.reset(); + assert_eq!(eskf.phase(), 0.0); + assert_eq!(eskf.a(), BIG_BUFF_A_INIT); + assert_eq!(eskf.w(), BIG_BUFF_W_INIT); + } + + #[test] + fn phi_seed_fires_once_at_seed_frame_count() { + let mut cfg = default_cfg(); + cfg.big_phi_seed_frames = 8; + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&cfg); + eskf.set_direction(1); + // 构造一个合理的速度历史，使 seed 有合法的 sin_arg。 + for i in 0..8 { + eskf.record_roll(i as f64 * 0.02, i as f64 * 0.04); + } + eskf.predict(0.02); + // seed 后 phase 不再是 0 + assert!(eskf.phase().abs() > 1e-6 || eskf.a() > 0.0); + } + + #[test] + fn curve_ekf_fit_disabled_skips_phase_advance() { + // big_curve_ekf_fit_enabled = false 时 predict 不推进 phase，退化为常速。 + let mut cfg = default_cfg(); + cfg.big_curve_ekf_fit_enabled = false; + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&cfg); + eskf.set_direction(1); + let phase_before = eskf.phase(); + eskf.record_roll(0.0, 0.0); + eskf.record_roll(0.02, 0.05); + eskf.predict(0.02); + assert!((eskf.phase() - phase_before).abs() < 1e-9, "phase 不应推进"); + } + + #[test] + fn speed_measurement_disabled_returns_skipped() { + // big_speed_measurement_enabled = false 时 update_with_speed 直接 Skipped。 + let mut cfg = default_cfg(); + cfg.big_speed_measurement_enabled = false; + cfg.big_speed_measurement_min_history = 2; + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&cfg); + eskf.set_direction(1); + for i in 0..10 { + eskf.record_roll(i as f64 * 0.02, i as f64 * 0.04); + } + assert_eq!(eskf.update_with_speed(0.02), SpeedUpdateResult::Skipped); + } + + #[test] + fn measurement_noise_scale_changes_gate_tolerance() { + // 放大 measurement_noise_scale 后，同一速度测量更容易被接受（R 更大，门控相对宽松）。 + // 这里只验证开关被读取且配置生效：把 scale 调大后 update_with_speed 仍正常返回。 + let mut cfg = default_cfg(); + cfg.big_speed_measurement_min_history = 2; + cfg.big_speed_measurement_gate = 5.0; + cfg.big_measurement_noise_scale = 10.0; + let mut eskf = BigBuffCurveEskf::from_tracker_cfg(&cfg); + eskf.set_direction(1); + for i in 0..10 { + eskf.record_roll(i as f64 * 0.02, i as f64 * 0.04); + } + let result = eskf.update_with_speed(0.02); + // 大 gate + 大噪声下应被接受（不 Gated）。 + assert_ne!(result, SpeedUpdateResult::Gated); + } +} diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs b/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs index 05317d4..a9deb14 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs @@ -1,15 +1,17 @@ +use crate::rbt_infra::rbt_cfg::{EnergyMechanismAimerCfg, EnergyMechanismMpcCfg}; use crate::rbt_mod::rbt_comm::rbt_comm_frame::{ AimingState, CtrlData, DEFAULT_BULLET_SPEED_MPS, SensData, ShotBuffMode, ShotMode, TaskMode, }; -use crate::rbt_mod::rbt_fire_control::SecondOrderPositionMpc; +use crate::rbt_mod::rbt_fire_control::{ + SECOND_ORDER_POSITION_MPC_HORIZON, SecondOrderPositionMpc, SecondOrderPositionMpcConfig, +}; use super::detected::EnergyMechanismMode; use super::tracker::EnergyMechanismTrackSnapshot; -const SNAPSHOT_STALE_MS: f64 = 180.0; -const BASE_PREDICT_TIME_S: f64 = 0.10; -const FIRE_GAP_S: f64 = 0.20; const GRAVITY_MPS2: f64 = 9.78; +const DEFAULT_BASE_PREDICT_TIME_S: f64 = 0.10; +const PITCH_RATE_SAMPLE_DT_S: f64 = 0.01; #[derive(Debug, Clone, Copy)] pub struct EnergyMechanismControlInput { @@ -48,15 +50,55 @@ pub struct EnergyMechanismController { last_fire_t: Option, last_valid_command: Option, last_stats: EnergyMechanismControlStats, + predict_time_s: f64, + fire_gap_s: f64, + yaw_offset_deg: f64, + pitch_offset_deg: f64, + pitch_velocity_lead_time_s: f64, + snapshot_stale_ms: f64, + mpc_model_dt_s: f64, + mpc_horizon: usize, } impl EnergyMechanismController { + /// 无参默认构造（保持向后兼容，使用默认配置常量）。 pub fn new() -> Self { + Self::from_aimer_cfg( + &EnergyMechanismAimerCfg::default(), + &EnergyMechanismMpcCfg::default(), + ) + } + + /// 从 aimer / mpc 配置构造。 + pub fn from_aimer_cfg( + aimer_cfg: &EnergyMechanismAimerCfg, + mpc_cfg: &EnergyMechanismMpcCfg, + ) -> Self { Self { - yaw_mpc: SecondOrderPositionMpc::default(), + yaw_mpc: SecondOrderPositionMpc::new(Self::mpc_config(mpc_cfg)) + .unwrap_or_else(|_| SecondOrderPositionMpc::default()), last_fire_t: None, last_valid_command: None, last_stats: EnergyMechanismControlStats::default(), + predict_time_s: aimer_cfg.predict_time_s.max(0.0), + fire_gap_s: aimer_cfg.fire_gap_s.max(0.0), + yaw_offset_deg: aimer_cfg.yaw_offset_deg, + pitch_offset_deg: aimer_cfg.pitch_offset_deg, + pitch_velocity_lead_time_s: aimer_cfg.pitch_velocity_lead_time_s.max(0.0), + snapshot_stale_ms: aimer_cfg.snapshot_stale_ms.max(1.0), + mpc_model_dt_s: mpc_cfg.model_dt_s.max(1e-3), + mpc_horizon: mpc_cfg.horizon.clamp(4, SECOND_ORDER_POSITION_MPC_HORIZON), + } + } + + fn mpc_config(mpc_cfg: &EnergyMechanismMpcCfg) -> SecondOrderPositionMpcConfig { + SecondOrderPositionMpcConfig { + model_dt_s: mpc_cfg.model_dt_s.max(1e-3), + track_q: mpc_cfg.track_q.max(0.0), + rate_q: mpc_cfg.rate_q.max(0.0), + command_q: mpc_cfg.command_q.max(0.0), + delta_r: mpc_cfg.delta_r.max(1e-6), + ..Default::default() } } @@ -75,38 +117,55 @@ impl EnergyMechanismController { let Some(snapshot) = input.target else { return self.no_target(input.feedback); }; - let stale = input.snapshot_age_ms > SNAPSHOT_STALE_MS; + let stale = input.snapshot_age_ms > self.snapshot_stale_ms; if stale || !snapshot.track_valid || !input.feedback_fresh { return self.no_target(input.feedback); } let bullet_speed = feedback_bullet_speed(input.feedback); - let predict_time_s = BASE_PREDICT_TIME_S + approximate_fly_time(snapshot, bullet_speed); - let predicted = snapshot.predict_target_center_world_m(predict_time_s); - let yaw_rad = predicted.y.atan2(predicted.x); - let yaw_deg = normalize_angle_deg((-yaw_rad).to_degrees()); - let pitch_deg = solve_pitch_deg(predicted, bullet_speed); - let shot_mode = self.next_shot_mode(); + let (yaw_deg, pitch_deg, fly_time_s) = self.solve_trajectory(snapshot, bullet_speed); + + // 用 tracker 预瞄 horizon 生成 MPC yaw 参考（替代固定 yaw 常量）。 + // horizon 的预测时间 = 基础延迟 + 最近一次解算的飞行时间。 + let base_dt_s = self.base_predict_time_s(); + let horizon_dt: Vec = (0..self.mpc_horizon) + .map(|i| base_dt_s + fly_time_s + i as f64 * self.mpc_model_dt_s) + .collect(); + let horizon_points = snapshot.predict_target_horizon(&horizon_dt); + let (yaw_ref, yaw_rate_ref) = self.build_yaw_reference(&horizon_points); + + // 用 MPC 输出替代 raw yaw：update_trajectory 返回滤波后的 command_deg。 + // MPC 失败或输出非有限时回退到 raw yaw_deg，保证控制不中断。 + let mpc_command_deg = self + .yaw_mpc + .update_trajectory( + &yaw_ref, + &yaw_rate_ref, + input.feedback.gimbal_yaw as f64, + input.feedback.yaw_speed as f64, + input.dt_s, + ) + .ok() + .map(|output| output.command_deg) + .filter(|deg| deg.is_finite()); + let gimbal_yaw_deg = mpc_command_deg.unwrap_or(yaw_deg); + + // 开火需要 MCU 允许 + 反馈新鲜 + 满足 fire_gap。 + let fire_permit = input.feedback.mcu_fire_permit; + let shot_mode = self.next_shot_mode(fire_permit); let control = CtrlData { - gimbal_yaw: yaw_deg as f32, + gimbal_yaw: gimbal_yaw_deg as f32, gimbal_pitch: pitch_deg as f32, shot_mode, shot_buff_mode: shot_mode_for_task(input.feedback.task_mode), aiming_state: AimingState::AimingWithTarget, }; - let _ = self.yaw_mpc.update_trajectory( - &[yaw_deg; 8], - &[0.0; 8], - input.feedback.gimbal_yaw as f64, - input.feedback.yaw_speed as f64, - input.dt_s, - ); self.last_valid_command = Some(control); self.last_stats = EnergyMechanismControlStats { target_detected: true, track_valid: snapshot.track_valid, - predicted_yaw_deg: yaw_deg, + predicted_yaw_deg: gimbal_yaw_deg, predicted_pitch_deg: pitch_deg, shot_mode, snapshot_stale: stale, @@ -114,6 +173,85 @@ impl EnergyMechanismController { control } + /// 两轮弹道飞行时间迭代 + pitch lead，迁移自 vivsionn `buff_aimer::solve_trajectory`。 + fn solve_trajectory( + &self, + snapshot: EnergyMechanismTrackSnapshot, + bullet_speed_mps: f64, + ) -> (f64, f64, f64) { + let base_dt_s = self.base_predict_time_s(); + let mut predicted = snapshot.predict_target_center_world_m(base_dt_s); + let mut last_fly_time = 0.0_f64; + let mut solved_yaw_deg = 0.0_f64; + let mut solved_pitch_deg = 0.0_f64; + + for pass in 0..2 { + let horizontal = predicted.x.hypot(predicted.y); + let height = predicted.z; + let Some(fly_time) = fly_time(bullet_speed_mps, horizontal, height) else { + return (solved_yaw_deg, solved_pitch_deg, last_fly_time); + }; + last_fly_time = fly_time; + + let total_dt = base_dt_s + fly_time; + predicted = snapshot.predict_target_center_world_m(total_dt); + + if pass == 1 { + let horizontal = predicted.x.hypot(predicted.y); + let height = predicted.z; + let ballistic_pitch = solve_pitch_deg(horizontal, height, bullet_speed_mps); + // estimate_pitch_rate 返回 rad/s；pitch_offset/弹道角都是 deg，lead 需转 deg。 + let pitch_rate_rad_s = self.estimate_pitch_rate(snapshot, total_dt); + let pitch_lead_deg = + (pitch_rate_rad_s * self.pitch_velocity_lead_time_s).to_degrees(); + solved_yaw_deg = command_yaw_deg(predicted, self.yaw_offset_deg); + solved_pitch_deg = ballistic_pitch + self.pitch_offset_deg + pitch_lead_deg; + } + } + (solved_yaw_deg, solved_pitch_deg, last_fly_time) + } + + fn base_predict_time_s(&self) -> f64 { + (DEFAULT_BASE_PREDICT_TIME_S + self.predict_time_s).clamp(0.0, 0.5) + } + + /// 用相邻预测点的 pitch 差分估 pitch 变化率（rad/s）。 + fn estimate_pitch_rate( + &self, + snapshot: EnergyMechanismTrackSnapshot, + predict_dt_s: f64, + ) -> f64 { + let p0 = snapshot.predict_target_center_world_m(predict_dt_s); + let p1 = snapshot.predict_target_center_world_m(predict_dt_s + PITCH_RATE_SAMPLE_DT_S); + let pitch0 = xyz_pitch_rad(p0); + let pitch1 = xyz_pitch_rad(p1); + if pitch0.is_finite() && pitch1.is_finite() { + (pitch1 - pitch0) / PITCH_RATE_SAMPLE_DT_S + } else { + 0.0 + } + } + + /// 从预瞄 horizon 点序列构建 MPC yaw 参考（deg）与 yaw 速率参考（deg/s）。 + fn build_yaw_reference(&self, horizon_points: &[na::Point3]) -> (Vec, Vec) { + let n = horizon_points.len().max(1); + let yaw_ref_rad: Vec = horizon_points + .iter() + .map(|p| command_yaw_rad(*p, self.yaw_offset_deg)) + .collect(); + let yaw_ref_deg: Vec = yaw_ref_rad.iter().map(|r| r.to_degrees()).collect(); + + let mut yaw_rate_deg_s = vec![0.0; n]; + if n > 1 { + for i in 1..n { + let delta = normalize_angle_rad(yaw_ref_rad[i] - yaw_ref_rad[i - 1]); + yaw_rate_deg_s[i] = delta.to_degrees() / self.mpc_model_dt_s; + } + yaw_rate_deg_s[0] = yaw_rate_deg_s[1]; + } + (yaw_ref_deg, yaw_rate_deg_s) + } + fn no_target(&mut self, feedback: SensData) -> CtrlData { self.last_stats = EnergyMechanismControlStats::default(); if let Some(command) = self.last_valid_command { @@ -132,11 +270,15 @@ impl EnergyMechanismController { } } - fn next_shot_mode(&mut self) -> ShotMode { + fn next_shot_mode(&mut self, fire_permit: bool) -> ShotMode { + // MCU 禁火时不下发 ShotOnce，但仍保持瞄准。 + if !fire_permit { + return ShotMode::AimOnly; + } let now = std::time::Instant::now(); if self .last_fire_t - .is_none_or(|last| now.duration_since(last).as_secs_f64() >= FIRE_GAP_S) + .is_none_or(|last| now.duration_since(last).as_secs_f64() >= self.fire_gap_s) { self.last_fire_t = Some(now); ShotMode::ShotOnce @@ -160,37 +302,47 @@ fn feedback_bullet_speed(feedback: SensData) -> f64 { } } -fn approximate_fly_time(snapshot: EnergyMechanismTrackSnapshot, bullet_speed_mps: f64) -> f64 { - let target = snapshot.target_center_world_m; - let distance = target.x.hypot(target.y).max(0.0); - if bullet_speed_mps <= 1.0 { - 0.0 - } else { - (distance / bullet_speed_mps).clamp(0.0, 0.4) +/// 解弹道飞行时间（秒）。距离过近或弹速过低时返回 `None` 表示无法求解。 +fn fly_time(bullet_speed_mps: f64, horizontal_m: f64, height_m: f64) -> Option { + if bullet_speed_mps <= 1.0 || !horizontal_m.is_finite() { + return None; } + // 近似：用直线距离 / 弹速，再夹到合理范围。 + let distance = horizontal_m.hypot(height_m).max(0.0); + Some((distance / bullet_speed_mps).clamp(0.0, 0.4)) } -fn solve_pitch_deg(target_world_m: na::Point3, bullet_speed_mps: f64) -> f64 { - let horizontal = target_world_m.x.hypot(target_world_m.y); - let height = target_world_m.z; +fn solve_pitch_deg(horizontal_m: f64, height_m: f64, bullet_speed_mps: f64) -> f64 { let v2 = bullet_speed_mps * bullet_speed_mps; let discriminant = - v2 * v2 - GRAVITY_MPS2 * (GRAVITY_MPS2 * horizontal * horizontal + 2.0 * height * v2); - if horizontal <= 1e-6 || discriminant < 0.0 || !discriminant.is_finite() { - return height.atan2(horizontal.max(1e-6)).to_degrees(); + v2 * v2 - GRAVITY_MPS2 * (GRAVITY_MPS2 * horizontal_m * horizontal_m + 2.0 * height_m * v2); + if horizontal_m <= 1e-6 || discriminant < 0.0 || !discriminant.is_finite() { + return height_m.atan2(horizontal_m.max(1e-6)).to_degrees(); } - let pitch = ((v2 - discriminant.sqrt()) / (GRAVITY_MPS2 * horizontal)).atan(); + let pitch = ((v2 - discriminant.sqrt()) / (GRAVITY_MPS2 * horizontal_m)).atan(); pitch.to_degrees() } -fn normalize_angle_deg(mut angle: f64) -> f64 { - while angle > 180.0 { - angle -= 360.0; - } - while angle < -180.0 { - angle += 360.0; +fn xyz_pitch_rad(point: na::Point3) -> f64 { + point.z.atan2(point.x.hypot(point.y)) +} + +/// 世界坐标 → 命令 yaw（弧度）。与主自瞄 planner 一致：world +y 映射到负 gimbal yaw。 +fn command_yaw_rad(point: na::Point3, yaw_offset_deg: f64) -> f64 { + let yaw_offset_rad = yaw_offset_deg.to_radians(); + normalize_angle_rad((-point.y).atan2(point.x) + yaw_offset_rad) +} + +fn command_yaw_deg(point: na::Point3, yaw_offset_deg: f64) -> f64 { + command_yaw_rad(point, yaw_offset_deg).to_degrees() +} + +fn normalize_angle_rad(angle: f64) -> f64 { + let mut normalized = (angle + std::f64::consts::PI) % std::f64::consts::TAU; + if normalized < 0.0 { + normalized += std::f64::consts::TAU; } - angle + normalized - std::f64::consts::PI } fn shot_mode_for_task(task_mode: TaskMode) -> ShotBuffMode { @@ -204,8 +356,13 @@ fn shot_mode_for_task(task_mode: TaskMode) -> ShotBuffMode { mod tests { use super::*; use crate::rbt_mod::rbt_comm::rbt_comm_frame::SelfFraction; + use crate::rbt_mod::rbt_energy_mechanism::tracker::CurveSnapshot; fn feedback(task_mode: TaskMode) -> SensData { + feedback_with_permit(task_mode, true) + } + + fn feedback_with_permit(task_mode: TaskMode, mcu_fire_permit: bool) -> SensData { SensData { task_mode, self_fraction: SelfFraction::Blue, @@ -214,16 +371,14 @@ mod tests { gimbal_yaw: 0.0, gimbal_pitch: 0.0, yaw_speed: 0.0, - mcu_fire_permit: true, + mcu_fire_permit, raw_task_mode: task_mode.into(), mapped_task_mode: task_mode, } } - #[test] - fn controller_outputs_energy_mechanism_control() { - let mut controller = EnergyMechanismController::new(); - let snapshot = EnergyMechanismTrackSnapshot { + fn small_snapshot() -> EnergyMechanismTrackSnapshot { + EnergyMechanismTrackSnapshot { mode: EnergyMechanismMode::Small, target_center_world_m: na::Point3::new(4.0, -1.0, 0.5), rune_center_world_m: na::Point3::new(4.0, 0.0, 0.0), @@ -238,10 +393,15 @@ mod tests { target_switched: false, selected_phase_index: Some(0), selected_roll_offset_rad: Some(0.0), - }; + curve: None, + } + } + #[test] + fn controller_outputs_energy_mechanism_control() { + let mut controller = EnergyMechanismController::new(); let control = controller.update(EnergyMechanismControlInput { - target: Some(snapshot), + target: Some(small_snapshot()), feedback: feedback(TaskMode::HitSmallBuff), feedback_fresh: true, dt_s: 0.004, @@ -267,4 +427,123 @@ mod tests { assert_eq!(control.shot_mode, ShotMode::DoNothing); assert_eq!(control.aiming_state, AimingState::AimingNoTarget); } + + #[test] + fn from_aimer_cfg_reads_offsets() { + let aimer = EnergyMechanismAimerCfg { + predict_time_s: 0.05, + fire_gap_s: 0.3, + yaw_offset_deg: 2.0, + pitch_offset_deg: -1.5, + pitch_velocity_lead_time_s: 0.02, + snapshot_stale_ms: 200.0, + }; + let mpc = EnergyMechanismMpcCfg::default(); + let controller = EnergyMechanismController::from_aimer_cfg(&aimer, &mpc); + assert!((controller.predict_time_s - 0.05).abs() < 1e-9); + assert!((controller.fire_gap_s - 0.3).abs() < 1e-9); + assert!((controller.yaw_offset_deg - 2.0).abs() < 1e-9); + assert!((controller.pitch_offset_deg + 1.5).abs() < 1e-9); + assert!((controller.snapshot_stale_ms - 200.0).abs() < 1e-9); + } + + #[test] + fn solve_trajectory_uses_curve_predict_for_large_mode() { + // 大符快照：roll_rate 故意为 0，确保走曲线分支并产生非零 yaw。 + let controller = EnergyMechanismController::new(); + let snapshot = EnergyMechanismTrackSnapshot { + mode: EnergyMechanismMode::Large, + target_center_world_m: na::Point3::new(4.0, 1.0, 0.5), + rune_center_world_m: na::Point3::new(4.0, 0.0, 0.0), + roll_rad: 0.0, + roll_rate_rad_s: 0.0, + direction: 1, + history_size: 10, + lost: false, + track_valid: true, + state_age_s: 0.0, + switch_deferred: false, + target_switched: false, + selected_phase_index: Some(0), + selected_roll_offset_rad: Some(0.0), + curve: Some(CurveSnapshot { + phase: 0.0, + a: 0.9125, + w: 1.942, + curve_speed_rad_s: 1.1775, + }), + }; + let (yaw_deg, _pitch_deg, _fly_time) = controller.solve_trajectory(snapshot, 24.0); + assert!(yaw_deg.is_finite()); + // world +y → 负 gimbal yaw，预测后 y 增大 → yaw 为负。 + assert!(yaw_deg < 0.0, "yaw_deg={yaw_deg}"); + } + + #[test] + fn fly_time_returns_zero_for_close_target() { + assert_eq!(fly_time(24.0, 0.0, 0.0), Some(0.0)); + } + + #[test] + fn fly_time_returns_none_for_low_bullet_speed() { + assert_eq!(fly_time(0.5, 4.0, 0.5), None); + } + + #[test] + fn mcu_fire_permit_blocks_shot_once() { + // MCU 禁火时即使 fire_gap 满足也不应下发 ShotOnce，但仍瞄准。 + let mut controller = EnergyMechanismController::new(); + let control = controller.update(EnergyMechanismControlInput { + target: Some(small_snapshot()), + feedback: feedback_with_permit(TaskMode::HitSmallBuff, false), + feedback_fresh: true, + dt_s: 0.004, + snapshot_age_ms: 5.0, + }); + + assert_eq!(control.aiming_state, AimingState::AimingWithTarget); + assert_eq!(control.shot_mode, ShotMode::AimOnly); + } + + #[test] + fn mcu_fire_permit_allows_shot_once_when_enabled() { + // MCU 允许开火 + fire_gap 满足 → ShotOnce。 + let mut controller = EnergyMechanismController::new(); + let control = controller.update(EnergyMechanismControlInput { + target: Some(small_snapshot()), + feedback: feedback_with_permit(TaskMode::HitSmallBuff, true), + feedback_fresh: true, + dt_s: 0.004, + snapshot_age_ms: 5.0, + }); + + assert_eq!(control.shot_mode, ShotMode::ShotOnce); + } + + #[test] + fn pitch_lead_converts_rad_per_s_to_deg() { + // pitch_rate = 1.0 rad/s, lead_time = 0.1s → lead = 0.1 rad = 5.729... deg。 + // 直接验证单位转换：estimate_pitch_rate 返回 rad/s，最终加到 deg 的 pitch 上。 + let lead_rad_s = 1.0_f64; + let lead_time_s = 0.1_f64; + let lead_deg = (lead_rad_s * lead_time_s).to_degrees(); + assert!((lead_deg - (0.1_f64).to_degrees()).abs() < 1e-9); + assert!((lead_deg - 5.729577951308232).abs() < 1e-6); + } + + #[test] + fn yaw_mpc_command_supersedes_raw_yaw_when_finite() { + // 有目标时 gimbal_yaw 应来自 MPC 输出（有限值），而非 raw solve_trajectory 的 yaw。 + // MPC 在 measured_yaw=0、ref 非零时输出应与 raw 不同。这里只断言控制下发的是有限值， + // 且当 raw yaw 与 measured 差距大时 MPC 会把它拉向参考（不会原样回吐 raw）。 + let mut controller = EnergyMechanismController::new(); + let control = controller.update(EnergyMechanismControlInput { + target: Some(small_snapshot()), + feedback: feedback(TaskMode::HitSmallBuff), + feedback_fresh: true, + dt_s: 0.004, + snapshot_age_ms: 5.0, + }); + assert!(control.gimbal_yaw.is_finite()); + } } diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs b/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs index bf09b40..3ff7c13 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs @@ -1,12 +1,13 @@ use std::collections::VecDeque; +#[cfg(test)] +use super::big_buff_curve_ekf::{BIG_BUFF_BASE_SPEED, big_buff_speed}; +use super::big_buff_curve_ekf::{BigBuffCurveEskf, big_buff_angle_delta}; use super::detected::EnergyMechanismMode; use super::solved::EnergyMechanismSolvedTarget; +use crate::rbt_infra::rbt_cfg::EnergyMechanismTrackerCfg; const HISTORY_CAPACITY: usize = 48; -const LOST_TIMEOUT_S: f64 = 0.35; -const LARGE_CURVE_A: f64 = 0.78; -const LARGE_CURVE_W: f64 = 1.884; const TARGET_SWITCH_SEGMENT_RAD: f64 = std::f64::consts::TAU / 5.0 * 0.45; const TARGET_REACQUIRE_ROLL_GATE_RAD: f64 = 0.12; const TARGET_REACQUIRE_DISTANCE_GATE_M: f64 = 0.45; @@ -28,18 +29,30 @@ pub struct EnergyMechanismTrackSnapshot { pub target_switched: bool, pub selected_phase_index: Option, pub selected_roll_offset_rad: Option, + /// 大符曲线参数（小符时为 `None`）。 + pub curve: Option, +} + +/// 大符曲线 EKF 的对外快照（供 aimer 做曲线预测预瞄）。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct CurveSnapshot { + pub phase: f64, + pub a: f64, + pub w: f64, + pub curve_speed_rad_s: f64, } impl EnergyMechanismTrackSnapshot { + /// 预测 `dt_s` 秒后的目标中心世界坐标。大符用曲线积分，小符用常速线性外推。 pub fn predict_target_center_world_m(self, dt_s: f64) -> na::Point3 { let radius = self.target_center_world_m - self.rune_center_world_m; - let angle = if self.mode == EnergyMechanismMode::Large { - self.roll_rad - + self.direction as f64 - * LARGE_CURVE_A - * ((LARGE_CURVE_W * dt_s.max(0.0)).sin() / LARGE_CURVE_W) - } else { - self.roll_rad + self.roll_rate_rad_s * dt_s.max(0.0) + let angle = match (self.mode, self.curve) { + (EnergyMechanismMode::Large, Some(curve)) => { + // 大符：用曲线 EKF 的角度增量积分。 + let delta = big_buff_angle_delta(curve.a, curve.w, curve.phase, dt_s.max(0.0)); + self.roll_rad + self.direction as f64 * delta + } + _ => self.roll_rad + self.roll_rate_rad_s * dt_s.max(0.0), }; let radius_norm = radius.norm(); if radius_norm <= 1e-9 { @@ -51,6 +64,29 @@ impl EnergyMechanismTrackSnapshot { self.rune_center_world_m.z + radius_norm * angle.sin(), ) } + + /// 为 aimer 生成 yaw 预瞄 horizon：对每个 `dt_s` 步，预测目标中心并返回世界坐标。 + /// 大符用曲线 EKF 推进相位，小符用常速外推。 + pub fn predict_target_horizon(&self, dt_steps: &[f64]) -> Vec> { + let radius = self.target_center_world_m - self.rune_center_world_m; + let radius_norm = radius.norm().max(1e-9); + let mut out = Vec::with_capacity(dt_steps.len()); + for &dt_s in dt_steps { + let angle = match (self.mode, self.curve) { + (EnergyMechanismMode::Large, Some(curve)) => { + let delta = big_buff_angle_delta(curve.a, curve.w, curve.phase, dt_s.max(0.0)); + self.roll_rad + self.direction as f64 * delta + } + _ => self.roll_rad + self.roll_rate_rad_s * dt_s.max(0.0), + }; + out.push(na::Point3::new( + self.rune_center_world_m.x, + self.rune_center_world_m.y + radius_norm * angle.cos(), + self.rune_center_world_m.z + radius_norm * angle.sin(), + )); + } + out + } } #[derive(Debug, Clone)] @@ -78,9 +114,20 @@ pub struct EnergyMechanismTracker { pending_switch_streak: usize, last_switch_deferred: bool, last_target_switched: bool, + /// 大符曲线 EKF。小符时为 `None`；大符时持有并承担相位/曲线预测。 + curve_eskf: Option, + lost_timeout_s: f64, + /// 大符专用丢失超时（比小符更短，激活时目标扇叶会熄灭切换）。 + big_lost_timeout_s: f64, + /// 大符模型完全重置超时：丢失超过这个时间才清掉曲线 EKF，避免短暂遮挡就重学。 + big_model_reset_timeout_s: f64, + /// 保存构造时的 tracker 配置，模式切换到 Large 时用它重建曲线 EKF， + /// 避免从 Small 切 Large 时丢失 rbt_cfg.toml 里的大符参数。 + tracker_cfg: EnergyMechanismTrackerCfg, } impl EnergyMechanismTracker { + /// 无参默认构造（保持向后兼容）。 pub fn new(mode: EnergyMechanismMode) -> Self { Self { mode, @@ -100,11 +147,45 @@ impl EnergyMechanismTracker { pending_switch_streak: 0, last_switch_deferred: false, last_target_switched: false, + curve_eskf: None, + lost_timeout_s: 0.35, + big_lost_timeout_s: 0.08, + big_model_reset_timeout_s: 0.35, + tracker_cfg: EnergyMechanismTrackerCfg::default(), + } + } + + /// 从配置构造（读 `lost_timeout_s` 等）。大符模式会预建曲线 EKF。 + pub fn from_tracker_cfg(mode: EnergyMechanismMode, cfg: &EnergyMechanismTrackerCfg) -> Self { + let mut tracker = Self::new(mode); + tracker.lost_timeout_s = cfg.lost_timeout_s.max(0.0); + tracker.big_lost_timeout_s = cfg.big_lost_timeout_s.max(0.0); + // model reset timeout 必须 >= lost timeout，否则会在 lost 之前就重置。 + tracker.big_model_reset_timeout_s = cfg + .big_model_reset_timeout_s + .max(tracker.big_lost_timeout_s); + tracker.tracker_cfg = cfg.clone(); + if mode == EnergyMechanismMode::Large { + tracker.curve_eskf = Some(BigBuffCurveEskf::from_tracker_cfg(cfg)); } + tracker } pub fn reset(&mut self, mode: EnergyMechanismMode) { - *self = Self::new(mode); + let lost_timeout_s = self.lost_timeout_s; + let big_lost_timeout_s = self.big_lost_timeout_s; + let big_model_reset_timeout_s = self.big_model_reset_timeout_s; + let tracker_cfg = self.tracker_cfg.clone(); + let mut next = Self::new(mode); + next.lost_timeout_s = lost_timeout_s; + next.big_lost_timeout_s = big_lost_timeout_s; + next.big_model_reset_timeout_s = big_model_reset_timeout_s; + next.tracker_cfg = tracker_cfg; + if mode == EnergyMechanismMode::Large { + // 始终用保存的真实配置重建曲线 EKF，避免 Small→Large 切换时丢 rbt_cfg.toml 参数。 + next.curve_eskf = Some(BigBuffCurveEskf::from_tracker_cfg(&next.tracker_cfg)); + } + *self = next; } pub fn update( @@ -126,6 +207,19 @@ impl EnergyMechanismTracker { self.last_switch_deferred = false; self.last_target_switched = false; + // target 缺失时按 mode 判定丢失，大符长时间丢失则清掉曲线 EKF。 + if target.is_none() + && self.mode == EnergyMechanismMode::Large + && self + .last_seen_tp + .map(|last| now.duration_since(last).as_secs_f64()) + .unwrap_or(f64::INFINITY) + > self.big_model_reset_timeout_s + && let Some(eskf) = &mut self.curve_eskf + { + eskf.reset(); + } + if let Some(target) = target { if self.mode == EnergyMechanismMode::Large && self.should_defer_target_switch(target) @@ -150,7 +244,18 @@ impl EnergyMechanismTracker { .last_seen_tp .map(|last| now.duration_since(last).as_secs_f64()) .unwrap_or(f64::INFINITY); - let lost = state_age_s > LOST_TIMEOUT_S; + // 大符用更短的 big_lost_timeout_s 判 lost（激活时目标切换快）。 + let lost_timeout = match self.mode { + EnergyMechanismMode::Large => self.big_lost_timeout_s, + EnergyMechanismMode::Small => self.lost_timeout_s, + }; + let lost = state_age_s > lost_timeout; + let curve = self.curve_eskf.as_ref().map(|eskf| CurveSnapshot { + phase: eskf.phase(), + a: eskf.a(), + w: eskf.w(), + curve_speed_rad_s: eskf.curve_speed(), + }); Some(EnergyMechanismTrackSnapshot { mode: self.mode, target_center_world_m: self.last_target_center_world_m, @@ -166,6 +271,7 @@ impl EnergyMechanismTracker { target_switched: self.last_target_switched, selected_phase_index: self.selected_phase_index, selected_roll_offset_rad: self.selected_roll_offset_rad, + curve, }) } @@ -191,6 +297,10 @@ impl EnergyMechanismTracker { self.direction = retained_direction; self.history.clear(); self.last_target_switched = reinitialize; + if let Some(eskf) = &mut self.curve_eskf { + eskf.reset(); + eskf.set_direction(retained_direction); + } } else { let delta = normalize_angle(observed_roll - self.filtered_roll_rad); let raw_rate = delta / dt_s; @@ -200,6 +310,13 @@ impl EnergyMechanismTracker { if raw_rate.abs() > 0.05 { self.direction = if raw_rate > 0.0 { 1 } else { -1 }; } + // 大符：推进曲线 EKF 并尝试速度测量更新。 + if let Some(eskf) = &mut self.curve_eskf { + eskf.set_direction(self.direction); + eskf.predict(dt_s); + let _ = eskf.update_with_speed(dt_s); + eskf.refresh_smoothed_speed(dt_s); + } } self.last_target_center_world_m = target.pose.target_center_world_m; @@ -216,6 +333,10 @@ impl EnergyMechanismTracker { while self.history.len() > HISTORY_CAPACITY { self.history.pop_front(); } + // 大符：记录 roll 样本供曲线 EKF 的 rolling-window 估速与 φ seed。 + if let Some(eskf) = &mut self.curve_eskf { + eskf.record_roll(time_s, observed_roll); + } self.fit_direction_from_history(); } @@ -342,6 +463,10 @@ mod tests { } } + fn default_tracker_cfg() -> EnergyMechanismTrackerCfg { + EnergyMechanismTrackerCfg::default() + } + #[test] fn tracker_reports_valid_after_two_observations() { let mut tracker = EnergyMechanismTracker::new(EnergyMechanismMode::Small); @@ -354,6 +479,8 @@ mod tests { assert!(snapshot.track_valid); assert_eq!(snapshot.direction, 1); assert!(snapshot.roll_rate_rad_s > 0.0); + // 小符没有曲线 EKF 快照。 + assert!(snapshot.curve.is_none()); } #[test] @@ -368,7 +495,10 @@ mod tests { #[test] fn large_tracker_defers_then_rebinds_confirmed_target_switch() { - let mut tracker = EnergyMechanismTracker::new(EnergyMechanismMode::Large); + let mut tracker = EnergyMechanismTracker::from_tracker_cfg( + EnergyMechanismMode::Large, + &default_tracker_cfg(), + ); tracker.update( EnergyMechanismMode::Large, Some(&target_with_mode(EnergyMechanismMode::Large, 0.0, 0)), @@ -407,4 +537,164 @@ mod tests { assert!(confirmed_switch.target_switched); assert_eq!(confirmed_switch.selected_phase_index, Some(1)); } + + #[test] + fn large_tracker_exposes_curve_snapshot_after_observations() { + let mut tracker = EnergyMechanismTracker::from_tracker_cfg( + EnergyMechanismMode::Large, + &default_tracker_cfg(), + ); + for i in 0..6 { + tracker.update( + EnergyMechanismMode::Large, + Some(&target_with_mode( + EnergyMechanismMode::Large, + i as f64 * 0.05, + 0, + )), + ); + } + let snapshot = tracker.snapshot(std::time::Instant::now()).unwrap(); + let curve = snapshot + .curve + .expect("curve snapshot present for large mode"); + assert!(curve.curve_speed_rad_s > 0.0); + assert!(curve.curve_speed_rad_s < BIG_BUFF_BASE_SPEED * 2.0); + } + + #[test] + fn predict_horizon_returns_one_point_per_step() { + let tracker = EnergyMechanismTracker::new(EnergyMechanismMode::Small); + // 直接构造一个快照测试 horizon。 + let snapshot = EnergyMechanismTrackSnapshot { + mode: EnergyMechanismMode::Small, + target_center_world_m: na::Point3::new(1.0, 1.0, 0.0), + rune_center_world_m: na::Point3::origin(), + roll_rad: 0.0, + roll_rate_rad_s: 1.0, + direction: 1, + history_size: 4, + lost: false, + track_valid: true, + state_age_s: 0.0, + switch_deferred: false, + target_switched: false, + selected_phase_index: Some(0), + selected_roll_offset_rad: Some(0.0), + curve: None, + }; + let horizon = snapshot.predict_target_horizon(&[0.0, 0.1, 0.2]); + assert_eq!(horizon.len(), 3); + // 半径 = target_center - rune_center = √2，angle = roll_rate·dt = 0.1。 + let radius = 2_f64.sqrt(); + assert!((horizon[1].y - radius * 0.1_f64.cos()).abs() < 1e-6); + let _ = tracker; + } + + #[test] + fn from_tracker_cfg_reads_lost_timeout() { + let mut cfg = default_tracker_cfg(); + cfg.lost_timeout_s = 0.5; + let tracker = EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Small, &cfg); + assert!((tracker.lost_timeout_s - 0.5).abs() < 1e-9); + } + + #[test] + fn small_to_large_switch_preserves_tracker_cfg() { + // 生产路径：先以 Small + 自定义配置构造，再切 Large。 + // 切换后必须用真实配置（而非 default）重建曲线 EKF。 + let mut cfg = default_tracker_cfg(); + cfg.big_phase_process_noise = 0.123; + cfg.lost_timeout_s = 0.42; + let mut tracker = + EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Small, &cfg); + // Small 时无曲线 EKF。 + assert!(tracker.curve_eskf.is_none()); + + // 切 Large：reset 后应有用真实配置重建的曲线 EKF，且 lost_timeout 保留。 + tracker.reset(EnergyMechanismMode::Large); + assert!(tracker.curve_eskf.is_some()); + assert!((tracker.lost_timeout_s - 0.42).abs() < 1e-9); + // tracker_cfg 应保留自定义值。 + assert!((tracker.tracker_cfg.big_phase_process_noise - 0.123).abs() < 1e-9); + } + + #[test] + fn large_to_small_then_back_to_large_keeps_cfg() { + let mut cfg = default_tracker_cfg(); + cfg.big_a_process_noise = 0.007; + let mut tracker = + EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Large, &cfg); + tracker.reset(EnergyMechanismMode::Small); + tracker.reset(EnergyMechanismMode::Large); + assert!(tracker.curve_eskf.is_some()); + assert!((tracker.tracker_cfg.big_a_process_noise - 0.007).abs() < 1e-9); + } + + #[test] + fn from_tracker_cfg_reads_big_timeout_fields() { + let mut cfg = default_tracker_cfg(); + cfg.big_lost_timeout_s = 0.05; + cfg.big_model_reset_timeout_s = 0.4; + let tracker = EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Large, &cfg); + assert!((tracker.big_lost_timeout_s - 0.05).abs() < 1e-9); + assert!((tracker.big_model_reset_timeout_s - 0.4).abs() < 1e-9); + } + + #[test] + fn big_model_reset_timeout_clamped_above_big_lost_timeout() { + // model reset timeout 必须 >= big_lost_timeout，配置里写更小值时自动抬升。 + let mut cfg = default_tracker_cfg(); + cfg.big_lost_timeout_s = 0.3; + cfg.big_model_reset_timeout_s = 0.1; // 比 big_lost 还小 + let tracker = EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Large, &cfg); + assert!(tracker.big_model_reset_timeout_s >= tracker.big_lost_timeout_s); + } + + #[test] + fn predict_target_center_uses_curve_for_large_mode() { + // 大符快照用曲线 EKF 的 angle_delta，而非 roll_rate 线性外推。 + let snapshot = EnergyMechanismTrackSnapshot { + mode: EnergyMechanismMode::Large, + target_center_world_m: na::Point3::new(4.0, 1.0, 0.0), + rune_center_world_m: na::Point3::new(4.0, 0.0, 0.0), + roll_rad: 0.0, + roll_rate_rad_s: 0.0, // 故意设 0，确保大符不走常速分支 + direction: 1, + history_size: 10, + lost: false, + track_valid: true, + state_age_s: 0.0, + switch_deferred: false, + target_switched: false, + selected_phase_index: Some(0), + selected_roll_offset_rad: Some(0.0), + curve: Some(CurveSnapshot { + phase: 0.0, + a: 0.9125, + w: 1.942, + curve_speed_rad_s: 1.1775, + }), + }; + let predicted = snapshot.predict_target_center_world_m(0.1); + // 曲线分支：dt=0 时 angle=0 → y=cos(0)=1.0；dt=0.1 时 angle≈0.118 → y 略小于 1。 + // 关键是大符用了曲线 delta（非 roll_rate=0 的零外推），y 应严格小于 1。 + assert!( + predicted.y < 1.0, + "y={} should be < 1.0 (curve delta applied)", + predicted.y + ); + assert!( + predicted.y > 0.98, + "y={} should be > 0.98 (curve delta is small)", + predicted.y + ); + } + + #[test] + fn curve_speed_via_big_buff_speed_helper() { + // 确认 big_buff_speed 导出可用且与曲线快照一致。 + let speed = big_buff_speed(0.9125, std::f64::consts::FRAC_PI_2); + assert!((speed - BIG_BUFF_BASE_SPEED).abs() < 1e-9); + } } diff --git a/lib/src/rbt_mod/rbt_estimator/rbt_ypd_angle_tracker.rs b/lib/src/rbt_mod/rbt_estimator/rbt_ypd_angle_tracker.rs index 3917413..3cee5df 100644 --- a/lib/src/rbt_mod/rbt_estimator/rbt_ypd_angle_tracker.rs +++ b/lib/src/rbt_mod/rbt_estimator/rbt_ypd_angle_tracker.rs @@ -1,5 +1,6 @@ use std::collections::VecDeque; +use crate::rbt_base::rbt_algorithm::rbt_ekf::ExtendedKalmanFilter; use crate::rbt_infra::rbt_cfg::EstimatorCfg; const STATE_DIM: usize = 11; @@ -134,8 +135,7 @@ pub struct YpdAngleTracker { update_count: usize, tracker_time_s: f64, is_converged: bool, - x: na::SVector, - p: na::SMatrix, + ekf: ExtendedKalmanFilter, last_nis: f64, recent_nis_failures: VecDeque, last_batch_match_ids: Vec, @@ -153,6 +153,26 @@ pub struct YpdAngleTracker { } impl YpdAngleTracker { + /// 当前名义状态（11 维 CV 模型）。 + pub fn x(&self) -> &na::DVector { + &self.ekf.x + } + + /// 当前协方差矩阵。 + pub fn p(&self) -> &na::DMatrix { + &self.ekf.p + } + + /// 可变借用名义状态。 + fn x_mut(&mut self) -> &mut na::DVector { + &mut self.ekf.x + } + + /// 可变借用协方差矩阵。 + fn p_mut(&mut self) -> &mut na::DMatrix { + &mut self.ekf.p + } + pub fn new() -> Self { let mut tracker = Self { initialized: false, @@ -162,8 +182,10 @@ impl YpdAngleTracker { update_count: 0, tracker_time_s: 0.0, is_converged: false, - x: na::SVector::::zeros(), - p: na::SMatrix::::identity(), + ekf: ExtendedKalmanFilter::with_initial( + na::DVector::zeros(STATE_DIM), + na::DMatrix::identity(STATE_DIM, STATE_DIM), + ), last_nis: 0.0, recent_nis_failures: VecDeque::from([false]), last_batch_match_ids: Vec::new(), @@ -191,8 +213,10 @@ impl YpdAngleTracker { self.update_count = 0; self.tracker_time_s = 0.0; self.is_converged = false; - self.x.fill(0.0); - self.p = na::SMatrix::::identity(); + self.ekf.init( + na::DVector::zeros(STATE_DIM), + na::DMatrix::identity(STATE_DIM, STATE_DIM), + ); self.last_nis = 0.0; self.recent_nis_failures.clear(); self.recent_nis_failures.push_back(false); @@ -224,13 +248,16 @@ impl YpdAngleTracker { let yaw = radial_yaw_from_observed_yaw(observation.yaw_rad, self.armor_num); let sign = radial_sign(self.armor_num); - self.x[0] = observation.position_mm.x - sign * radius * yaw.cos(); - self.x[2] = observation.position_mm.y - sign * radius * yaw.sin(); - self.x[4] = observation.position_mm.z; - self.x[6] = yaw; - self.x[8] = radius; - - self.p = if self.is_outpost { + let x0 = { + let mut v = na::DVector::zeros(STATE_DIM); + v[0] = observation.position_mm.x - sign * radius * yaw.cos(); + v[2] = observation.position_mm.y - sign * radius * yaw.sin(); + v[4] = observation.position_mm.z; + v[6] = yaw; + v[8] = radius; + v + }; + let p0 = if self.is_outpost { diagonal_matrix([ 1_000.0, 64_000.0, 1_000.0, 64_000.0, 1_000.0, 81_000.0, 0.4, 100.0, 100.0, 90_000.0, 90_000.0, @@ -241,6 +268,7 @@ impl YpdAngleTracker { 10_000.0, 10_000.0, ]) }; + self.ekf.init(x0, p0); self.initialized = true; self.tracked_id = self.select_best_armor_id(observation); @@ -263,19 +291,19 @@ impl YpdAngleTracker { }; self.tracker_time_s += dt; - if self.is_outpost && self.converged() && self.x[7].abs() > 2.0 { - self.x[7] = self.x[7].signum() * 2.51; + if self.is_outpost && self.converged() && self.x()[7].abs() > 2.0 { + self.x_mut()[7] = self.x()[7].signum() * 2.51; } - let mut f = na::SMatrix::::identity(); + let mut f = na::DMatrix::::identity(STATE_DIM, STATE_DIM); f[(0, 1)] = dt; f[(2, 3)] = dt; f[(4, 5)] = dt; f[(6, 7)] = dt; + let q = self.process_noise(dt); - self.x = f * self.x; - self.x[6] = normalize_angle(self.x[6]); - self.p = symmetrize(f * self.p * f.transpose() + self.process_noise(dt)); + self.ekf.predict(&f, &q); + self.x_mut()[6] = normalize_angle(self.x()[6]); self.clamp_geometry(); } @@ -360,17 +388,17 @@ impl YpdAngleTracker { let tracked_armor_xyza = self.predicted_armor_state(self.tracked_id); let mut state11d = [0.0; STATE_DIM]; - state11d.copy_from_slice(self.x.as_slice()); + state11d.copy_from_slice(self.x().as_slice()); let mut state9 = [0.0; 9]; - state9[0] = self.x[0]; - state9[1] = self.x[1]; - state9[2] = self.x[2]; - state9[3] = self.x[3]; + state9[0] = self.x()[0]; + state9[1] = self.x()[1]; + state9[2] = self.x()[2]; + state9[3] = self.x()[3]; state9[4] = tracked_armor_xyza[2]; - state9[5] = self.x[5]; + state9[5] = self.x()[5]; state9[6] = tracked_armor_xyza[3]; - state9[7] = self.x[7]; + state9[7] = self.x()[7]; state9[8] = self.armor_radius(self.tracked_id); Some(YpdTrackerSnapshot { @@ -397,18 +425,18 @@ impl YpdAngleTracker { return false; } let primary_ok = - self.x[PRIMARY_RADIUS] > MIN_RADIUS_MM && self.x[PRIMARY_RADIUS] < MAX_RADIUS_MM; + self.x()[PRIMARY_RADIUS] > MIN_RADIUS_MM && self.x()[PRIMARY_RADIUS] < MAX_RADIUS_MM; if !primary_ok { return true; } if self.armor_num == 4 { - let secondary = self.x[PRIMARY_RADIUS] + self.x[DELTA_RADIUS]; + let secondary = self.x()[PRIMARY_RADIUS] + self.x()[DELTA_RADIUS]; !(secondary > MIN_RADIUS_MM && secondary < MAX_RADIUS_MM) } else { - !self.x[DELTA_RADIUS].is_finite() - || !self.x[HEIGHT_DIFF].is_finite() - || self.x[DELTA_RADIUS].abs() > OUTPOST_MAX_HEIGHT_OFFSET_MM - || self.x[HEIGHT_DIFF].abs() > OUTPOST_MAX_HEIGHT_OFFSET_MM + !self.x()[DELTA_RADIUS].is_finite() + || !self.x()[HEIGHT_DIFF].is_finite() + || self.x()[DELTA_RADIUS].abs() > OUTPOST_MAX_HEIGHT_OFFSET_MM + || self.x()[HEIGHT_DIFF].abs() > OUTPOST_MAX_HEIGHT_OFFSET_MM } } @@ -430,14 +458,14 @@ impl YpdAngleTracker { self.is_converged } - fn process_noise(&self, dt: f64) -> na::SMatrix { + fn process_noise(&self, dt: f64) -> na::DMatrix { let pos_noise = if self.is_outpost { 10_000.0 } else { 100_000.0 }; let yaw_noise = if self.is_outpost { 0.1 } else { 400.0 }; let a = dt.powi(4) / 4.0; let b = dt.powi(3) / 2.0; let c = dt * dt; - let mut q = na::SMatrix::::zeros(); + let mut q = na::DMatrix::::zeros(STATE_DIM, STATE_DIM); for (pos, vel, noise) in [ (0, 1, pos_noise), (2, 3, pos_noise), @@ -452,11 +480,7 @@ impl YpdAngleTracker { q } - fn predicted_armor_position( - &self, - state: &na::SVector, - id: usize, - ) -> na::Point3 { + fn predicted_armor_position(&self, state: &na::DVector, id: usize) -> na::Point3 { let angle = normalize_angle(state[6] + id as f64 * std::f64::consts::TAU / self.armor_num as f64); let radius = radius_from_state(state, self.armor_num, id); @@ -471,9 +495,9 @@ impl YpdAngleTracker { fn predicted_armor_state(&self, id: usize) -> [f64; 4] { let clamped_id = id.min(self.armor_num.saturating_sub(1)); let angle = normalize_angle( - self.x[6] + clamped_id as f64 * std::f64::consts::TAU / self.armor_num as f64, + self.x()[6] + clamped_id as f64 * std::f64::consts::TAU / self.armor_num as f64, ); - let position = self.predicted_armor_position(&self.x, clamped_id); + let position = self.predicted_armor_position(self.x(), clamped_id); [ position.x, position.y, @@ -482,28 +506,20 @@ impl YpdAngleTracker { ] } - fn predicted_measurement( - &self, - state: &na::SVector, - id: usize, - ) -> na::SVector { + fn predicted_measurement(&self, state: &na::DVector, id: usize) -> na::DVector { let position = self.predicted_armor_position(state, id); let ypd = xyz_to_ypd(position); let angle = normalize_angle(state[6] + id as f64 * std::f64::consts::TAU / self.armor_num as f64); - na::SVector::::new( + na::DVector::from_vec(vec![ ypd.x, ypd.y, ypd.z, observed_yaw_from_radial_yaw(angle, self.armor_num), - ) + ]) } - fn measurement_jacobian( - &self, - state: &na::SVector, - id: usize, - ) -> na::SMatrix { + fn measurement_jacobian(&self, state: &na::DVector, id: usize) -> na::DMatrix { let angle = normalize_angle(state[6] + id as f64 * std::f64::consts::TAU / self.armor_num as f64); let use_secondary_radius = self.armor_num == 4 && (id == 1 || id == 3); @@ -514,7 +530,7 @@ impl YpdAngleTracker { state[PRIMARY_RADIUS] }; - let mut h_xyza = na::SMatrix::::zeros(); + let mut h_xyza = na::DMatrix::::zeros(4, STATE_DIM); h_xyza[(0, 0)] = 1.0; h_xyza[(0, 6)] = -sign * radius * angle.sin(); h_xyza[(0, PRIMARY_RADIUS)] = sign * angle.cos(); @@ -550,19 +566,23 @@ impl YpdAngleTracker { h_xyza[(3, 6)] = 1.0; let h_ypd = xyz_to_ypd_jacobian(self.predicted_armor_position(state, id)); - let mut h_ypda = na::SMatrix::::zeros(); - h_ypda.fixed_view_mut::<3, 3>(0, 0).copy_from(&h_ypd); + let mut h_ypda = na::DMatrix::::zeros(4, 4); + for row in 0..3 { + for col in 0..3 { + h_ypda[(row, col)] = h_ypd[(row, col)]; + } + } h_ypda[(3, 3)] = 1.0; h_ypda * h_xyza } - fn measurement_noise(&self, observation: &YpdObservation) -> na::SMatrix { + fn measurement_noise(&self, observation: &YpdObservation) -> na::DMatrix { let ypd = xyz_to_ypd(observation.position_mm); let center_yaw = observation.position_mm.y.atan2(observation.position_mm.x); let delta_angle = normalize_angle(observation.yaw_rad - center_yaw).abs(); let distance_sigma_mm = (ypd.z.abs() * 0.03).clamp(10.0, 250.0); - let mut r = na::SMatrix::::zeros(); + let mut r = na::DMatrix::::zeros(4, 4); r[(0, 0)] = 4e-3; r[(1, 1)] = 4e-3; r[(2, 2)] = distance_sigma_mm * distance_sigma_mm; @@ -590,22 +610,19 @@ impl YpdAngleTracker { fn correct_with_observation(&mut self, observation: &YpdObservation, id: usize) -> bool { let matched_id = id.min(self.armor_num.saturating_sub(1)); let ypd = xyz_to_ypd(observation.position_mm); - let z = na::SVector::::new(ypd.x, ypd.y, ypd.z, observation.yaw_rad); - let h = self.measurement_jacobian(&self.x, matched_id); + let z = na::DVector::from_vec(vec![ypd.x, ypd.y, ypd.z, observation.yaw_rad]); + let h = self.measurement_jacobian(self.x(), matched_id); let r = self.measurement_noise(observation); - let predicted = self.predicted_measurement(&self.x, matched_id); - let mut residual = z - predicted; - residual[0] = normalize_angle(residual[0]); - residual[1] = normalize_angle(residual[1]); - residual[3] = normalize_angle(residual[3]); - - let s = h * self.p * h.transpose() + r; - let Some(s_inv) = s.try_inverse() else { - self.record_nis(f64::INFINITY); - return false; + let predicted = self.predicted_measurement(self.x(), matched_id); + let residual_fn = |a: &na::DVector, b: &na::DVector| { + let mut diff = a - b; + diff[0] = normalize_angle(diff[0]); + diff[1] = normalize_angle(diff[1]); + diff[3] = normalize_angle(diff[3]); + diff }; - let prior_nis = residual.transpose() * s_inv * residual; - let prior_nis = prior_nis[(0, 0)]; + let prior_nis = self.ekf.nis(&z, &h, &r, &predicted, residual_fn); + if self.is_outpost && self.update_count >= OUTPOST_PRIOR_GATE_MIN_UPDATES && prior_nis.is_finite() @@ -616,15 +633,16 @@ impl YpdAngleTracker { return false; } - let k = self.p * h.transpose() * s_inv; - let i = na::SMatrix::::identity(); - self.x += k * residual; - self.x[6] = normalize_angle(self.x[6]); - self.p = symmetrize((i - k * h) * self.p * (i - k * h).transpose() + k * r * k.transpose()); + let (accepted, nis) = self.ekf.update(&z, &h, &r, &predicted, residual_fn); + if !accepted { + self.record_nis(f64::INFINITY); + return false; + } + self.x_mut()[6] = normalize_angle(self.x()[6]); self.apply_outpost_radius_prior(); self.clamp_geometry(); - self.record_nis(prior_nis); + self.record_nis(nis); self.update_count += 1; self.consecutive_rejected_updates = 0; true @@ -635,7 +653,7 @@ impl YpdAngleTracker { observation: &YpdObservation, id: usize, ) -> GeometryRecoverySample { - let predicted = self.predicted_armor_position(&self.x, id.min(self.armor_num - 1)); + let predicted = self.predicted_armor_position(self.x(), id.min(self.armor_num - 1)); let residual = observation.position_mm - predicted; GeometryRecoverySample { xy_residual_mm: residual.x.hypot(residual.y), @@ -666,8 +684,8 @@ impl YpdAngleTracker { .map(|sample| sample.z_residual_mm) .sum::() / samples.len() as f64; - let sigma_dr = self.p[(DELTA_RADIUS, DELTA_RADIUS)].max(1e-9).sqrt(); - let sigma_h = self.p[(HEIGHT_DIFF, HEIGHT_DIFF)].max(1e-9).sqrt(); + let sigma_dr = self.p()[(DELTA_RADIUS, DELTA_RADIUS)].max(1e-9).sqrt(); + let sigma_h = self.p()[(HEIGHT_DIFF, HEIGHT_DIFF)].max(1e-9).sqrt(); let xy_over_sigma_dr = mean_xy / sigma_dr; let z_over_sigma_h = mean_z / sigma_h; let mismatch = z_over_sigma_h.is_finite() @@ -701,17 +719,18 @@ impl YpdAngleTracker { let min_dr_var_mm2 = cfg.ypd_geometry_recovery_min_dr_variance.max(0.0) * M2_TO_MM2; let min_h_var_mm2 = cfg.ypd_geometry_recovery_min_h_variance.max(0.0) * M2_TO_MM2; + let p = self.p_mut(); for index in [DELTA_RADIUS, HEIGHT_DIFF] { for col in 0..STATE_DIM { - self.p[(index, col)] *= scale.sqrt(); - self.p[(col, index)] = self.p[(index, col)]; + p[(index, col)] *= scale.sqrt(); + p[(col, index)] = p[(index, col)]; } } - self.p[(DELTA_RADIUS, DELTA_RADIUS)] = - (self.p[(DELTA_RADIUS, DELTA_RADIUS)] * scale).max(min_dr_var_mm2); - self.p[(HEIGHT_DIFF, HEIGHT_DIFF)] = - (self.p[(HEIGHT_DIFF, HEIGHT_DIFF)] * scale).max(min_h_var_mm2); - self.p = symmetrize(self.p); + p[(DELTA_RADIUS, DELTA_RADIUS)] = + (p[(DELTA_RADIUS, DELTA_RADIUS)] * scale).max(min_dr_var_mm2); + p[(HEIGHT_DIFF, HEIGHT_DIFF)] = (p[(HEIGHT_DIFF, HEIGHT_DIFF)] * scale).max(min_h_var_mm2); + let sym = symmetrize_dynamic(p); + *p = sym; } fn assign_armor_ids(&self, observations: &[YpdObservation]) -> Vec> { @@ -736,10 +755,10 @@ impl YpdAngleTracker { } fn armor_radius(&self, id: usize) -> f64 { - radius_from_state(&self.x, self.armor_num, id) + radius_from_state(self.x(), self.armor_num, id) } - fn height_offset_for_id(&self, state: &na::SVector, id: usize) -> f64 { + fn height_offset_for_id(&self, state: &na::DVector, id: usize) -> f64 { if self.outpost_height_phase_locked() { self.outpost_height_offset_for_id(id) } else { @@ -764,22 +783,28 @@ impl YpdAngleTracker { } fn clamp_geometry(&mut self) { - self.x[PRIMARY_RADIUS] = self.x[PRIMARY_RADIUS].clamp(MIN_RADIUS_MM, MAX_RADIUS_MM); - if self.armor_num == 4 { - let secondary = - (self.x[PRIMARY_RADIUS] + self.x[DELTA_RADIUS]).clamp(MIN_RADIUS_MM, MAX_RADIUS_MM); - self.x[DELTA_RADIUS] = secondary - self.x[PRIMARY_RADIUS]; - } else { - self.x[PRIMARY_RADIUS] = OUTPOST_RADIUS_MM; - if self.outpost_height_phase_locked() { - self.apply_locked_outpost_height_offsets(); + let armor_num = self.armor_num; + let locked = self.outpost_height_phase_locked(); + { + let x = self.x_mut(); + x[PRIMARY_RADIUS] = x[PRIMARY_RADIUS].clamp(MIN_RADIUS_MM, MAX_RADIUS_MM); + if armor_num == 4 { + let secondary = + (x[PRIMARY_RADIUS] + x[DELTA_RADIUS]).clamp(MIN_RADIUS_MM, MAX_RADIUS_MM); + x[DELTA_RADIUS] = secondary - x[PRIMARY_RADIUS]; } else { - self.x[DELTA_RADIUS] = self.x[DELTA_RADIUS] - .clamp(-OUTPOST_MAX_HEIGHT_OFFSET_MM, OUTPOST_MAX_HEIGHT_OFFSET_MM); - self.x[HEIGHT_DIFF] = self.x[HEIGHT_DIFF] - .clamp(-OUTPOST_MAX_HEIGHT_OFFSET_MM, OUTPOST_MAX_HEIGHT_OFFSET_MM); + x[PRIMARY_RADIUS] = OUTPOST_RADIUS_MM; + if !locked { + x[DELTA_RADIUS] = x[DELTA_RADIUS] + .clamp(-OUTPOST_MAX_HEIGHT_OFFSET_MM, OUTPOST_MAX_HEIGHT_OFFSET_MM); + x[HEIGHT_DIFF] = x[HEIGHT_DIFF] + .clamp(-OUTPOST_MAX_HEIGHT_OFFSET_MM, OUTPOST_MAX_HEIGHT_OFFSET_MM); + } } } + if armor_num == 3 && locked { + self.apply_locked_outpost_height_offsets(); + } } fn reset_outpost_height_phase(&mut self) { @@ -807,10 +832,10 @@ impl YpdAngleTracker { if !self.outpost_height_phase_locked() { return; } - self.x[DELTA_RADIUS] = self.outpost_height_offset_for_id(1); - self.x[HEIGHT_DIFF] = self.outpost_height_offset_for_id(2); - self.p[(DELTA_RADIUS, DELTA_RADIUS)] = OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2; - self.p[(HEIGHT_DIFF, HEIGHT_DIFF)] = OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2; + self.x_mut()[DELTA_RADIUS] = self.outpost_height_offset_for_id(1); + self.x_mut()[HEIGHT_DIFF] = self.outpost_height_offset_for_id(2); + self.p_mut()[(DELTA_RADIUS, DELTA_RADIUS)] = OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2; + self.p_mut()[(HEIGHT_DIFF, HEIGHT_DIFF)] = OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2; } fn update_outpost_height_phase(&mut self, observation: &YpdObservation, matched_id: usize) { @@ -868,7 +893,7 @@ impl YpdAngleTracker { self.outpost_height_phase_scores[second] - self.outpost_height_phase_scores[best]; if candidate_center_z[best].is_finite() && margin >= OUTPOST_HEIGHT_PHASE_LOCK_MARGIN { self.outpost_height_phase = Some(best); - self.x[4] = candidate_center_z[best]; + self.x_mut()[4] = candidate_center_z[best]; self.apply_locked_outpost_height_offsets(); } } @@ -910,29 +935,37 @@ impl YpdAngleTracker { return; } let prior_variance = OUTPOST_RADIUS_PRIOR_SIGMA_MM * OUTPOST_RADIUS_PRIOR_SIGMA_MM; - let innovation_variance = self.p[(PRIMARY_RADIUS, PRIMARY_RADIUS)] + prior_variance; + let innovation_variance = self.p()[(PRIMARY_RADIUS, PRIMARY_RADIUS)] + prior_variance; if !innovation_variance.is_finite() || innovation_variance <= 1e-9 { return; } - let k = self.p.column(PRIMARY_RADIUS) / innovation_variance; - self.x += k * (OUTPOST_RADIUS_MM - self.x[PRIMARY_RADIUS]); - self.x[6] = normalize_angle(self.x[6]); + let k = self.p().column(PRIMARY_RADIUS).into_owned() / innovation_variance; + let innovation = OUTPOST_RADIUS_MM - self.x()[PRIMARY_RADIUS]; + { + let x = self.x_mut(); + for row in 0..STATE_DIM { + x[row] += k[row] * innovation; + } + x[6] = normalize_angle(x[6]); + } - let mut i_kh = na::SMatrix::::identity(); + let mut i_kh = na::DMatrix::::identity(STATE_DIM, STATE_DIM); for row in 0..STATE_DIM { i_kh[(row, PRIMARY_RADIUS)] -= k[row]; } - let rk = prior_variance * (k * k.transpose()); - self.p = symmetrize(i_kh * self.p * i_kh.transpose() + rk); + let rk = prior_variance * (&k * k.transpose()); + let p_new = symmetrize_dynamic(&(&i_kh * self.p() * &i_kh.transpose() + &rk)); + *self.p_mut() = p_new; } fn append_motion_sample(&mut self) { + let (cx, cy, yaw_rate) = (self.x()[0], self.x()[2], self.x()[7]); self.motion_history.push_back(MotionSample { t_s: self.tracker_time_s, - center_x: self.x[0], - center_y: self.x[2], - yaw_rate: self.x[7], + center_x: cx, + center_y: cy, + yaw_rate, }); while self.motion_history.len() > MOTION_HISTORY_CAPACITY { self.motion_history.pop_front(); @@ -978,17 +1011,16 @@ impl Default for YpdAngleTracker { } } -fn diagonal_matrix(values: [f64; STATE_DIM]) -> na::SMatrix { - let mut matrix = na::SMatrix::::zeros(); +fn diagonal_matrix(values: [f64; STATE_DIM]) -> na::DMatrix { + let mut matrix = na::DMatrix::::zeros(STATE_DIM, STATE_DIM); for (index, value) in values.into_iter().enumerate() { matrix[(index, index)] = value; } matrix } -fn symmetrize( - matrix: na::SMatrix, -) -> na::SMatrix { +/// 对称化动态大小协方差矩阵：`(M + Mᵀ) / 2`。 +fn symmetrize_dynamic(matrix: &na::DMatrix) -> na::DMatrix { (matrix + matrix.transpose()) * 0.5 } @@ -1012,7 +1044,7 @@ fn observed_yaw_from_radial_yaw(radial_yaw: f64, armor_num: usize) -> f64 { } } -fn radius_from_state(state: &na::SVector, armor_num: usize, id: usize) -> f64 { +fn radius_from_state(state: &na::DVector, armor_num: usize, id: usize) -> f64 { if armor_num == 4 && (id == 1 || id == 3) { state[PRIMARY_RADIUS] + state[DELTA_RADIUS] } else { @@ -1020,11 +1052,7 @@ fn radius_from_state(state: &na::SVector, armor_num: usize, id: } } -fn height_offset_from_state( - state: &na::SVector, - armor_num: usize, - id: usize, -) -> f64 { +fn height_offset_from_state(state: &na::DVector, armor_num: usize, id: usize) -> f64 { if armor_num == 4 { if id == 1 || id == 3 { state[HEIGHT_DIFF] @@ -1233,8 +1261,8 @@ ypd_geometry_recovery_min_h_variance = 0.000625 let obs = observation(na::Point3::new(1_000.0, 0.0, 100.0), 0.0, 200.0); let mut tracker = YpdAngleTracker::new(); tracker.init(&obs, 4); - tracker.x[1] = 100.0; - tracker.x[7] = 1.0; + tracker.x_mut()[1] = 100.0; + tracker.x_mut()[7] = 1.0; tracker.predict(0.05); let snapshot = tracker.snapshot().unwrap(); @@ -1249,10 +1277,10 @@ ypd_geometry_recovery_min_h_variance = 0.000625 let center = na::Point3::new(1_000.0, 0.0, 100.0); let mut tracker = YpdAngleTracker::new(); tracker.init(&observation(center, 0.0, 200.0), 4); - tracker.p[(DELTA_RADIUS, DELTA_RADIUS)] = 10.0; - tracker.p[(HEIGHT_DIFF, HEIGHT_DIFF)] = 10.0; - let before_dr = tracker.p[(DELTA_RADIUS, DELTA_RADIUS)]; - let before_h = tracker.p[(HEIGHT_DIFF, HEIGHT_DIFF)]; + tracker.p_mut()[(DELTA_RADIUS, DELTA_RADIUS)] = 10.0; + tracker.p_mut()[(HEIGHT_DIFF, HEIGHT_DIFF)] = 10.0; + let before_dr = tracker.p()[(DELTA_RADIUS, DELTA_RADIUS)]; + let before_h = tracker.p()[(HEIGHT_DIFF, HEIGHT_DIFF)]; let mismatched = [ observation(na::Point3::new(1_000.0, 0.0, 260.0), 0.0, 320.0), observation( @@ -1267,8 +1295,8 @@ ypd_geometry_recovery_min_h_variance = 0.000625 tracker.note_observation_jump(true, &cfg); tracker.update_batch(&mismatched, Some(0), &cfg); - assert!(tracker.p[(DELTA_RADIUS, DELTA_RADIUS)] > before_dr); - assert!(tracker.p[(HEIGHT_DIFF, HEIGHT_DIFF)] > before_h); + assert!(tracker.p()[(DELTA_RADIUS, DELTA_RADIUS)] > before_dr); + assert!(tracker.p()[(HEIGHT_DIFF, HEIGHT_DIFF)] > before_h); } #[test] @@ -1305,20 +1333,20 @@ ypd_geometry_recovery_min_h_variance = 0.000625 assert_eq!(tracker.outpost_height_phase, Some(phase)); assert_eq!( - tracker.x[DELTA_RADIUS], + tracker.x()[DELTA_RADIUS], outpost_height_offset_from_phase(phase, 1).unwrap() ); assert_eq!( - tracker.x[HEIGHT_DIFF], + tracker.x()[HEIGHT_DIFF], outpost_height_offset_from_phase(phase, 2).unwrap() ); assert_eq!( - tracker.p[(DELTA_RADIUS, DELTA_RADIUS)], + tracker.p()[(DELTA_RADIUS, DELTA_RADIUS)], OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2 ); - let h_id1 = tracker.measurement_jacobian(&tracker.x, 1); - let h_id2 = tracker.measurement_jacobian(&tracker.x, 2); + let h_id1 = tracker.measurement_jacobian(tracker.x(), 1); + let h_id2 = tracker.measurement_jacobian(tracker.x(), 2); assert_eq!(h_id1[(2, DELTA_RADIUS)], 0.0); assert_eq!(h_id2[(2, HEIGHT_DIFF)], 0.0); }