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.
how do you compile example_gravity.c?
You simply compile it with the rest of the sources as a binary. Nothing special here.
That is not helpful at all.
I’m sad you feel that way. Have a look at the develop branch on GitHub; that might help you more. You’ll find a Visual Studio solution and project there that includes libfixmath and libfixkalman and statically compiles them with libfixkalman into one binary that has the example code as its entry point.
How do I compile this in cygwin? I don’t have visual C.
I did not do this, but a simple makefile should do it. You’ll require libfixmath and libfixmatrix of course.
Does this compile with gcc in Windows?
It’s been a while and I didn’t try, but I have no reason to doubt that.
Hallo,
vielen Dank für libfixkalman. Ich teste sie zur Zeit auf einem ARM7 Autopilot. Ich habe es mir inzwischen selbst recht gut zusammengereimt, aber hast du vielleicht noch Beispielcode mit mehreren Messwerten und/oder Controls?
Hi Martin,
leider wenig …
Für ein Roboterprojekt habe ich kürzlich das Filter für die Odometrie eingesetzt, aber der Code ist aus verschiedenen Gründen nicht auf GitHub. Die relevanten Dateien (insbesondere die zur Geschwindigkeitsfusion) habe ich hier als Gist öffentlich gemacht, vielleicht hilft Dir das etwas.