From bc413db761d5217578335331bf02afaf5b7de941 Mon Sep 17 00:00:00 2001 From: XiaoPengYouCode <120481176+XiaoPengYouCode@users.noreply.github.com> Date: Mon, 15 Jun 2026 00:10:44 +0800 Subject: [PATCH 1/3] feat: gate energy mechanism target switches --- docs/vivsionn-gap-checklist.md | 2 +- .../rbt_energy_mechanism/fire_control.rs | 4 + .../rbt_mod/rbt_energy_mechanism/solved.rs | 139 ++++++++++++++- .../rbt_mod/rbt_energy_mechanism/tracker.rs | 167 +++++++++++++++++- 4 files changed, 299 insertions(+), 13 deletions(-) diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index 447b397..daf0ee3 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/docs/vivsionn-gap-checklist.md @@ -8,4 +8,4 @@ This checklist tracks the functional gaps found while comparing this Rust worksp | [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 | -| [ ] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust energy mechanism decode still needs R center correction and switching gates | Required for stable energy mechanism detection | +| [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 | 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 a7d5467..05317d4 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs @@ -234,6 +234,10 @@ mod tests { 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), }; let control = controller.update(EnergyMechanismControlInput { diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/solved.rs b/lib/src/rbt_mod/rbt_energy_mechanism/solved.rs index 24e1b45..687467a 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/solved.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/solved.rs @@ -2,12 +2,15 @@ use crate::rbt_infra::rbt_err::{RbtError, RbtResult}; use super::detected::{EnergyMechanismMode, EnergyMechanismObject}; +const R_CENTER_KEYPOINT_INDEX: usize = 4; const MODEL_TOP_M: na::Point3 = na::Point3::new(0.0, 0.0, 0.827); const MODEL_LEFT_M: na::Point3 = na::Point3::new(0.0, -0.127, 0.700); const MODEL_BOTTOM_M: na::Point3 = na::Point3::new(0.0, 0.0, 0.573); const MODEL_RIGHT_M: na::Point3 = na::Point3::new(0.0, 0.127, 0.700); const MODEL_BLADE_CENTER_M: na::Point3 = na::Point3::new(0.0, 0.0, 0.700); const MODEL_R_CENTER_M: na::Point3 = na::Point3::new(0.0, 0.0, 0.0); +const MODEL_BLADE_RADIUS_M: f32 = 0.700; +const R_CENTER_GEOMETRY_MAX_ERROR_RATIO: f32 = 0.35; #[derive(Debug, Clone, PartialEq)] pub struct EnergyMechanismPose { @@ -25,9 +28,13 @@ pub struct EnergyMechanismSolvedTarget { pub pose: EnergyMechanismPose, pub image_r_center: na::Point2, pub image_target_center: na::Point2, + pub image_r_center_corrected: bool, pub confidence: f32, pub selected_phase_index: usize, pub observed_roll_rad: f64, + pub switch_deferred: bool, + pub target_switched: bool, + pub selected_roll_offset_rad: Option, } #[derive(Debug, Clone, PartialEq)] @@ -43,18 +50,30 @@ pub fn solve_energy_mechanism( cam_k: &na::Matrix3, ) -> RbtResult { let mut best = None; - for (index, candidate) in candidates.iter().enumerate() { - let Some(pose) = solve_candidate_pose(candidate, cam_k) else { + let mut corrected_candidates = Vec::with_capacity(candidates.len()); + for candidate in &candidates { + let corrected = correct_candidate_r_center(candidate); + let Some(pose) = solve_candidate_pose(&corrected.object, cam_k) else { + corrected_candidates.push(corrected.object); continue; }; + let observed_roll_rad = observed_roll(&corrected.object); + let selected_phase_index = phase_index_from_roll(observed_roll_rad); let target = EnergyMechanismSolvedTarget { mode, - observed_roll_rad: observed_roll(candidate), + observed_roll_rad, pose, - image_r_center: candidate.r_center(), - image_target_center: candidate.target_center(), + image_r_center: corrected.object.r_center(), + image_target_center: corrected.object.target_center(), + image_r_center_corrected: corrected.corrected, confidence: candidate.confidence, - selected_phase_index: index, + selected_phase_index, + switch_deferred: false, + target_switched: false, + selected_roll_offset_rad: Some(phase_offset_rad( + observed_roll_rad, + selected_phase_index, + )), }; if best .as_ref() @@ -65,15 +84,67 @@ pub fn solve_energy_mechanism( { best = Some(target); } + corrected_candidates.push(corrected.object); } Ok(EnergyMechanismSolvedFrame { mode, target: best, - candidates, + candidates: corrected_candidates, }) } +#[derive(Debug, Clone)] +struct CorrectedCandidate { + object: EnergyMechanismObject, + corrected: bool, +} + +fn correct_candidate_r_center(candidate: &EnergyMechanismObject) -> CorrectedCandidate { + let target = candidate.target_center(); + let observed_r = candidate.r_center(); + let geometry_r = geometry_r_center(candidate); + let observed_radius = (observed_r - target).norm(); + let geometry_radius = geometry_radius(candidate); + let corrected = observed_radius <= 1e-3 + || (observed_radius - geometry_radius).abs() + > geometry_radius * R_CENTER_GEOMETRY_MAX_ERROR_RATIO; + if corrected { + let mut object = candidate.clone(); + object.keypoints[R_CENTER_KEYPOINT_INDEX] = geometry_r; + CorrectedCandidate { object, corrected } + } else { + CorrectedCandidate { + object: candidate.clone(), + corrected, + } + } +} + +fn geometry_r_center(candidate: &EnergyMechanismObject) -> na::Point2 { + let target = candidate.target_center(); + let observed_r = candidate.r_center(); + let radius = geometry_radius(candidate); + let direction = observed_r - target; + let direction = if direction.norm() > 1e-3 { + direction.normalize() + } else { + na::Vector2::new(0.0, -1.0) + }; + target + direction * radius +} + +fn geometry_radius(candidate: &EnergyMechanismObject) -> f32 { + let top = candidate.keypoints[0]; + let left = candidate.keypoints[1]; + let bottom = candidate.keypoints[2]; + let right = candidate.keypoints[3]; + let vertical = (top - bottom).norm(); + let horizontal = (left - right).norm(); + let blade_span_px = ((vertical + horizontal) * 0.5).max(1.0); + blade_span_px * MODEL_BLADE_RADIUS_M / 0.254 +} + fn solve_candidate_pose( candidate: &EnergyMechanismObject, cam_k: &na::Matrix3, @@ -225,6 +296,25 @@ fn observed_roll(candidate: &EnergyMechanismObject) -> f64 { (target.y - r_center.y).atan2(target.x - r_center.x) as f64 } +fn phase_index_from_roll(roll_rad: f64) -> usize { + let normalized = roll_rad.rem_euclid(std::f64::consts::TAU); + ((normalized / (std::f64::consts::TAU / 5.0)).round() as usize) % 5 +} + +fn phase_offset_rad(roll_rad: f64, phase_index: usize) -> f64 { + normalize_angle(roll_rad - phase_index as f64 * std::f64::consts::TAU / 5.0) +} + +fn normalize_angle(mut angle: f64) -> f64 { + while angle > std::f64::consts::PI { + angle -= std::f64::consts::TAU; + } + while angle < -std::f64::consts::PI { + angle += std::f64::consts::TAU; + } + angle +} + fn to_f64(point: na::Point2) -> na::Point2 { na::Point2::new(point.x as f64, point.y as f64) } @@ -253,6 +343,21 @@ mod tests { ) } + fn image_object_with_r_center(r_center: na::Point2) -> EnergyMechanismObject { + EnergyMechanismObject { + bbox: EnergyMechanismBBox::from_center_size(320.0, 320.0, 140.0, 140.0), + class: EnergyMechanismClass::RedTarget, + confidence: 0.9, + keypoints: [ + na::Point2::new(320.0, 260.0), + na::Point2::new(290.0, 320.0), + na::Point2::new(320.0, 380.0), + na::Point2::new(350.0, 320.0), + r_center, + ], + } + } + #[test] fn solves_valid_energy_mechanism_target() { let cam_k = camera_matrix(); @@ -303,4 +408,24 @@ mod tests { assert!(solved.target.is_none()); } + + #[test] + fn corrects_bad_r_center_to_geometry_radius() { + let object = image_object_with_r_center(na::Point2::new(320.0, 319.0)); + let corrected = correct_candidate_r_center(&object); + let target = object.target_center(); + let expected_radius = geometry_radius(&object); + + assert!(corrected.corrected); + assert!(((corrected.object.r_center() - target).norm() - expected_radius).abs() < 1e-3); + } + + #[test] + fn keeps_consistent_r_center_without_correction() { + let object = image_object_with_r_center(na::Point2::new(320.0, 650.0)); + let corrected = correct_candidate_r_center(&object); + + assert!(!corrected.corrected); + assert_eq!(corrected.object.r_center(), object.r_center()); + } } diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs b/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs index eaab1e9..bf09b40 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/tracker.rs @@ -7,6 +7,10 @@ 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; +const TARGET_SWITCH_CONFIRM_FRAMES: usize = 2; #[derive(Debug, Clone, Copy, PartialEq)] pub struct EnergyMechanismTrackSnapshot { @@ -20,6 +24,10 @@ pub struct EnergyMechanismTrackSnapshot { pub lost: bool, pub track_valid: bool, pub state_age_s: f64, + pub switch_deferred: bool, + pub target_switched: bool, + pub selected_phase_index: Option, + pub selected_roll_offset_rad: Option, } impl EnergyMechanismTrackSnapshot { @@ -64,6 +72,12 @@ pub struct EnergyMechanismTracker { filtered_roll_rate_rad_s: f64, direction: i32, history: VecDeque, + selected_phase_index: Option, + selected_roll_offset_rad: Option, + pending_switch_phase_index: Option, + pending_switch_streak: usize, + last_switch_deferred: bool, + last_target_switched: bool, } impl EnergyMechanismTracker { @@ -80,6 +94,12 @@ impl EnergyMechanismTracker { filtered_roll_rate_rad_s: 0.0, direction: 0, history: VecDeque::new(), + selected_phase_index: None, + selected_roll_offset_rad: None, + pending_switch_phase_index: None, + pending_switch_streak: 0, + last_switch_deferred: false, + last_target_switched: false, } } @@ -103,9 +123,20 @@ impl EnergyMechanismTracker { .map(|last| now.duration_since(last).as_secs_f64().clamp(0.001, 0.08)) .unwrap_or(0.01); self.last_update_tp = Some(now); + self.last_switch_deferred = false; + self.last_target_switched = false; if let Some(target) = target { - self.correct(target, time_s, dt_s, now); + if self.mode == EnergyMechanismMode::Large + && self.should_defer_target_switch(target) + && self.pending_switch_streak < TARGET_SWITCH_CONFIRM_FRAMES + { + self.last_switch_deferred = true; + } else { + let reinitialize = self.mode == EnergyMechanismMode::Large + && self.should_reinitialize_for_target_switch(target); + self.correct(target, time_s, dt_s, now, reinitialize); + } } self.snapshot(now) @@ -131,6 +162,10 @@ impl EnergyMechanismTracker { lost, track_valid: !lost && self.history.len() >= 2, state_age_s, + switch_deferred: self.last_switch_deferred, + target_switched: self.last_target_switched, + selected_phase_index: self.selected_phase_index, + selected_roll_offset_rad: self.selected_roll_offset_rad, }) } @@ -140,12 +175,22 @@ impl EnergyMechanismTracker { time_s: f64, dt_s: f64, now: std::time::Instant, + reinitialize: bool, ) { let observed_roll = normalize_angle(target.observed_roll_rad); - if !self.initialized { + if !self.initialized || reinitialize { + let retained_rate = if reinitialize { + self.filtered_roll_rate_rad_s + } else { + 0.0 + }; + let retained_direction = self.direction; self.initialized = true; self.filtered_roll_rad = observed_roll; - self.filtered_roll_rate_rad_s = 0.0; + self.filtered_roll_rate_rad_s = retained_rate; + self.direction = retained_direction; + self.history.clear(); + self.last_target_switched = reinitialize; } else { let delta = normalize_angle(observed_roll - self.filtered_roll_rad); let raw_rate = delta / dt_s; @@ -159,6 +204,10 @@ impl EnergyMechanismTracker { self.last_target_center_world_m = target.pose.target_center_world_m; self.last_rune_center_world_m = target.pose.rune_center_world_m; + self.selected_phase_index = Some(target.selected_phase_index); + self.selected_roll_offset_rad = target.selected_roll_offset_rad; + self.pending_switch_phase_index = None; + self.pending_switch_streak = 0; self.last_seen_tp = Some(now); self.history.push_back(RollSample { time_s, @@ -170,6 +219,58 @@ impl EnergyMechanismTracker { self.fit_direction_from_history(); } + fn should_defer_target_switch(&mut self, target: &EnergyMechanismSolvedTarget) -> bool { + if !self.initialized { + return false; + } + let switching = target.switch_deferred || self.is_target_switch_candidate(target); + if !switching { + self.pending_switch_phase_index = None; + self.pending_switch_streak = 0; + return false; + } + + if self.pending_switch_phase_index == Some(target.selected_phase_index) { + self.pending_switch_streak = self.pending_switch_streak.saturating_add(1); + } else { + self.pending_switch_phase_index = Some(target.selected_phase_index); + self.pending_switch_streak = 1; + } + true + } + + fn should_reinitialize_for_target_switch(&self, target: &EnergyMechanismSolvedTarget) -> bool { + self.initialized + && (target.target_switched + || self.pending_switch_streak >= TARGET_SWITCH_CONFIRM_FRAMES + || self.is_target_switch_candidate(target)) + } + + fn is_target_switch_candidate(&self, target: &EnergyMechanismSolvedTarget) -> bool { + if !self.initialized { + return false; + } + if self + .selected_phase_index + .is_some_and(|phase| phase != target.selected_phase_index) + { + return true; + } + let roll_jump = normalize_angle(target.observed_roll_rad - self.filtered_roll_rad).abs(); + let target_jump = + (target.pose.target_center_world_m - self.last_target_center_world_m).norm(); + let offset_jump = match ( + self.selected_roll_offset_rad, + target.selected_roll_offset_rad, + ) { + (Some(previous), Some(current)) => normalize_angle(current - previous).abs(), + _ => 0.0, + }; + roll_jump > TARGET_REACQUIRE_ROLL_GATE_RAD + || target_jump > TARGET_REACQUIRE_DISTANCE_GATE_M + || offset_jump > TARGET_SWITCH_SEGMENT_RAD + } + fn fit_direction_from_history(&mut self) { if self.history.len() < 3 { return; @@ -206,8 +307,16 @@ mod tests { use crate::rbt_mod::rbt_energy_mechanism::solved::EnergyMechanismPose; fn target(roll_rad: f64) -> EnergyMechanismSolvedTarget { + target_with_mode(EnergyMechanismMode::Small, roll_rad, 0) + } + + fn target_with_mode( + mode: EnergyMechanismMode, + roll_rad: f64, + selected_phase_index: usize, + ) -> EnergyMechanismSolvedTarget { EnergyMechanismSolvedTarget { - mode: EnergyMechanismMode::Small, + mode, pose: EnergyMechanismPose { rune_center_world_m: na::Point3::origin(), target_center_world_m: na::Point3::new(1.0, roll_rad.cos(), roll_rad.sin()), @@ -221,9 +330,15 @@ mod tests { (320.0 + 100.0 * roll_rad.cos()) as f32, (320.0 + 100.0 * roll_rad.sin()) as f32, ), + image_r_center_corrected: false, confidence: 0.9, - selected_phase_index: 0, + selected_phase_index, observed_roll_rad: roll_rad, + switch_deferred: false, + target_switched: false, + selected_roll_offset_rad: Some(normalize_angle( + roll_rad - selected_phase_index as f64 * std::f64::consts::TAU / 5.0, + )), } } @@ -250,4 +365,46 @@ mod tests { assert!(snapshot.is_none()); } + + #[test] + fn large_tracker_defers_then_rebinds_confirmed_target_switch() { + let mut tracker = EnergyMechanismTracker::new(EnergyMechanismMode::Large); + tracker.update( + EnergyMechanismMode::Large, + Some(&target_with_mode(EnergyMechanismMode::Large, 0.0, 0)), + ); + tracker.update( + EnergyMechanismMode::Large, + Some(&target_with_mode(EnergyMechanismMode::Large, 0.05, 0)), + ); + + let first_switch = tracker + .update( + EnergyMechanismMode::Large, + Some(&target_with_mode( + EnergyMechanismMode::Large, + std::f64::consts::TAU / 5.0, + 1, + )), + ) + .unwrap(); + assert!(first_switch.switch_deferred); + assert!(!first_switch.target_switched); + assert_eq!(first_switch.selected_phase_index, Some(0)); + + let confirmed_switch = tracker + .update( + EnergyMechanismMode::Large, + Some(&target_with_mode( + EnergyMechanismMode::Large, + std::f64::consts::TAU / 5.0, + 1, + )), + ) + .unwrap(); + + assert!(!confirmed_switch.switch_deferred); + assert!(confirmed_switch.target_switched); + assert_eq!(confirmed_switch.selected_phase_index, Some(1)); + } } From 341eaf059d749e1bcebaacd49dc2e545618b4ce9 Mon Sep 17 00:00:00 2001 From: XiaoPengYouCode <120481176+XiaoPengYouCode@users.noreply.github.com> Date: Mon, 15 Jun 2026 00:00:02 +0800 Subject: [PATCH 2/3] feat: add outpost tracker specialization --- docs/vivsionn-gap-checklist.md | 2 +- lib/src/rbt_mod/rbt_estimator.rs | 4 +- .../rbt_estimator/rbt_ypd_angle_tracker.rs | 422 +++++++++++++++++- 3 files changed, 407 insertions(+), 21 deletions(-) diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index 9ab264d..447b397 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/docs/vivsionn-gap-checklist.md @@ -7,5 +7,5 @@ This checklist tracks the functional gaps found while comparing this Rust worksp | [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 | -| [ ] P0 outpost specialization | Outpost path has height phase lock, radius prior, and outpost yaw recovery | Rust outpost handling still needs target-specific recovery and yaw logic | Required for stable outpost mode | +| [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 | | [ ] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust energy mechanism decode still needs R center correction and switching gates | Required for stable energy mechanism detection | diff --git a/lib/src/rbt_mod/rbt_estimator.rs b/lib/src/rbt_mod/rbt_estimator.rs index 7641add..509f59b 100644 --- a/lib/src/rbt_mod/rbt_estimator.rs +++ b/lib/src/rbt_mod/rbt_estimator.rs @@ -356,7 +356,9 @@ impl RbtEstimator { } else { radius_from_center }; - let yaw_rad = if radius_from_center > 1e-6 { + let yaw_rad = if armor_num == 3 { + armor.observed_yaw_rad() + } else if radius_from_center > 1e-6 { (dy / sign).atan2(dx / sign) } else { armor.observed_yaw_rad() 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 0c0ca0c..3917413 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 @@ -10,6 +10,27 @@ const MIN_RADIUS_MM: f64 = 50.0; const MAX_RADIUS_MM: f64 = 500.0; const OUTPOST_RADIUS_MM: f64 = 276.5; const OUTPOST_MAX_HEIGHT_OFFSET_MM: f64 = 600.0; +const OUTPOST_RADIUS_PRIOR_SIGMA_MM: f64 = 18.0; +const OUTPOST_PLANE_TO_RADIAL_YAW_OFFSET_RAD: f64 = 153.0 * std::f64::consts::PI / 180.0; +const OUTPOST_HEIGHT_STEP_MM: f64 = 105.0; +const OUTPOST_HEIGHT_PHASE_SIGMA_MM: f64 = 35.0; +const OUTPOST_HEIGHT_PHASE_LOCK_MARGIN: f64 = 9.0; +const OUTPOST_HEIGHT_PHASE_CENTER_CAPACITY: usize = 96; +const OUTPOST_HEIGHT_PHASE_MIN_CENTER_SAMPLES: usize = 36; +const OUTPOST_HEIGHT_PHASE_MIN_SAMPLES_PER_ID: usize = 6; +const OUTPOST_PRIOR_GATE_MIN_UPDATES: usize = 8; +const OUTPOST_REINIT_AFTER_REJECTED_UPDATES: usize = 4; +const OUTPOST_PRIOR_NIS_REJECT_THRESHOLD: f64 = 16.0; +const OUTPOST_PRIMARY_ONLY_IMAGE_Y_THRESHOLD: f64 = 600.0; +const OUTPOST_LOCKED_HEIGHT_VARIANCE_MM2: f64 = 1.0; +const OUTPOST_HEIGHT_RANK_CANDIDATES: [[i8; 3]; 6] = [ + [1, 0, -1], + [0, -1, 1], + [-1, 1, 0], + [1, -1, 0], + [-1, 0, 1], + [0, 1, -1], +]; const MOTION_HISTORY_CAPACITY: usize = 128; const NIS_WINDOW_SIZE: usize = 100; const M2_TO_MM2: f64 = 1_000_000.0; @@ -122,6 +143,13 @@ pub struct YpdAngleTracker { geometry_recovery_window_remaining: usize, geometry_recovery_cooldown_remaining: usize, geometry_mismatch_streak: usize, + consecutive_rejected_updates: usize, + outpost_height_phase_scores: [f64; 6], + outpost_height_phase_observations: usize, + outpost_height_phase: Option, + outpost_height_phase_id_samples: VecDeque, + outpost_height_phase_id_counts: [usize; 3], + outpost_height_phase_center_samples: [VecDeque; 6], } impl YpdAngleTracker { @@ -143,6 +171,13 @@ impl YpdAngleTracker { geometry_recovery_window_remaining: 0, geometry_recovery_cooldown_remaining: 0, geometry_mismatch_streak: 0, + consecutive_rejected_updates: 0, + outpost_height_phase_scores: [0.0; 6], + outpost_height_phase_observations: 0, + outpost_height_phase: None, + outpost_height_phase_id_samples: VecDeque::new(), + outpost_height_phase_id_counts: [0; 3], + outpost_height_phase_center_samples: std::array::from_fn(|_| VecDeque::new()), }; tracker.reset(); tracker @@ -166,6 +201,8 @@ impl YpdAngleTracker { self.geometry_recovery_window_remaining = 0; self.geometry_recovery_cooldown_remaining = 0; self.geometry_mismatch_streak = 0; + self.consecutive_rejected_updates = 0; + self.reset_outpost_height_phase(); } pub fn init(&mut self, observation: &YpdObservation, armor_num: usize) { @@ -185,7 +222,7 @@ impl YpdAngleTracker { 200.0 }; - let yaw = normalize_angle(observation.yaw_rad); + 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(); @@ -195,8 +232,8 @@ impl YpdAngleTracker { self.p = 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, 1.0, 90_000.0, - 90_000.0, + 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, ]) } else { diagonal_matrix([ @@ -238,8 +275,8 @@ impl YpdAngleTracker { self.x = f * self.x; self.x[6] = normalize_angle(self.x[6]); - self.clamp_geometry(); self.p = symmetrize(f * self.p * f.transpose() + self.process_noise(dt)); + self.clamp_geometry(); } pub fn note_observation_jump(&mut self, jumped: bool, cfg: &EstimatorCfg) { @@ -268,8 +305,18 @@ impl YpdAngleTracker { let tracked_index = preferred_index.filter(|index| *index < limit).unwrap_or(0); let mut primary_match = None; let mut recovery_samples = Vec::new(); + let use_primary_only = self.is_outpost + && limit > 1 + && preferred_index.is_some_and(|index| { + index < limit + && observations[index].image_center.y < OUTPOST_PRIMARY_ONLY_IMAGE_Y_THRESHOLD + }); + let mut accepted_count = 0; for index in 0..limit { + if use_primary_only && index != tracked_index { + continue; + } let matched_id = assignment[index] .unwrap_or_else(|| self.select_best_armor_id(&observations[index])); if self.geometry_recovery_window_remaining > 0 && self.armor_num == 4 { @@ -278,12 +325,26 @@ impl YpdAngleTracker { } if self.correct_with_observation(&observations[index], matched_id) { self.last_batch_match_ids[index] = matched_id as isize; + self.update_outpost_height_phase(&observations[index], matched_id); + accepted_count += 1; if index == tracked_index { primary_match = Some(matched_id); } } } + if self.is_outpost + && accepted_count == 0 + && self.consecutive_rejected_updates >= OUTPOST_REINIT_AFTER_REJECTED_UPDATES + { + self.init(&observations[tracked_index], self.armor_num); + self.tracked_id = self.select_best_armor_id(&observations[tracked_index]); + self.last_batch_match_ids = vec![-1; observations.len()]; + self.last_batch_match_ids[tracked_index] = self.tracked_id as isize; + return; + } + + self.apply_locked_outpost_height_offsets(); if let Some(matched_id) = primary_match { self.tracked_id = matched_id; } @@ -403,7 +464,7 @@ impl YpdAngleTracker { na::Point3::new( state[0] + sign * radius * angle.cos(), state[2] + sign * radius * angle.sin(), - state[4] + height_offset_from_state(state, self.armor_num, id), + state[4] + self.height_offset_for_id(state, id), ) } @@ -413,7 +474,12 @@ impl YpdAngleTracker { 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); - [position.x, position.y, position.z, angle] + [ + position.x, + position.y, + position.z, + observed_yaw_from_radial_yaw(angle, self.armor_num), + ] } fn predicted_measurement( @@ -425,7 +491,12 @@ impl YpdAngleTracker { 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(ypd.x, ypd.y, ypd.z, angle) + na::SVector::::new( + ypd.x, + ypd.y, + ypd.z, + observed_yaw_from_radial_yaw(angle, self.armor_num), + ) } fn measurement_jacobian( @@ -463,17 +534,19 @@ impl YpdAngleTracker { }; h_xyza[(2, 4)] = 1.0; - h_xyza[(2, DELTA_RADIUS)] = if self.armor_num == 3 && id == 1 { + let outpost_height_locked = self.outpost_height_phase_locked(); + h_xyza[(2, DELTA_RADIUS)] = if self.armor_num == 3 && !outpost_height_locked && id == 1 { + 1.0 + } else { + 0.0 + }; + h_xyza[(2, HEIGHT_DIFF)] = if (self.armor_num == 4 && (id == 1 || id == 3)) + || (self.armor_num == 3 && !outpost_height_locked && id == 2) + { 1.0 } else { 0.0 }; - h_xyza[(2, HEIGHT_DIFF)] = - if (self.armor_num == 4 && (id == 1 || id == 3)) || (self.armor_num == 3 && id == 2) { - 1.0 - } else { - 0.0 - }; h_xyza[(3, 6)] = 1.0; let h_ypd = xyz_to_ypd_jacobian(self.predicted_armor_position(state, id)); @@ -494,6 +567,23 @@ impl YpdAngleTracker { r[(1, 1)] = 4e-3; r[(2, 2)] = distance_sigma_mm * distance_sigma_mm; r[(3, 3)] = (delta_angle.abs() + 1.0).ln() / 20.0 + 9e-2; + if self.is_outpost { + let mut distance_scale = 1.0; + if observation.image_center.y < 600.0 { + distance_scale *= 3.0; + } + if observation.image_center.y < 450.0 { + distance_scale *= 2.0; + } + let obs_pitch_deg = ypd.y.to_degrees(); + if obs_pitch_deg < 10.0 { + distance_scale *= 2.0; + } + if obs_pitch_deg < 5.0 { + distance_scale *= 1.5; + } + r[(2, 2)] *= distance_scale; + } r } @@ -516,16 +606,27 @@ impl YpdAngleTracker { }; let prior_nis = residual.transpose() * s_inv * residual; let prior_nis = prior_nis[(0, 0)]; + if self.is_outpost + && self.update_count >= OUTPOST_PRIOR_GATE_MIN_UPDATES + && prior_nis.is_finite() + && prior_nis > OUTPOST_PRIOR_NIS_REJECT_THRESHOLD + { + self.consecutive_rejected_updates = self.consecutive_rejected_updates.saturating_add(1); + self.record_nis(prior_nis); + 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.clamp_geometry(); self.p = symmetrize((i - k * h) * self.p * (i - k * h).transpose() + k * r * k.transpose()); + self.apply_outpost_radius_prior(); + self.clamp_geometry(); self.record_nis(prior_nis); self.update_count += 1; + self.consecutive_rejected_updates = 0; true } @@ -638,6 +739,14 @@ impl YpdAngleTracker { radius_from_state(&self.x, self.armor_num, id) } + fn height_offset_for_id(&self, state: &na::SVector, id: usize) -> f64 { + if self.outpost_height_phase_locked() { + self.outpost_height_offset_for_id(id) + } else { + height_offset_from_state(state, self.armor_num, id) + } + } + fn record_nis(&mut self, nis: f64) { self.last_nis = nis; self.recent_nis_failures @@ -662,13 +771,162 @@ impl YpdAngleTracker { self.x[DELTA_RADIUS] = secondary - self.x[PRIMARY_RADIUS]; } else { self.x[PRIMARY_RADIUS] = OUTPOST_RADIUS_MM; - 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); + if self.outpost_height_phase_locked() { + self.apply_locked_outpost_height_offsets(); + } 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); + } + } + } + + fn reset_outpost_height_phase(&mut self) { + self.outpost_height_phase_scores = [0.0; 6]; + self.outpost_height_phase_observations = 0; + self.outpost_height_phase = None; + self.outpost_height_phase_id_samples.clear(); + self.outpost_height_phase_id_counts = [0; 3]; + for samples in &mut self.outpost_height_phase_center_samples { + samples.clear(); + } + } + + fn outpost_height_phase_locked(&self) -> bool { + self.is_outpost && self.outpost_height_phase.is_some() + } + + fn outpost_height_offset_for_id(&self, id: usize) -> f64 { + self.outpost_height_phase + .and_then(|phase| outpost_height_offset_from_phase(phase, id)) + .unwrap_or(0.0) + } + + fn apply_locked_outpost_height_offsets(&mut self) { + 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; + } + + fn update_outpost_height_phase(&mut self, observation: &YpdObservation, matched_id: usize) { + if !self.is_outpost || matched_id >= 3 || !observation.position_mm.z.is_finite() { + return; + } + + self.outpost_height_phase_id_samples.push_back(matched_id); + self.outpost_height_phase_id_counts[matched_id] += 1; + for phase in 0..OUTPOST_HEIGHT_RANK_CANDIDATES.len() { + let center_z = observation.position_mm.z + - outpost_height_offset_from_phase(phase, matched_id).unwrap_or(0.0); + self.outpost_height_phase_center_samples[phase].push_back(center_z); + } + + while self.outpost_height_phase_id_samples.len() > OUTPOST_HEIGHT_PHASE_CENTER_CAPACITY { + if let Some(old_id) = self.outpost_height_phase_id_samples.pop_front() + && old_id < self.outpost_height_phase_id_counts.len() + { + self.outpost_height_phase_id_counts[old_id] = + self.outpost_height_phase_id_counts[old_id].saturating_sub(1); + } + for samples in &mut self.outpost_height_phase_center_samples { + samples.pop_front(); + } + } + + self.outpost_height_phase_observations = self.outpost_height_phase_id_samples.len(); + let mut candidate_center_z = [f64::NAN; 6]; + for (phase, center_slot) in candidate_center_z + .iter_mut() + .enumerate() + .take(self.outpost_height_phase_scores.len()) + { + let (score, center_z) = self.outpost_height_phase_score_from_samples(phase); + self.outpost_height_phase_scores[phase] = score; + *center_slot = center_z; + } + + if self.outpost_height_phase_locked() + || self.outpost_height_phase_observations < OUTPOST_HEIGHT_PHASE_MIN_CENTER_SAMPLES + || !self.outpost_height_phase_has_enough_id_coverage() + { + return; + } + + let mut phases: Vec<_> = (0..self.outpost_height_phase_scores.len()).collect(); + phases.sort_by(|lhs, rhs| { + self.outpost_height_phase_scores[*lhs] + .total_cmp(&self.outpost_height_phase_scores[*rhs]) + }); + let best = phases[0]; + let second = phases[1]; + let margin = + 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.apply_locked_outpost_height_offsets(); + } + } + + fn outpost_height_phase_score_from_samples(&self, phase: usize) -> (f64, f64) { + let Some(center_z) = median_value( + self.outpost_height_phase_center_samples[phase] + .iter() + .copied(), + ) else { + return (f64::INFINITY, f64::NAN); + }; + let sigma_sq = OUTPOST_HEIGHT_PHASE_SIGMA_MM * OUTPOST_HEIGHT_PHASE_SIGMA_MM; + let mut score = 0.0; + let mut count = 0; + for sample in &self.outpost_height_phase_center_samples[phase] { + if !sample.is_finite() { + continue; + } + let residual = sample - center_z; + score += residual * residual / sigma_sq; + count += 1; + } + if count > 0 { + (score, center_z) + } else { + (f64::INFINITY, f64::NAN) } } + fn outpost_height_phase_has_enough_id_coverage(&self) -> bool { + self.outpost_height_phase_id_counts + .iter() + .all(|count| *count >= OUTPOST_HEIGHT_PHASE_MIN_SAMPLES_PER_ID) + } + + fn apply_outpost_radius_prior(&mut self) { + if !self.is_outpost { + 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; + 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 mut i_kh = na::SMatrix::::identity(); + 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); + } + fn append_motion_sample(&mut self) { self.motion_history.push_back(MotionSample { t_s: self.tracker_time_s, @@ -738,6 +996,22 @@ fn radial_sign(armor_num: usize) -> f64 { if armor_num == 3 { 1.0 } else { -1.0 } } +fn radial_yaw_from_observed_yaw(observed_yaw: f64, armor_num: usize) -> f64 { + if armor_num == 3 { + normalize_angle(observed_yaw + OUTPOST_PLANE_TO_RADIAL_YAW_OFFSET_RAD) + } else { + normalize_angle(observed_yaw) + } +} + +fn observed_yaw_from_radial_yaw(radial_yaw: f64, armor_num: usize) -> f64 { + if armor_num == 3 { + normalize_angle(radial_yaw - OUTPOST_PLANE_TO_RADIAL_YAW_OFFSET_RAD) + } else { + normalize_angle(radial_yaw) + } +} + fn radius_from_state(state: &na::SVector, armor_num: usize, id: usize) -> f64 { if armor_num == 4 && (id == 1 || id == 3) { state[PRIMARY_RADIUS] + state[DELTA_RADIUS] @@ -766,6 +1040,13 @@ fn height_offset_from_state( } } +fn outpost_height_offset_from_phase(phase: usize, id: usize) -> Option { + OUTPOST_HEIGHT_RANK_CANDIDATES + .get(phase) + .and_then(|candidate| candidate.get(id)) + .map(|rank| f64::from(*rank) * OUTPOST_HEIGHT_STEP_MM) +} + fn normalize_angle(angle: f64) -> f64 { let mut normalized = (angle + std::f64::consts::PI) % std::f64::consts::TAU; if normalized < 0.0 { @@ -805,6 +1086,20 @@ fn xyz_to_ypd_jacobian(position: na::Point3) -> na::SMatrix { ]) } +fn median_value(values: impl Iterator) -> Option { + let mut values: Vec<_> = values.filter(|value| value.is_finite()).collect(); + if values.is_empty() { + return None; + } + values.sort_by(f64::total_cmp); + let mid = values.len() / 2; + if values.len() % 2 == 0 { + Some((values[mid - 1] + values[mid]) * 0.5) + } else { + Some(values[mid]) + } +} + fn linear_slope_abs(samples: impl Iterator) -> f64 { let values: Vec<_> = samples.collect(); if values.len() < 2 { @@ -892,6 +1187,27 @@ ypd_geometry_recovery_min_h_variance = 0.000625 } } + fn outpost_phase_observation(center_z: f64, phase: usize, id: usize) -> YpdObservation { + let radial_yaw = id as f64 * std::f64::consts::TAU / 3.0; + let position = na::Point3::new( + 1_000.0 + OUTPOST_RADIUS_MM * radial_yaw.cos(), + OUTPOST_RADIUS_MM * radial_yaw.sin(), + center_z + outpost_height_offset_from_phase(phase, id).unwrap(), + ); + YpdObservation { + position_mm: position, + yaw_rad: observed_yaw_from_radial_yaw(radial_yaw, 3), + image_center: na::Point2::new(320.0, 520.0), + radius_hint_mm: OUTPOST_RADIUS_MM, + } + } + + fn initialize_outpost_tracker() -> YpdAngleTracker { + let mut tracker = YpdAngleTracker::new(); + tracker.init(&outpost_phase_observation(200.0, 0, 0), 3); + tracker + } + #[test] fn batch_update_assigns_unique_armor_ids() { let center = na::Point3::new(1_000.0, 0.0, 100.0); @@ -954,4 +1270,72 @@ ypd_geometry_recovery_min_h_variance = 0.000625 assert!(tracker.p[(DELTA_RADIUS, DELTA_RADIUS)] > before_dr); assert!(tracker.p[(HEIGHT_DIFF, HEIGHT_DIFF)] > before_h); } + + #[test] + fn outpost_observed_yaw_converts_to_radial_state_and_back() { + let observed_yaw = 0.25; + let mut tracker = YpdAngleTracker::new(); + let obs = YpdObservation { + position_mm: na::Point3::new(1_000.0, 0.0, 100.0), + yaw_rad: observed_yaw, + image_center: na::Point2::new(320.0, 500.0), + radius_hint_mm: OUTPOST_RADIUS_MM, + }; + + tracker.init(&obs, 3); + let snapshot = tracker.snapshot().unwrap(); + + assert!( + (normalize_angle(snapshot.state11d[6] - observed_yaw) + - OUTPOST_PLANE_TO_RADIAL_YAW_OFFSET_RAD) + .abs() + < 1e-9 + ); + assert!((snapshot.tracked_armor_xyza[3] - observed_yaw).abs() < 1e-9); + } + + #[test] + fn outpost_height_phase_locks_offsets_and_freezes_jacobian() { + let mut tracker = initialize_outpost_tracker(); + let phase = 0; + for round in 0..OUTPOST_HEIGHT_PHASE_MIN_CENTER_SAMPLES { + let id = round % 3; + tracker.update_outpost_height_phase(&outpost_phase_observation(200.0, phase, id), id); + } + + assert_eq!(tracker.outpost_height_phase, Some(phase)); + assert_eq!( + tracker.x[DELTA_RADIUS], + outpost_height_offset_from_phase(phase, 1).unwrap() + ); + assert_eq!( + tracker.x[HEIGHT_DIFF], + outpost_height_offset_from_phase(phase, 2).unwrap() + ); + assert_eq!( + 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); + assert_eq!(h_id1[(2, DELTA_RADIUS)], 0.0); + assert_eq!(h_id2[(2, HEIGHT_DIFF)], 0.0); + } + + #[test] + fn outpost_low_image_primary_only_skips_secondary_observations() { + let cfg = estimator_cfg(); + let mut tracker = initialize_outpost_tracker(); + let mut primary = outpost_phase_observation(200.0, 0, 0); + primary.image_center.y = 500.0; + let mut secondary = outpost_phase_observation(200.0, 0, 1); + secondary.image_center.y = 650.0; + + tracker.update_batch(&[primary, secondary], Some(0), &cfg); + + assert_eq!(tracker.last_batch_match_ids().len(), 2); + assert!(tracker.last_batch_match_ids()[0] >= 0); + assert_eq!(tracker.last_batch_match_ids()[1], -1); + } } From e962c81cdc832a27f46e7cf2da39fbd9f5e67350 Mon Sep 17 00:00:00 2001 From: XiaoPengYouCode <120481176+XiaoPengYouCode@users.noreply.github.com> Date: Sun, 14 Jun 2026 23:42:44 +0800 Subject: [PATCH 3/3] feat: add YPD geometry recovery --- cfg/rbt_cfg.toml | 9 + docs/vivsionn-gap-checklist.md | 2 +- lib/src/rbt_infra/rbt_cfg.rs | 54 ++++++ lib/src/rbt_mod/rbt_estimator.rs | 17 +- .../rbt_estimator/rbt_ypd_angle_tracker.rs | 167 +++++++++++++++++- 5 files changed, 242 insertions(+), 7 deletions(-) diff --git a/cfg/rbt_cfg.toml b/cfg/rbt_cfg.toml index 5ddc1dc..70d684b 100644 --- a/cfg/rbt_cfg.toml +++ b/cfg/rbt_cfg.toml @@ -47,3 +47,12 @@ cam_k = [1600.0, 0.0, 320.0, 0.0, 1705.7, 192.0, 0.0, 0.0, 1.0] [estimator_cfg] armor_lost_wait_duration_ms = 100 enemy_lost_wait_duration_ms = 1000 +ypd_geometry_recovery_window_frames = 24 +ypd_geometry_recovery_cooldown_frames = 12 +ypd_geometry_recovery_mismatch_required_streak = 2 +ypd_geometry_recovery_min_matched_count = 2 +ypd_geometry_recovery_z_sigma_threshold = 3.0 +ypd_geometry_recovery_xy_sigma_threshold = 2.0 +ypd_geometry_recovery_cov_inflation_scale = 48.0 +ypd_geometry_recovery_min_dr_variance = 0.0025 +ypd_geometry_recovery_min_h_variance = 0.000625 diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index b5f79ea..9ab264d 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/docs/vivsionn-gap-checklist.md @@ -6,6 +6,6 @@ This checklist tracks the functional gaps found while comparing this Rust worksp |---|---|---|---| | [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 | -| [ ] P0 YPD geometry recovery | Tracker recovers after armor jump/mismatch by gating windows and inflating geometry covariance | Rust tracker still needs online geometry recovery logic | Required for robust reacquisition | +| [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 | | [ ] P0 outpost specialization | Outpost path has height phase lock, radius prior, and outpost yaw recovery | Rust outpost handling still needs target-specific recovery and yaw logic | Required for stable outpost mode | | [ ] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust energy mechanism decode still needs R center correction and switching gates | Required for stable energy mechanism detection | diff --git a/lib/src/rbt_infra/rbt_cfg.rs b/lib/src/rbt_infra/rbt_cfg.rs index 0d8d5b1..ff9dbb9 100644 --- a/lib/src/rbt_infra/rbt_cfg.rs +++ b/lib/src/rbt_infra/rbt_cfg.rs @@ -136,10 +136,64 @@ impl CamCfg { pub struct EstimatorCfg { armor_lost_wait_duration_ms: u64, enemy_lost_wait_duration_ms: u64, + #[serde(default = "default_ypd_geometry_recovery_window_frames")] + pub ypd_geometry_recovery_window_frames: usize, + #[serde(default = "default_ypd_geometry_recovery_cooldown_frames")] + pub ypd_geometry_recovery_cooldown_frames: usize, + #[serde(default = "default_ypd_geometry_recovery_mismatch_required_streak")] + pub ypd_geometry_recovery_mismatch_required_streak: usize, + #[serde(default = "default_ypd_geometry_recovery_min_matched_count")] + pub ypd_geometry_recovery_min_matched_count: usize, + #[serde(default = "default_ypd_geometry_recovery_z_sigma_threshold")] + pub ypd_geometry_recovery_z_sigma_threshold: f64, + #[serde(default = "default_ypd_geometry_recovery_xy_sigma_threshold")] + pub ypd_geometry_recovery_xy_sigma_threshold: f64, + #[serde(default = "default_ypd_geometry_recovery_cov_inflation_scale")] + pub ypd_geometry_recovery_cov_inflation_scale: f64, + #[serde(default = "default_ypd_geometry_recovery_min_dr_variance")] + pub ypd_geometry_recovery_min_dr_variance: f64, + #[serde(default = "default_ypd_geometry_recovery_min_h_variance")] + pub ypd_geometry_recovery_min_h_variance: f64, // top1_activate_w: f64, // top2_activate_w: f64, } +fn default_ypd_geometry_recovery_window_frames() -> usize { + 24 +} + +fn default_ypd_geometry_recovery_cooldown_frames() -> usize { + 12 +} + +fn default_ypd_geometry_recovery_mismatch_required_streak() -> usize { + 2 +} + +fn default_ypd_geometry_recovery_min_matched_count() -> usize { + 2 +} + +fn default_ypd_geometry_recovery_z_sigma_threshold() -> f64 { + 3.0 +} + +fn default_ypd_geometry_recovery_xy_sigma_threshold() -> f64 { + 2.0 +} + +fn default_ypd_geometry_recovery_cov_inflation_scale() -> f64 { + 48.0 +} + +fn default_ypd_geometry_recovery_min_dr_variance() -> f64 { + 2.5e-3 +} + +fn default_ypd_geometry_recovery_min_h_variance() -> f64 { + 6.25e-4 +} + impl EstimatorCfg { #[inline(always)] pub fn lost_wait_duration_ms(&self) -> tokio::time::Duration { diff --git a/lib/src/rbt_mod/rbt_estimator.rs b/lib/src/rbt_mod/rbt_estimator.rs index 51ef279..7641add 100644 --- a/lib/src/rbt_mod/rbt_estimator.rs +++ b/lib/src/rbt_mod/rbt_estimator.rs @@ -211,7 +211,7 @@ impl RbtEstimator { } self.update_global_vars(solved_enemy); - self.update_tracker(solved_enemy.as_ref(), dt_s); + self.update_tracker(cfg, solved_enemy.as_ref(), dt_s); } pub fn snapshot(&self) -> Option { @@ -282,7 +282,12 @@ impl RbtEstimator { dt_s.clamp(0.001, 0.05) } - fn update_tracker(&mut self, solved_enemy: Option<&RbtSolvedResult>, dt_s: f64) { + fn update_tracker( + &mut self, + cfg: &EstimatorCfg, + solved_enemy: Option<&RbtSolvedResult>, + dt_s: f64, + ) { use EstimatorStateMachine::*; match &self.state { @@ -290,7 +295,7 @@ impl RbtEstimator { WakeUp | Recovery | Track { .. } => { self.predict_or_reset_tracker(dt_s); if let Some(solved) = solved_enemy { - self.correct_tracker_with_solution(solved); + self.correct_tracker_with_solution(cfg, solved); } self.sync_tracker_snapshot(); } @@ -310,7 +315,7 @@ impl RbtEstimator { self.ypd_angle_tracker.predict(dt_s); } - fn correct_tracker_with_solution(&mut self, solved: &RbtSolvedResult) { + fn correct_tracker_with_solution(&mut self, cfg: &EstimatorCfg, solved: &RbtSolvedResult) { let observations = self.ypd_observations(solved); let Some(preferred_index) = preferred_observation_index(&observations) else { return; @@ -322,7 +327,9 @@ impl RbtEstimator { .init(&observations[preferred_index], armor_num); } else { self.ypd_angle_tracker - .update_batch(&observations, Some(preferred_index)); + .note_observation_jump(self.single_or_double, cfg); + self.ypd_angle_tracker + .update_batch(&observations, Some(preferred_index), cfg); } } 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 cbd160a..0c0ca0c 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,7 @@ use std::collections::VecDeque; +use crate::rbt_infra::rbt_cfg::EstimatorCfg; + const STATE_DIM: usize = 11; const PRIMARY_RADIUS: usize = 8; const DELTA_RADIUS: usize = 9; @@ -10,6 +12,7 @@ const OUTPOST_RADIUS_MM: f64 = 276.5; const OUTPOST_MAX_HEIGHT_OFFSET_MM: f64 = 600.0; const MOTION_HISTORY_CAPACITY: usize = 128; const NIS_WINDOW_SIZE: usize = 100; +const M2_TO_MM2: f64 = 1_000_000.0; #[derive(Debug, Clone, Copy)] pub struct YpdObservation { @@ -44,6 +47,12 @@ struct MotionSample { yaw_rate: f64, } +#[derive(Debug, Clone, Copy)] +struct GeometryRecoverySample { + xy_residual_mm: f64, + z_residual_mm: f64, +} + struct ArmorAssignmentSearch<'a> { tracker: &'a YpdAngleTracker, observations: &'a [YpdObservation], @@ -110,6 +119,9 @@ pub struct YpdAngleTracker { recent_nis_failures: VecDeque, last_batch_match_ids: Vec, motion_history: VecDeque, + geometry_recovery_window_remaining: usize, + geometry_recovery_cooldown_remaining: usize, + geometry_mismatch_streak: usize, } impl YpdAngleTracker { @@ -128,6 +140,9 @@ impl YpdAngleTracker { recent_nis_failures: VecDeque::from([false]), last_batch_match_ids: Vec::new(), motion_history: VecDeque::new(), + geometry_recovery_window_remaining: 0, + geometry_recovery_cooldown_remaining: 0, + geometry_mismatch_streak: 0, }; tracker.reset(); tracker @@ -148,6 +163,9 @@ impl YpdAngleTracker { self.recent_nis_failures.push_back(false); self.last_batch_match_ids.clear(); self.motion_history.clear(); + self.geometry_recovery_window_remaining = 0; + self.geometry_recovery_cooldown_remaining = 0; + self.geometry_mismatch_streak = 0; } pub fn init(&mut self, observation: &YpdObservation, armor_num: usize) { @@ -224,10 +242,20 @@ impl YpdAngleTracker { self.p = symmetrize(f * self.p * f.transpose() + self.process_noise(dt)); } + pub fn note_observation_jump(&mut self, jumped: bool, cfg: &EstimatorCfg) { + if jumped && self.initialized && self.armor_num == 4 { + self.geometry_recovery_window_remaining = + cfg.ypd_geometry_recovery_window_frames.max(1); + } else if !jumped && self.geometry_recovery_window_remaining == 0 { + self.geometry_mismatch_streak = 0; + } + } + pub fn update_batch( &mut self, observations: &[YpdObservation], preferred_index: Option, + cfg: &EstimatorCfg, ) { self.last_batch_match_ids.clear(); if !self.initialized || observations.is_empty() { @@ -239,10 +267,15 @@ impl YpdAngleTracker { let assignment = self.assign_armor_ids(&observations[..limit]); let tracked_index = preferred_index.filter(|index| *index < limit).unwrap_or(0); let mut primary_match = None; + let mut recovery_samples = Vec::new(); for index in 0..limit { let matched_id = assignment[index] .unwrap_or_else(|| self.select_best_armor_id(&observations[index])); + if self.geometry_recovery_window_remaining > 0 && self.armor_num == 4 { + recovery_samples + .push(self.geometry_recovery_sample(&observations[index], matched_id)); + } if self.correct_with_observation(&observations[index], matched_id) { self.last_batch_match_ids[index] = matched_id as isize; if index == tracked_index { @@ -254,6 +287,7 @@ impl YpdAngleTracker { if let Some(matched_id) = primary_match { self.tracked_id = matched_id; } + self.update_geometry_recovery(&recovery_samples, cfg); self.clamp_geometry(); self.append_motion_sample(); } @@ -495,6 +529,90 @@ impl YpdAngleTracker { true } + fn geometry_recovery_sample( + &self, + observation: &YpdObservation, + id: usize, + ) -> GeometryRecoverySample { + 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), + z_residual_mm: residual.z.abs(), + } + } + + fn update_geometry_recovery(&mut self, samples: &[GeometryRecoverySample], cfg: &EstimatorCfg) { + if self.geometry_recovery_cooldown_remaining > 0 { + self.geometry_recovery_cooldown_remaining -= 1; + } + if self.geometry_recovery_window_remaining == 0 || self.armor_num != 4 { + return; + } + self.geometry_recovery_window_remaining -= 1; + if samples.len() < cfg.ypd_geometry_recovery_min_matched_count.max(1) { + self.geometry_mismatch_streak = 0; + return; + } + + let mean_xy = samples + .iter() + .map(|sample| sample.xy_residual_mm) + .sum::() + / samples.len() as f64; + let mean_z = samples + .iter() + .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 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() + && xy_over_sigma_dr.is_finite() + && ((z_over_sigma_h >= cfg.ypd_geometry_recovery_z_sigma_threshold + && xy_over_sigma_dr >= cfg.ypd_geometry_recovery_xy_sigma_threshold) + || z_over_sigma_h >= cfg.ypd_geometry_recovery_z_sigma_threshold + 1.0); + + if mismatch { + self.geometry_mismatch_streak = self.geometry_mismatch_streak.saturating_add(1); + } else { + self.geometry_mismatch_streak = 0; + } + + if self.geometry_recovery_cooldown_remaining == 0 + && self.geometry_mismatch_streak + >= cfg.ypd_geometry_recovery_mismatch_required_streak.max(1) + { + self.inflate_geometry_covariance(cfg); + self.geometry_mismatch_streak = 0; + self.geometry_recovery_cooldown_remaining = + cfg.ypd_geometry_recovery_cooldown_frames.max(1); + self.geometry_recovery_window_remaining = 0; + } + } + + fn inflate_geometry_covariance(&mut self, cfg: &EstimatorCfg) { + let scale = cfg + .ypd_geometry_recovery_cov_inflation_scale + .clamp(1.0, 1_000.0); + 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; + + 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)]; + } + } + 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); + } + fn assign_armor_ids(&self, observations: &[YpdObservation]) -> Vec> { ArmorAssignmentSearch::new(self, observations).run() } @@ -741,6 +859,25 @@ fn quadratic_accel(samples: impl Iterator) -> f64 { mod tests { use super::*; + fn estimator_cfg() -> EstimatorCfg { + toml::from_str( + "\ +armor_lost_wait_duration_ms = 100 +enemy_lost_wait_duration_ms = 1000 +ypd_geometry_recovery_window_frames = 24 +ypd_geometry_recovery_cooldown_frames = 12 +ypd_geometry_recovery_mismatch_required_streak = 2 +ypd_geometry_recovery_min_matched_count = 2 +ypd_geometry_recovery_z_sigma_threshold = 3.0 +ypd_geometry_recovery_xy_sigma_threshold = 2.0 +ypd_geometry_recovery_cov_inflation_scale = 48.0 +ypd_geometry_recovery_min_dr_variance = 0.0025 +ypd_geometry_recovery_min_h_variance = 0.000625 +", + ) + .unwrap() + } + fn observation(center: na::Point3, yaw: f64, radius: f64) -> YpdObservation { let position = na::Point3::new( center.x - radius * yaw.cos(), @@ -766,7 +903,7 @@ mod tests { observation(center, 0.0, 200.0), observation(center, std::f64::consts::FRAC_PI_2, 200.0), ]; - tracker.update_batch(&observations, Some(0)); + tracker.update_batch(&observations, Some(0), &estimator_cfg()); assert_eq!(tracker.last_batch_match_ids().len(), 2); assert_ne!( @@ -789,4 +926,32 @@ mod tests { assert!((snapshot.state11d[0] - 1_005.0).abs() < 1e-6); assert!((snapshot.state11d[6] - 0.05).abs() < 1e-6); } + + #[test] + fn geometry_recovery_inflates_dr_and_height_covariance_after_mismatch() { + let cfg = estimator_cfg(); + 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)]; + let mismatched = [ + observation(na::Point3::new(1_000.0, 0.0, 260.0), 0.0, 320.0), + observation( + na::Point3::new(1_000.0, 0.0, 260.0), + std::f64::consts::FRAC_PI_2, + 320.0, + ), + ]; + + tracker.note_observation_jump(true, &cfg); + tracker.update_batch(&mismatched, Some(0), &cfg); + 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); + } }