This is a library that provides an extended Kalman filter implementation for tracking orientation, expressed as a quaternion. It is designed to be used with a 6DOF IMU. This means that the filter expects angular velocity and acceleration as input.
In addition to the C implementation, you will also find a Python version that was used for initial prototyping and design. The C implementation is in kfclib, and the Python prototype can be found in kfplib.
Warning
WHILE I HAVE MADE EFFORTS TO ENSURE THAT THE FILTER WORKS AS INTENDED, I CANNOT GUARANTEE THE CORRECTNESS OF THE FILTER DESIGN.
The 9dof version is coming soon.
- https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
- https://ahrs.readthedocs.io/en/latest/filters/ekf.html#
- Basile Graf, arXiv: Dynamical Systems, 2008
The filter should be initialized using the kf_init
function:
struct kalman_filter *kf_init(float dt, float var_a, float var_w, float var_P);
This function takes the following parameters:
dt
: Time difference between measurements (in seconds).var_a
: Variance of the accelerometer measurements.var_w
: Variance of the angular velocity measurements.var_P
: Variance of the initial state.
The filter is designed in a way such that there is no need to know the initial orientation in relation to a fixed frame of reference (e.g. NED, ENU, etc.). This leaves the user with two approaches for initializing the state and expressing the orientation.
In this approach, you might average the first few accelerometer readings (without inducing any translation or rotation) and use those as the reference acceleration. The initial orientation, i.e., the initial state vector, should be the identity quaternion.
If you want to express the orientation relative to a fixed frame of reference (e.g., NED, ENU, etc.), you should use the gravitational vector of that frame as the reference acceleration. The initial state vector should be set to the orientation relative to the fixed frame. This might yield better results depending on the state of the IMU and how accurately the initial orientation can be determined
The quaternion that expresses the orientation can be set with:
void kf_set_q(struct kalman_filter *kf, float qw, float qx, float qy, float qz);
The reference acceleration can be set with:
void kf_set_aref(struct kalman_filter *kf, float ax, float ay, float az);
Once the filter is initialized, you can use the kf_filt
function to
perform a single filter step (predict + update).
int kf_filt(struct kalman_filter *kf,
float wx, float wy, float wz,
float ax, float ay, float az);
The function takes a pointer to the struct kalman_filter
that was
obtained from kf_init
along with the angular velocity measurements (wx, wy, wz
in ax, ay, az
).
After each filter step, the orientation quaternion is updated and can be
accessed via the kf->q
member, which is of type struct quaternion
:
struct quaternion
{
float w, x, y, z;
};
To release the filter's allocated memory, use kf_free
.
void kf_free(struct kalman_filter *kf);
Under construction.
The state vector is a quaternion that tracks the orientation.
The state covariance matrix is initialized as an identity matrix since there should be no covariance between the quaternion elements.
The gyroscope measurements are used as control input to predict the next state (i.e., compute the prior). Therefore the quaternion integration equation is used to create the state transition matrix:
Where:
Here
This leads to:
In the beginning I just used an identity matrix with the variance of the
gyroscope measurements
Where
The normalized accelerometer measurements are used for the measurement vector.
The state vector is converted into measurement space by rotating the reference acceleration vector using the quaternion of the current state vector. If the prediction was accurate, the rotated
This results in:
With
The measurement covariance matrix is an identity matrix with the accelerometer
variance
Coming soon.