Generic Kalman filter implementation with C and C++ interfaces.
You simply need to add kalman.cc to your build process. The library contains its own copy of the Eigen linear algebra library to do all the mathematical heavy lifting. The library provides 3 different interfaces:
- A simple (AND DEPRECATED!) C interface that uses statically-sized structures. These functions take fixed-size kalman_vec_t and kalman_mat_t arguments. If the filter needs fewer values than are in the structure, it will ignore the extraneous rows. Please note that the fixed-size structures are limited to 9-variable filters. If you need more variables in the filter, use the dynamic interface below. Please note that this interface is deprecated. Use the dynamically- sizeable C interface below for future development.
- A C interface that uses dynamically-sized structures. These functions
take kalman_dmat_t arguments. These are single-block allocation
structures created using kalman_dmat_alloc. Once created using the
kalman_dmat_alloc
(and similar) functions, the matrix dimensions cannot be altered. Free the allocated matrix using the standard C library free() function. Dynamic interface functions have a 'd' in their name (e.g.kalman_set_dstate
instead ofkalman_set_state
). - A C++ interface that uses dynamically-sized matrices with RAII. These
structures dynamically allocate memory as necessary and are aliases
for the Eigen::Matrix type. All features of the Eigen::Matrix template
type are available.
The C++ interface functions are declared in
kalman.hh
and are located inside of thekalman
namespace.
Should you need to, you can mix-and-match these interfaces. They all operate on the same underlying state.
To construct a filter, use kalman_alloc
with the number of state
variables you wish to track. Before being able to use the filter you
must initialize it:
- Setup the filter's initial state vector (x_k). Use the
kalman_set_state
,kalman_set_dstate
orkalman::set_state
functions for this task. - Setup the filter's initial covariance matrix (P_k). Use the
kalman_set_cov_mat
,kalman_set_dcov_mat
orkalman::set_cov_mat
functions for this task. - Setup the filter's prediction matrix (A_k). Use the
kalman_set_pred_mat
,kalman_set_dpred_mat
orkalman::set_pred_mat
functions for this task.
This is the minimum amount of setup required to get the filter working.
See kalman.h
or kalman.hh
for the other functions available to
initialize other useful properties of the filter (such as the control
vector, process noise, etc.). To evolve the filter by one step, use one
of the following functions:
kalman_step(kal, measurement, measurement_covariance, observation_model);
kalman_dstep(kal, measurement, measurement_covariance, observation_model);
kalman::step(kal, measurement, measurement_covariance, observation_model);
measurement
and measurement_covariance
are the new measurement
vector and covariance matrix. If your measurement uses a different layout
than your filter's vector, you can pass the observation_model
matrix,
which will be used to adapt the measurement to fit the filter's internal
state vector layout. If your measurement vector matches the filter's
state vector, simply pass NULL for the observation model (use the
2-argument version of kalman::step for the C++ interface).
In the C interfaces, you can also pass NULL for the measurement
vector and measurement_covariance
matrix. This will perform a
predictive-only (NULL) update of the filter. This is useful for cases
when your filter needs to be updated at fixed intervals, but your
measurement system doesn't always supply new measurements to update the
filter. Mathematically, this simply performs the predicted measurement
(x_k') and predicted covariance (P_k') calculations and then simply
installs these new predictions as the filter's current state vector and
covariance.
In the C++ interface, you can perform a NULL update by calling
kalman::step
with just the filter argument and no measurement.
You can retrieve the filter's current state vector and covariance matrix
using kalman_get_state
and kalman_get_cov_mat
.
You will no doubt want to observe the internals of the filter during
development. The KALMAN_DEBUG_STATE
and KALMAN_DEBUG_COV_MAT
macros give you a quick shorthand that dumps the filter's current state
vector and covariance matrix to stdout. You can also use the more
generic KALMAN_DEBUG_VEC
and KALMAN_DEBUG_MAT
macros to extract
any other vector or matrix in the filter, e.g. to extract the control
vector:
kalman_t *kal;
...
KALMAN_DEBUG_VEC(kal, "cont_vect", kalman_get_cont);