Custom Flight Controller Part 2.2: Getting the Orientation --- The Quaternion Kalman Filter
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
- 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
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:
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.