yakf - Yet Another Kalman Filter

Yet Another Kalman Filter Implementation, with [no_std] supported

Current implementation status

Filter Status

• UKF ✅

Sampling Method Status

• Minimal Skew Simplex Sampling (n+2) ✅

Usage

Add this to your Cargo.toml: [dependencies] yakf = "0.1"

Example: ``` /// import yakf crate extern crate yakf; /// import State trait, UKF filter struct, and MSSS sampling method struct use yakf::kf::{sigma_points::MinimalSkewSimplexSampling as MSSS, state::State, ukf::UKF};

/// import Re-exports of hifitime (for time) and nalgebra (for matrix) use yakf::{ linalg, time::{Duration, Epoch, Unit}, };

fn main() { use crate::linalg::{Const, OMatrix, OVector, U2}; use rand::prelude::*;

#[derive(Debug)]
/// define a custom struct to be the state. e.g., BikeState, has a 2-D vector x and a timestamped time t.
pub struct BikeState {
    pub x: OVector<f64, U2>,
    pub t: Epoch,
}

/// for example, you can define your own methods.
impl BikeState {
    pub fn new(state: OVector<f64, U2>, epoch: Epoch) -> Self {
        BikeState { x: state, t: epoch }
    }
    pub fn zeros() -> Self {
        Self {
            x: OVector::<f64, U2>::zeros(),
            t: Epoch::from_gregorian_tai(2022, 5, 10, 0, 0, 0, 0),
        }
    }
}

/// you **MUST** implement State<T,U> for your custom state struct.
///
impl State<U2, Const<1>> for BikeState {
    fn state(&self) -> OVector<f64, U2> {
        self.x.clone()
    }
    fn set_state(&mut self, state: OVector<f64, U2>) {
        self.x = state;
    }

    fn epoch(&self) -> Epoch {
        self.t
    }
    fn set_epoch(&mut self, epoch: Epoch) {
        self.t = epoch;
    }
}
// you SHOULD provide a function `dynamics` for propagating the state.
//
// for example,
let dynamics = |x: &OVector<f64, U2>, _ext: &OVector<f64, Const<1>>, dt: Duration| {
    OVector::<f64, U2>::new(x[0] + x[1] * dt.in_seconds(), x[1])
};

// you SHOULD ALSO provide a function for yielding measurements based on given state.
//
// for example, assume the measuring has a 2-D measurement.
let measure_model =
    |x: &OVector<f64, U2>| OVector::<f64, U2>::new(5.0 * x[0] * x[0] * x[1], x[1] * x[0] * 2.0);

// Finally, build the UKF
let mut ukf = UKF::<U2, Const<4>, U2, Const<1>, BikeState>::build(
    Box::new(dynamics),
    Box::new(measure_model),
    Box::new(MSSS::build(0.6)),
    BikeState::zeros(),
    OMatrix::<f64, U2, U2>::from_diagonal_element(1.0),
    OMatrix::<f64, U2, U2>::from_diagonal_element(0.01),
    OMatrix::<f64, U2, U2>::from_diagonal(&OVector::<f64, U2>::new(1.0, 0.01)),
);

// You can then use ukf to estimate the state vector.

let mut rng = rand::thread_rng();
let mut add_noisies = |mut y: OVector<f64, U2>| {
    y[0] += rng.gen_range(-1.0..1.0);
    y[1] += rng.gen_range(-0.1..0.1);
    y
};
let s = OVector::<f64, U2>::new(-5.0, 1.0);
let t = Epoch::now().unwrap();
let mut bike_actual = BikeState::new(s, t);

println!(
    "bike actual = {:?}, ukf estimate = {:?}",
    &bike_actual,
    &ukf.current_estimate()
);

let ukf_base_epoch = ukf.current_estimate().epoch();
for _i in 0..1000 {
    let dt = Duration::from_f64(1.0, Unit::Second);
    let m_epoch = ukf_base_epoch + dt;
    let _ = bike_actual.propagate(&dynamics, dt, OVector::<f64, Const<1>>::zeros());
    let mut meas = measure_model(&bike_actual.state());
    meas = add_noisies(meas);
    ukf.feed_and_update(meas, m_epoch, OVector::<f64, Const<1>>::zeros());

    println!(
        "bike actual = {:?}, meas = {:.3?}, ukf estimate = {:.3?}",
        &bike_actual.state(),
        meas,
        &ukf.current_estimate().state(),
    );
}
let error = &ukf.current_estimate().state() - &bike_actual.state();
assert!(error.norm() < 0.5);

} ```

You can see the output as .. .. .. bike actual = [[992.0, 1.0]], meas = [[4920320.466, 1983.914]], ukf estimate = [[992.208, 1.000]] bike actual = [[993.0, 1.0]], meas = [[4930244.722, 1986.052]], ukf estimate = [[993.127, 1.000]] bike actual = [[994.0, 1.0]], meas = [[4940180.252, 1987.912]], ukf estimate = [[994.227, 1.000]] bike actual = [[995.0, 1.0]], meas = [[4950125.125, 1989.992]], ukf estimate = [[995.159, 1.000]]