↓Skip to main content
  1. posts/

A Kalman Filter implementation in Rust

·16 mins
Control Software Rust
Table of Contents
The Kalman Filter is a ubiquitous algorithm for estimating the state of a system from noisy measurements and an uncertain model. This is my attempt to implement it in Rust and make a useful library out of it.

I won’t detail how the Kalman Filter works as there are many great explanations out there (Bzarg, rlabbe, kalmanfilter.net). I am aware that there are Kalman filter implementations in Rust already, but it’s always fun to make something new, and I think this is a little bit different.

The repo is available at https://github.com/dw-labs-org/kfilter, the crate at https://crates.io/crates/kfilter/ and the documentation at https://docs.rs/kfilter/latest/kfilter/.

Version 1: Basic Implementation #

The notation I am using matches that on wikipedia, repeated below for convenience. The state vector x and each of the matrices are represented using the statically sized SMatrix from the excellent nalgebra crate for linear algebra.

I have used upper case variable names (F, P, Q, etc) to match the notation against rust convention, please forgive me, it helped to ensure the maths is correct.

KF Equations from wikipedia.
KF Equations from wikipedia.

In the code below, the Kalman struct contains the state and matrices and is generic over the matrix element type T (f32 or f64), the number of states N and the number of measurements M.

use nalgebra::{RealField, SMatrix, SVector};

/// Representation of Kalman filter
#[derive(Debug)]
struct Kalman<T, const N: usize, const M: usize> {
    /// Estimated state
    pub x: SVector<T, N>,
    /// Discrete state transition
    F: SMatrix<T, N, N>,
    /// Covariance
    pub P: SMatrix<T, N, N>,
    /// Process Covariance
    Q: SMatrix<T, N, N>,
    /// Observation Matrix
    H: SMatrix<T, M, N>,
    /// Measurment covariance
    R: SMatrix<T, M, M>,
}

The Kalman filter equations are split into predict and update. Predict estimates the state and covariance based on the model and its covariance. Update does the same using a measurement and its covariance.

impl<T: RealField + Copy, const N: usize, const M: usize> Kalman<T, N, M> {
    pub fn predict(&mut self) {
        self.x = self.F * self.x;
        self.P = self.F * self.P * self.F.transpose() + self.Q;
    }

    pub fn update(&mut self, z: &SVector<T, M>) {
        // innovation
        let y = z - (self.H * self.x);
        // innovation covariance
        let S = self.H * self.P * self.H.transpose() + self.R;
        // Optimal gain
        let K = self.P * self.H.transpose() * S.try_inverse().unwrap();
        // state update
        self.x += K * y;
        // covariance update
        self.P = (SMatrix::identity() - K * self.H) * self.P;
    }

    pub fn new(
        F: SMatrix<T, N, N>,
        Q: SMatrix<T, N, N>,
        H: SMatrix<T, M, N>,
        R: SMatrix<T, M, M>,
    ) -> Self {
        Kalman {
            x: SMatrix::zeros(),
            F,
            P: SMatrix::zeros(),
            Q,
            H,
            R,
        }
    }
}

Below is example usage with a 1D constant velocity model and noisy position measurments.

fn const_velocity() {
    // Example usage with position and velocity states and measured position
    // Model assumes constant velocity

    // Timestep
    let dt = 0.1;
    let F = Matrix2::new(1.0, 0.1, 0.0, 1.0);
    let Q = Matrix2::identity();
    let H = Matrix1x2::new(1.0, 0.0);
    let R = Matrix1::identity() * 0.5;
    let mut kalman = Kalman::new(F, Q, H, R);

    // Create a random number generator (using rand_distr crate)
    let position_error = Normal::new(0.0, 1.0).unwrap();
    let mut rng = rand::thread_rng();

    // set initial state and covariance
    kalman.x = Vector2::new(position_error.sample(&mut rng), 0.0);
    kalman.P = Q;

    // Record state evolution
    const T: usize = 40;
    const VELOCITY: f64 = 1.0;
    let positions = (0..T).map(|t| (t as f64) * VELOCITY).collect::<Vec<f64>>();
    let mut measured = Vec::with_capacity(T);
    let mut predicted = Vec::with_capacity(T);
    let mut updated = Vec::with_capacity(T);
    let mut covariance = Vec::with_capacity(T);

    // iterate though time in seconds
    for position in &positions {
        // The noise corrupted measurement
        let measurement = position + position_error.sample(&mut rng);
        measured.push(measurement);
        // kalman predict and update
        kalman.predict();
        predicted.push(kalman.x.x);
        kalman.update(&Matrix1::new(measurement));
        updated.push(kalman.x.x);
        covariance.push(kalman.P.m11);
    }
}

