diff --git a/src/ffi.rs b/src/ffi.rs index 1159b52..0b4ca77 100644 --- a/src/ffi.rs +++ b/src/ffi.rs @@ -507,93 +507,3 @@ pub unsafe extern "C" fn aegis_fusion4_get_sensor_health( .sensor_health(sensor_id) .unwrap_or(Real::NAN) } - -#[cfg(test)] -mod tests { - use super::*; - use core::mem::MaybeUninit; - - #[test] - fn test_c_ffi_scalar_filter() { - unsafe { - // Null pointer safety - assert_eq!( - aegis_scalar_filter_init(core::ptr::null_mut(), 0.0, 1.0, 0.01, 0.5), - AegisStatusCode::NullPointer - ); - - let mut filter = MaybeUninit::::uninit(); - let status = aegis_scalar_filter_init(filter.as_mut_ptr(), 0.0, 1.0, 0.01, 0.5); - assert_eq!(status, AegisStatusCode::Ok); - let mut filter = filter.assume_init(); - - let status = aegis_scalar_filter_predict(&mut filter, 0.1); - assert_eq!(status, AegisStatusCode::Ok); - - let mut out = 0.0; - let status = aegis_scalar_filter_update(&mut filter, 1.05, 3.0, &mut out); - assert_eq!(status, AegisStatusCode::Ok); - assert!(out > 0.0); - assert!((aegis_scalar_filter_get_state(&filter) - out).abs() < 1e-12); - assert!(aegis_scalar_filter_get_variance(&filter) > 0.0); - - // Rejection of adversary spike - let status = aegis_scalar_filter_update(&mut filter, 9999.0, 3.0, &mut out); - assert_eq!(status, AegisStatusCode::MeasurementGated); - - // Test fail-closed default gate configuration - let status = aegis_scalar_filter_set_default_gate(&mut filter, 3.0); - assert_eq!(status, AegisStatusCode::Ok); - // Passing 0.0 falls back to configured 3.0 default gate -> rejects spike - let status = aegis_scalar_filter_update(&mut filter, 9999.0, 0.0, &mut out); - assert_eq!(status, AegisStatusCode::MeasurementGated); - // Passing explicit AEGIS_GATE_DISABLE (-1.0) allows through - let status = - aegis_scalar_filter_update(&mut filter, 1.05, AEGIS_GATE_DISABLE, &mut out); - assert_eq!(status, AegisStatusCode::Ok); - } - } - - #[test] - fn test_c_ffi_h_infinity_filter() { - unsafe { - let mut hinf = MaybeUninit::::uninit(); - let status = aegis_h_inf_filter_init(hinf.as_mut_ptr(), 0.0, 1.0, 0.01, 0.5, 5.0); - assert_eq!(status, AegisStatusCode::Ok); - let mut hinf = hinf.assume_init(); - - let status = aegis_h_inf_filter_predict(&mut hinf, 0.1); - assert_eq!(status, AegisStatusCode::Ok); - - let mut out = 0.0; - let status = aegis_h_inf_filter_update(&mut hinf, 1.05, &mut out); - assert_eq!(status, AegisStatusCode::Ok); - assert!(out > 0.0); - assert!(aegis_h_inf_filter_get_bound(&hinf) > 0.0); - } - } - - #[test] - fn test_c_ffi_fusion4_and_fdir() { - unsafe { - let mut fusion = MaybeUninit::::uninit(); - let status = aegis_fusion4_init(fusion.as_mut_ptr(), 0.0, 1.0, 0.01); - assert_eq!(status, AegisStatusCode::Ok); - let mut fusion = fusion.assume_init(); - - // Register Sensor 1 - let status = aegis_fusion4_register_sensor(&mut fusion, 1, 0.2, 2, 3.0, 2); - assert_eq!(status, AegisStatusCode::Ok); - - let status = aegis_fusion4_predict(&mut fusion, 0.5); - assert_eq!(status, AegisStatusCode::Ok); - - let mut fusion_status = AegisFusionStatusCode::UnregisteredSensor; - let status = aegis_fusion4_fuse(&mut fusion, 1, 0.52, &mut fusion_status); - assert_eq!(status, AegisStatusCode::Ok); - assert_eq!(fusion_status, AegisFusionStatusCode::Fused); - assert!((aegis_fusion4_get_state(&fusion) - 0.52).abs() < 0.2); - assert_eq!(aegis_fusion4_get_sensor_health(&fusion, 1), 1.0); - } - } -} diff --git a/src/lib.rs b/src/lib.rs index c8d4ffa..aae5724 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -44,571 +44,3 @@ pub use linear::{LinearKalmanFilter, ScalarKalmanFilter}; pub use robust::{AdaptiveFilter1D, HInfinityFilter1D}; pub use stats::WindowedStats; pub use unscented::UnscentedKalmanFilter2D; - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_scalar_filter_convergence() { - let mut filter = ScalarKalmanFilter::new(0.0, 10.0, 0.01, 0.5).unwrap(); - let target = 25.0; - for _ in 0..50 { - filter.predict(0.0).unwrap(); - let _ = filter.update(target + 0.05, None).unwrap(); - } - assert!((filter.state() - target).abs() < 0.2); - } - - #[test] - fn test_scalar_filter_outlier_gating() { - let mut filter = ScalarKalmanFilter::new(10.0, 1.0, 0.01, 0.5).unwrap(); - // Adversarial spike — must be rejected by 3-sigma gating - let err = filter.update(9999.0, Some(3.0)); - assert_eq!(err, Err(FilterError::MeasurementGated)); - assert!((filter.state() - 10.0).abs() < 1e-9); - } - - #[test] - fn test_ukf_linear_observation_multi_step() { - // UKF with h(x) = x[0] (linear) — deterministically tracks a constant signal. - // Multiple updates allow covariance to shrink and state to lock onto the target. - let mut ukf = UnscentedKalmanFilter2D::new( - [0.0, 0.0], - [[2.0, 0.0], [0.0, 2.0]], - [[0.001, 0.0], [0.0, 0.001]], - 0.5, - ) - .unwrap(); - - let target = 5.0; - for _ in 0..30 { - ukf.predict(0.1, |x, _dt| [x[0], x[1]]).unwrap(); - let _ = ukf.update(target, |x| x[0], None).unwrap(); - } - - // After 30 updates the position estimate must be close to target - assert!((ukf.x[0] - target).abs() < 0.3, "ukf.x[0] = {}", ukf.x[0]); - } - - #[test] - fn test_ukf_nonlinear_tight_covariance() { - // h(x) = x[0]^2 — non-linear. With tight initial covariance sigma - // points stay close to mean, so linearisation error is small and a - // single update step visibly moves the state toward the true value. - let true_pos: Real = 3.0; - let mut ukf = UnscentedKalmanFilter2D::new( - [true_pos + 0.5, 0.0], - [[0.04, 0.0], [0.0, 0.04]], // tight — sigma points ±0.2 from mean - [[0.001, 0.0], [0.0, 0.001]], - 0.1, - ) - .unwrap(); - - ukf.predict(0.0, |x, _dt| [x[0], x[1]]).unwrap(); - - let z = true_pos * true_pos; // 9.0 - let before = ukf.x[0]; - let res = ukf.update(z, |x| x[0] * x[0], None).unwrap(); - - // Update must move x[0] toward true_pos (3.0) from initial 3.5 - assert!( - res[0] < before, - "Expected x[0] to decrease toward 3.0, before={before}, after={}", - res[0] - ); - assert!(res[0] > 2.5, "Expected x[0] > 2.5, got {}", res[0]); - } - - #[test] - fn test_ukf_predict_update_characterization() { - // Golden pins for the unscented propagation and update paths, guarding the - // iterator refactor in issue #19. Captured from the current implementation; - // reordering the weighted sums would shift these beyond 1e-12. - let mut ukf = UnscentedKalmanFilter2D::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[0.01, 0.0], [0.0, 0.01]], - 0.5, - ) - .unwrap(); - - #[cfg(feature = "single-precision")] - let tol: Real = 1e-4; - #[cfg(not(feature = "single-precision"))] - let tol: Real = 1e-12; - - ukf.predict(0.1, |x, _dt| [x[0] + 0.1, x[1]]).unwrap(); - let est = ukf.update(1.0, |x| x[0], None).unwrap(); - assert!( - (est[0] - 0.7019867549668872).abs() < tol, - "est[0] = {}", - est[0] - ); - assert!((est[1] - 0.0).abs() < tol, "est[1] = {}", est[1]); - assert!( - (ukf.p[0][0] - 0.3344370860927156).abs() < tol, - "p00 = {}", - ukf.p[0][0] - ); - assert!( - (ukf.p[1][1] - 1.0099999999999998).abs() < tol, - "p11 = {}", - ukf.p[1][1] - ); - - // A second step with a non-trivial transition exercises the covariance - // propagation (x1 drift) and the innovation / cross-covariance sums. - ukf.predict(0.1, |x, _dt| [x[0] + 0.1, x[1] + 0.05]) - .unwrap(); - let est2 = ukf.update(1.2, |x| x[0], None).unwrap(); - assert!( - (est2[0] - 0.9643322092384911).abs() < tol, - "est2[0] = {}", - est2[0] - ); - assert!( - (est2[1] - 0.050000000000000044).abs() < tol, - "est2[1] = {}", - est2[1] - ); - } - - #[test] - fn test_h_infinity_robustness() { - let mut h_inf = HInfinityFilter1D::new(10.0, 1.0, 0.05, 0.5, 5.0).unwrap(); - - for _ in 0..20 { - h_inf.predict(0.0).unwrap(); - let _ = h_inf.update(10.1).unwrap(); - } - - assert!((h_inf.state() - 10.1).abs() < 0.3); - assert!(h_inf.bound() > 0.0); - } - - #[test] - fn test_adaptive_filter_noise_tracking() { - let mut af = AdaptiveFilter1D::new(0.0, 5.0, 0.01, 1.0, 0.05).unwrap(); - for _ in 0..40 { - af.predict(0.0).unwrap(); - let _ = af.update(20.0).unwrap(); - } - assert!((af.x - 20.0).abs() < 0.5, "af.x = {}", af.x); - } - - #[test] - fn test_nan_inf_rejection() { - let result = ScalarKalmanFilter::new(Real::NAN, 1.0, 0.1, 1.0); - assert_eq!(result.err(), Some(FilterError::NonFiniteInput)); - - let mut f = ScalarKalmanFilter::new(0.0, 1.0, 0.1, 1.0).unwrap(); - assert_eq!( - f.update(Real::INFINITY, None).err(), - Some(FilterError::NonFiniteInput) - ); - } - - #[test] - fn test_multi_sensor_fusion_and_quarantine() { - // Setup fusion engine with capacity for 3 sensors - let mut fusion = MultiSensorFusion1D::<3>::new(0.0, 5.0, 0.05).unwrap(); - - // Sensor 1: Accurate Barometer (R=0.2, max 3 anomalies) - let baro = SensorSource::new(1, 0.2, 3).unwrap(); - // Sensor 2: Noisy GPS (R=1.5, max 2 anomalies) - let gps = SensorSource::new(2, 1.5, 2).unwrap(); - - fusion.register_sensor(baro).unwrap(); - fusion.register_sensor(gps).unwrap(); - - // Nominal fusion - fusion.predict(1.0).unwrap(); - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 1.05, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Fused); - - // Adversarial spoofing on GPS (id=2) - // Attack 1: Gated - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 2, - value: 500.0, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Gated); - - // Attack 2: Second consecutive anomaly triggers Quarantine! - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 2, - value: 500.0, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Quarantined); - assert_eq!(fusion.sensor_health(2), Some(0.0)); - - // Subsequent measurements from quarantined GPS are safely ignored - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 2, - value: 1.0, - }) - .unwrap(); - assert_eq!(status, FusionStatus::IgnoredQuarantined); - - // Barometer continues nominal operation unhindered - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 1.02, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Fused); - } - - #[test] - fn test_sensor_auto_recovery() { - let mut fusion = MultiSensorFusion1D::<2>::new(10.0, 1.0, 0.01).unwrap(); - // Sensor with auto-recovery threshold of 2 consecutive healthy observations - let gps = SensorSource::new(1, 0.5, 2).unwrap().with_auto_recovery(2); - fusion.register_sensor(gps).unwrap(); - - // 2 anomalies force quarantine - let _ = fusion.fuse(SensorMeasurement { - sensor_id: 1, - value: 100.0, - }); - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 100.0, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Quarantined); - - // 1st healthy sample while quarantined - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 10.05, - }) - .unwrap(); - assert_eq!(status, FusionStatus::IgnoredQuarantined); - - // 2nd healthy sample triggers auto-recovery! - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 10.02, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Recovered); - - // Subsequent sample is actively fused again - let status = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 10.01, - }) - .unwrap(); - assert_eq!(status, FusionStatus::Fused); - } - - #[test] - fn test_windowed_stats_rolling_metrics() { - let mut stats = WindowedStats::<4>::new(); - assert!(stats.is_empty()); - assert_eq!(stats.len(), 0); - - stats.push(2.0).unwrap(); - stats.push(4.0).unwrap(); - assert_eq!(stats.len(), 2); - assert!((stats.mean().unwrap() - 3.0).abs() < 1e-9); - assert_eq!(stats.min(), Some(2.0)); - assert_eq!(stats.max(), Some(4.0)); - assert!((stats.peak_to_peak().unwrap() - 2.0).abs() < 1e-9); - - // Push more samples to exceed window capacity of 4 - stats.push(6.0).unwrap(); - stats.push(8.0).unwrap(); - assert_eq!(stats.len(), 4); - assert!((stats.mean().unwrap() - 5.0).abs() < 1e-9); - - // Overwrite oldest (2.0) with 10.0 -> window has [10, 4, 6, 8], mean = 7.0 - stats.push(10.0).unwrap(); - assert_eq!(stats.len(), 4); - assert!((stats.mean().unwrap() - 7.0).abs() < 1e-9); - assert_eq!(stats.min(), Some(4.0)); - assert_eq!(stats.max(), Some(10.0)); - assert!(stats.variance().unwrap() > 0.0); - } - - #[test] - fn test_fdir_consecutive_anomaly_reset() { - let mut fusion = MultiSensorFusion1D::<2>::new(0.0, 1.0, 0.01).unwrap(); - // Sensor requires 2 consecutive anomalies to quarantine - let sensor = SensorSource::new(1, 0.2, 2).unwrap(); - fusion.register_sensor(sensor).unwrap(); - - // 1. First anomaly -> Gated - let st1 = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 100.0, - }) - .unwrap(); - assert_eq!(st1, FusionStatus::Gated); - - // 2. Interleaved healthy measurement -> Fused (breaks streak, resets anomaly_count) - let st2 = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 0.01, - }) - .unwrap(); - assert_eq!(st2, FusionStatus::Fused); - - // 3. Second anomaly (isolated, not consecutive) -> Gated, NOT Quarantined - let st3 = fusion - .fuse(SensorMeasurement { - sensor_id: 1, - value: 100.0, - }) - .unwrap(); - assert_eq!(st3, FusionStatus::Gated); - assert!((fusion.sensor_health(1).unwrap() - 0.3).abs() < 1e-9); - } - - #[test] - fn test_fusion_singularity_guard() { - let mut fusion = MultiSensorFusion1D::<1>::new(0.0, 1.0, 0.01).unwrap(); - let sensor = SensorSource::new(1, 0.5, 3).unwrap(); - fusion.register_sensor(sensor).unwrap(); - - // Force degenerate/singular variance - fusion.p = -0.5; - let err = fusion.fuse(SensorMeasurement { - sensor_id: 1, - value: 1.0, - }); - assert_eq!(err, Err(FilterError::SingularInnovation)); - } - - #[test] - fn test_h_infinity_known_reference_gain() { - // Analytical benchmark test pinning Simon (2006, Eq. 11.45-11.48) - // Given: x0 = 0.0, P0 = 1.0, Q = 0.0, R = 0.5, gamma = 5.0 - // Riccati Denominator: D = 1 - P/gamma^2 + P/R = 1 - 1/25 + 1/0.5 = 2.96 - // Minimax Gain: K = P / (R * D) = 1.0 / (0.5 * 2.96) = 1.0 / 1.48 - // When measurement z = 1.48, delta = K * residual = (1.0 / 1.48) * 1.48 = 1.0 exactly. - let mut h_inf = HInfinityFilter1D::new(0.0, 1.0, 0.0, 0.5, 5.0).unwrap(); - let new_state = h_inf.update(1.48).unwrap(); - assert!( - (new_state - 1.0).abs() < 1e-12, - "Expected exactly 1.0, got {}", - new_state - ); - let expected_post_cov = 1.0 / 2.96; - assert!( - (h_inf.bound() - expected_post_cov).abs() < 1e-12, - "Expected post-update bound {}, got {}", - expected_post_cov, - h_inf.bound() - ); - } - - #[test] - fn test_h_infinity_riccati_instability_guard() { - // When gamma bound is tighter than physically admissible under system noise: - // P = 10.0, R = 1.0, gamma = 0.5 => gamma^2 = 0.25 - // D = 1 - 10/0.25 + 10/1 = 1 - 40 + 10 = -29 <= 0 - let mut h_inf = HInfinityFilter1D::new(0.0, 10.0, 0.0, 1.0, 0.5).unwrap(); - let err = h_inf.update(1.0); - assert_eq!(err, Err(FilterError::NumericalInstability)); - } - - #[test] - fn test_adaptive_filter_bounds_and_prior_covariance() { - let mut af = AdaptiveFilter1D::new(0.0, 2.0, 0.01, 1.0, 0.1).unwrap(); - // Severe adversarial jump: residual is 1000.0, y^2 = 1e6 - af.predict(0.0).unwrap(); - let _ = af.update(1000.0).unwrap(); - // Online R estimate must be clamped within upper bound (<= 1e6) - assert!(af.estimated_measurement_noise() <= 1e6); - assert!(af.estimated_measurement_noise() >= 1e-6); - - // Sub-noise tiny measurement where y^2 < P_prior: - for _ in 0..20 { - af.predict(0.0).unwrap(); - let _ = af.update(af.x).unwrap(); - } - // Online R estimate must stay strictly positive, never collapsing below 1e-6 - assert!(af.estimated_measurement_noise() >= 1e-6); - } - - #[test] - fn test_capacity_exceeded_error() { - let mut fusion = MultiSensorFusion1D::<2>::new(0.0, 1.0, 0.01).unwrap(); - let s1 = SensorSource::new(1, 0.2, 3).unwrap(); - let s2 = SensorSource::new(2, 0.3, 3).unwrap(); - let s3 = SensorSource::new(3, 0.4, 3).unwrap(); - - assert_eq!(fusion.register_sensor(s1), Ok(())); - assert_eq!(fusion.register_sensor(s2), Ok(())); - // Array capacity is 2; 3rd sensor must return CapacityExceeded - assert_eq!( - fusion.register_sensor(s3), - Err(FilterError::CapacityExceeded) - ); - } - - #[test] - fn test_invalid_parameter_error() { - assert_eq!( - SensorSource::new(1, 0.0, 3).err(), - Some(FilterError::InvalidParameter) - ); - assert_eq!( - SensorSource::new(1, -0.5, 3).err(), - Some(FilterError::InvalidParameter) - ); - let s = SensorSource::new(1, 0.5, 3).unwrap(); - assert_eq!( - s.with_gate_threshold(0.0).err(), - Some(FilterError::InvalidParameter) - ); - assert_eq!( - MultiSensorFusion1D::<1>::new(0.0, -1.0, 0.01).err(), - Some(FilterError::InvalidParameter) - ); - } - - #[test] - fn test_invalid_covariance_error() { - assert_eq!( - ScalarKalmanFilter::new(0.0, 0.0, 0.01, 0.5).err(), - Some(FilterError::InvalidCovariance) - ); - assert_eq!( - ScalarKalmanFilter::new(0.0, -1.0, 0.01, 0.5).err(), - Some(FilterError::InvalidCovariance) - ); - assert_eq!( - HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 0.0).err(), - Some(FilterError::InvalidCovariance) - ); - assert_eq!( - AdaptiveFilter1D::new(0.0, 1.0, 0.01, 0.5, 1.5).err(), - Some(FilterError::InvalidCovariance) - ); - } - - #[test] - fn test_zero_panics_property_stress() { - let extreme_floats = [ - Real::NAN, - Real::INFINITY, - Real::NEG_INFINITY, - 0.0, - -0.0, - Real::MIN_POSITIVE, - Real::MAX, - -Real::MAX, - #[cfg(not(feature = "single-precision"))] - 1e-300, - #[cfg(not(feature = "single-precision"))] - 1e300, - #[cfg(feature = "single-precision")] - 1e-35, - #[cfg(feature = "single-precision")] - 1e35, - ]; - - let mut kf = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap(); - let mut hinf = HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 5.0).unwrap(); - let mut af = AdaptiveFilter1D::new(0.0, 1.0, 0.01, 0.5, 0.05).unwrap(); - - for &val in &extreme_floats { - // Predict calls must never panic - let _ = kf.predict(val); - let _ = hinf.predict(val); - let _ = af.predict(val); - - // Update calls must return Result, never panic - let _ = kf.update(val, Some(3.0)); - let _ = hinf.update(val); - let _ = af.update(val); - } - } - - #[test] - fn test_eskf_stationary_kinematics() { - let init_state = EskfNominalState::new( - Vector3::new(0.0, 0.0, 100.0), - Vector3::ZERO, - Quaternion::IDENTITY, - Vector3::ZERO, - Vector3::ZERO, - ); - let cov_diag = [0.1; 15]; - let noise = EskfNoiseParameters::new(0.01, 0.001, 0.0001, 0.00001); - let mut eskf = ErrorStateKalmanFilter::new( - init_state, - cov_diag, - noise, - Vector3::new(0.0, 0.0, -9.80665), - ) - .unwrap(); - - // Stationary IMU on Earth surface: accelerometer reads +1g upward reaction - let accel = Vector3::new(0.0, 0.0, 9.80665); - let gyro = Vector3::ZERO; - let dt = 0.01; - - for _ in 0..100 { - eskf.predict(&accel, &gyro, dt).unwrap(); - } - - // Stationary drone should remain at altitude 100m - assert!((eskf.state().position.z - 100.0).abs() < 1e-4); - assert!(eskf.state().velocity.norm() < 1e-4); - assert!((eskf.state().attitude.w - 1.0).abs() < 1e-5); - } - - #[test] - fn test_eskf_position_update_and_outlier_gating() { - let init_state = EskfNominalState::new( - Vector3::new(10.0, 5.0, -2.0), - Vector3::ZERO, - Quaternion::IDENTITY, - Vector3::ZERO, - Vector3::ZERO, - ); - let cov_diag = [1.0; 15]; - let noise = EskfNoiseParameters::new(0.05, 0.005, 0.0001, 0.00001); - let mut eskf = ErrorStateKalmanFilter::new( - init_state, - cov_diag, - noise, - Vector3::new(0.0, 0.0, -9.80665), - ) - .unwrap(); - - // Legitimate GNSS position observation (sub-meter drift) - let gnss_pos = Vector3::new(10.2, 5.1, -1.9); - eskf.update_position(&gnss_pos, 0.25, None).unwrap(); - - assert!((eskf.state().position.x - 10.2).abs() < 0.2); - assert!((eskf.state().position.y - 5.1).abs() < 0.2); - - // Adversarial GNSS spoofing / multipath spike (10,000 meters away) - let spoofed_pos = Vector3::new(10000.0, 0.0, 0.0); - let err = eskf.update_position(&spoofed_pos, 0.25, Some(3.0)); - assert_eq!(err, Err(FilterError::MeasurementGated)); - } -} diff --git a/src/linear.rs b/src/linear.rs index c68ef45..171d463 100644 --- a/src/linear.rs +++ b/src/linear.rs @@ -620,267 +620,3 @@ impl LinearKalmanFilter { Ok(inv) } } - -#[cfg(test)] -mod tests { - use super::*; - - // (1) predict regression: a constant-velocity model must still propagate x. - // F = [[1, 0.1], [0, 1]] -> position advances by 0.1*velocity per step; with - // velocity held at 100, x[0] grows by 10.0 each predict. Deterministic. - #[test] - fn test_linear_predict_kinematic() { - let mut f = LinearKalmanFilter::<2, 1>::new( - [0.0, 100.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.1], [0.0, 1.0]], - [[0.001, 0.0], [0.0, 0.001]], - [[1.0, 0.0]], // H: one row, position only - [[0.5]], - ) - .unwrap(); - for _ in 0..3 { - f.predict().unwrap(); - } - assert!((f.x[0] - 30.0).abs() < 1e-9, "x[0] = {}", f.x[0]); - } - - // (2) M=1 measurement update converges the state to a constant target, exactly - // like the 1D scalar filter. S=1, M=1 with F=I: x tracks the target as the - // posterior variance shrinks. - #[test] - fn test_linear_update_m1_convergence() { - let mut f = - LinearKalmanFilter::<1, 1>::new([0.0], [[10.0]], [[1.0]], [[0.01]], [[1.0]], [[0.5]]) - .unwrap(); - let target = 25.0; - for _ in 0..50 { - f.predict().unwrap(); - let _ = f.update(&[target], None).unwrap(); - } - assert!((f.x[0] - target).abs() < 0.2, "x[0] = {}", f.x[0]); - } - - // (3) The H=I, P=I, R=I case has an exact closed-form first update: K = P H^T - // S^-1 = I . I . (2I)^-1 = 0.5 I, so x moves halfway to the measurement in one step. - #[test] - fn test_linear_update_m2_single_step_halfway() { - let mut f = LinearKalmanFilter::<2, 2>::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], // F = I - [[0.0, 0.0], [0.0, 0.0]], // Q = 0 - [[1.0, 0.0], [0.0, 1.0]], // H = I - [[1.0, 0.0], [0.0, 1.0]], // R = I - ) - .unwrap(); - let z = [10.0, 20.0]; - let out = f.update(&z, None).unwrap(); - assert!((out[0] - 5.0).abs() < 1e-9, "out[0] = {}", out[0]); - assert!((out[1] - 10.0).abs() < 1e-9, "out[1] = {}", out[1]); - } - - // (4) Repeated M=2 updates converge to the true measurement vector. - #[test] - fn test_linear_update_m2_convergence() { - let mut f = LinearKalmanFilter::<2, 2>::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - [[0.01, 0.0], [0.0, 0.01]], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - ) - .unwrap(); - let z = [10.0, 20.0]; - for _ in 0..50 { - f.predict().unwrap(); - f.update(&z, None).unwrap(); - } - assert!((f.x[0] - 10.0).abs() < 0.1, "x[0] = {}", f.x[0]); - assert!((f.x[1] - 20.0).abs() < 0.1, "x[1] = {}", f.x[1]); - } - - // (5) Joseph form must keep P symmetric and positive on the diagonals. For the - // H=I, P=I, R=I case the posterior covariance is the analytic 0.50 * I: - // P = (I - KH) P (I - KH)^T + K R K^T = 0.25 I + 0.25 I = 0.50 I. - #[test] - fn test_linear_update_joseph_symmetry_and_psd() { - let mut f = LinearKalmanFilter::<2, 2>::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - [[0.0, 0.0], [0.0, 0.0]], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - ) - .unwrap(); - f.update(&[5.0, 5.0], None).unwrap(); - // Symmetry: off-diagonals match on both triangles. - assert!((f.p[0][1] - f.p[1][0]).abs() < 1e-9, "off-diag mismatch"); - assert!((f.p[0][1]).abs() < 1e-9, "off-diag = {}", f.p[0][1]); - // Diagonals: analytic 0.50, and strictly positive (PSD lower bound). - assert!((f.p[0][0] - 0.50).abs() < 1e-9, "p00 = {}", f.p[0][0]); - assert!((f.p[1][1] - 0.50).abs() < 1e-9, "p11 = {}", f.p[1][1]); - } - - // (6) Gating: a far-off M=1 measurement is rejected (3-sigma gate) and the state - // is left untouched, mirroring the scalar filter's MeasurementGated behaviour. - #[test] - fn test_linear_update_m1_gating_rejects_outlier() { - let mut f = - LinearKalmanFilter::<1, 1>::new([0.0], [[1.0]], [[1.0]], [[0.0]], [[1.0]], [[0.01]]) - .unwrap(); - let err = f.update(&[1000.0], Some(3.0)); - assert_eq!(err, Err(FilterError::MeasurementGated)); - assert!((f.x[0] - 0.0).abs() < 1e-12, "state moved on gated update"); - } - - // (7) A rank-deficient observation (both rows identical) with zero process and - // measurement noise makes the innovation covariance singular -> NumericalInstability. - #[test] - fn test_linear_update_singular_innovation() { - let mut f = LinearKalmanFilter::<2, 2>::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - [[0.0, 0.0], [0.0, 0.0]], - [[1.0, 0.0], [1.0, 0.0]], // rank-deficient H - [[0.0, 0.0], [0.0, 0.0]], // R = 0, so S = H P H^T is singular - ) - .unwrap(); - assert_eq!( - f.update(&[1.0, 1.0], None).err(), - Some(FilterError::NumericalInstability) - ); - } - - // (8) Non-finite measurements are rejected at the boundary, like the 1D path. - #[test] - fn test_linear_update_nonfinite_measurement() { - let mut f = - LinearKalmanFilter::<1, 1>::new([0.0], [[1.0]], [[1.0]], [[0.01]], [[1.0]], [[0.5]]) - .unwrap(); - assert_eq!( - f.update(&[Real::NAN], None).err(), - Some(FilterError::NonFiniteInput) - ); - assert_eq!( - f.update(&[Real::INFINITY], None).err(), - Some(FilterError::NonFiniteInput) - ); - } - - // (9) Non-diagonal H with correlated (non-diagonal) measurement error must use the true - // K = P H^T S^-1, not a column-sum of S^-1. With P=I, H=[[1,1],[0,1]], R=I the - // innovation covariance is S = H H^T + I = [[3,1],[1,2]], inverse [[2/5,-1/5],[-1/5,3/5]], - // so K = H^T S^-1 = [[0.4,-0.2],[0.2,0.4]]; with x=0, x_new = K * z. - // z=[4,2] is chosen so the off-diagonal K[0][1] (correct -0.2, old bug 0) is exposed: - // the old index bug gave x_new[0] = 0.8 instead of 1.2. - #[test] - fn test_linear_update_k_non_diagonal_correlated() { - let mut f = LinearKalmanFilter::<2, 2>::new( - [0.0, 0.0], - [[1.0, 0.0], [0.0, 1.0]], - [[1.0, 0.0], [0.0, 1.0]], - [[0.0, 0.0], [0.0, 0.0]], - [[1.0, 1.0], [0.0, 1.0]], // non-diagonal H - [[1.0, 0.0], [0.0, 1.0]], - ) - .unwrap(); - let out = f.update(&[4.0, 2.0], None).unwrap(); - #[cfg(feature = "single-precision")] - let tol: Real = 1e-4; - #[cfg(not(feature = "single-precision"))] - let tol: Real = 1e-9; - assert!((out[0] - 1.2).abs() < tol, "out[0] = {}", out[0]); - assert!((out[1] - 1.6).abs() < tol, "out[1] = {}", out[1]); - } - // (9) Factored O(S^3) Joseph update test for S=4, M=2: verifies that the factored - // (E*P)*E^T + (K*S)*K^T produces identical positive semi-definite, symmetric covariance. - #[test] - #[allow(clippy::needless_range_loop)] - fn test_linear_update_higher_dim_factored_joseph() { - let x0 = [10.0, 0.5, 20.0, -0.2]; - let p0 = [ - [2.0, 0.1, 0.0, 0.0], - [0.1, 1.5, 0.0, 0.0], - [0.0, 0.0, 2.0, 0.1], - [0.0, 0.0, 0.1, 1.5], - ]; - let f_mat = [ - [1.0, 0.1, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.1], - [0.0, 0.0, 0.0, 1.0], - ]; - let q_mat = [ - [0.01, 0.0, 0.0, 0.0], - [0.0, 0.01, 0.0, 0.0], - [0.0, 0.0, 0.01, 0.0], - [0.0, 0.0, 0.01, 0.0], - ]; - // Measure position x and position y - let h_mat = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]]; - let r_mat = [[0.25, 0.02], [0.02, 0.25]]; - - let mut kf = LinearKalmanFilter::<4, 2>::new(x0, p0, f_mat, q_mat, h_mat, r_mat).unwrap(); - kf.predict().unwrap(); - let z = [10.15, 19.95]; - let updated_state = kf - .update(&z, Some(crate::DEFAULT_OUTLIER_GATE_SIGMA)) - .unwrap(); - - // State moves toward measurement - assert!((updated_state[0] - 10.15).abs() < 1.0); - assert!((updated_state[2] - 19.95).abs() < 1.0); - - // Verify covariance symmetry P[i][j] == P[j][i] - for i in 0..4 { - for j in 0..4 { - assert!( - (kf.p[i][j] - kf.p[j][i]).abs() < 1e-12, - "Covariance asymmetry at ({}, {}): {} vs {}", - i, - j, - kf.p[i][j], - kf.p[j][i] - ); - } - // Strict positive variance on diagonal - assert!(kf.p[i][i] > 0.0, "Diagonal variance non-positive at {}", i); - assert!( - kf.p[i][i] < p0[i][i] + 0.1, - "Variance did not contract on measurement update" - ); - } - } - - // (10) Fail-closed security validation: when initialized with new_resilient or with_default_gate, - // calls to update(..., None) automatically enforce 3-sigma innovation gating. - #[test] - fn test_fail_closed_gating_behavior() { - let mut f_scalar = ScalarKalmanFilter::new_resilient(0.0, 1.0, 0.01, 0.5).unwrap(); - // 1000-sigma outlier passed with None: MUST be rejected by default 3-sigma gate! - let err = f_scalar.update(1000.0, None); - assert_eq!(err, Err(FilterError::MeasurementGated)); - assert!((f_scalar.state() - 0.0).abs() < 1e-12); - - // Explicit opt-out via Some(-1.0) allows through - let accepted = f_scalar.update(1.0, Some(-1.0)).unwrap(); - assert!(accepted > 0.0); - - // Same for LinearKalmanFilter - let mut f_linear = LinearKalmanFilter::<1, 1>::new_resilient( - [0.0], - [[1.0]], - [[1.0]], - [[0.01]], - [[1.0]], - [[0.5]], - ) - .unwrap(); - let err_linear = f_linear.update(&[1000.0], None); - assert_eq!(err_linear, Err(FilterError::MeasurementGated)); - } -} diff --git a/tests/adaptive.rs b/tests/adaptive.rs new file mode 100644 index 0000000..379eb5c --- /dev/null +++ b/tests/adaptive.rs @@ -0,0 +1,32 @@ +#[cfg(test)] +mod adaptive_filter { + use aegis_filter::AdaptiveFilter1D; + #[test] + fn noise_tracking() { + let mut af = AdaptiveFilter1D::new(0.0, 5.0, 0.01, 1.0, 0.05).unwrap(); + for _ in 0..40 { + af.predict(0.0).unwrap(); + let _ = af.update(20.0).unwrap(); + } + assert!((af.x - 20.0).abs() < 0.5, "af.x = {}", af.x); + } + + #[test] + fn bounds_and_prior_covariance() { + let mut af = AdaptiveFilter1D::new(0.0, 2.0, 0.01, 1.0, 0.1).unwrap(); + // Severe adversarial jump: residual is 1000.0, y^2 = 1e6 + af.predict(0.0).unwrap(); + let _ = af.update(1000.0).unwrap(); + // Online R estimate must be clamped within upper bound (<= 1e6) + assert!(af.estimated_measurement_noise() <= 1e6); + assert!(af.estimated_measurement_noise() >= 1e-6); + + // Sub-noise tiny measurement where y^2 < P_prior: + for _ in 0..20 { + af.predict(0.0).unwrap(); + let _ = af.update(af.x).unwrap(); + } + // Online R estimate must stay strictly positive, never collapsing below 1e-6 + assert!(af.estimated_measurement_noise() >= 1e-6); + } +} diff --git a/tests/c_ffi.rs b/tests/c_ffi.rs new file mode 100644 index 0000000..62bc0d9 --- /dev/null +++ b/tests/c_ffi.rs @@ -0,0 +1,109 @@ +#[cfg(test)] +mod c_ffi { + use aegis_filter::ffi::AegisStatusCode; + use core::mem::MaybeUninit; + + #[test] + fn scalar_filter() { + use aegis_filter::ffi::{ + aegis_scalar_filter_get_state, aegis_scalar_filter_get_variance, + aegis_scalar_filter_init, aegis_scalar_filter_predict, + aegis_scalar_filter_set_default_gate, aegis_scalar_filter_update, AegisScalarFilter, + AEGIS_GATE_DISABLE, + }; + + unsafe { + // Null pointer safety + assert_eq!( + aegis_scalar_filter_init(core::ptr::null_mut(), 0.0, 1.0, 0.01, 0.5), + AegisStatusCode::NullPointer + ); + + let mut filter = MaybeUninit::::uninit(); + let status = aegis_scalar_filter_init(filter.as_mut_ptr(), 0.0, 1.0, 0.01, 0.5); + assert_eq!(status, AegisStatusCode::Ok); + let mut filter = filter.assume_init(); + + let status = aegis_scalar_filter_predict(&mut filter, 0.1); + assert_eq!(status, AegisStatusCode::Ok); + + let mut out = 0.0; + let status = aegis_scalar_filter_update(&mut filter, 1.05, 3.0, &mut out); + assert_eq!(status, AegisStatusCode::Ok); + assert!(out > 0.0); + assert!((aegis_scalar_filter_get_state(&filter) - out).abs() < 1e-12); + assert!(aegis_scalar_filter_get_variance(&filter) > 0.0); + + // Rejection of adversary spike + let status = aegis_scalar_filter_update(&mut filter, 9999.0, 3.0, &mut out); + assert_eq!(status, AegisStatusCode::MeasurementGated); + + // Test fail-closed default gate configuration + let status = aegis_scalar_filter_set_default_gate(&mut filter, 3.0); + assert_eq!(status, AegisStatusCode::Ok); + + // Passing 0.0 falls back to configured 3.0 default gate -> rejects spike + let status = aegis_scalar_filter_update(&mut filter, 9999.0, 0.0, &mut out); + assert_eq!(status, AegisStatusCode::MeasurementGated); + + // Passing explicit AEGIS_GATE_DISABLE (-1.0) allows through + let status = + aegis_scalar_filter_update(&mut filter, 1.05, AEGIS_GATE_DISABLE, &mut out); + assert_eq!(status, AegisStatusCode::Ok); + } + } + + #[test] + fn h_infinity_filter() { + use aegis_filter::ffi::{ + aegis_h_inf_filter_get_bound, aegis_h_inf_filter_init, aegis_h_inf_filter_predict, + aegis_h_inf_filter_update, AegisHInfFilter, + }; + + unsafe { + let mut hinf = MaybeUninit::::uninit(); + let status = aegis_h_inf_filter_init(hinf.as_mut_ptr(), 0.0, 1.0, 0.01, 0.5, 5.0); + assert_eq!(status, AegisStatusCode::Ok); + let mut hinf = hinf.assume_init(); + + let status = aegis_h_inf_filter_predict(&mut hinf, 0.1); + assert_eq!(status, AegisStatusCode::Ok); + + let mut out = 0.0; + let status = aegis_h_inf_filter_update(&mut hinf, 1.05, &mut out); + assert_eq!(status, AegisStatusCode::Ok); + assert!(out > 0.0); + assert!(aegis_h_inf_filter_get_bound(&hinf) > 0.0); + } + } + + #[test] + fn fusion4_and_fdir() { + use aegis_filter::ffi::{ + aegis_fusion4_fuse, aegis_fusion4_get_sensor_health, aegis_fusion4_get_state, + aegis_fusion4_init, aegis_fusion4_predict, aegis_fusion4_register_sensor, AegisFusion4, + AegisFusionStatusCode, + }; + + unsafe { + let mut fusion = MaybeUninit::::uninit(); + let status = aegis_fusion4_init(fusion.as_mut_ptr(), 0.0, 1.0, 0.01); + assert_eq!(status, AegisStatusCode::Ok); + let mut fusion = fusion.assume_init(); + + // Register Sensor 1 + let status = aegis_fusion4_register_sensor(&mut fusion, 1, 0.2, 2, 3.0, 2); + assert_eq!(status, AegisStatusCode::Ok); + + let status = aegis_fusion4_predict(&mut fusion, 0.5); + assert_eq!(status, AegisStatusCode::Ok); + + let mut fusion_status = AegisFusionStatusCode::UnregisteredSensor; + let status = aegis_fusion4_fuse(&mut fusion, 1, 0.52, &mut fusion_status); + assert_eq!(status, AegisStatusCode::Ok); + assert_eq!(fusion_status, AegisFusionStatusCode::Fused); + assert!((aegis_fusion4_get_state(&fusion) - 0.52).abs() < 0.2); + assert_eq!(aegis_fusion4_get_sensor_health(&fusion, 1), 1.0); + } + } +} diff --git a/tests/error.rs b/tests/error.rs new file mode 100644 index 0000000..b250451 --- /dev/null +++ b/tests/error.rs @@ -0,0 +1,102 @@ +#[cfg(test)] +mod err { + use aegis_filter::{ + AdaptiveFilter1D, FilterError, HInfinityFilter1D, MultiSensorFusion1D, Real, + ScalarKalmanFilter, SensorSource, + }; + + #[test] + fn capacity_exceeded() { + let mut fusion = MultiSensorFusion1D::<2>::new(0.0, 1.0, 0.01).unwrap(); + let s1 = SensorSource::new(1, 0.2, 3).unwrap(); + let s2 = SensorSource::new(2, 0.3, 3).unwrap(); + let s3 = SensorSource::new(3, 0.4, 3).unwrap(); + + assert_eq!(fusion.register_sensor(s1), Ok(())); + assert_eq!(fusion.register_sensor(s2), Ok(())); + // Array capacity is 2; 3rd sensor must return CapacityExceeded + assert_eq!( + fusion.register_sensor(s3), + Err(FilterError::CapacityExceeded) + ); + } + + #[test] + fn invalid_parameter() { + assert_eq!( + SensorSource::new(1, 0.0, 3).err(), + Some(FilterError::InvalidParameter) + ); + assert_eq!( + SensorSource::new(1, -0.5, 3).err(), + Some(FilterError::InvalidParameter) + ); + let s = SensorSource::new(1, 0.5, 3).unwrap(); + assert_eq!( + s.with_gate_threshold(0.0).err(), + Some(FilterError::InvalidParameter) + ); + assert_eq!( + MultiSensorFusion1D::<1>::new(0.0, -1.0, 0.01).err(), + Some(FilterError::InvalidParameter) + ); + } + + #[test] + fn invalid_covariance() { + assert_eq!( + ScalarKalmanFilter::new(0.0, 0.0, 0.01, 0.5).err(), + Some(FilterError::InvalidCovariance) + ); + assert_eq!( + ScalarKalmanFilter::new(0.0, -1.0, 0.01, 0.5).err(), + Some(FilterError::InvalidCovariance) + ); + assert_eq!( + HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 0.0).err(), + Some(FilterError::InvalidCovariance) + ); + assert_eq!( + AdaptiveFilter1D::new(0.0, 1.0, 0.01, 0.5, 1.5).err(), + Some(FilterError::InvalidCovariance) + ); + } + + #[test] + fn zero_panics_property_stress() { + let extreme_floats = [ + Real::NAN, + Real::INFINITY, + Real::NEG_INFINITY, + 0.0, + -0.0, + Real::MIN_POSITIVE, + Real::MAX, + -Real::MAX, + #[cfg(not(feature = "single-precision"))] + 1e-300, + #[cfg(not(feature = "single-precision"))] + 1e300, + #[cfg(feature = "single-precision")] + 1e-35, + #[cfg(feature = "single-precision")] + 1e35, + ]; + + let mut kf = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap(); + let mut hinf = HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 5.0).unwrap(); + let mut af = AdaptiveFilter1D::new(0.0, 1.0, 0.01, 0.5, 0.05).unwrap(); + + for &val in &extreme_floats { + // Predict calls must never panic + let _ = kf.predict(val); + let _ = hinf.predict(val); + let _ = af.predict(val); + + // Update calls must return Result, never panic + let _ = kf.update(val, Some(3.0)); + let _ = hinf.update(val); + let _ = af.update(val); + } + } +} diff --git a/tests/hinfinity.rs b/tests/hinfinity.rs new file mode 100644 index 0000000..30840a5 --- /dev/null +++ b/tests/hinfinity.rs @@ -0,0 +1,50 @@ +#[cfg(test)] +mod h_infinity { + use aegis_filter::{FilterError, HInfinityFilter1D}; + + #[test] + fn robustness() { + let mut h_inf = HInfinityFilter1D::new(10.0, 1.0, 0.05, 0.5, 5.0).unwrap(); + + for _ in 0..20 { + h_inf.predict(0.0).unwrap(); + let _ = h_inf.update(10.1).unwrap(); + } + + assert!((h_inf.state() - 10.1).abs() < 0.3); + assert!(h_inf.bound() > 0.0); + } + + #[test] + fn known_reference_gain() { + // Analytical benchmark test pinning Simon (2006, Eq. 11.45-11.48) + // Given: x0 = 0.0, P0 = 1.0, Q = 0.0, R = 0.5, gamma = 5.0 + // Riccati Denominator: D = 1 - P/gamma^2 + P/R = 1 - 1/25 + 1/0.5 = 2.96 + // Minimax Gain: K = P / (R * D) = 1.0 / (0.5 * 2.96) = 1.0 / 1.48 + // When measurement z = 1.48, delta = K * residual = (1.0 / 1.48) * 1.48 = 1.0 exactly. + let mut h_inf = HInfinityFilter1D::new(0.0, 1.0, 0.0, 0.5, 5.0).unwrap(); + let new_state = h_inf.update(1.48).unwrap(); + assert!( + (new_state - 1.0).abs() < 1e-12, + "Expected exactly 1.0, got {}", + new_state + ); + let expected_post_cov = 1.0 / 2.96; + assert!( + (h_inf.bound() - expected_post_cov).abs() < 1e-12, + "Expected post-update bound {}, got {}", + expected_post_cov, + h_inf.bound() + ); + } + + #[test] + fn riccati_instability_guard() { + // When gamma bound is tighter than physically admissible under system noise: + // P = 10.0, R = 1.0, gamma = 0.5 => gamma^2 = 0.25 + // D = 1 - 10/0.25 + 10/1 = 1 - 40 + 10 = -29 <= 0 + let mut h_inf = HInfinityFilter1D::new(0.0, 10.0, 0.0, 1.0, 0.5).unwrap(); + let err = h_inf.update(1.0); + assert_eq!(err, Err(FilterError::NumericalInstability)); + } +} diff --git a/tests/kalman_filter.rs b/tests/kalman_filter.rs new file mode 100644 index 0000000..963cd0d --- /dev/null +++ b/tests/kalman_filter.rs @@ -0,0 +1,239 @@ +#[cfg(test)] +mod ukf { + use aegis_filter::{Real, UnscentedKalmanFilter2D}; + #[test] + fn linear_observation_multi_step() { + // UKF with h(x) = x[0] (linear) — deterministically tracks a constant signal. + // Multiple updates allow covariance to shrink and state to lock onto the target. + let mut ukf = UnscentedKalmanFilter2D::new( + [0.0, 0.0], + [[2.0, 0.0], [0.0, 2.0]], + [[0.001, 0.0], [0.0, 0.001]], + 0.5, + ) + .unwrap(); + + let target = 5.0; + for _ in 0..30 { + ukf.predict(0.1, |x, _dt| [x[0], x[1]]).unwrap(); + let _ = ukf.update(target, |x| x[0], None).unwrap(); + } + + // After 30 updates the position estimate must be close to target + assert!((ukf.x[0] - target).abs() < 0.3, "ukf.x[0] = {}", ukf.x[0]); + } + + #[test] + fn nonlinear_tight_covariance() { + // h(x) = x[0]^2 — non-linear. With tight initial covariance sigma + // points stay close to mean, so linearisation error is small and a + // single update step visibly moves the state toward the true value. + let true_pos: Real = 3.0; + let mut ukf = UnscentedKalmanFilter2D::new( + [true_pos + 0.5, 0.0], + [[0.04, 0.0], [0.0, 0.04]], // tight — sigma points ±0.2 from mean + [[0.001, 0.0], [0.0, 0.001]], + 0.1, + ) + .unwrap(); + + ukf.predict(0.0, |x, _dt| [x[0], x[1]]).unwrap(); + + let z = true_pos * true_pos; // 9.0 + let before = ukf.x[0]; + let res = ukf.update(z, |x| x[0] * x[0], None).unwrap(); + + // Update must move x[0] toward true_pos (3.0) from initial 3.5 + assert!( + res[0] < before, + "Expected x[0] to decrease toward 3.0, before={before}, after={}", + res[0] + ); + assert!(res[0] > 2.5, "Expected x[0] > 2.5, got {}", res[0]); + } + + #[test] + fn predict_update_characterization() { + // Golden pins for the unscented propagation and update paths, guarding the + // iterator refactor in issue #19. Captured from the current implementation; + // reordering the weighted sums would shift these beyond 1e-12. + let mut ukf = UnscentedKalmanFilter2D::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[0.01, 0.0], [0.0, 0.01]], + 0.5, + ) + .unwrap(); + + #[cfg(feature = "single-precision")] + let tol: Real = 1e-4; + #[cfg(not(feature = "single-precision"))] + let tol: Real = 1e-12; + + ukf.predict(0.1, |x, _dt| [x[0] + 0.1, x[1]]).unwrap(); + let est = ukf.update(1.0, |x| x[0], None).unwrap(); + assert!( + (est[0] - 0.7019867549668872).abs() < tol, + "est[0] = {}", + est[0] + ); + assert!((est[1] - 0.0).abs() < tol, "est[1] = {}", est[1]); + assert!( + (ukf.p[0][0] - 0.3344370860927156).abs() < tol, + "p00 = {}", + ukf.p[0][0] + ); + assert!( + (ukf.p[1][1] - 1.0099999999999998).abs() < tol, + "p11 = {}", + ukf.p[1][1] + ); + + // A second step with a non-trivial transition exercises the covariance + // propagation (x1 drift) and the innovation / cross-covariance sums. + ukf.predict(0.1, |x, _dt| [x[0] + 0.1, x[1] + 0.05]) + .unwrap(); + let est2 = ukf.update(1.2, |x| x[0], None).unwrap(); + assert!( + (est2[0] - 0.9643322092384911).abs() < tol, + "est2[0] = {}", + est2[0] + ); + assert!( + (est2[1] - 0.050000000000000044).abs() < tol, + "est2[1] = {}", + est2[1] + ); + } +} + +#[cfg(test)] +mod eskf { + use aegis_filter::{ + ErrorStateKalmanFilter, EskfNoiseParameters, EskfNominalState, FilterError, Quaternion, + Vector3, + }; + + #[test] + fn stationary_kinematics() { + let init_state = EskfNominalState::new( + Vector3::new(0.0, 0.0, 100.0), + Vector3::ZERO, + Quaternion::IDENTITY, + Vector3::ZERO, + Vector3::ZERO, + ); + let cov_diag = [0.1; 15]; + let noise = EskfNoiseParameters::new(0.01, 0.001, 0.0001, 0.00001); + let mut eskf = ErrorStateKalmanFilter::new( + init_state, + cov_diag, + noise, + Vector3::new(0.0, 0.0, -9.80665), + ) + .unwrap(); + + // Stationary IMU on Earth surface: accelerometer reads +1g upward reaction + let accel = Vector3::new(0.0, 0.0, 9.80665); + let gyro = Vector3::ZERO; + let dt = 0.01; + + for _ in 0..100 { + eskf.predict(&accel, &gyro, dt).unwrap(); + } + + // Stationary drone should remain at altitude 100m + assert!((eskf.state().position.z - 100.0).abs() < 1e-4); + assert!(eskf.state().velocity.norm() < 1e-4); + assert!((eskf.state().attitude.w - 1.0).abs() < 1e-5); + } + + #[test] + fn position_update_and_outlier_gating() { + let init_state = EskfNominalState::new( + Vector3::new(10.0, 5.0, -2.0), + Vector3::ZERO, + Quaternion::IDENTITY, + Vector3::ZERO, + Vector3::ZERO, + ); + let cov_diag = [1.0; 15]; + let noise = EskfNoiseParameters::new(0.05, 0.005, 0.0001, 0.00001); + let mut eskf = ErrorStateKalmanFilter::new( + init_state, + cov_diag, + noise, + Vector3::new(0.0, 0.0, -9.80665), + ) + .unwrap(); + + // Legitimate GNSS position observation (sub-meter drift) + let gnss_pos = Vector3::new(10.2, 5.1, -1.9); + eskf.update_position(&gnss_pos, 0.25, None).unwrap(); + + assert!((eskf.state().position.x - 10.2).abs() < 0.2); + assert!((eskf.state().position.y - 5.1).abs() < 0.2); + + // Adversarial GNSS spoofing / multipath spike (10,000 meters away) + let spoofed_pos = Vector3::new(10000.0, 0.0, 0.0); + let err = eskf.update_position(&spoofed_pos, 0.25, Some(3.0)); + assert_eq!(err, Err(FilterError::MeasurementGated)); + } +} + +#[cfg(test)] +mod fusion { + use aegis_filter::{ + FilterError, FusionStatus, MultiSensorFusion1D, SensorMeasurement, SensorSource, + }; + #[test] + fn consecutive_anomaly_reset() { + let mut fusion = MultiSensorFusion1D::<2>::new(0.0, 1.0, 0.01).unwrap(); + // Sensor requires 2 consecutive anomalies to quarantine + let sensor = SensorSource::new(1, 0.2, 2).unwrap(); + fusion.register_sensor(sensor).unwrap(); + + // 1. First anomaly -> Gated + let st1 = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 100.0, + }) + .unwrap(); + assert_eq!(st1, FusionStatus::Gated); + + // 2. Interleaved healthy measurement -> Fused (breaks streak, resets anomaly_count) + let st2 = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 0.01, + }) + .unwrap(); + assert_eq!(st2, FusionStatus::Fused); + + // 3. Second anomaly (isolated, not consecutive) -> Gated, NOT Quarantined + let st3 = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 100.0, + }) + .unwrap(); + assert_eq!(st3, FusionStatus::Gated); + assert!((fusion.sensor_health(1).unwrap() - 0.3).abs() < 1e-9); + } + + #[test] + fn singularity_guard() { + let mut fusion = MultiSensorFusion1D::<1>::new(0.0, 1.0, 0.01).unwrap(); + let sensor = SensorSource::new(1, 0.5, 3).unwrap(); + fusion.register_sensor(sensor).unwrap(); + + // Force degenerate/singular variance + fusion.p = -0.5; + let err = fusion.fuse(SensorMeasurement { + sensor_id: 1, + value: 1.0, + }); + assert_eq!(err, Err(FilterError::SingularInnovation)); + } +} diff --git a/tests/linear.rs b/tests/linear.rs new file mode 100644 index 0000000..392c56b --- /dev/null +++ b/tests/linear.rs @@ -0,0 +1,288 @@ +#[cfg(test)] +mod linear { + use aegis_filter::{ + common::{FilterError, Real}, + linear::*, + DEFAULT_OUTLIER_GATE_SIGMA, + }; + + // (1) predict regression: a constant-velocity model must still propagate x. + // F = [[1, 0.1], [0, 1]] -> position advances by 0.1*velocity per step; with + // velocity held at 100, x[0] grows by 10.0 each predict. Deterministic. + #[test] + fn predict_kinematic() { + let mut f = LinearKalmanFilter::<2, 1>::new( + [0.0, 100.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.1], [0.0, 1.0]], + [[0.001, 0.0], [0.0, 0.001]], + [[1.0, 0.0]], // H: one row, position only + [[0.5]], + ) + .unwrap(); + for _ in 0..3 { + f.predict().unwrap(); + } + assert!((f.x[0] - 30.0).abs() < 1e-9, "x[0] = {}", f.x[0]); + } + + #[cfg(test)] + mod update { + use super::*; + // (2) M=1 measurement update converges the state to a constant target, exactly + // like the 1D scalar filter. S=1, M=1 with F=I: x tracks the target as the + // posterior variance shrinks. + #[test] + fn update_m1_convergence() { + let mut f = LinearKalmanFilter::<1, 1>::new( + [0.0], + [[10.0]], + [[1.0]], + [[0.01]], + [[1.0]], + [[0.5]], + ) + .unwrap(); + let target = 25.0; + for _ in 0..50 { + f.predict().unwrap(); + let _ = f.update(&[target], None).unwrap(); + } + assert!((f.x[0] - target).abs() < 0.2, "x[0] = {}", f.x[0]); + } + + // (3) The H=I, P=I, R=I case has an exact closed-form first update: K = P H^T + // S^-1 = I . I . (2I)^-1 = 0.5 I, so x moves halfway to the measurement in one step. + #[test] + fn update_m2_single_step_halfway() { + let mut f = LinearKalmanFilter::<2, 2>::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], // F = I + [[0.0, 0.0], [0.0, 0.0]], // Q = 0 + [[1.0, 0.0], [0.0, 1.0]], // H = I + [[1.0, 0.0], [0.0, 1.0]], // R = I + ) + .unwrap(); + let z = [10.0, 20.0]; + let out = f.update(&z, None).unwrap(); + assert!((out[0] - 5.0).abs() < 1e-9, "out[0] = {}", out[0]); + assert!((out[1] - 10.0).abs() < 1e-9, "out[1] = {}", out[1]); + } + + // (4) Repeated M=2 updates converge to the true measurement vector. + #[test] + fn update_m2_convergence() { + let mut f = LinearKalmanFilter::<2, 2>::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + [[0.01, 0.0], [0.0, 0.01]], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + ) + .unwrap(); + let z = [10.0, 20.0]; + for _ in 0..50 { + f.predict().unwrap(); + f.update(&z, None).unwrap(); + } + assert!((f.x[0] - 10.0).abs() < 0.1, "x[0] = {}", f.x[0]); + assert!((f.x[1] - 20.0).abs() < 0.1, "x[1] = {}", f.x[1]); + } + + // (5) Joseph form must keep P symmetric and positive on the diagonals. For the + // H=I, P=I, R=I case the posterior covariance is the analytic 0.50 * I: + // P = (I - KH) P (I - KH)^T + K R K^T = 0.25 I + 0.25 I = 0.50 I. + #[test] + fn update_joseph_symmetry_and_psd() { + let mut f = LinearKalmanFilter::<2, 2>::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + [[0.0, 0.0], [0.0, 0.0]], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + ) + .unwrap(); + f.update(&[5.0, 5.0], None).unwrap(); + // Symmetry: off-diagonals match on both triangles. + assert!((f.p[0][1] - f.p[1][0]).abs() < 1e-9, "off-diag mismatch"); + assert!((f.p[0][1]).abs() < 1e-9, "off-diag = {}", f.p[0][1]); + // Diagonals: analytic 0.50, and strictly positive (PSD lower bound). + assert!((f.p[0][0] - 0.50).abs() < 1e-9, "p00 = {}", f.p[0][0]); + assert!((f.p[1][1] - 0.50).abs() < 1e-9, "p11 = {}", f.p[1][1]); + } + + // (6) Gating: a far-off M=1 measurement is rejected (3-sigma gate) and the state + // is left untouched, mirroring the scalar filter's MeasurementGated behaviour. + #[test] + fn update_m1_gating_rejects_outlier() { + let mut f = LinearKalmanFilter::<1, 1>::new( + [0.0], + [[1.0]], + [[1.0]], + [[0.0]], + [[1.0]], + [[0.01]], + ) + .unwrap(); + let err = f.update(&[1000.0], Some(3.0)); + assert_eq!(err, Err(FilterError::MeasurementGated)); + assert!((f.x[0] - 0.0).abs() < 1e-12, "state moved on gated update"); + } + + // (7) A rank-deficient observation (both rows identical) with zero process and + // measurement noise makes the innovation covariance singular -> NumericalInstability. + #[test] + fn update_singular_innovation() { + let mut f = LinearKalmanFilter::<2, 2>::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + [[0.0, 0.0], [0.0, 0.0]], + [[1.0, 0.0], [1.0, 0.0]], // rank-deficient H + [[0.0, 0.0], [0.0, 0.0]], // R = 0, so S = H P H^T is singular + ) + .unwrap(); + assert_eq!( + f.update(&[1.0, 1.0], None).err(), + Some(FilterError::NumericalInstability) + ); + } + + // (8) Non-finite measurements are rejected at the boundary, like the 1D path. + #[test] + fn update_nonfinite_measurement() { + let mut f = LinearKalmanFilter::<1, 1>::new( + [0.0], + [[1.0]], + [[1.0]], + [[0.01]], + [[1.0]], + [[0.5]], + ) + .unwrap(); + assert_eq!( + f.update(&[Real::NAN], None).err(), + Some(FilterError::NonFiniteInput) + ); + assert_eq!( + f.update(&[Real::INFINITY], None).err(), + Some(FilterError::NonFiniteInput) + ); + } + + // (9) Non-diagonal H with correlated (non-diagonal) measurement error must use the true + // K = P H^T S^-1, not a column-sum of S^-1. With P=I, H=[[1,1],[0,1]], R=I the + // innovation covariance is S = H H^T + I = [[3,1],[1,2]], inverse [[2/5,-1/5],[-1/5,3/5]], + // so K = H^T S^-1 = [[0.4,-0.2],[0.2,0.4]]; with x=0, x_new = K * z. + // z=[4,2] is chosen so the off-diagonal K[0][1] (correct -0.2, old bug 0) is exposed: + // the old index bug gave x_new[0] = 0.8 instead of 1.2. + #[test] + fn update_k_non_diagonal_correlated() { + let mut f = LinearKalmanFilter::<2, 2>::new( + [0.0, 0.0], + [[1.0, 0.0], [0.0, 1.0]], + [[1.0, 0.0], [0.0, 1.0]], + [[0.0, 0.0], [0.0, 0.0]], + [[1.0, 1.0], [0.0, 1.0]], // non-diagonal H + [[1.0, 0.0], [0.0, 1.0]], + ) + .unwrap(); + let out = f.update(&[4.0, 2.0], None).unwrap(); + #[cfg(feature = "single-precision")] + let tol: Real = 1e-4; + #[cfg(not(feature = "single-precision"))] + let tol: Real = 1e-9; + assert!((out[0] - 1.2).abs() < tol, "out[0] = {}", out[0]); + assert!((out[1] - 1.6).abs() < tol, "out[1] = {}", out[1]); + } + // (9) Factored O(S^3) Joseph update test for S=4, M=2: verifies that the factored + // (E*P)*E^T + (K*S)*K^T produces identical positive semi-definite, symmetric covariance. + #[test] + #[allow(clippy::needless_range_loop)] + fn update_higher_dim_factored_joseph() { + let x0 = [10.0, 0.5, 20.0, -0.2]; + let p0 = [ + [2.0, 0.1, 0.0, 0.0], + [0.1, 1.5, 0.0, 0.0], + [0.0, 0.0, 2.0, 0.1], + [0.0, 0.0, 0.1, 1.5], + ]; + let f_mat = [ + [1.0, 0.1, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.1], + [0.0, 0.0, 0.0, 1.0], + ]; + let q_mat = [ + [0.01, 0.0, 0.0, 0.0], + [0.0, 0.01, 0.0, 0.0], + [0.0, 0.0, 0.01, 0.0], + [0.0, 0.0, 0.01, 0.0], + ]; + // Measure position x and position y + let h_mat = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]]; + let r_mat = [[0.25, 0.02], [0.02, 0.25]]; + + let mut kf = + LinearKalmanFilter::<4, 2>::new(x0, p0, f_mat, q_mat, h_mat, r_mat).unwrap(); + kf.predict().unwrap(); + let z = [10.15, 19.95]; + let updated_state = kf.update(&z, Some(DEFAULT_OUTLIER_GATE_SIGMA)).unwrap(); + + // State moves toward measurement + assert!((updated_state[0] - 10.15).abs() < 1.0); + assert!((updated_state[2] - 19.95).abs() < 1.0); + + // Verify covariance symmetry P[i][j] == P[j][i] + for i in 0..4 { + for j in 0..4 { + assert!( + (kf.p[i][j] - kf.p[j][i]).abs() < 1e-12, + "Covariance asymmetry at ({}, {}): {} vs {}", + i, + j, + kf.p[i][j], + kf.p[j][i] + ); + } + // Strict positive variance on diagonal + assert!(kf.p[i][i] > 0.0, "Diagonal variance non-positive at {}", i); + assert!( + kf.p[i][i] < p0[i][i] + 0.1, + "Variance did not contract on measurement update" + ); + } + } + } + + // (10) Fail-closed security validation: when initialized with new_resilient or with_default_gate, + // calls to update(..., None) automatically enforce 3-sigma innovation gating. + #[test] + fn fail_closed_gating_behavior() { + let mut f_scalar = ScalarKalmanFilter::new_resilient(0.0, 1.0, 0.01, 0.5).unwrap(); + // 1000-sigma outlier passed with None: MUST be rejected by default 3-sigma gate! + let err = f_scalar.update(1000.0, None); + assert_eq!(err, Err(FilterError::MeasurementGated)); + assert!((f_scalar.state() - 0.0).abs() < 1e-12); + + // Explicit opt-out via Some(-1.0) allows through + let accepted = f_scalar.update(1.0, Some(-1.0)).unwrap(); + assert!(accepted > 0.0); + + // Same for LinearKalmanFilter + let mut f_linear = LinearKalmanFilter::<1, 1>::new_resilient( + [0.0], + [[1.0]], + [[1.0]], + [[0.01]], + [[1.0]], + [[0.5]], + ) + .unwrap(); + let err_linear = f_linear.update(&[1000.0], None); + assert_eq!(err_linear, Err(FilterError::MeasurementGated)); + } +} diff --git a/tests/other.rs b/tests/other.rs new file mode 100644 index 0000000..5ceb689 --- /dev/null +++ b/tests/other.rs @@ -0,0 +1,32 @@ +#[cfg(test)] +mod tests { + use aegis_filter::WindowedStats; + #[test] + fn windowed_stats_rolling_metrics() { + let mut stats = WindowedStats::<4>::new(); + assert!(stats.is_empty()); + assert_eq!(stats.len(), 0); + + stats.push(2.0).unwrap(); + stats.push(4.0).unwrap(); + assert_eq!(stats.len(), 2); + assert!((stats.mean().unwrap() - 3.0).abs() < 1e-9); + assert_eq!(stats.min(), Some(2.0)); + assert_eq!(stats.max(), Some(4.0)); + assert!((stats.peak_to_peak().unwrap() - 2.0).abs() < 1e-9); + + // Push more samples to exceed window capacity of 4 + stats.push(6.0).unwrap(); + stats.push(8.0).unwrap(); + assert_eq!(stats.len(), 4); + assert!((stats.mean().unwrap() - 5.0).abs() < 1e-9); + + // Overwrite oldest (2.0) with 10.0 -> window has [10, 4, 6, 8], mean = 7.0 + stats.push(10.0).unwrap(); + assert_eq!(stats.len(), 4); + assert!((stats.mean().unwrap() - 7.0).abs() < 1e-9); + assert_eq!(stats.min(), Some(4.0)); + assert_eq!(stats.max(), Some(10.0)); + assert!(stats.variance().unwrap() > 0.0); + } +} diff --git a/tests/scalar.rs b/tests/scalar.rs new file mode 100644 index 0000000..92bdfe3 --- /dev/null +++ b/tests/scalar.rs @@ -0,0 +1,39 @@ +#[cfg(test)] +mod scalar { + use aegis_filter::{FilterError, Real, ScalarKalmanFilter}; + #[cfg(test)] + mod filter { + use super::*; + #[test] + fn filter_convergence() { + let mut filter = ScalarKalmanFilter::new(0.0, 10.0, 0.01, 0.5).unwrap(); + let target = 25.0; + for _ in 0..50 { + filter.predict(0.0).unwrap(); + let _ = filter.update(target + 0.05, None).unwrap(); + } + assert!((filter.state() - target).abs() < 0.2); + } + + #[test] + fn filter_outlier_gating() { + let mut filter = ScalarKalmanFilter::new(10.0, 1.0, 0.01, 0.5).unwrap(); + // Adversarial spike — must be rejected by 3-sigma gating + let err = filter.update(9999.0, Some(3.0)); + assert_eq!(err, Err(FilterError::MeasurementGated)); + assert!((filter.state() - 10.0).abs() < 1e-9); + } + } + + #[test] + fn nan_inf_rejection() { + let result = ScalarKalmanFilter::new(Real::NAN, 1.0, 0.1, 1.0); + assert_eq!(result.err(), Some(FilterError::NonFiniteInput)); + + let mut f = ScalarKalmanFilter::new(0.0, 1.0, 0.1, 1.0).unwrap(); + assert_eq!( + f.update(Real::INFINITY, None).err(), + Some(FilterError::NonFiniteInput) + ); + } +} diff --git a/tests/sensor.rs b/tests/sensor.rs new file mode 100644 index 0000000..1c309cd --- /dev/null +++ b/tests/sensor.rs @@ -0,0 +1,113 @@ +#[cfg(test)] +mod sensor { + use aegis_filter::{FusionStatus, MultiSensorFusion1D, SensorMeasurement, SensorSource}; + #[test] + fn multi_fusion_and_quarantine() { + // Setup fusion engine with capacity for 3 sensors + let mut fusion = MultiSensorFusion1D::<3>::new(0.0, 5.0, 0.05).unwrap(); + + // Sensor 1: Accurate Barometer (R=0.2, max 3 anomalies) + let baro = SensorSource::new(1, 0.2, 3).unwrap(); + // Sensor 2: Noisy GPS (R=1.5, max 2 anomalies) + let gps = SensorSource::new(2, 1.5, 2).unwrap(); + + fusion.register_sensor(baro).unwrap(); + fusion.register_sensor(gps).unwrap(); + + // Nominal fusion + fusion.predict(1.0).unwrap(); + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 1.05, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Fused); + + // Adversarial spoofing on GPS (id=2) + // Attack 1: Gated + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 2, + value: 500.0, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Gated); + + // Attack 2: Second consecutive anomaly triggers Quarantine! + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 2, + value: 500.0, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Quarantined); + assert_eq!(fusion.sensor_health(2), Some(0.0)); + + // Subsequent measurements from quarantined GPS are safely ignored + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 2, + value: 1.0, + }) + .unwrap(); + assert_eq!(status, FusionStatus::IgnoredQuarantined); + + // Barometer continues nominal operation unhindered + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 1.02, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Fused); + } + + #[test] + fn auto_recovery() { + let mut fusion = MultiSensorFusion1D::<2>::new(10.0, 1.0, 0.01).unwrap(); + // Sensor with auto-recovery threshold of 2 consecutive healthy observations + let gps = SensorSource::new(1, 0.5, 2).unwrap().with_auto_recovery(2); + fusion.register_sensor(gps).unwrap(); + + // 2 anomalies force quarantine + let _ = fusion.fuse(SensorMeasurement { + sensor_id: 1, + value: 100.0, + }); + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 100.0, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Quarantined); + + // 1st healthy sample while quarantined + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 10.05, + }) + .unwrap(); + assert_eq!(status, FusionStatus::IgnoredQuarantined); + + // 2nd healthy sample triggers auto-recovery! + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 10.02, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Recovered); + + // Subsequent sample is actively fused again + let status = fusion + .fuse(SensorMeasurement { + sensor_id: 1, + value: 10.01, + }) + .unwrap(); + assert_eq!(status, FusionStatus::Fused); + } +}