Skip to content
/ kfclib Public

Kalman filter in C for quaternion based orientation estimation with 6-dof IMU's.

License

Notifications You must be signed in to change notification settings

NMIK69/kfclib

Folders and files

NameName
Last commit message
Last commit date

Latest commit

770501f · Sep 27, 2024

History

3 Commits
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

kfclib

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.

Main Resources and References used

Usage

Initializing the filter

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.

Setting the initial orientation and acceleration reference vector.

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.

Approach 1: Unknown Orientation relative to a fixed reference frame.

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.

Approach 2: Known Orientation relative to a fixed reference frame.

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);

Running the filter

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 rad s and the acceleration measurements (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;
};

Releasing the filter

To release the filter's allocated memory, use kf_free.

void kf_free(struct kalman_filter *kf);

Example

Under construction.

Kalman Filter Design (6dof-IMU)

State vector

The state vector is a quaternion that tracks the orientation.

x = [ q w q x q y q z ]

State Covariance Matrix

The state covariance matrix is initialized as an identity matrix since there should be no covariance between the quaternion elements.

P = σ p 2 I 4

State Transition Matrix

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:

q n = q n 1 + Δ t d q n d t , d q n d t = 1 2 q n 1 Ω n ,

Where:

Ω n = [ 0 , w x , w y , w z ] T

Here w x , w y and w z are the angular velocity measurements from the gyroscope in rad s . The operator denotes quaternion multiplication. For this kalman filter, q n is the current prior, and q n 1 is the previous posterior. The state transition matrix is derived from the quaternion integration equation such that F is linear and such that:

x n = F x n 1 = x n 1 + Δ t ( 1 2 x n 1 Ω n )

This leads to:

F = [ 1 Δ t 2 ω x Δ t 2 ω y Δ t 2 ω z Δ t 2 ω x 1 Δ t 2 ω z Δ t 2 ω y Δ t 2 ω y Δ t 2 ω z 1 Δ t 2 ω x Δ t 2 ω z Δ t 2 ω y Δ t 2 ω x 1 ]

Process Noise Covariance Matrix

In the beginning I just used an identity matrix with the variance of the gyroscope measurements σ ω 2 on the diagonal. But after using a design initially found here which is linked to this publication I had better results. They designed Q the following way:

Q n = σ ω 2 W n W n T

W n = Δ t 2 [ q x q y q z q w q z q y q z q w q x q y q x q w ]

Where q w , q x , q y , q z are taken from the current state vector.

Measurement vector

The normalized accelerometer measurements are used for the measurement vector.

z = [ a x ^ a y ^ a z ^ ]

Observation matrix

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 a ref ^ should closely match the measured acceleration z . The observation matrix H is computed such that H is linear and such that:

H x = x [ 0 a ref ^ ] x

This results in:

H = [ p x p w p z p y p y p z p w p x p z p y p x p w ]

With p = [ p w , p x , p y , p z ] T and:

p = x [ 0 a ref ^ ]

Measurement Covariance Matrix

The measurement covariance matrix is an identity matrix with the accelerometer variance σ a 2 on the diagonal.

R = σ a 2 I 3 ,

Kalman Filter Design (9dof-IMU)

Coming soon.

About

Kalman filter in C for quaternion based orientation estimation with 6-dof IMU's.

Topics

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published