The recorded real, measured, predicted and updated positions look like this (plotted using plotters):

Plot of real, measured, predicted and updated positions against time
Plot of real, measured, predicted and updated positions against time.
It takes a few samples but eventually the position estimate ‘catches up’ after its incorrect initial conditions.

See version1 for the full code including plotting the graph.

This is probably enough for a lot of projects, but I would like to take it further. There are some problems that need addressing…

Issues #

  1. Some systems have inputs which modify their state via a control matrix.
  2. The EKF requires a non-linear state transition function for estimating x and the covariance is updated using the Jacobian of this function for F.
  3. Even linear systems may have varying process and measurement covariances, which are currently immutable.
  4. The measurement vector does not have to be a fixed size. The KF is often used for sensor fusion and each sensor could have different noise and observation matrices (including a different shape, which doesn’t work with the current const generics)
  5. H.transpose() gets called twice in update() - an easy optimisation.

Version 2: Adding Systems #

The aim here is to address issues 1, 2 and 3 by making the system that the KF operates on generic. The system has 2 roles to fulfill:

  • Transition from the current state to the next
  • Generate a transition matrix for updating the covariance

For this to work, the system is required to implement the following trait. The new const generic U is the size of the input vector.

pub trait System<T, const N: usize, const U: usize> {
    /// transition to the next state, returning a reference to it
    fn step(&mut self) -> &SVector<T, N>;
    /// transition to the next state with an input, returning a reference to it
    fn step_with_input(&mut self, u: SVector<T, U>) -> &SVector<T, N>;
    /// Get a reference to the transition matrix (Jacobian)
    fn transition(&self) -> &SMatrix<T, N, N>;
    /// Get a reference to the process covariance matrix
    fn covariance(&self) -> &SMatrix<T, N, N>;
    /// Get a reference to the state
    fn state(&self) -> &SVector<T, N>;
    /// Get a mutable reference to the state
    fn state_mut(&mut self) -> &mut SVector<T, N>;
}

And some changes are made to the KF to use a type that implements System.

/// Representation of Kalman filter
#[derive(Debug)]
struct Kalman<T, const N: usize, const M: usize, const U: usize, S: System<T, N, U>> {
    /// Covariance
    pub P: SMatrix<T, N, N>,
    /// The associated system
    pub system: S,
    /// Observation Matrix
    H: SMatrix<T, M, N>,
    /// Measurment covariance
    R: SMatrix<T, M, M>,
}

impl<T: RealField + Copy, const N: usize, const M: usize, const U: usize, S: System<T, N, U>>
    Kalman<T, N, M, U, S>
{
    pub fn predict(&mut self) {
        self.system.step();
        let F = self.system.transition();
        self.P = F * self.P * F.transpose() + self.system.covariance();
    }

    pub fn predict_with_input(&mut self, u: SVector<T, U>) {
        self.system.step_with_input(u);
        let F = self.system.transition();
        self.P = F * self.P * F.transpose() + self.system.covariance();
    }

    pub fn update(&mut self, z: &SVector<T, M>) {
        // innovation
        let y = z - (self.H * self.system.state());
        // innovation covariance
        let S = self.H * self.P * self.H.transpose() + self.R;
        // Optimal gain
        let K = self.P * self.H.transpose() * S.try_inverse().unwrap();
        // state update
        *self.system.state_mut() += K * y;
        // covariance update
        self.P = (SMatrix::identity() - K * self.H) * self.P;
    }

    pub fn new(system: S, H: SMatrix<T, M, N>, R: SMatrix<T, M, M>) -> Self {
        Kalman {
            P: SMatrix::zeros(),
            system,
            H,
            R,
        }
    }
}

A linear system that implements System:

pub struct LinearSystem<T, const N: usize, const U: usize> {
    x: SVector<T, N>,
    F: SMatrix<T, N, N>,
    Q: SMatrix<T, N, N>,
    B: SMatrix<T, N, U>,
}

impl<T: RealField + Copy, const N: usize, const U: usize> LinearSystem<T, N, U> {
    pub fn new(F: SMatrix<T, N, N>, Q: SMatrix<T, N, N>, B: SMatrix<T, N, U>) -> Self {
        LinearSystem {
            x: SMatrix::zeros(),
            F,
            Q,
            B,
        }
    }
}

impl<T: RealField + Copy, const N: usize> LinearSystem<T, N, 0> {
    pub fn new_no_input(F: SMatrix<T, N, N>, Q: SMatrix<T, N, N>) -> Self {
        LinearSystem {
            x: SMatrix::zeros(),
            F,
            Q,
            B: SMatrix::zeros(),
        }
    }
}

