Module Description

The Ahrs_ekf modules updates the ahrs structure using an extended kalman filter with updates from the accelerometer and magnetometer.

Accelerometer Update Derivation

We need to determine the measurement matrix (H) for the kalman filter. This is done by predicting what the measurement would be at any given attitude (h) and then obtaining the Jacobian matrix of that.

The state vector given in the Ahrs_ekf module is given as:

where b in the gyroscope biases and q is the ahrs quaternion.

The acceleration vector is the following:

g is positive as the positive z direction points towards the ground.

As we want to rotate the vector in the opposite direction of the ahrs quaternion, we can negate the i, j, and k component of the quaternion to get this rotation.

As per the quaternions_rotate_vector function in quaternions.h, we compute the following steps in order to rotate the vector.

Our final rotated vector is:

In order to obtain the measurement matrix in the kalman filter, we take the Jacobian of h with respect to the state vector.

Magnetometer Update Derivation

We need to determine the measurement matrix (H) for the kalman filter. This is done by predicting what the measurement would be at any given attitude (h) and then obtaining the Jacobian matrix of that.

The state vector given in the Ahrs_ekf module is given as:

where b in the gyroscope biases and q is the ahrs quaternion.

The magnetometer vector is the following:

As we want to rotate the vector in the opposite direction of the ahrs quaternion, we can negate the i, j, and k component of the quaternion to get this rotation.

As per the quaternions_rotate_vector function in quaternions.h, we compute the following steps in order to rotate the vector.

Our final rotated vector is:

In order to obtain the measurement matrix in the kalman filter, we take the Jacobian of h with respect to the state vector.