State estimation for tracking states that can not be directly perceived.

This library helps you track states when your measurements of the state are imperfect or noisy, as happens when you use one or more sensors to detect the state, rather than having perfect knowledge of it.

It provides general traits, and two implementations:

• Extended Kalman Filter (EKF): Best when you have a good mathematical model of how your system behaves. Fast and memory-efficient.

• Particle Filter: More flexible, works well when your system is highly nonlinear or you have complex noise. Uses more memory and CPU.

Quick Start

You only need to answer two questions:

1. How does your system change over time? (the process model)
2. What do your sensors measure? (the measurement model)

Example: Tracking a Drone

use estimators::ekf::{ProcessModel, MeasurementModel, ExtendedKalmanFilter};
use estimators::StateEstimator;

// State: [x, y, altitude]
// Control: [vx, vy, vz] velocity commands
// We measure: [x, y] from a camera

struct DroneMotion;

impl ProcessModel<3, 3> for DroneMotion {
    fn predict_next(&self, state: &[f64; 3], velocity: &[f64; 3], dt: f64) -> [f64; 3] {
        // Simple motion: new_position = old_position + velocity * time
        [
            state[0] + velocity[0] * dt,
            state[1] + velocity[1] * dt,
            state[2] + velocity[2] * dt,
        ]
    }

    fn process_noise_std(&self) -> [f64; 3] {
        [0.1, 0.1, 0.1]  // How much random drift we expect per second
    }
}

struct CameraSensor;

impl MeasurementModel<3, 2> for CameraSensor {
    fn expected_measurement(&self, state: &[f64; 3]) -> [f64; 2] {
        [state[0], state[1]]  // Camera sees x, y position
    }

    fn measurement_noise_std(&self) -> [f64; 2] {
        [0.5, 0.5]  // Camera accurate to about ±0.5 meters
    }
}

// Create the filter
let mut ekf = ExtendedKalmanFilter::new(
    [0.0, 0.0, 1.0],    // Initial position guess
    [1.0, 1.0, 0.5],    // Initial uncertainty
    DroneMotion,
    CameraSensor,
);

// Simulate one step
let velocity = [0.5, 0.0, 0.0];  // Moving in x direction
ekf.predict(&velocity, 0.1);     // 100ms time step

let camera_reading = [0.06, 0.01];
ekf.update(&camera_reading).unwrap();

let position = ekf.estimate();
println!("Estimated position: {:?}", position);