aegis_filter/
lib.rs

1//! aegis-filter: High-Integrity, Cyber-Resilient State Estimation & Robust Filtering Engine in pure Rust.
2//!
3//! # Architecture
4//! - [`linear`]: Classical and Joseph-stabilized linear Kalman filters with Mahalanobis gating.
5//! - [`unscented`]: Unscented Kalman Filter (UKF) for non-linear systems without analytical Jacobians.
6//! - [`robust`]: $H_\infty$ (H-infinity) Minimax filters for bounded worst-case disturbances and online adaptive noise estimation.
7//! - [`common`]: Core error definitions and `#![no_std]` deterministic mathematical traits.
8
9#![cfg_attr(not(feature = "std"), no_std)]
10
11pub mod common;
12pub mod fusion;
13pub mod linear;
14pub mod robust;
15pub mod stats;
16pub mod unscented;
17
18pub use common::FilterError;
19pub use fusion::{FusionStatus, MultiSensorFusion1D, SensorMeasurement, SensorSource};
20pub use linear::{LinearKalmanFilter, ScalarKalmanFilter};
21pub use robust::{AdaptiveFilter1D, HInfinityFilter1D};
22pub use stats::WindowedStats;
23pub use unscented::UnscentedKalmanFilter2D;
24
25#[cfg(test)]
26mod tests {
27    use super::*;
28
29    #[test]
30    fn test_scalar_filter_convergence() {
31        let mut filter = ScalarKalmanFilter::new(0.0, 10.0, 0.01, 0.5).unwrap();
32        let target = 25.0;
33        for _ in 0..50 {
34            filter.predict(0.0).unwrap();
35            let _ = filter.update(target + 0.05, None).unwrap();
36        }
37        assert!((filter.state() - target).abs() < 0.2);
38    }
39
40    #[test]
41    fn test_scalar_filter_outlier_gating() {
42        let mut filter = ScalarKalmanFilter::new(10.0, 1.0, 0.01, 0.5).unwrap();
43        // Adversarial spike — must be rejected by 3-sigma gating
44        let err = filter.update(9999.0, Some(3.0));
45        assert_eq!(err, Err(FilterError::MeasurementGated));
46        assert!((filter.state() - 10.0).abs() < 1e-9);
47    }
48
49    #[test]
50    fn test_ukf_linear_observation_multi_step() {
51        // UKF with h(x) = x[0] (linear) — deterministically tracks a constant signal.
52        // Multiple updates allow covariance to shrink and state to lock onto the target.
53        let mut ukf = UnscentedKalmanFilter2D::new(
54            [0.0, 0.0],
55            [[2.0, 0.0], [0.0, 2.0]],
56            [[0.001, 0.0], [0.0, 0.001]],
57            0.5,
58        )
59        .unwrap();
60
61        let target = 5.0;
62        for _ in 0..30 {
63            ukf.predict(0.1, |x, _dt| [x[0], x[1]]).unwrap();
64            let _ = ukf.update(target, |x| x[0], None).unwrap();
65        }
66
67        // After 30 updates the position estimate must be close to target
68        assert!((ukf.x[0] - target).abs() < 0.3, "ukf.x[0] = {}", ukf.x[0]);
69    }
70
71    #[test]
72    fn test_ukf_nonlinear_tight_covariance() {
73        // h(x) = x[0]^2 — non-linear. With tight initial covariance sigma
74        // points stay close to mean, so linearisation error is small and a
75        // single update step visibly moves the state toward the true value.
76        let true_pos: f64 = 3.0;
77        let mut ukf = UnscentedKalmanFilter2D::new(
78            [true_pos + 0.5, 0.0],
79            [[0.04, 0.0], [0.0, 0.04]], // tight — sigma points ±0.2 from mean
80            [[0.001, 0.0], [0.0, 0.001]],
81            0.1,
82        )
83        .unwrap();
84
85        ukf.predict(0.0, |x, _dt| [x[0], x[1]]).unwrap();
86
87        let z = true_pos * true_pos; // 9.0
88        let before = ukf.x[0];
89        let res = ukf.update(z, |x| x[0] * x[0], None).unwrap();
90
91        // Update must move x[0] toward true_pos (3.0) from initial 3.5
92        assert!(
93            res[0] < before,
94            "Expected x[0] to decrease toward 3.0, before={before}, after={}",
95            res[0]
96        );
97        assert!(res[0] > 2.5, "Expected x[0] > 2.5, got {}", res[0]);
98    }
99
100    #[test]
101    fn test_h_infinity_robustness() {
102        let mut h_inf = HInfinityFilter1D::new(10.0, 1.0, 0.05, 0.5, 5.0).unwrap();
103
104        for _ in 0..20 {
105            h_inf.predict(0.0).unwrap();
106            let _ = h_inf.update(10.1).unwrap();
107        }
108
109        assert!((h_inf.state() - 10.1).abs() < 0.3);
110        assert!(h_inf.bound() > 0.0);
111    }
112
113    #[test]
114    fn test_adaptive_filter_noise_tracking() {
115        let mut af = AdaptiveFilter1D::new(0.0, 5.0, 0.01, 1.0, 0.05).unwrap();
116        for _ in 0..40 {
117            af.predict(0.0);
118            let _ = af.update(20.0).unwrap();
119        }
120        assert!((af.x - 20.0).abs() < 0.5, "af.x = {}", af.x);
121    }
122
123    #[test]
124    fn test_nan_inf_rejection() {
125        let result = ScalarKalmanFilter::new(f64::NAN, 1.0, 0.1, 1.0);
126        assert_eq!(result.err(), Some(FilterError::NonFiniteInput));
127
128        let mut f = ScalarKalmanFilter::new(0.0, 1.0, 0.1, 1.0).unwrap();
129        assert_eq!(
130            f.update(f64::INFINITY, None).err(),
131            Some(FilterError::NonFiniteInput)
132        );
133    }
134
135    #[test]
136    fn test_multi_sensor_fusion_and_quarantine() {
137        // Setup fusion engine with capacity for 3 sensors
138        let mut fusion = MultiSensorFusion1D::<3>::new(0.0, 5.0, 0.05).unwrap();
139
140        // Sensor 1: Accurate Barometer (R=0.2, max 3 anomalies)
141        let baro = SensorSource::new(1, 0.2, 3).unwrap();
142        // Sensor 2: Noisy GPS (R=1.5, max 2 anomalies)
143        let gps = SensorSource::new(2, 1.5, 2).unwrap();
144
145        fusion.register_sensor(baro).unwrap();
146        fusion.register_sensor(gps).unwrap();
147
148        // Nominal fusion
149        fusion.predict(1.0).unwrap();
150        let status = fusion
151            .fuse(SensorMeasurement {
152                sensor_id: 1,
153                value: 1.05,
154            })
155            .unwrap();
156        assert_eq!(status, FusionStatus::Fused);
157
158        // Adversarial spoofing on GPS (id=2)
159        // Attack 1: Gated
160        let status = fusion
161            .fuse(SensorMeasurement {
162                sensor_id: 2,
163                value: 500.0,
164            })
165            .unwrap();
166        assert_eq!(status, FusionStatus::Gated);
167
168        // Attack 2: Second consecutive anomaly triggers Quarantine!
169        let status = fusion
170            .fuse(SensorMeasurement {
171                sensor_id: 2,
172                value: 500.0,
173            })
174            .unwrap();
175        assert_eq!(status, FusionStatus::Quarantined);
176        assert_eq!(fusion.sensor_health(2), Some(0.0));
177
178        // Subsequent measurements from quarantined GPS are safely ignored
179        let status = fusion
180            .fuse(SensorMeasurement {
181                sensor_id: 2,
182                value: 1.0,
183            })
184            .unwrap();
185        assert_eq!(status, FusionStatus::IgnoredQuarantined);
186
187        // Barometer continues nominal operation unhindered
188        let status = fusion
189            .fuse(SensorMeasurement {
190                sensor_id: 1,
191                value: 1.02,
192            })
193            .unwrap();
194        assert_eq!(status, FusionStatus::Fused);
195    }
196
197    #[test]
198    fn test_sensor_auto_recovery() {
199        let mut fusion = MultiSensorFusion1D::<2>::new(10.0, 1.0, 0.01).unwrap();
200        // Sensor with auto-recovery threshold of 2 consecutive healthy observations
201        let gps = SensorSource::new(1, 0.5, 2).unwrap().with_auto_recovery(2);
202        fusion.register_sensor(gps).unwrap();
203
204        // 2 anomalies force quarantine
205        let _ = fusion.fuse(SensorMeasurement {
206            sensor_id: 1,
207            value: 100.0,
208        });
209        let status = fusion
210            .fuse(SensorMeasurement {
211                sensor_id: 1,
212                value: 100.0,
213            })
214            .unwrap();
215        assert_eq!(status, FusionStatus::Quarantined);
216
217        // 1st healthy sample while quarantined
218        let status = fusion
219            .fuse(SensorMeasurement {
220                sensor_id: 1,
221                value: 10.05,
222            })
223            .unwrap();
224        assert_eq!(status, FusionStatus::IgnoredQuarantined);
225
226        // 2nd healthy sample triggers auto-recovery!
227        let status = fusion
228            .fuse(SensorMeasurement {
229                sensor_id: 1,
230                value: 10.02,
231            })
232            .unwrap();
233        assert_eq!(status, FusionStatus::Recovered);
234
235        // Subsequent sample is actively fused again
236        let status = fusion
237            .fuse(SensorMeasurement {
238                sensor_id: 1,
239                value: 10.01,
240            })
241            .unwrap();
242        assert_eq!(status, FusionStatus::Fused);
243    }
244
245    #[test]
246    fn test_windowed_stats_rolling_metrics() {
247        let mut stats = WindowedStats::<4>::new();
248        assert!(stats.is_empty());
249        assert_eq!(stats.len(), 0);
250
251        stats.push(2.0).unwrap();
252        stats.push(4.0).unwrap();
253        assert_eq!(stats.len(), 2);
254        assert!((stats.mean().unwrap() - 3.0).abs() < 1e-9);
255        assert_eq!(stats.min(), Some(2.0));
256        assert_eq!(stats.max(), Some(4.0));
257        assert!((stats.peak_to_peak().unwrap() - 2.0).abs() < 1e-9);
258
259        // Push more samples to exceed window capacity of 4
260        stats.push(6.0).unwrap();
261        stats.push(8.0).unwrap();
262        assert_eq!(stats.len(), 4);
263        assert!((stats.mean().unwrap() - 5.0).abs() < 1e-9);
264
265        // Overwrite oldest (2.0) with 10.0 -> window has [10, 4, 6, 8], mean = 7.0
266        stats.push(10.0).unwrap();
267        assert_eq!(stats.len(), 4);
268        assert!((stats.mean().unwrap() - 7.0).abs() < 1e-9);
269        assert_eq!(stats.min(), Some(4.0));
270        assert_eq!(stats.max(), Some(10.0));
271        assert!(stats.variance().unwrap() > 0.0);
272    }
273}
aegis_filter/lib.rs

aegis_filter/
lib.rs
