Kalman Filters and Generic Structs in Rust

A couple weeks ago I found myself getting a little frustrated with Python and Matlab, for reasons that don’t necessitate a long-winded explanation. I fell into the rabbit hole of languages, and emerged with Julia, Rust and Haskell rolling around, half-formed, in my head. My adventures with both Julia and Haskell deserve their own posts, which I can’t guarantee will come anytime soon.

Anyway, Rust is a relatively new systems programming language, and if you want to learn more, there’s no better place to start than diving into the Rust book . I found myself falling in love with both the language and with the incredibly helpful community. It also got me into thinking about type theory, but there’s enough about that to also fill another post.

Along the way, I figured the best way to really hammer home the newer concepts (to me) of Rust’s approaches to mutability and ownership was to just write something of my own. After a couple weeks of hammering out the details, I published kalmanrs on crates.io . Getting there was a little bit of a hassle, tangentially related to the way that Rust handles generics.

I used the nalgebra crate as the backend for the linear algebra, and the matrix dimension parameters are defined using type-level unsigned integer constants. This means that to define a 3x3 matrix, you’d use the U3 type. Nalgebra uses type-level integers because Rust doesn’t support parameterization over integer values (you can’t have systems that are generic over integer inputs). Since I designed kalmanrs with generic dimensions in mind, this made things pretty challenging.

I got around with it by writing a macro that ‘builds’ the implementation for you for any given set of dimensions:

#[macro_export]
macro_rules! lkf_builder {
    ($lkf_wrapper: ty) => {
        impl $lkf_wrapper {                
            fn predict(&mut self) {
                // projects state ahead 
               self.state.x = self.lk.A * self.state.x + self.lk.B*self.state.u;
                // project error covariance ahead
                self.lk.P = self.lk.A * self.lk.P * self.lk.A.transpose() + self.lk.Q;
            }
            
             fn update(&mut self) {
                // compute Kalman gain
                let mut inner_val = self.lk.H * self.lk.P * self.lk.H.transpose() + self.lk.R;
                inner_val.try_inverse_mut();
                self.lk.K = self.lk.P * self.lk.H.transpose() * inner_val;

                // update estimate with measurement z
                self.state.x = self.state.x + self.lk.K*(self.state.z - self.lk.H * self.state.x);

                // update error covariance
                self.lk.P = (self.lk.I - self.lk.K * self.lk.H) * self.lk.P;
            }
        }
    }
}

The user just has to create a struct that encapsulates/wraps the two other structs that form the Kalman Filter, one that represents the state at any given time and another that holds the necessary covariance, mapping and kalman gain matrices. Then all you’ve got to do is fire off the macro, and the predict and update functions for the filter are implemented for your encapsulating struct. Hopefully this helps anyone that’s grappling with dimensional genericity in Rust.

As an addendum, I’m really enjoying the language. I gotta find the time to start throwing it at some embedded applications, and I should have something up related to that soon (given I don’t find something else to distract me first).