impl<T: RealField + Copy, const N: usize, const U: usize> System<T, N, U>
    for LinearSystem<T, N, U>
{
    fn step(&mut self) -> &SVector<T, N> {
        self.x = self.F * self.x;
        &self.x
    }
    fn step_with_input(&mut self, u: SVector<T, U>) -> &SVector<T, N> {
        self.x = self.F * self.x + self.B * u;
        &self.x
    }

    fn transition(&self) -> &SMatrix<T, N, N> {
        &self.F
    }

    fn covariance(&self) -> &SMatrix<T, N, N> {
        &self.Q
    }

    fn state(&self) -> &SVector<T, N> {
        &self.x
    }

    fn state_mut(&mut self) -> &mut SVector<T, N> {
        &mut self.x
    }
}

This feels a little bit overcomplicated and convoluted, however it does allow for systems with inputs to be modelled and indeed any non-linear state transition, provided the Jacobian is calculable and the System trait is implemented.

Type aliases can make it a bit more pleasant to use. Getting back to the simplicity of the first implementation:

type LinearSystemNoInput<T, const N: usize> = LinearSystem<T, N, 0>;

type KalmanLinearNoInput<T, const N: usize, const M: usize> =
    Kalman<T, N, M, 0, LinearSystemNoInput<T, N>>;

impl<T: RealField + Copy, const N: usize, const M: usize> KalmanLinearNoInput<T, N, M> {
    pub fn new_linear_no_input(
        F: SMatrix<T, N, N>,
        Q: SMatrix<T, N, N>,
        H: SMatrix<T, M, N>,
        R: SMatrix<T, M, M>,
    ) -> Self {
        Kalman::new(LinearSystemNoInput::new(F, Q, SMatrix::zeros()), H, R)
    }
}

And the predict and update functions can be used as before.

So that is 1, 2 and part of 3 sorted. Now to look at the measurement and update stage of the KF. The source code can found here.

Version 3: Measurements #

The problem outlined in Issue 4 stems from the fact that the const generic M is fixed for a specific instance of the Kalman struct. Therefore the update function needs to be modified to accept any size measurement, along with its corresponding observation and covariance matrices H and R.

There are probably a few ways to do this and I certainly tried a lot that didn’t work. Ultimately I settled on using a KalmanUpdate trait to specify the required behaviour of the KF when updating with a Measurement struct.

trait KalmanUpdate<T, const N: usize, const M: usize> {
    fn update(&mut self, measurement: &Measurement<T, N, M>);
}

impl<T: RealField + Copy, const N: usize, const M: usize, const U: usize, S: System<T, N, U>>
    KalmanUpdate<T, N, M> for Kalman<T, N, U, S>
{
    fn update(&mut self, measurement: &Measurement<T, N, M>) {
        let H_transpose = measurement.H.transpose();
        // innovation
        let y = measurement.z - (measurement.H * self.system.state());
        // innovation covariance
        let S = measurement.H * self.P * H_transpose + measurement.R;
        // Optimal gain
        let K = self.P * H_transpose * S.try_inverse().unwrap();
        // state update
        *self.system.state_mut() += K * y;
        // covariance update
        self.P = (SMatrix::identity() - K * measurement.H) * self.P;
    }
}

pub struct Measurement<T, const N: usize, const M: usize> {
    pub z: SVector<T, M>,
    pub H: SMatrix<T, M, N>,
    pub R: SMatrix<T, M, M>,
}

The Kalman struct is no longer constrained by the measurement vector length M and measurements can vary in length, fixing issue 4 and the varying measurement covariance in issue 3. Issue 5 has also been fixed by calling the H.transpose() function just once. There are still some problems however…

  1. For a proper EKF implementation, the map from state space to observation space performed by the H matrix can be non-linear. Similar to the F matrix in the system model, there should be a function to perform the mapping and another to return the Jacobian.

  2. For a simple linear KF with constant transition/observation/covariances, this is now more complicated to use as a Measurement instance needs to be kept around or created each timestep. The only way around this is probably a wrapper around Kalman for single measurement systems.

  3. The transition and observation matrix transposes are still calculated every time-step which is unnecessary for linear systems

Source code: version3.

Version 4: EKF #

A nonlinear observation means creating a trait for measurements analogous to the System trait in Version 2.


