In need for a Kalman fil­ter on an embed­ded sys­tem I was look­ing for a lin­ear alge­bra library. It turned out that there are quite a bunch of libraries writ­ten in C++, most­ly tem­plate based, yet noth­ing lean and mean writ­ten in ANSI C. I end­ed up port­ing parts of EJML to C and picked only what I need­ed for the Kalman fil­ter (see the result, kalman-clib, if you are inter­est­ed). The result was a work­ing, rel­a­tive­ly fast library using floats. Or dou­bles, if the user prefers.

How­ev­er, a big prob­lem is that many embed­ded tar­gets (say, ARM Cor­tex-M0/3) don’t sport a float­ing-point unit, so the search went on for a fixed-point lin­ear alge­bra library. Thanks to a hint on Stack­Over­flow it end­ed at lib­fix­math, which imple­ments Q16 num­bers (i.e. 16.16 on 32bit inte­ger) and is released under an MIT license.

Luck­i­ly, some oth­er guy cre­at­ed a lin­ear alge­bra library around it, called lib­fix­ma­trix (MIT, too). I start­ed to port my Kalman library to lib­fix­ma­trix and, voilà, lib­fixkalman was born (you can find it in the lib­fixkalman GitHub repos­i­to­ry).

Libfixkalman Usage Example

Using lib­fixkalman should be rel­a­tive­ly sim­ple. For the fil­ter there’s a struc­ture kalman16_t and for each dif­fer­ent kind of obser­va­tion, there’s kalman16_observation_t.

kalman16_t kf;
kalman16_observation_t kfm;

Defin­ing the fil­ter is pret­ty straight­for­ward (albeit a lit­tle nasty because of a lot of calls to float-to-fixed con­ver­sion meth­ods, 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, fil­ter­ing works by recur­sive­ly call­ing 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 inver­sion of the resid­ual covari­ance matrix in the obser­va­tion update step, is imple­ment­ed using a Cholesky decom­po­si­tion and an inver­sion method that is spe­cial­ized for work­ing on low­er-tri­an­gu­lar­ized pos­i­tive-def­i­nite sym­met­ric matri­ces.

The whole exam­ple can be found in example_gravity.c.

My afore­men­tioned float­ing-point based library kalman-clib works sim­i­lar­ly, but is a bit more dif­fi­cult to set up because of the use of shared tem­po­rary buffers. Like lib­fixkalman it avoids dynam­ic mem­o­ry allo­ca­tion, but takes anoth­er approach than lib­fix­ma­trix does.