Struct ExtendedKalmanFilter 

pub struct ExtendedKalmanFilter<const N: usize, const M: usize, Model> {
    pub x: [f32; N],
    pub p: [[f32; N]; N],
    pub q: [[f32; N]; N],
    pub r: [[f32; M]; M],
    pub model: Model,
}

Extended Kalman filter with compile-time dimensions and a user EkfModel.

Measurement dimension M must be ≤ 16. Covariance update uses P ← (I − KH) P.

Fields

x: [f32; N]

State estimate

p: [[f32; N]; N]

State covariance P (N×N)

q: [[f32; N]; N]

Process noise covariance Q (N×N)

r: [[f32; M]; M]

Measurement noise covariance R (M×M)

model: Model

Nonlinear process / measurement model

Implementations

impl<const N: usize, const M: usize, Model: EkfModel<N, M>> ExtendedKalmanFilter<N, M, Model>

pub fn new( x0: [f32; N], p0: [[f32; N]; N], q: [[f32; N]; N], r: [[f32; M]; M], model: Model, ) -> Self

Create an EKF with initial state, covariances, and model.

pub fn from_variances( x0: [f32; N], p_var: f32, q_var: f32, r_var: f32, model: Model, ) -> Self

Create an EKF with diagonal covariances from scalar variances.

Examples found in repository

examples/sensor_fusion_navigation.rs (lines 234-240)

