Kalman filter

Why ?

On the board, we have three accelerometers and three magnetometers. Thanks to FQA algorithm, we can find a first approximation of the attitude (the orientation of the copter coordinate system in comparison with the earth coordinate system). However, sensors are noisy:

Besides, accelerometers are not accurate in movement. Indeed, when there are slow motions, accelerometers measure the opposite of the gravity vector in the coordinate system of the copter, but in movement, they measure also the real acceleration.

To find a good approximation of the attitude, even if we move, we use three gyroscopes. They are precise in fast motions but not very precise in slow motions. Furthermore, these sensors have bias which must be deleted. We must design a filter which takes into account accelerometers and magnetometers at slow speed and gyroscopes at fast speed. The Kalman filter can find a compromise between all these sensors and it can removed the bias of the gyroscopes.

How ?

As it is explained in the FQA article, we have chosen to describe the attitude of the copter thanks to a quaternion q. In addition to the quaternion, we have gyroscopes bias bx, by and bz in our state vector. So our state vector has seven components.

The prediction step takes into account the gyroscopes values (ωx, ωy, ωz) and the previous state to obtain the a priori state. The predicted state is based on the formula :

where

Then the update step uses the values returned by the FQA algorithm to modify the a priori state to obtain the a posteriori state. Since the covariance of our measurement matrix is diagonal, we use the sequential Kalman filter to avoid a matrix inversion in the computation of Kalman gain. We chose the covariance process less significant than the covariance of measurement to give more importance to the process. We can see that the result is very good compared with the FQA algorithm only:

Source: Mathieu Marmion thesis