In need for a Kalman filter on an embedded system I was looking for a linear algebra library. It turned out that there are quite a bunch of libraries written in C++, mostly template based, yet nothing lean and mean written in ANSI C. I ended up porting parts of EJML to C and picked only what I needed for the Kalman filter (see the result, kalman-clib, if you are interested). The result was a working, relatively fast library using floats. Or doubles, if the user prefers.

However, a big problem is that many embedded targets (say, ARM Cortex-M0/3) don’t sport a floating-point unit, so the search went on for a fixed-point linear algebra library. Thanks to a hint on StackOverflow it ended at libfixmath, which implements Q16 numbers (i.e. 16.16 on 32bit integer) and is released under an MIT license.

Luckily, some other guy created a linear algebra library around it, called libfixmatrix (MIT, too). I started to port my Kalman library to libfixmatrix and, voilà, libfixkalman was born (you can find it in the libfixkalman GitHub repository).

Libfixkalman Usage Example

Using libfixkalman should be relatively simple. For the filter there’s a structure kalman16_t and for each different kind of observation, there’s kalman16_observation_t.

kalman16_t kf;
kalman16_observation_t kfm;

Defining the filter is pretty straightforward (albeit a little nasty because of a lot of calls to float-to-fixed conversion methods, like fix16_from_float()):

#define KALMAN_NUM_STATES 3
#define KALMAN_NUM_INPUTS 0
#define KALMAN_NUM_MEASUREMENTS 1

#define matrix_set(matrix, row, column, value) \
    matrix->data[row][column] = value

static void kalman_gravity_init()
{
    /************************************************************************/
    /* initialize the filter structures                                     */
    /************************************************************************/
    kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
    kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);

    /************************************************************************/
    /* set initial state                                                    */
    /************************************************************************/
    mf16 *x = kalman_get_state_vector(&kf);
    matrix_set(x, 0, 0, 0); // s_i
    matrix_set(x, 1, 0, 0); // v_i
    matrix_set(x, 2, 0, F16(6)); // g_i, initial estimation

    /************************************************************************/
    /* set state transition                                                 */
    /************************************************************************/
    mf16 *A = kalman_get_state_transition(&kf);
   
    // set time constant and helper
    const fix16_t T = fix16_one;
    const fix16_t Tsquare = fix16_sq(T);
    const fix16_t fix16_half = F16(0.5);

    // transition of x to s
    matrix_set(A, 0, 0, fix16_one);   // 1
    matrix_set(A, 0, 1, T);   // T
    matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
    
    /* ... snip ... */

    /************************************************************************/
    /* set covariance                                                       */
    /************************************************************************/
    mf16 *P = kalman_get_system_covariance(&kf);

    /* ... snip ... */

    /************************************************************************/
    /* set measurement transformation                                       */
    /************************************************************************/
    mf16 *H = kalman_get_observation_transformation(&kfm);

    /* ... snip ... */

    /************************************************************************/
    /* set process noise                                                    */
    /************************************************************************/
    mf16 *R = kalman_get_observation_process_noise(&kfm);

    /* ... snip ... */
}

After that, filtering works by recursively calling kalman_predict() and kalman_correct()

void kalman_gravity_demo()
{
    // initialize the filter
    kalman_gravity_init();

    mf16 *x = kalman_get_state_vector(&kf);
    mf16 *z = kalman_get_observation_vector(&kfm);

    // filter!
    uint_fast16_t i;
    for (i = 0; i < MEAS_COUNT; ++i)
    {
        // prediction.
        kalman_predict(&kf);

        // measure ...
        fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
        matrix_set(z, 0, 0, measurement);

        // update
        kalman_correct(&kf, &kfm);
    }

    // fetch estimated g
    const fix16_t g_estimated = x->data[2][0];
    const float value = fix16_to_float(g_estimated);
}

The hot spot, the inversion of the residual covariance matrix in the observation update step, is implemented using a Cholesky decomposition and an inversion method that is specialized for working on lower-triangularized positive-definite symmetric matrices.

The whole example can be found in example_gravity.c.

My aforementioned floating-point based library kalman-clib works similarly, but is a bit more difficult to set up because of the use of shared temporary buffers. Like libfixkalman it avoids dynamic memory allocation, but takes another approach than libfixmatrix does.