fn main() {
    println!("===============================================================================");
    println!("        embedded-dsp Sensor Fusion, Navigation & Attitude Estimation           ");
    println!("===============================================================================");
    println!();

    // -----------------------------------------------------------------------------------------
    // 1. IMU Sensor Outlier Rejection via 1D Conditional Median Filtering
    // -----------------------------------------------------------------------------------------
    println!("--- 1. Sensor Conditioning & Glitch Removal (Conditional Median Filter) ---");
    // Accelerometer readings experiencing occasional mechanical shock / communication glitches
    let raw_accel_z = [
        9.81f32, 9.80, 9.82, 105.4, 9.81, 9.79, 9.83, 9.81, -45.0, 9.82, 9.80, 9.81,
    ];
    let mut cleaned_accel_z = [0.0f32; 12];
    // Replaces spike only if it deviates by more than threshold (10.0 m/s^2) from local median
    median_filter_1d_f32(&raw_accel_z, &mut cleaned_accel_z, 3, 10.0);

    println!("  Raw Accel Z (with spikes)    : {:?}", raw_accel_z);
    println!("  Cleaned Accel Z (spikes fixed): {:?}", cleaned_accel_z);

    // -----------------------------------------------------------------------------------------
    // 2. Sensor Factory Calibration via Weighted Polynomial Least Squares
    // -----------------------------------------------------------------------------------------
    println!("\n--- 2. Sensor Factory Calibration (Polynomial Least-Squares Fit) ---");
    // Calibration fixture measurements: ADC readings vs Known physical quantities (e.g. pressure/temp)
    let adc_counts = [100.0f32, 200.0, 300.0, 400.0, 500.0];
    // True relationship: Output = 12.5 + 0.45 * ADC
    let ref_values = [57.5f32, 102.5, 147.5, 192.5, 237.5];
    let mut calib_params = [0.0f32; 2]; // [offset c0, gain c1]

    let status = polynomial_least_squares_fit(&adc_counts, &ref_values, None, 1, &mut calib_params);
    if status == Status::Success {
        println!(
            "  Fitted Calibration Model: y = {:.4} + {:.4} * x",
            calib_params[0], calib_params[1]
        );
    } else {
        println!("  Least squares fitting error: {:?}", status);
    }

    // -----------------------------------------------------------------------------------------
    // 3. 3D Orientation & Attitude Tracking with Quaternions
    // -----------------------------------------------------------------------------------------
    println!("\n--- 3. 3D Attitude Estimation with Unit Quaternions ---");
    // Initial attitude: Identity (no rotation)
    let q_current = [1.0f32, 0.0, 0.0, 0.0]; // [w, x, y, z]

    // Incremental rotation: Pitch 90 degrees around Y-axis (cos(45°), 0, sin(45°), 0)
    let angle_y = core::f32::consts::FRAC_PI_2;
    let q_pitch_90 = [(angle_y / 2.0).cos(), 0.0, (angle_y / 2.0).sin(), 0.0];

    let mut q_rotated = [0.0f32; 4];
    quaternion_product_f32(&q_pitch_90, &q_current, &mut q_rotated);
    quaternion_normalize_f32(&mut q_rotated);
    println!(
        "  Quaternion after 90° Pitch: [{:.4}, {:.4}, {:.4}, {:.4}]",
        q_rotated[0], q_rotated[1], q_rotated[2], q_rotated[3]
    );

    // Convert attitude to 3x3 Rotation Matrix
    let mut rot_matrix = [0.0f32; 9];
    quaternion_to_rotmat_f32(&q_rotated, &mut rot_matrix);
    println!("  Converted 3x3 Direction Cosine Matrix (DCM):");
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[0], rot_matrix[1], rot_matrix[2]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[3], rot_matrix[4], rot_matrix[5]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[6], rot_matrix[7], rot_matrix[8]
    );

    // Rotate body-frame vector (e.g. forward velocity [1.0, 0.0, 0.0]) to navigation-frame
    // v_rot = q * v * q^*
    let mut q_conj = [0.0f32; 4];
    quaternion_conjugate_f32(&q_rotated, &mut q_conj);
    let v_body = [0.0f32, 1.0, 0.0, 0.0]; // pure imaginary quaternion [0, vx, vy, vz]
    let mut q_temp = [0.0f32; 4];
    let mut v_nav_q = [0.0f32; 4];
    quaternion_product_f32(&q_rotated, &v_body, &mut q_temp);
    quaternion_product_f32(&q_temp, &q_conj, &mut v_nav_q);
    println!(
        "  Body Vector [1, 0, 0] rotated to Navigation Frame: [{:.4}, {:.4}, {:.4}]",
        v_nav_q[1], v_nav_q[2], v_nav_q[3]
    );

    // -----------------------------------------------------------------------------------------
    // 4. Linear 2D Kinematic Kalman Filter (GPS Position + Velocity Fusion)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 4. 2D Kinematic Kalman Filter (Position + Velocity Fusion) ---");
    // True trajectory: moving at constant speed 2.0 m/s from pos = 0
    const DT: f32 = 0.1; // 100 ms time step
    const STEPS: usize = 30;

    let mut kf2d = KalmanFilter2D::new(0.0, 0.0, 0.1, 4.0); // q_var = 0.1, r_var = 4.0 (noisy GPS)
    let mut estimated_pos = [0.0f32; STEPS];
    let mut true_pos = [0.0f32; STEPS];

    for k in 0..STEPS {
        let t = k as f32 * DT;
        let true_p = 2.0 * t;
        true_pos[k] = true_p;

        // Noisy GPS reading: True pos + random noise
        let prng =
            ((k as u64).wrapping_mul(1664525).wrapping_add(1013904223) % 1000) as f32 / 1000.0;
        let noisy_gps = true_p + (prng - 0.5) * 3.0;

        kf2d.predict(DT);
        let est = kf2d.update(noisy_gps);
        estimated_pos[k] = est[0];

        if k % 10 == 0 || k == STEPS - 1 {
            println!(
                "  Step {:>2} (t={:.1}s): True Pos={:>5.2}m, Noisy GPS={:>5.2}m, KF Pos={:>5.2}m, KF Vel={:>5.2}m/s",
                k, t, true_p, noisy_gps, est[0], est[1]
            );
        }
    }

    // -----------------------------------------------------------------------------------------
    // 5. Const-Generic Linear Kalman Filter (4-State, 2-Measurement)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 5. Const-Generic Linear Kalman Filter (4x2 Tracking) ---");
    // State: [px, vx, py, vy]
    let f_matrix = [
        [1.0, DT, 0.0, 0.0],
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, 1.0, DT],
        [0.0, 0.0, 0.0, 1.0],
    ];
    // Measurement matrix: GPS measures [px, py]
    let h_matrix = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]];
    let mut p_cov = [[0.0f32; 4]; 4];
    let mut q_cov = [[0.0f32; 4]; 4];
    for i in 0..4 {
        p_cov[i][i] = 1.0;
        q_cov[i][i] = 0.05;
    }
    let r_cov = [[1.5, 0.0], [0.0, 1.5]];

    let mut kf_4x2 = KalmanFilter::<4, 2>::new(
        [0.0, 1.5, 0.0, -1.0], // Initial state
        p_cov,
        q_cov,
        r_cov,
    );

    kf_4x2.predict(&f_matrix);
    let meas_z = [0.18, -0.09];
    let kf_status = kf_4x2.update(&h_matrix, &meas_z);
    println!(
        "  Const-generic KalmanFilter<4, 2> update status: {:?}",
        kf_status
    );
    println!(
        "  Updated State Vector: px={:.3}m, vx={:.3}m/s, py={:.3}m, vy={:.3}m/s",
        kf_4x2.x[0], kf_4x2.x[1], kf_4x2.x[2], kf_4x2.x[3]
    );

    // -----------------------------------------------------------------------------------------
    // 6. Non-Linear Extended Kalman Filter (Radar Range + Bearing Tracking)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 6. Non-Linear Extended Kalman Filter (Radar Tracking) ---");
    let ekf_model = RadarTrackingModel;
    let mut ekf = ExtendedKalmanFilter::<4, 2, RadarTrackingModel>::from_variances(
        [100.0, 10.0, 50.0, 5.0], // Target starts at (100m, 50m), speed (10m/s, 5m/s)
        10.0,                     // Initial P variance
        0.2,                      // Process Q variance
        1.0,                      // Measurement R variance
        ekf_model,
    );

    println!("  Target True Trajectory vs EKF Non-Linear Estimate:");
    for step in 1..=5 {
        let dt = 0.5;
        // True target state
        let true_px = 100.0 + step as f32 * dt * 10.0;
        let true_py = 50.0 + step as f32 * dt * 5.0;

        // Polar radar measurements with sensor noise
        let true_range = (true_px * true_px + true_py * true_py).sqrt();
        let true_bearing = true_py.atan2(true_px);

        ekf.predict(dt);
        let z = [true_range + 0.5, true_bearing - 0.005]; // Noisy radar ping
        let status = ekf.update(&z);

        println!(
            "    Step {}: Meas [Range={:>6.1}m, Azimuth={:>6.3} rad] -> EKF Est [X={:>6.1}m, Y={:>6.1}m] (Status: {:?})",
            step, z[0], z[1], ekf.x[0], ekf.x[2], status
        );
    }

    // -----------------------------------------------------------------------------------------
    // 7. Statistical Performance Evaluation
    // -----------------------------------------------------------------------------------------
    println!("\n--- 7. Tracking Error Statistical Metrics ---");
    let mut tracking_errors = [0.0f32; STEPS];
    for i in 0..STEPS {
        tracking_errors[i] = estimated_pos[i] - true_pos[i];
    }

    let mut mean_err = 0.0f32;
    let mut std_err = 0.0f32;
    let mut rms_err = 0.0f32;
    let mut max_err = 0.0f32;
    let mut max_idx = 0usize;

    mean_f32(&tracking_errors, &mut mean_err);
    std_f32(&tracking_errors, &mut std_err);
    rms_f32(&tracking_errors, &mut rms_err);
    max_f32(&tracking_errors, &mut max_err, &mut max_idx);

    println!("  Position Tracking Error Metrics (over {} steps):", STEPS);
    println!("    • Mean Error      : {:>7.4} m", mean_err);
    println!("    • Std Deviation   : {:>7.4} m", std_err);
    println!("    • RMS Error       : {:>7.4} m", rms_err);
    println!(
        "    • Max Error       : {:>7.4} m (at step {})",
        max_err, max_idx
    );

    println!();
    println!("===============================================================================");
    println!("             Sensor Fusion & Navigation Execution Complete!                    ");
    println!("===============================================================================");
}