pub trait Measurement<T, const N: usize, const M: usize> {
    /// Calculate the innovation (y) based on the measurement and the observation mapping
    fn innovation(&self, x: &SVector<T, N>) -> SVector<T, M>;
    /// Get the measurement covariance
    fn covariance(&self) -> &SMatrix<T, M, M>;
    /// Get the observation matrix
    fn observation(&self) -> &SMatrix<T, M, N>;
}

pub struct LinearMeasurement<T, const N: usize, const M: usize> {
    pub z: SVector<T, M>,
    pub H: SMatrix<T, M, N>,
    pub R: SMatrix<T, M, M>,
}

impl<T: RealField + Copy, const N: usize, const M: usize> Measurement<T, N, M>
    for LinearMeasurement<T, N, M>
{
    fn innovation(&self, x: &SVector<T, N>) -> SVector<T, M> {
        self.z - (self.H * x)
    }

    fn covariance(&self) -> &SMatrix<T, M, M> {
        &self.R
    }

    fn observation(&self) -> &SMatrix<T, M, N> {
        &self.H
    }
}

And the Kalman struct and KalmanUpdate trait are modified accordingly:

trait KalmanUpdate<T, const N: usize, const M: usize, ME: Measurement<T, N, M>> {
    fn update(&mut self, measurement: &ME);
}

impl<
        T: RealField + Copy,
        const N: usize,
        const M: usize,
        const U: usize,
        S: System<T, N, U>,
        ME: Measurement<T, N, M>,
    > KalmanUpdate<T, N, M, ME> for Kalman<T, N, U, S>
{
    fn update(&mut self, measurement: &ME) {
        let H_transpose = measurement.observation().transpose();
        // innovation
        let y = measurement.innovation(self.system.state());
        // innovation covariance
        let S = measurement.observation() * self.P * H_transpose + measurement.covariance();
        // Optimal gain
        let K = self.P * H_transpose * S.try_inverse().unwrap();
        // state update
        *self.system.state_mut() += K * y;
        // covariance update
        self.P = (SMatrix::identity() - K * measurement.observation()) * self.P;
    }
}
And that is issue 6 done!

Source code: version4.

Version 5: Back to basics #

With all the additional features to make the KF and EKF functionality as configurable as possible, creating a basic fixed linear KF is now quite complicated compared to Version 1. To make this easier, Kalman1M wraps the Kalman struct, storing the measurement matrices.

/// Kalman filter with a fixed shape measurement
struct Kalman1M<
    T,
    const N: usize,
    const U: usize,
    const M: usize,
    S: System<T, N, U>,
    ME: Measurement<T, N, M>,
> {
    kalman: Kalman<T, N, U, S>,
    measurement: ME,
}

impl<T: RealField + Copy, const N: usize, const M: usize>
    Kalman1M<T, N, 0, M, LinearSystemNoInput<T, N>, LinearMeasurement<T, N, M>>
{
    pub fn new_linear_no_input(
        F: SMatrix<T, N, N>,
        Q: SMatrix<T, N, N>,
        H: SMatrix<T, M, N>,
        R: SMatrix<T, M, M>,
    ) -> Self {
        Kalman1M {
            kalman: Kalman::new_linear_no_input(F, Q),
            measurement: LinearMeasurement {
                z: SMatrix::zeros(),
                H,
                R,
            },
        }
    }

    pub fn predict(&mut self) {
        self.kalman.predict();
    }

    pub fn update(&mut self, z: SVector<T, M>) {
        self.measurement.z = z;
        self.kalman.update(&self.measurement);
    }
}

This looks far from basic but the end result hopefully is. Kalman1M can be constructed with the 4 matrices and predict() and update() can be used as before.

Source code: version5.

Version 6: Performance #

The aim here is to remove the unnecessary transpose calculations that are performed at every timestep, which for a linear system do not change. Adding an extra function the System trait:

pub trait System<T, const N: usize, const U: usize> {
    ...
    /// Get the transpose of the transition matrix
    fn transition_transpose(&self) -> &SMatrix<T, N, N>;
    ...
}

Then for the LinearSystem, the transpose can be recorded on creation:


impl<T: RealField + Copy, const N: usize, const U: usize> LinearSystem<T, N, U> {
    pub fn new(F: SMatrix<T, N, N>, Q: SMatrix<T, N, N>, B: SMatrix<T, N, U>) -> Self {
        LinearSystem {
            x: SMatrix::zeros(),
            F,
            F_t: F.transpose(),
            Q,
            B,
        }
    }
}

And be retrieved each timestep:

impl<T: RealField + Copy, const N: usize, const U: usize> System<T, N, U>
    for LinearSystem<T, N, U>
{
    ...
    fn transition_transpose(&self) -> &SMatrix<T, N, N> {
        &self.F_t
    }
    ...
}

