cnkalman

CNKalman

This is a relatively low level implementation of a kalman filter; with support for extended and iterative extended kalman filters. The goals of the project are to provide a numerically stable, robust EKF implementation which is both fast and portable.

The main logic is written in C and only needs the associated matrix library to work; and there are C++ wrappers provided for convenience.

Tutorial on Kalman Filter Theory

There was originally going to be a more in depth discussion of the theoretical side but there isn't much one could do to improve on the very in depth tutorial by Roger Labbe.

Features

• Support for extended kalman filter, linear kalman filters, and Iterate Extended Kalman Filter (paper)
• Support for adaptive measurement covariance
• Support for error-state kalman filter
• Built-in support for numerical-based jacobians, and an option to debug user provided jacobians by using the numerical results
• Minimal heap allocations
• Supports multiple measurement models per filter, which can be integrated at varying frequencies
• C++ bindings for objected oriented applications
• Automatic code generation for analytical jacobians

Quick start

kalman_example.c

#include <cnkalman/kalman.h>
#include <stdio.h>

static inline void kalman_transition_model_fn(FLT dt, const struct cnkalman_state_s *k, const struct CnMat *x0,
struct CnMat *x1, struct CnMat *F) {
    // Logic to fill in the next state x1 and the associated transition matrix F
}

static inline void kalman_process_noise_fn(void *user, FLT dt, const struct CnMat *x, struct CnMat *Q) {
    // Logic to fill in the process covariance Q
}

static inline bool kalman_measurement_model_fn(void *user, const struct CnMat *Z, const struct CnMat *x_t,
struct CnMat *y, struct CnMat *H_k) {
    // Logic to fill in the residuals `y`, and the jacobian of the predicted measurement function `h`
    return false; // This should return true if the jacobian and evaluation were valid.
}

int main() {
    int state_cnt = 1;
    cnkalman_state_t kalman_state = { 0 };
    cnkalman_state_init(&kalman_state, state_cnt, kalman_transition_model_fn, kalman_process_noise_fn, 0, 0);
    // Uncomment the next line if you want to use numerical jacobians for the transition matrix
    //kalman_state.transition_jacobian_mode = cnkalman_jacobian_mode_two_sided; 
    
    cnkalman_meas_model_t kalman_meas_model = { 0 };
    cnkalman_meas_model_init(&kalman_state, "Example Measurement", &kalman_meas_model, kalman_measurement_model_fn);
    // Uncomment the next line if you want to use numerical jacobians for this measurement
    // kalman_meas_model.meas_jacobian_mode = cnkalman_jacobian_mode_two_sided; 
    
    CnMat Z, R;
    // Logic to fill in measurement matrix Z and measurement covariance matrix R
    cnkalman_meas_model_predict_update(1, &kalman_meas_model, 0, &Z, &R);
    
    printf("Output:%f\n", cn_as_vector(&kalman_state.state)[0]);
    return 0;
}

Code Generation

One of the more difficult things about extended kalman filters is the fact that calculating the jacobian for even a simple measurement or prediction function can be tedious and error prone. An optional portion of cnkalman is easy integration of symengine in such a way that you can write the objective function in python and it'll generate the C implementation of both the function itself as well as it's jacobian with each of it's inputs.

BikeLandmarks.py

from symengine import atan2, asin, cos, sin, tan, sqrt
import cnkalman.codegen as cg

@cg.generate_code(state = 3, u = 2)
def predict_function(dt, wheelbase, state, u):
    x, y, theta = state
    v, alpha = u
    d = v * dt
    R = wheelbase/tan(alpha)
    beta = d / wheelbase * tan(alpha)

    return [x + -R * sin(theta) + R * sin(theta + beta),
            y + R * cos(theta) - R * cos(theta + beta),
            theta + beta]

@cg.generate_code(state = 3, landmark = 2)
def meas_function(state, landmark):
    x, y, theta = state
    px, py = landmark

    hyp = (px-x)**2 + (py-y)**2
    dist = sqrt(hyp)

    return [dist, atan2(py - y, px - x) - theta]

There are limitations in what type of logic is permissible here -- it must be something that is analytically tractable -- but most objective functions themselves are not too complicated to write.

Notice that for array-type input like state and u above, you must specify a size hint.

When ran, this python script generates the companion BikeLandmarks.gen.h which has the generated code, and callouts such as:

static inline void gen_predict_function(CnMat* out, const FLT dt, const FLT wheelbase, const FLT* state, const FLT* u);
static inline void gen_predict_function_jac_state(CnMat* Hx, const FLT dt, const FLT wheelbase, const FLT* state, const FLT* u);

If you include this project in as a cmake project, a cmake function cnkalman_generate_code is available that makes this an optional part of your build process.