pub fn predict(&mut self, dt: f32)

EKF predict: x ← f(x, dt), P ← F P Fᵀ + Q with F = ∂f/∂x.

Examples found in repository

examples/sensor_fusion_navigation.rs (line 253)

fn main() {
    println!("===============================================================================");
    println!("        embedded-dsp Sensor Fusion, Navigation & Attitude Estimation           ");
    println!("===============================================================================");
    println!();

    // -----------------------------------------------------------------------------------------
    // 1. IMU Sensor Outlier Rejection via 1D Conditional Median Filtering
    // -----------------------------------------------------------------------------------------
    println!("--- 1. Sensor Conditioning & Glitch Removal (Conditional Median Filter) ---");
    // Accelerometer readings experiencing occasional mechanical shock / communication glitches
    let raw_accel_z = [
        9.81f32, 9.80, 9.82, 105.4, 9.81, 9.79, 9.83, 9.81, -45.0, 9.82, 9.80, 9.81,
    ];
    let mut cleaned_accel_z = [0.0f32; 12];
    // Replaces spike only if it deviates by more than threshold (10.0 m/s^2) from local median
    median_filter_1d_f32(&raw_accel_z, &mut cleaned_accel_z, 3, 10.0);

    println!("  Raw Accel Z (with spikes)    : {:?}", raw_accel_z);
    println!("  Cleaned Accel Z (spikes fixed): {:?}", cleaned_accel_z);

    // -----------------------------------------------------------------------------------------
    // 2. Sensor Factory Calibration via Weighted Polynomial Least Squares
    // -----------------------------------------------------------------------------------------
    println!("\n--- 2. Sensor Factory Calibration (Polynomial Least-Squares Fit) ---");
    // Calibration fixture measurements: ADC readings vs Known physical quantities (e.g. pressure/temp)
    let adc_counts = [100.0f32, 200.0, 300.0, 400.0, 500.0];
    // True relationship: Output = 12.5 + 0.45 * ADC
    let ref_values = [57.5f32, 102.5, 147.5, 192.5, 237.5];
    let mut calib_params = [0.0f32; 2]; // [offset c0, gain c1]

    let status = polynomial_least_squares_fit(&adc_counts, &ref_values, None, 1, &mut calib_params);
    if status == Status::Success {
        println!(
            "  Fitted Calibration Model: y = {:.4} + {:.4} * x",
            calib_params[0], calib_params[1]
        );
    } else {
        println!("  Least squares fitting error: {:?}", status);
    }

    // -----------------------------------------------------------------------------------------
    // 3. 3D Orientation & Attitude Tracking with Quaternions
    // -----------------------------------------------------------------------------------------
    println!("\n--- 3. 3D Attitude Estimation with Unit Quaternions ---");
    // Initial attitude: Identity (no rotation)
    let q_current = [1.0f32, 0.0, 0.0, 0.0]; // [w, x, y, z]

    // Incremental rotation: Pitch 90 degrees around Y-axis (cos(45°), 0, sin(45°), 0)
    let angle_y = core::f32::consts::FRAC_PI_2;
    let q_pitch_90 = [(angle_y / 2.0).cos(), 0.0, (angle_y / 2.0).sin(), 0.0];

    let mut q_rotated = [0.0f32; 4];
    quaternion_product_f32(&q_pitch_90, &q_current, &mut q_rotated);
    quaternion_normalize_f32(&mut q_rotated);
    println!(
        "  Quaternion after 90° Pitch: [{:.4}, {:.4}, {:.4}, {:.4}]",
        q_rotated[0], q_rotated[1], q_rotated[2], q_rotated[3]
    );

    // Convert attitude to 3x3 Rotation Matrix
    let mut rot_matrix = [0.0f32; 9];
    quaternion_to_rotmat_f32(&q_rotated, &mut rot_matrix);
    println!("  Converted 3x3 Direction Cosine Matrix (DCM):");
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[0], rot_matrix[1], rot_matrix[2]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[3], rot_matrix[4], rot_matrix[5]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[6], rot_matrix[7], rot_matrix[8]
    );

    // Rotate body-frame vector (e.g. forward velocity [1.0, 0.0, 0.0]) to navigation-frame
    // v_rot = q * v * q^*
    let mut q_conj = [0.0f32; 4];
    quaternion_conjugate_f32(&q_rotated, &mut q_conj);
    let v_body = [0.0f32, 1.0, 0.0, 0.0]; // pure imaginary quaternion [0, vx, vy, vz]
    let mut q_temp = [0.0f32; 4];
    let mut v_nav_q = [0.0f32; 4];
    quaternion_product_f32(&q_rotated, &v_body, &mut q_temp);
    quaternion_product_f32(&q_temp, &q_conj, &mut v_nav_q);
    println!(
        "  Body Vector [1, 0, 0] rotated to Navigation Frame: [{:.4}, {:.4}, {:.4}]",
        v_nav_q[1], v_nav_q[2], v_nav_q[3]
    );

    // -----------------------------------------------------------------------------------------
    // 4. Linear 2D Kinematic Kalman Filter (GPS Position + Velocity Fusion)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 4. 2D Kinematic Kalman Filter (Position + Velocity Fusion) ---");
    // True trajectory: moving at constant speed 2.0 m/s from pos = 0
    const DT: f32 = 0.1; // 100 ms time step
    const STEPS: usize = 30;

    let mut kf2d = KalmanFilter2D::new(0.0, 0.0, 0.1, 4.0); // q_var = 0.1, r_var = 4.0 (noisy GPS)
    let mut estimated_pos = [0.0f32; STEPS];
    let mut true_pos = [0.0f32; STEPS];

    for k in 0..STEPS {
        let t = k as f32 * DT;
        let true_p = 2.0 * t;
        true_pos[k] = true_p;

        // Noisy GPS reading: True pos + random noise
        let prng =
            ((k as u64).wrapping_mul(1664525).wrapping_add(1013904223) % 1000) as f32 / 1000.0;
        let noisy_gps = true_p + (prng - 0.5) * 3.0;

        kf2d.predict(DT);
        let est = kf2d.update(noisy_gps);
        estimated_pos[k] = est[0];

        if k % 10 == 0 || k == STEPS - 1 {
            println!(
                "  Step {:>2} (t={:.1}s): True Pos={:>5.2}m, Noisy GPS={:>5.2}m, KF Pos={:>5.2}m, KF Vel={:>5.2}m/s",
                k, t, true_p, noisy_gps, est[0], est[1]
            );
        }
    }

    // -----------------------------------------------------------------------------------------
    // 5. Const-Generic Linear Kalman Filter (4-State, 2-Measurement)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 5. Const-Generic Linear Kalman Filter (4x2 Tracking) ---");
    // State: [px, vx, py, vy]
    let f_matrix = [
        [1.0, DT, 0.0, 0.0],
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, 1.0, DT],
        [0.0, 0.0, 0.0, 1.0],
    ];
    // Measurement matrix: GPS measures [px, py]
    let h_matrix = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]];
    let mut p_cov = [[0.0f32; 4]; 4];
    let mut q_cov = [[0.0f32; 4]; 4];
    for i in 0..4 {
        p_cov[i][i] = 1.0;
        q_cov[i][i] = 0.05;
    }
    let r_cov = [[1.5, 0.0], [0.0, 1.5]];

    let mut kf_4x2 = KalmanFilter::<4, 2>::new(
        [0.0, 1.5, 0.0, -1.0], // Initial state
        p_cov,
        q_cov,
        r_cov,
    );

    kf_4x2.predict(&f_matrix);
    let meas_z = [0.18, -0.09];
    let kf_status = kf_4x2.update(&h_matrix, &meas_z);
    println!(
        "  Const-generic KalmanFilter<4, 2> update status: {:?}",
        kf_status
    );
    println!(
        "  Updated State Vector: px={:.3}m, vx={:.3}m/s, py={:.3}m, vy={:.3}m/s",
        kf_4x2.x[0], kf_4x2.x[1], kf_4x2.x[2], kf_4x2.x[3]
    );

    // -----------------------------------------------------------------------------------------
    // 6. Non-Linear Extended Kalman Filter (Radar Range + Bearing Tracking)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 6. Non-Linear Extended Kalman Filter (Radar Tracking) ---");
    let ekf_model = RadarTrackingModel;
    let mut ekf = ExtendedKalmanFilter::<4, 2, RadarTrackingModel>::from_variances(
        [100.0, 10.0, 50.0, 5.0], // Target starts at (100m, 50m), speed (10m/s, 5m/s)
        10.0,                     // Initial P variance
        0.2,                      // Process Q variance
        1.0,                      // Measurement R variance
        ekf_model,
    );

    println!("  Target True Trajectory vs EKF Non-Linear Estimate:");
    for step in 1..=5 {
        let dt = 0.5;
        // True target state
        let true_px = 100.0 + step as f32 * dt * 10.0;
        let true_py = 50.0 + step as f32 * dt * 5.0;

        // Polar radar measurements with sensor noise
        let true_range = (true_px * true_px + true_py * true_py).sqrt();
        let true_bearing = true_py.atan2(true_px);

        ekf.predict(dt);
        let z = [true_range + 0.5, true_bearing - 0.005]; // Noisy radar ping
        let status = ekf.update(&z);

        println!(
            "    Step {}: Meas [Range={:>6.1}m, Azimuth={:>6.3} rad] -> EKF Est [X={:>6.1}m, Y={:>6.1}m] (Status: {:?})",
            step, z[0], z[1], ekf.x[0], ekf.x[2], status
        );
    }

    // -----------------------------------------------------------------------------------------
    // 7. Statistical Performance Evaluation
    // -----------------------------------------------------------------------------------------
    println!("\n--- 7. Tracking Error Statistical Metrics ---");
    let mut tracking_errors = [0.0f32; STEPS];
    for i in 0..STEPS {
        tracking_errors[i] = estimated_pos[i] - true_pos[i];
    }

    let mut mean_err = 0.0f32;
    let mut std_err = 0.0f32;
    let mut rms_err = 0.0f32;
    let mut max_err = 0.0f32;
    let mut max_idx = 0usize;

    mean_f32(&tracking_errors, &mut mean_err);
    std_f32(&tracking_errors, &mut std_err);
    rms_f32(&tracking_errors, &mut rms_err);
    max_f32(&tracking_errors, &mut max_err, &mut max_idx);

    println!("  Position Tracking Error Metrics (over {} steps):", STEPS);
    println!("    • Mean Error      : {:>7.4} m", mean_err);
    println!("    • Std Deviation   : {:>7.4} m", std_err);
    println!("    • RMS Error       : {:>7.4} m", rms_err);
    println!(
        "    • Max Error       : {:>7.4} m (at step {})",
        max_err, max_idx
    );

    println!();
    println!("===============================================================================");
    println!("             Sensor Fusion & Navigation Execution Complete!                    ");
    println!("===============================================================================");
}

