During my last year, I have been working with affordable autonomous drones and the biggest problem has always been having an accurate heading. The aim was to build a drone able to drive around by itself keeping the price as low as possible.

The Problem

I needed a way to know in which direction the drone is heading, and after a fast google search for less than 50$ I bought a "BNO055 Absolute Position Sensor", or "APS" as I call it, which should fuse magnetometer, gyroscope, and accelerometer to estimate the 3d position of the sensor in the space.
This worked really well, until with took the drone for a ride outside where the heading started shifting reaching errors up to 90 degrees.

The Failed Attempts

Storing the calibration data

The BNO055 it's quite nice to work with (except for the wrong heading), using the Adafruit library to communicate with the APS it is really easy to read the calibration status of each sensor and it's even possible to save and load calibration data.
The calibration goes from 0 which means useless readings to 3. Usually, after connecting the APS and moving it a bit to calibrate it, it would always reach 3 and the heading would be correct. This made me think that if I store the calibration data when the calibration is at 3 and reload them when the calibration goes down it would fix the heading.
This worked in the office but it was useless outside where the calibration would not rise to 3 when reloading the calibration data.

Automatic Calibration System (ACS)

After the previous failed attempt i noticed that rotating the senor on the 3 axis by 90 degree was enough to calibrate the magnetometer and to have an accurate heading so after a joking about it with some colleague I ended up building a mechanism to move the sensor on 2 axes using 2 servo motor since the drone could rotate on the 3rd axis.
This solution gave us high hope and it was working wonderfully in the office, taking the calibration from 0 to 3 in half-second, unfortunately even this was failing while the drone was moving, the weirdest part was that the calibration would rise to 3 but the heading was still incorrect.

These are the two ACS design I tested without success.

The solution

After giving up trying to fix the sensor I noticed that the GPS can give you a really accurate heading when the drone is moving but it's totally useless if the drone it's rotating in place, so we ended up merging the heading from the GPS with the heading given by the APS to have a reliable reading.

Kalman filter

From Wikipedia "The Kalman filter is an algorithm which uses a series of measurements observed over time and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone"
I'm not going to explain you the math behind the Kalman filter but you can easily find it online, I will instead explain how I use it to merge the GPS and APS heading.

The Kalman filter has 2 steps, a prediction step, where it takes as input some sensor data which can be used to predict X given the previous value of X, and an update step, where it takes as input the value of X with some noise and it uses it to fix the prediction. The filter will always produce the most likely value of X.

How to apply it

To be able to use the Kalman filter we need an unknown variable X, which will be our heading, a noisy reading of X, which will be the GPS heading, and some sensor which can be used to calculate the next heading knowing the previous one.
For this last part, we will use the APS, but we will not use the heading directly, since it is subject to shifting, instead, we will use the difference in heading between two consecutive reading as a rotation, this can be added to the previous heading to estimate the next one.

Practical example

For my project, I used a Kalman filter implementation found on Github
https://github.com/zziz/kalman-filter

As you can see the Kalman filter is controlled by 7 matrices:

x is the unknown variable we are trying to estimate using Kalman filter, x0 is the initial value we want to assign to it, in our case since the heading is a single value we can give it [[0]] or leave it empty.

F and B are used to calculate the new value of x during the prediction phase using this formula x = F*x + B*u, where u is the value read from the sensor, in our case it will be how much the compass value has changed since the last Kalman filter prediction. This makes things easy because F and B can be identity matrices ([[1]]) which will make the formula x = x+u

H is used to calculate the error between the Kalman prediction and the real value z during the update phase using this formula: e = z - H*x as before in our case we don't need to modify the shape of x so we can use an identity matrix [[1]]

Q R and P are covariances matrices used to indicate how accurate are our sensors and calculation, where R indicate the covariance on the sensor reading while Q and P indicate the error in the calculation process. For our purpose we can set P to [[0]], Q to [[0.1]] to indicate that our calculation given the correct data are pretty accurate while we can test different values for R to give more or less confidence to the GPS reading.

Code

F = np.array([[1]])
B = np.array([[1]])
H = np.array([[1]])
Q = np.array([[0.1]])
R = np.array([[50]])
P = np.array([[0]])
kf = KalmanFilter(F=F, B=B, H=H, Q=Q, R=R, P=P)

old_heading = compass.getHeading()
counter = 0

while True:
    counter += 1

    new_heading = compass.getHeading()
    delta_heading = new_heading - old_heading
    old_heading = new_heading

    predicted_heading = kf.predict(delta_heading)[0][0]

    # normalize heading
    predicted_heading = predicted_heading % 360
    kf.x = predicted_heading

    # read GPS heading only once per second since
    # the sensor has 1 Hz update rate
    if counter % 10:
        gps_heading = gps.getHeading()

        # since the heading goes from 359 to 0 
        # this make the error always < 180 between
        # the predicted heading and the gps heading
        if gps_heading - predicted_heading > 180:
            gps_heading -= 360
        if gps_heading - predicted_heading < -180:
            gps_heading += 360

        kf.update(np.array[[gps_heading]])

    time.sleep(0.1)

Theoretically, this should work but the real world doesn't care, so we need some more tweaks. The problem with the GPS is that if we are not moving the heading is random and it also takes some times to realize we are moving. To fix this I change the last if as follow

# read GPS heading only once per second since
# the sensor has 1 Hz update rate
if is_drone_going_straght and gps.getSpeed() > 0.5 and counter % 10:
    gps_heading = gps.getHeading()

    # since the heading goes from 359 to 0 
    # this make the error always < 180 between
    # the predicted heading and the gps heading
    if gps_heading - predicted_heading > 180:
        gps_heading -= 360
    if gps_heading - predicted_heading < -180:
        gps_heading += 360

    kf.update(np.array[[gps_heading]])

time.sleep(0.1)

where is_drone_going_straight is a boolean calculate using the value that we are sending to the engines, so if the values are both positive and very similar it means we are going straight.
Using this we got a quite reliable heading sensor which works even if the compass starts shifting.

We did some test on the field with the drone using this algorithm and adding random shifts to the compass data to simulate a bad reading and it's possible to see the drone start moving in the wrong direction and going back to the original position after just a few seconds.