Struct HInfinityFilter1D

Source

pub struct HInfinityFilter1D { /* private fields */ }

Expand description

1D Robust H-infinity (H_inf) Filter.

Unlike the Kalman Filter, which minimizes the mean-square error under the assumption of known Gaussian noise, the H-infinity filter solves a minimax game: it minimizes the worst-case estimation error over all possible bounded disturbances.

Implementations§

Source §

impl HInfinityFilter1D

Source

pub fn new( initial_state: f64, initial_variance: f64, process_bound_q: f64, measurement_bound_r: f64, gamma: f64, ) -> Result<Self, FilterError>

Creates a new H-infinity filter. gamma must be strictly greater than 0 (typically >= 1.0).

Examples found in repository ?

examples/benchmark.rs (line 32)

6fn main() {
7    println!("===============================================================================");
8    println!("  AEGIS-FILTER :: PERFORMANCE & EXECUTION LATENCY BENCHMARK");
9    println!("  Developed under Exploidus (https://github.com/Exploidus)");
10    println!("===============================================================================\n");
11
12    const ITERATIONS: u64 = 1_000_000;
13
14    // 1. Benchmark: Scalar Kalman Filter (Joseph Form + Predict + Update)
15    {
16        let mut filter = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap();
17        let start = Instant::now();
18        for i in 0..ITERATIONS {
19            filter.predict(0.01).unwrap();
20            let _ = filter.update(10.0 + (i as f64 * 0.0001), None).unwrap();
21        }
22        let elapsed = start.elapsed();
23        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
24        println!(
25            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
26            "ScalarKalmanFilter (Predict+Update)", elapsed, per_op_ns
27        );
28    }
29
30    // 2. Benchmark: H-Infinity Robust Minimax Filter
31    {
32        let mut h_inf = HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 5.0).unwrap();
33        let start = Instant::now();
34        for i in 0..ITERATIONS {
35            h_inf.predict(0.01).unwrap();
36            let _ = h_inf.update(10.0 + (i as f64 * 0.0001)).unwrap();
37        }
38        let elapsed = start.elapsed();
39        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
40        println!(
41            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
42            "HInfinityFilter1D (Predict+Update)", elapsed, per_op_ns
43        );
44    }
45
46    // 3. Benchmark: 2D Unscented Kalman Filter (Sigma Points Propagation)
47    {
48        let mut ukf = UnscentedKalmanFilter2D::new(
49            [0.0, 0.0],
50            [[1.0, 0.0], [0.0, 1.0]],
51            [[0.01, 0.0], [0.0, 0.01]],
52            0.5,
53        )
54        .unwrap();
55        let start = Instant::now();
56        for _ in 0..ITERATIONS {
57            ukf.predict(0.01, |x, _dt| [x[0] + 0.01, x[1]]).unwrap();
58            let _ = ukf.update(1.0, |x| x[0], None).unwrap();
59        }
60        let elapsed = start.elapsed();
61        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
62        println!(
63            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
64            "UnscentedKalmanFilter2D (5 Sigma Pts)", elapsed, per_op_ns
65        );
66    }
67
68    println!("\n-------------------------------------------------------------------------------");
69    println!("  Zero heap allocations. Deterministic, hard real-time latency profile.");
70    println!("===============================================================================");
71}

Source

pub fn predict(&mut self, control_input: f64) -> Result<(), FilterError>

Time update: x = x + u P = P + Q

Examples found in repository ?

examples/benchmark.rs (line 35)

