# yakf - Yet Another Kalman Filter Yet Another Kalman Filter Implementation, as well as, Lie Theory (Lie group, algebra, vector) on SO(3), SE(3), SO(2), and SE(2). `[no_std]` is supported by default. # Current implementation status ## Filter Status * UKF ✅ * EKF (Only dynamically-sized version) ✅ ## Sampling Method Status * Minimal Skew Simplex Sampling (n+2) ✅ * Symmetrically-Distributed Sampling Method (2n+1) ✅ ## Static V.S Dynamic Cases * For ***statically***-sized state whose dimension is known in compile time, refer to `yakf::filters` * For ***dynamically***-sized state whose dimension may vary in run time, refer to `yakf::dfilters` ## Lie Group Status * SO(3) ✅, refer to `yakf::lie::so3` * SE(3) ✅, refer to `yakf::lie::se3` * SO(2) ✅, refer to `yakf::lie::so2` * SE(2) ✅, refer to `yakf::lie::se2` ## Lie Group Examples * (non-generic) Kalman Filter Example on SO(3) ✅ see `examples/so3_kf.rs` for instance * (non-generic) Kalman Filter Example on SE(3) ✅ see `examples/se3_kf.rs` for instance ***NOTE that some functions havn't been thoroughly tested, so please let me know if there is any error.*** # UKF Usage Add this to your Cargo.toml: ``` [dependencies] yakf = "0.1" ``` Example (statically-sized): ``` /// import yakf crate extern crate yakf; /// import State trait, UKF filter struct, and MSSS sampling method struct use yakf::kf::{ MinimalSkewSimplexSampling as MSSS, State, SymmetricallyDistributedSampling as SDS, 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 (x[0]: position, x[1]: velocity) and a timestamped time t. pub struct BikeState { pub x: OVector, pub t: Epoch, } /// for example, you can define your own methods. impl BikeState { pub fn new(state: OVector, epoch: Epoch) -> Self { BikeState { x: state, t: epoch } } pub fn zeros() -> Self { Self { x: OVector::::zeros(), t: Epoch::from_gregorian_tai(2022, 5, 10, 0, 0, 0, 0), } } } /// you **MUST** implement State for your custom state struct. /// impl State> for BikeState { fn state(&self) -> &OVector { &self.x } fn set_state(&mut self, state: OVector) { 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 UKF propagating the state. // // for example, let dynamics = |x: &OVector, _ext: &OVector>, dt: Duration| { OVector::::new(x[0] + x[1] * dt.in_seconds(), x[1]) }; // you SHOULD ALSO provide a function for UKF yielding measurements based on given state. // // for example, assume the measuring has a 2-D measurement. let measure_model = |x: &OVector| OVector::::new(x[0], x[1]); // you SHOULD ALSO specify a sampling method for UKF. // for example, you can specify a MSSS method type T2 = Const<4>; let samling_method = MSSS::::build(0.6).unwrap(); // or you can specify a SDS method as an alternative. type _T2 = Const<5>; let _samling_method = SDS::::build(1e-3, None, None).unwrap(); // finally, build the UKF. let mut ukf = UKF::, BikeState>::build( Box::new(dynamics), Box::new(measure_model), Box::new(samling_method), BikeState::zeros(), OMatrix::::from_diagonal_element(10.0), OMatrix::::from_diagonal_element(1.0), OMatrix::::from_diagonal(&OVector::::new(1.0, 0.001)), ); // you can then use ukf to estimate the state vector. let mut rng = rand::thread_rng(); let mut add_noisies = |mut y: OVector| { y[0] += rng.gen_range(-3.0..3.0); y[1] += rng.gen_range(-0.1..0.1); y }; let s = OVector::::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 mut actual_normed_noise: Vec = Vec::new(); let mut estimate_normed_error: Vec = Vec::new(); let nums_measure = 500_usize; // you can set an arbitary time base for ukf. // a timing system would help in aligning data. let ukf_base_epoch = ukf.current_estimate().epoch(); for i in 0..nums_measure { let dt = Duration::from_f64(1.0, Unit::Second); let m_epoch = ukf_base_epoch + dt; /* Remark 1. Note that the actual dynamics doesn't need to be exactly the same with that used by ukf. Actually, the dynamics used by ukf is only a Model abstracted from the actual one. But in this example, assume they are the same. Case is the same for measuring model. Remark 2. For the same reason, the delta_t used by actual dynamics is not neccesarily the same with dt (the one used by ukf estimation) and, actually, delta_t should be much smaller than dt in real world. However, for simplity, this example just let them be the same, i.e. delta_t = dt. */ let _ = bike_actual.propagate(&dynamics, dt, OVector::>::zeros()); // use measuring model to simulate a measurement, and add some noises on it. let mut meas = measure_model(&bike_actual.state()); meas = add_noisies(meas); // every time the measurement is ready, ukf is trigger to update. ukf.feed_and_update(meas, m_epoch, OVector::>::zeros()); if i > nums_measure / 3 { actual_normed_noise.push((&meas - bike_actual.state()).norm()); estimate_normed_error .push((ukf.current_estimate().state() - bike_actual.state()).norm()); } println!( "bike actual = {:?}, meas = {:.3?}, ukf estimate = {:.3?}", &bike_actual.state(), meas, &ukf.current_estimate().state(), ); } let nums = actual_normed_noise.len(); let noise_metric: f64 = actual_normed_noise .into_iter() .fold(0.0, |acc, x| acc + x / nums as f64); let error_metric: f64 = estimate_normed_error .into_iter() .fold(0.0, |acc, x| acc + x / nums as f64); println!( "noise_metric = {:?}, error_metric = {:?}", noise_metric, error_metric ); assert!(error_metric < noise_metric); } ``` You may see the output as ``` .. .. .. actual = [493.0, 1.0], meas = [493.281, 1.073], estimate = [492.553, 1.073] actual = [494.0, 1.0], meas = [492.615, 0.941], estimate = [492.598, 0.941] actual = [495.0, 1.0], meas = [496.849, 1.019], estimate = [495.710, 1.019] noise_metric = 1.5346849337852513, error_metric = 1.2218914483371828 ``` # SO(3)/SE(3) Usage The element in SO(3) is stored as an Enum type, named by `SO3`. One can create such an element by methods `from_vec`, `from_grp`, `from_alg`. Either will be Ok, since an element can be expressed in all the three forms, and there exists a one-on-one mapping relationship among them. For example, one can create a SO3 element `so` from a 3-d vector: ``` let so = SO3::from_vec(Vec3::new(0.3, 0.6, -0.9)); ``` If you want use its vector form, just call ``` let v = so.to_vec(); ``` if you want use its group form, just call ``` let g = so.to_group(); ``` and if you want use its algebra form, just call ``` let a = so.to_alg(); ``` As you know, all `v`, `g`, `a` stand for the same `SO3` element `so` (limited in a proper range). Currently, some neccessary methods have been implemented for `SO3`. The design of `SE(3)` is consensus with `SO(3)`.