impl<T: RealField + Copy, const N: usize, const U: usize, S: System<T, N, U>> Kalman<T, N, U, S> {
    pub fn predict(&mut self) {
        self.system.step();
        self.P = self.system.transition() * self.P * self.system.transition_transpose()
            + self.system.covariance();
    }
}

The same changes can be made to the Measurement trait to store the transpose of the observation matrix and return a reference to it at each update() call.

Source code: version6.

Version 7: Tidying up with traits #

The current implementation has one more issue I would like to address. The System trait contains both the step and step_with_input functions, which should be mutually exclusive as no type should implement both. It also means both predict and predict_with_input can be called on Kalman which does’t make sense and could lead to errors.

The step functionality is move into 2 new traits, InputSystem and NoInputSystem:

pub trait InputSystem<T, const N: usize, const U: usize>: System<T, N, U> {
    /// transition to the next state with input, 
    /// returning a reference to it
    fn step(&mut self, u: SVector<T, U>) -> &SVector<T, N>;
}

pub trait NoInputSystem<T, const N: usize>: System<T, N, 0> {
    /// transition to the next state, returning a reference to it
    fn step(&mut self) -> &SVector<T, N>;
}

Then systems can implement these according to whether they can take inputs or not.

Then to prevent both predict functions being available, 2 new traits are created for Kalman:

trait KalmanPredict<T, const N: usize> {
    /// predict the next state and return it. Also update covariance
    fn predict(&mut self) -> &SVector<T, N>;
}

trait KalmanPredictInput<T, const N: usize, const U: usize> {
    /// predict the next state with an input vector and return it. Also update covariance
    fn predict(&mut self, u: SVector<T, U>) -> &SVector<T, N>;
}

And Kalman only implements each trait when its associated system implements the corresponding system trait, as shown below:

/// Implement the predict stage for a system with no input
impl<T: RealField + Copy, const N: usize, S: NoInputSystem<T, N>> KalmanPredict<T, N>
    for Kalman<T, N, 0, S>
{
    fn predict(&mut self) -> &SVector<T, N> {
        self.system.step();
        self.P = self.system.transition() * self.P * self.system.transition_transpose()
            + self.system.covariance();
        self.system.state()
    }
}

/// Implement the predict stage for a system with input
impl<T: RealField + Copy, const N: usize, const U: usize, S: InputSystem<T, N, U>>
    KalmanPredictInput<T, N, U> for Kalman<T, N, U, S>
{
    fn predict(&mut self, u: SVector<T, U>) -> &SVector<T, N> {
        self.system.step(u);
        self.P = self.system.transition() * self.P * self.system.transition_transpose()
            + self.system.covariance();
        self.system.state()
    }
}

And I think that is enough messing with the library. The source code up to this point can be found at version7.

Nonlinear system #

As a final addition, this is the generic non-linear system in the library. By supplying a step_fn that implements f(x,u) as well as returning the jacobian and process covariance, an EKF


pub struct NonLinearSystem<T, const N: usize, const U: usize> {
    /// System state
    x: SVector<T, N>,
    /// Process Covariance, updated after step_fn() call.
    Q: SMatrix<T, N, N>,
    /// Jacobian, updated after jacobian() call.
    F: SMatrix<T, N, N>,
    /// Jacobian transpose, updated after jacobian() call.
    F_t: SMatrix<T, N, N>,
    /// Function that steps from current state to next with an input   
    /// Returns the new state, the jacobian and the process covariance
    step_fn: StepInput<T, N, U>,
}

impl<T: RealField + Copy, const N: usize, const U: usize> InputSystem<T, N, U>
    for NonLinearSystem<T, N, U>
{
    fn step(&mut self, u: SVector<T, U>) -> &SVector<T, N> {
        // Get updated state, jacobian and process covariance
        let r = (self.step_fn)(self.x, u);
        self.x = r.state;
        self.F = r.jacobian;
        self.F_t = self.F.transpose();
        self.Q = r.covariance;
        // Return state
        self.state()
    }
}

Conclusion and future work #

I am pretty happy with how things have worked out and I believe it meets to goal of a flexible library for both the KF and EKF.

Before publishing to crates.io I also added different constructors for common configurations, a nicer way of setting initial state and covariance values, some documentation and examples.

I would like to look at methods for ensuring the covariance remains symmetric as this can sometimes be necessary due to numerical issues. There are also other forms of the KF like the unscented Kalman filter that I would like to learn more about and implement.

←→ Benchmarking Typescript/Javascript on NodeJS on Linux
↑