Struct filter::kalman::kalman_filter::KalmanFilter

pub struct KalmanFilter<F, DimX, DimZ, DimU> where
    F: RealField,
    DimX: DimName,
    DimZ: DimName,
    DimU: DimName,
    DefaultAllocator: Allocator<F, DimX> + Allocator<F, DimZ> + Allocator<F, DimX, DimZ> + Allocator<F, DimZ, DimX> + Allocator<F, DimZ, DimZ> + Allocator<F, DimX, DimX> + Allocator<F, DimU> + Allocator<F, DimX, DimU>,  {
    pub x: VectorN<F, DimX>,
    pub P: MatrixMN<F, DimX, DimX>,
    pub x_prior: VectorN<F, DimX>,
    pub P_prior: MatrixMN<F, DimX, DimX>,
    pub x_post: VectorN<F, DimX>,
    pub P_post: MatrixMN<F, DimX, DimX>,
    pub z: Option<VectorN<F, DimZ>>,
    pub R: MatrixMN<F, DimZ, DimZ>,
    pub Q: MatrixMN<F, DimX, DimX>,
    pub B: Option<MatrixMN<F, DimX, DimU>>,
    pub F: MatrixMN<F, DimX, DimX>,
    pub H: MatrixMN<F, DimZ, DimX>,
    pub y: VectorN<F, DimZ>,
    pub K: MatrixMN<F, DimX, DimZ>,
    pub S: MatrixMN<F, DimZ, DimZ>,
    pub SI: MatrixMN<F, DimZ, DimZ>,
    pub alpha_sq: F,
}

Implements a Kalman filter. For a detailed explanation, see the excellent book Kalman and Bayesian Filters in Python [1]_. The book applies also for this Rust implementation and all functions should works similar with minor changes due to language differences.

References

.. [1] Roger Labbe. "Kalman and Bayesian Filters in Python" https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

Fields

x: VectorN<F, DimX>

Current state estimate.

P: MatrixMN<F, DimX, DimX>

Current state covariance matrix.

x_prior: VectorN<F, DimX>

Prior (predicted) state estimate.

P_prior: MatrixMN<F, DimX, DimX>

Prior (predicted) state covariance matrix.

x_post: VectorN<F, DimX>

Posterior (updated) state estimate.

P_post: MatrixMN<F, DimX, DimX>

Posterior (updated) state covariance matrix.

z: Option<VectorN<F, DimZ>>

Last measurement

R: MatrixMN<F, DimZ, DimZ>

Measurement noise matrix.

Q: MatrixMN<F, DimX, DimX>

MatrixMN<F, DimZ, DimZ>,

B: Option<MatrixMN<F, DimX, DimU>>

Control transition matrix

F: MatrixMN<F, DimX, DimX>

State Transition matrix.

H: MatrixMN<F, DimZ, DimX>

Measurement function.

y: VectorN<F, DimZ>

Residual of the update step.

K: MatrixMN<F, DimX, DimZ>

Kalman gain of the update step.

S: MatrixMN<F, DimZ, DimZ>

System uncertainty (P projected to measurement space).

SI: MatrixMN<F, DimZ, DimZ>

Inverse system uncertainty.

alpha_sq: F

Fading memory setting.

Methods

impl<F, DimX, DimZ, DimU> KalmanFilter<F, DimX, DimZ, DimU> where
    F: RealField,
    DimX: DimName,
    DimZ: DimName,
    DimU: DimName,
    DefaultAllocator: Allocator<F, DimX> + Allocator<F, DimZ> + Allocator<F, DimX, DimZ> + Allocator<F, DimZ, DimX> + Allocator<F, DimZ, DimZ> + Allocator<F, DimX, DimX> + Allocator<F, DimU> + Allocator<F, DimX, DimU>, 

pub fn predict(
    &mut self,
    u: Option<&VectorN<F, DimU>>,
    B: Option<&MatrixMN<F, DimX, DimU>>,
    F: Option<&MatrixMN<F, DimX, DimX>>,
    Q: Option<&MatrixMN<F, DimX, DimX>>
)

Predict next state (prior) using the Kalman filter state propagation equations.

pub fn update(
    &mut self,
    z: &VectorN<F, DimZ>,
    R: Option<&MatrixMN<F, DimZ, DimZ>>,
    H: Option<&MatrixMN<F, DimZ, DimX>>
)

Add a new measurement (z) to the Kalman filter.

pub fn predict_steadystate(
    &mut self,
    u: Option<&VectorN<F, DimU>>,
    B: Option<&MatrixMN<F, DimX, DimU>>
)

Predict state (prior) using the Kalman filter state propagation equations. Only x is updated, P is left unchanged.

pub fn update_steadystate(&mut self, z: &VectorN<F, DimZ>)

Add a new measurement (z) to the Kalman filter without recomputing the Kalman gain K, the state covariance P, or the system uncertainty S.

pub fn get_prediction(
    &self,
    u: Option<&VectorN<F, DimU>>
) -> (VectorN<F, DimX>, MatrixMN<F, DimX, DimX>)

Predicts the next state of the filter and returns it without altering the state of the filter.

pub fn get_update(
    &self,
    z: &VectorN<F, DimZ>
) -> (VectorN<F, DimX>, MatrixMN<F, DimX, DimX>)

Computes the new estimate based on measurement z and returns it without altering the state of the filter.

pub fn residual_of(&self, z: &VectorN<F, DimZ>) -> VectorN<F, DimZ>

Returns the residual for the given measurement (z). Does not alter the state of the filter.

pub fn measurement_of_state(&self, x: &VectorN<F, DimX>) -> VectorN<F, DimZ>

Helper function that converts a state into a measurement.

Trait Implementations

impl<F, DimX, DimZ, DimU> Default for KalmanFilter<F, DimX, DimZ, DimU> where
    F: RealField,
    DimX: DimName,
    DimZ: DimName,
    DimU: DimName,
    DefaultAllocator: Allocator<F, DimX> + Allocator<F, DimZ> + Allocator<F, DimX, DimZ> + Allocator<F, DimZ, DimX> + Allocator<F, DimZ, DimZ> + Allocator<F, DimX, DimX> + Allocator<F, DimU> + Allocator<F, DimX, DimU>, 

fn default() -> Self

Returns a Kalman filter initialised with default parameters.

impl<F: Debug, DimX: Debug, DimZ: Debug, DimU: Debug> Debug for KalmanFilter<F, DimX, DimZ, DimU> where
    F: RealField,
    DimX: DimName,
    DimZ: DimName,
    DimU: DimName,
    DefaultAllocator: Allocator<F, DimX> + Allocator<F, DimZ> + Allocator<F, DimX, DimZ> + Allocator<F, DimZ, DimX> + Allocator<F, DimZ, DimZ> + Allocator<F, DimX, DimX> + Allocator<F, DimU> + Allocator<F, DimX, DimU>, 

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

Formats the value using the given formatter.