6fn main() {
7    println!("===============================================================================");
8    println!("  AEGIS-FILTER :: PERFORMANCE & EXECUTION LATENCY BENCHMARK");
9    println!("  Developed under Exploidus (https://github.com/Exploidus)");
10    println!("===============================================================================\n");
11
12    const ITERATIONS: u64 = 1_000_000;
13
14    // 1. Benchmark: Scalar Kalman Filter (Joseph Form + Predict + Update)
15    {
16        let mut filter = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap();
17        let start = Instant::now();
18        for i in 0..ITERATIONS {
19            filter.predict(0.01).unwrap();
20            let _ = filter.update(10.0 + (i as f64 * 0.0001), None).unwrap();
21        }
22        let elapsed = start.elapsed();
23        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
24        println!(
25            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
26            "ScalarKalmanFilter (Predict+Update)", elapsed, per_op_ns
27        );
28    }
29
30    // 2. Benchmark: H-Infinity Robust Minimax Filter
31    {
32        let mut h_inf = HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 5.0).unwrap();
33        let start = Instant::now();
34        for i in 0..ITERATIONS {
35            h_inf.predict(0.01).unwrap();
36            let _ = h_inf.update(10.0 + (i as f64 * 0.0001)).unwrap();
37        }
38        let elapsed = start.elapsed();
39        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
40        println!(
41            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
42            "HInfinityFilter1D (Predict+Update)", elapsed, per_op_ns
43        );
44    }
45
46    // 3. Benchmark: 2D Unscented Kalman Filter (Sigma Points Propagation)
47    {
48        let mut ukf = UnscentedKalmanFilter2D::new(
49            [0.0, 0.0],
50            [[1.0, 0.0], [0.0, 1.0]],
51            [[0.01, 0.0], [0.0, 0.01]],
52            0.5,
53        )
54        .unwrap();
55        let start = Instant::now();
56        for _ in 0..ITERATIONS {
57            ukf.predict(0.01, |x, _dt| [x[0] + 0.01, x[1]]).unwrap();
58            let _ = ukf.update(1.0, |x| x[0], None).unwrap();
59        }
60        let elapsed = start.elapsed();
61        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
62        println!(
63            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
64            "UnscentedKalmanFilter2D (5 Sigma Pts)", elapsed, per_op_ns
65        );
66    }
67
68    println!("\n-------------------------------------------------------------------------------");
69    println!("  Zero heap allocations. Deterministic, hard real-time latency profile.");
70    println!("===============================================================================");
71}

Source

pub fn update(&mut self, measurement: f64) -> Result<f64, FilterError>

Measurement update under the H-infinity minimax criteria:

Stability condition requires: 1 - gamma^(-2) * P + (H^2 * P) / R > 0

Examples found in repository ?

examples/benchmark.rs (line 36)

6fn main() {
7    println!("===============================================================================");
8    println!("  AEGIS-FILTER :: PERFORMANCE & EXECUTION LATENCY BENCHMARK");
9    println!("  Developed under Exploidus (https://github.com/Exploidus)");
10    println!("===============================================================================\n");
11
12    const ITERATIONS: u64 = 1_000_000;
13
14    // 1. Benchmark: Scalar Kalman Filter (Joseph Form + Predict + Update)
15    {
16        let mut filter = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap();
17        let start = Instant::now();
18        for i in 0..ITERATIONS {
19            filter.predict(0.01).unwrap();
20            let _ = filter.update(10.0 + (i as f64 * 0.0001), None).unwrap();
21        }
22        let elapsed = start.elapsed();
23        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
24        println!(
25            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
26            "ScalarKalmanFilter (Predict+Update)", elapsed, per_op_ns
27        );
28    }
29
30    // 2. Benchmark: H-Infinity Robust Minimax Filter
31    {
32        let mut h_inf = HInfinityFilter1D::new(0.0, 1.0, 0.01, 0.5, 5.0).unwrap();
33        let start = Instant::now();
34        for i in 0..ITERATIONS {
35            h_inf.predict(0.01).unwrap();
36            let _ = h_inf.update(10.0 + (i as f64 * 0.0001)).unwrap();
37        }
38        let elapsed = start.elapsed();
39        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
40        println!(
41            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
42            "HInfinityFilter1D (Predict+Update)", elapsed, per_op_ns
43        );
44    }
45
46    // 3. Benchmark: 2D Unscented Kalman Filter (Sigma Points Propagation)
47    {
48        let mut ukf = UnscentedKalmanFilter2D::new(
49            [0.0, 0.0],
50            [[1.0, 0.0], [0.0, 1.0]],
51            [[0.01, 0.0], [0.0, 0.01]],
52            0.5,
53        )
54        .unwrap();
55        let start = Instant::now();
56        for _ in 0..ITERATIONS {
57            ukf.predict(0.01, |x, _dt| [x[0] + 0.01, x[1]]).unwrap();
58            let _ = ukf.update(1.0, |x| x[0], None).unwrap();
59        }
60        let elapsed = start.elapsed();
61        let per_op_ns = elapsed.as_nanos() as f64 / (ITERATIONS * 2) as f64;
62        println!(
63            "{:<35} | Total: {:>8.2?} | {:>8.2} ns / operation",
64            "UnscentedKalmanFilter2D (5 Sigma Pts)", elapsed, per_op_ns
65        );
66    }
67
68    println!("\n-------------------------------------------------------------------------------");
69    println!("  Zero heap allocations. Deterministic, hard real-time latency profile.");
70    println!("===============================================================================");
71}