pub fn predict_with_input<const U: usize>(&mut self, dt: f32, u: &[f32; U])

EKF predict with an exogenous input u, via EkfModel::f_with_input / EkfModel::jacobian_f_with_input. See the module docs for when this is needed instead of ExtendedKalmanFilter::predict.

pub fn update(&mut self, z: &[f32; M]) -> Status

EKF update with measurement z. Linearizes h at the current estimate.

On singular innovation covariance or M > 16, returns an error and leaves state unchanged.

Examples found in repository

examples/sensor_fusion_navigation.rs (line 255)

fn main() {
    println!("===============================================================================");
    println!("        embedded-dsp Sensor Fusion, Navigation & Attitude Estimation           ");
    println!("===============================================================================");
    println!();

    // -----------------------------------------------------------------------------------------
    // 1. IMU Sensor Outlier Rejection via 1D Conditional Median Filtering
    // -----------------------------------------------------------------------------------------
    println!("--- 1. Sensor Conditioning & Glitch Removal (Conditional Median Filter) ---");
    // Accelerometer readings experiencing occasional mechanical shock / communication glitches
    let raw_accel_z = [
        9.81f32, 9.80, 9.82, 105.4, 9.81, 9.79, 9.83, 9.81, -45.0, 9.82, 9.80, 9.81,
    ];
    let mut cleaned_accel_z = [0.0f32; 12];
    // Replaces spike only if it deviates by more than threshold (10.0 m/s^2) from local median
    median_filter_1d_f32(&raw_accel_z, &mut cleaned_accel_z, 3, 10.0);

    println!("  Raw Accel Z (with spikes)    : {:?}", raw_accel_z);
    println!("  Cleaned Accel Z (spikes fixed): {:?}", cleaned_accel_z);

    // -----------------------------------------------------------------------------------------
    // 2. Sensor Factory Calibration via Weighted Polynomial Least Squares
    // -----------------------------------------------------------------------------------------
    println!("\n--- 2. Sensor Factory Calibration (Polynomial Least-Squares Fit) ---");
    // Calibration fixture measurements: ADC readings vs Known physical quantities (e.g. pressure/temp)
    let adc_counts = [100.0f32, 200.0, 300.0, 400.0, 500.0];
    // True relationship: Output = 12.5 + 0.45 * ADC
    let ref_values = [57.5f32, 102.5, 147.5, 192.5, 237.5];
    let mut calib_params = [0.0f32; 2]; // [offset c0, gain c1]

    let status = polynomial_least_squares_fit(&adc_counts, &ref_values, None, 1, &mut calib_params);
    if status == Status::Success {
        println!(
            "  Fitted Calibration Model: y = {:.4} + {:.4} * x",
            calib_params[0], calib_params[1]
        );
    } else {
        println!("  Least squares fitting error: {:?}", status);
    }

    // -----------------------------------------------------------------------------------------
    // 3. 3D Orientation & Attitude Tracking with Quaternions
    // -----------------------------------------------------------------------------------------
    println!("\n--- 3. 3D Attitude Estimation with Unit Quaternions ---");
    // Initial attitude: Identity (no rotation)
    let q_current = [1.0f32, 0.0, 0.0, 0.0]; // [w, x, y, z]

    // Incremental rotation: Pitch 90 degrees around Y-axis (cos(45°), 0, sin(45°), 0)
    let angle_y = core::f32::consts::FRAC_PI_2;
    let q_pitch_90 = [(angle_y / 2.0).cos(), 0.0, (angle_y / 2.0).sin(), 0.0];

    let mut q_rotated = [0.0f32; 4];
    quaternion_product_f32(&q_pitch_90, &q_current, &mut q_rotated);
    quaternion_normalize_f32(&mut q_rotated);
    println!(
        "  Quaternion after 90° Pitch: [{:.4}, {:.4}, {:.4}, {:.4}]",
        q_rotated[0], q_rotated[1], q_rotated[2], q_rotated[3]
    );

    // Convert attitude to 3x3 Rotation Matrix
    let mut rot_matrix = [0.0f32; 9];
    quaternion_to_rotmat_f32(&q_rotated, &mut rot_matrix);
    println!("  Converted 3x3 Direction Cosine Matrix (DCM):");
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[0], rot_matrix[1], rot_matrix[2]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[3], rot_matrix[4], rot_matrix[5]
    );
    println!(
        "    [{:>7.4}, {:>7.4}, {:>7.4}]",
        rot_matrix[6], rot_matrix[7], rot_matrix[8]
    );

    // Rotate body-frame vector (e.g. forward velocity [1.0, 0.0, 0.0]) to navigation-frame
    // v_rot = q * v * q^*
    let mut q_conj = [0.0f32; 4];
    quaternion_conjugate_f32(&q_rotated, &mut q_conj);
    let v_body = [0.0f32, 1.0, 0.0, 0.0]; // pure imaginary quaternion [0, vx, vy, vz]
    let mut q_temp = [0.0f32; 4];
    let mut v_nav_q = [0.0f32; 4];
    quaternion_product_f32(&q_rotated, &v_body, &mut q_temp);
    quaternion_product_f32(&q_temp, &q_conj, &mut v_nav_q);
    println!(
        "  Body Vector [1, 0, 0] rotated to Navigation Frame: [{:.4}, {:.4}, {:.4}]",
        v_nav_q[1], v_nav_q[2], v_nav_q[3]
    );

    // -----------------------------------------------------------------------------------------
    // 4. Linear 2D Kinematic Kalman Filter (GPS Position + Velocity Fusion)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 4. 2D Kinematic Kalman Filter (Position + Velocity Fusion) ---");
    // True trajectory: moving at constant speed 2.0 m/s from pos = 0
    const DT: f32 = 0.1; // 100 ms time step
    const STEPS: usize = 30;

    let mut kf2d = KalmanFilter2D::new(0.0, 0.0, 0.1, 4.0); // q_var = 0.1, r_var = 4.0 (noisy GPS)
    let mut estimated_pos = [0.0f32; STEPS];
    let mut true_pos = [0.0f32; STEPS];

    for k in 0..STEPS {
        let t = k as f32 * DT;
        let true_p = 2.0 * t;
        true_pos[k] = true_p;

        // Noisy GPS reading: True pos + random noise
        let prng =
            ((k as u64).wrapping_mul(1664525).wrapping_add(1013904223) % 1000) as f32 / 1000.0;
        let noisy_gps = true_p + (prng - 0.5) * 3.0;

        kf2d.predict(DT);
        let est = kf2d.update(noisy_gps);
        estimated_pos[k] = est[0];

        if k % 10 == 0 || k == STEPS - 1 {
            println!(
                "  Step {:>2} (t={:.1}s): True Pos={:>5.2}m, Noisy GPS={:>5.2}m, KF Pos={:>5.2}m, KF Vel={:>5.2}m/s",
                k, t, true_p, noisy_gps, est[0], est[1]
            );
        }
    }

    // -----------------------------------------------------------------------------------------
    // 5. Const-Generic Linear Kalman Filter (4-State, 2-Measurement)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 5. Const-Generic Linear Kalman Filter (4x2 Tracking) ---");
    // State: [px, vx, py, vy]
    let f_matrix = [
        [1.0, DT, 0.0, 0.0],
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, 1.0, DT],
        [0.0, 0.0, 0.0, 1.0],
    ];
    // Measurement matrix: GPS measures [px, py]
    let h_matrix = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]];
    let mut p_cov = [[0.0f32; 4]; 4];
    let mut q_cov = [[0.0f32; 4]; 4];
    for i in 0..4 {
        p_cov[i][i] = 1.0;
        q_cov[i][i] = 0.05;
    }
    let r_cov = [[1.5, 0.0], [0.0, 1.5]];

    let mut kf_4x2 = KalmanFilter::<4, 2>::new(
        [0.0, 1.5, 0.0, -1.0], // Initial state
        p_cov,
        q_cov,
        r_cov,
    );

    kf_4x2.predict(&f_matrix);
    let meas_z = [0.18, -0.09];
    let kf_status = kf_4x2.update(&h_matrix, &meas_z);
    println!(
        "  Const-generic KalmanFilter<4, 2> update status: {:?}",
        kf_status
    );
    println!(
        "  Updated State Vector: px={:.3}m, vx={:.3}m/s, py={:.3}m, vy={:.3}m/s",
        kf_4x2.x[0], kf_4x2.x[1], kf_4x2.x[2], kf_4x2.x[3]
    );

    // -----------------------------------------------------------------------------------------
    // 6. Non-Linear Extended Kalman Filter (Radar Range + Bearing Tracking)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 6. Non-Linear Extended Kalman Filter (Radar Tracking) ---");
    let ekf_model = RadarTrackingModel;
    let mut ekf = ExtendedKalmanFilter::<4, 2, RadarTrackingModel>::from_variances(
        [100.0, 10.0, 50.0, 5.0], // Target starts at (100m, 50m), speed (10m/s, 5m/s)
        10.0,                     // Initial P variance
        0.2,                      // Process Q variance
        1.0,                      // Measurement R variance
        ekf_model,
    );

    println!("  Target True Trajectory vs EKF Non-Linear Estimate:");
    for step in 1..=5 {
        let dt = 0.5;
        // True target state
        let true_px = 100.0 + step as f32 * dt * 10.0;
        let true_py = 50.0 + step as f32 * dt * 5.0;

        // Polar radar measurements with sensor noise
        let true_range = (true_px * true_px + true_py * true_py).sqrt();
        let true_bearing = true_py.atan2(true_px);

        ekf.predict(dt);
        let z = [true_range + 0.5, true_bearing - 0.005]; // Noisy radar ping
        let status = ekf.update(&z);

        println!(
            "    Step {}: Meas [Range={:>6.1}m, Azimuth={:>6.3} rad] -> EKF Est [X={:>6.1}m, Y={:>6.1}m] (Status: {:?})",
            step, z[0], z[1], ekf.x[0], ekf.x[2], status
        );
    }

    // -----------------------------------------------------------------------------------------
    // 7. Statistical Performance Evaluation
    // -----------------------------------------------------------------------------------------
    println!("\n--- 7. Tracking Error Statistical Metrics ---");
    let mut tracking_errors = [0.0f32; STEPS];
    for i in 0..STEPS {
        tracking_errors[i] = estimated_pos[i] - true_pos[i];
    }

    let mut mean_err = 0.0f32;
    let mut std_err = 0.0f32;
    let mut rms_err = 0.0f32;
    let mut max_err = 0.0f32;
    let mut max_idx = 0usize;

    mean_f32(&tracking_errors, &mut mean_err);
    std_f32(&tracking_errors, &mut std_err);
    rms_f32(&tracking_errors, &mut rms_err);
    max_f32(&tracking_errors, &mut max_err, &mut max_idx);

    println!("  Position Tracking Error Metrics (over {} steps):", STEPS);
    println!("    • Mean Error      : {:>7.4} m", mean_err);
    println!("    • Std Deviation   : {:>7.4} m", std_err);
    println!("    • RMS Error       : {:>7.4} m", rms_err);
    println!(
        "    • Max Error       : {:>7.4} m (at step {})",
        max_err, max_idx
    );

    println!();
    println!("===============================================================================");
    println!("             Sensor Fusion & Navigation Execution Complete!                    ");
    println!("===============================================================================");
}

