Custom Flight Controller Part 2.3.1: Sensor Fusion --- (Optional) The Quaternion Kalman Filter

Part 2.3 consists a series of post explaining how to perform sensor fusion using Quaternion Kalman Filter. The complete code can be found in my Github.

Note that in the final implementation, I opted to use Madgwick Filter instead because it is more lightweight and perform equally well empirically. Therefore, the whole series about sensor fusion is completely optional. Madgwick filter is implemented in C and can be downloaded easily from https://github.com/arduino-libraries/MadgwickAHRS. Refer to this site to learn more about madgwick filter.

Alternatively, the quaternion kalman filter covered in this series of post works as well and since I have spent quite a lot of time implementing it, I believe it is worth documenting.

Accelerometer measures the acceleration experienced, gyroscope measures the angular velocity and magnetometer measures the magnetic field. In order to calculate the orientation from these IMU measurements, sensor fusion needs to be performed. Although there are numerous algorithm to achieve sensor fusion, the underlying principle should be similar.

An example of sensor fusion using complementary filter

Assuming the initial orientation is known, integrating the gyroscope measurement will indicate how much the device has rotated. Combining both information allows me to predict the current orientation. Furthermore, assuming that there is no external linear acceleration, the only acceleration the accelerometer should measure is gravity which is always pointing downward. With different orientation of the device gravity measured will give different xyz axis measurement while the magnitude is remained constant. Therefore, accelerometer measurement could be used to update the orientation predicted to give a better estimate. The same theory can be applied to magnetometer as well.

However, sensor measurements are not accurate. Not only does every sensor has intrinsic noise, there are numerous problems that affect the accuracy of measurement. To name a few, gyroscope tends to drift over time, meaning it will have a zero-state error. And external acceleration and magnetic field are always present in real life so the accelerometer and magnetometer measurement under such conditions would be contaminated. The purpose of sensor fusion is therefore to make us of all the available data and give the best possible estimate of the device's current orientation.

Popular sensor fusion methods include complimentary filter which is widely used in self-balancing robot, extended Kalman Filter which is used in Pixhawk. Similarly, there are countless papers explaining various types of filters to perform orientation estimation. I have decided to use a quaternion kalman filter for this project. This filter algorithm is proposed and explained in paper Novel Quaternion Kalman Filter and Adaptive Kalman Filter for MEMS-IMU based Attitude Estimation under External Acceleration and Parsimonious use of Gyroscopes.

In order to understand the filter algorithm, one has to understand the basics of Kalman Filter first. This amazing tutorial on Extended Kalman Filter by Simon D. Levy benefited me greatly. And if you are looking for mathematical proof, Wikipedia offers it. Alternatively, here contains a complete derivation of Extended Kalman Filter.

Since I was using a Quaternion Kalman Filter, I had to get myself familiar with quaternion as well. Wikipedia was my main teacher in this case.  

I do not have the confidence to say that I have completely understood the mathematical theories behind Kalman Filter and quaternion, but these tutorials have enabled me to implement the filter proposed in research paper. The implementation of the algorithm is explained as follows.  

• Let X be the state quaternion representing the orientation. X = [x0 x1 x2 x3] = [x0 x*], where x0 is the scalar and x* = [x1 x2 x3] forms the vector part of quaternion. 
• Let A be the state transition matrix. Quaternion algebra gives that 
  

where w =[gyroX gyroY gyroZ] is a vector that contains the gyroscope measurement. The proof of this equation can be found here. The weird operator in the middle represents quaternion multiplcation. Quaternion multiplication could be changed to matrix multiplication as shown in this figure

         

          where

         Therefore,

         and 

• Let Q be the process noise covariance matrix.Then 
  

where Rg is the gyroscope measurement noise covariance. Its diagonal elements indicate the variance of the measurement data in three axes. And

Basically, this formula relates the gyroscope measurement noise covariance to the noise of quaternion predicted. It is somewhat like a frame transformation. Similarly, let R be the observation noise convariance matrix. Then

where Ra is the accelerometer measurement noise covariance matrix.

• The acceleration observation model is 

Where Z is the accelerometer measurement, f(Xk) is a function of Xk, G is the gravitation acceleration vector and V represents noise. Normally, Extended Kalman Filter is used to handle this kind of nonlinear observation model. However, here the papers proposed a different method. Left-multiplying both sides of the above equation by Xk and rearranging yields

          Where 

Note that the left side of the equation becomes zero. And the observation model is linearized, allowing us to use a linear Kalman filter without having to compute Jacobians as required in Extended Kalman Filter. Now the measurement residual(the difference between calculated value and measurement) is -HXk.

• The rest are quite straight forward. Following the Kalman Filter Algorithm we get:

          Prediction:

          Update:

Since Arduino is not capable to handle matrix math and floating point calculation fast enough, I implement the full algorithm in matlab. The test script can be found in my Github.

Inside the script I have arbitrary set gyro measurement to some random values. I have also set dt to one for simplicity. 

gx_true = 20;

gy_true = 45;

gz_true = 90;

dt = 1;

Noises are added by utilizing random number generator. Note that Kalman Filter assumes noise to be normally distributed bu RNG gives uniform distribution. However, it does not affect much.

%add process noise

gx = gx_true+ 10*rand(1,1) + (-5);

gy = gy_true+ 10*rand(1,1) + (-5);

gz = gz_true+ 10*rand(1,1) + (-5);

%add measurement noise

tmp = 4*rand(3,1)+(-2);

Z = D_true*G +tmp;

D_true is the rotation matrix generated from quaternion. Matlab provides an existing function quat2eul() to calculate this rotation matrix.

Running the qkf.m gives the following result

The yellow dotted line is the true value, orange dashed line represent measurements and the blue curve represents the estimated value. This shows that the proposed quaternion kalman filter is able to reject noise and maintain a relatively good estimate.

That is all for the quaternion kalman filter. The next part will be about running the actual simulation on matlab.