Struct rust_ukf::UnscentedKalmanFilter

pub struct UnscentedKalmanFilter<const S: usize> {
    pub state: State<S>,
    pub covariance: Covariance<S>,
    /* private fields */
}

Unscented Kalman filter structure. Contains the state of the filter and its covariance as well as a set of weights calculated for the algorithm based on the initialization parameters.

Fields

state: State<S>

covariance: Covariance<S>

Implementations

impl<const S: usize> UnscentedKalmanFilter<S>

pub fn new(
    alpha: f64,
    beta: f64,
    kappa: f64,
    x0: State<S>,
    p0: Covariance<S>
) -> Self

Constructor for the filter. Takes three filter parameters and the initial filter state and its covariance.

The parameters are alpha, beta, and kappa. Alpha is usually a small positive constant which tunes the spread of the sigma points that the filter calculates. Beta helps incorporate knowledge about the underlying distribution of the filter. In most cases, this will be assumed to be Gaussian, and in this case beta=2.0 is optimal. Kappa is another parameter that tunes the sigma points and can usually be set to zero. Please see ‘the description of the algorithm by Mathworks’ for a more in-depth explanation of these parameters.

pub fn calc_sp_deltas(&self, p: &Covariance<S>) -> [State<S>; { _ }]

Calculate the sigmapoint deltas. Used for adjusting the sigmapoints at every iteration. Takes the current covariance of the filter state.

pub fn calc_sp(&self, x: &State<S>, p: &Covariance<S>) -> [State<S>; { _ }]

Calculate the sigma points using the filter state and its covariance.

pub fn update<const C: usize, D>(
    &mut self,
    control_k: &Control<C>,
    dt: f64,
    dynamics_model: D,
    dynamics_model_cov: &Covariance<S>
)where
    D: FnMut(&State<S>, &Control<C>, &f64) -> State<S>,

Run the filter state update. Takes the filter estimate from timestep k given all measurements up to and including k to timestep k+1 given all measurements up to and including k. Takes the current control of the system, the dynamics function, and the covariance of the assumed zero-mean process noise.

pub fn innovate<const C: usize, const Y: usize, M>(
    &mut self,
    control_k: &Control<C>,
    output_k: &Output<Y>,
    measurement_model: M,
    measurement_model_cov: &Covariance<Y>
)where
    M: FnMut(&State<S>, &Control<C>) -> Output<Y>,

Run a filter measurement update. Takes the filter estimate from timestep k given all measurements relevant to the provided measurement function up to and including k-1 to timestep k given all measurements relevant to the provided measurement function up to and including k. Takes the current control of the system, the relevant measurement values (output), the measurement function, and the covariance of the assumed zero-mean measurement noise.

pub fn progress<const C: usize, const Y: usize, D, M>(
    &mut self,
    dt: f64,
    control_k: Control<C>,
    output_k: Output<Y>,
    dynamics_model: D,
    dynamics_model_cov: &Covariance<S>,
    measurement_model: M,
    measurement_model_cov: &Covariance<Y>
)where
    D: FnMut(&State<S>, &Control<C>, &f64) -> State<S>,
    M: FnMut(&State<S>, &Control<C>) -> Output<Y>,

A convenience function. This runs both a measurement update and a state update in that order. The arguments are passed directly on to those functions.

This can be used when there is only one measurement function and therefore no need to run the steps separately.