pub fn update_with_input<const U: usize>( &mut self, z: &[f32; M], u: &[f32; U], ) -> Status

EKF update with an exogenous input u, via EkfModel::h_with_input / EkfModel::jacobian_h_with_input. See the module docs for when this is needed instead of ExtendedKalmanFilter::update.

On singular innovation covariance or M > 16, returns an error and leaves state unchanged.

Trait Implementations

impl<const N: usize, const M: usize, Model: Clone> Clone for ExtendedKalmanFilter<N, M, Model>

fn clone(&self) -> ExtendedKalmanFilter<N, M, Model>

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl<const N: usize, const M: usize, Model: Copy> Copy for ExtendedKalmanFilter<N, M, Model>

impl<const N: usize, const M: usize, Model: Debug> Debug for ExtendedKalmanFilter<N, M, Model>

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl<const N: usize, const M: usize, Model: PartialEq> PartialEq for ExtendedKalmanFilter<N, M, Model>

fn eq(&self, other: &ExtendedKalmanFilter<N, M, Model>) -> bool

Equality operator ==.

fn ne(&self, other: &Rhs) -> bool

Inequality operator !=.

impl<const N: usize, const M: usize, Model: PartialEq> StructuralPartialEq for ExtendedKalmanFilter<N, M, Model>