Saturday, 16 November 2013

Using a complementary filter to combine Accelerometer and Gyroscopic data

This post shows how to combine data from the accelerometer and gyroscope using a complementary filter to produce a better readings from the MPU-6050.

Complementary filter
The image above shows data for a negative rotation around the Y axis followed by a positive rotation around the X axis.  It includes the base accelerometer, gyroscope and the filtered data.  Lets look at things in a bit more detail.

The following graph show a simple rotation in X of roughly 90-100 degrees (I didn't measure it accurately).  The red line shows the accelerometer data and as we can see from the spikes it's a noisy data set.  The green line show the rotation angle calculated from summing the individual angles read from the gyroscope.  While this data is less noisy it is prone to drift over time, the gyroscope doesn't return back to zero when not moving.

The blue line shows the complementary filter at work.  It combines the two data sets by merging fast rotations from the gyroscope with the slower trends from the accelerometer and we get the best of both worlds. For a full explanation of the theory behind this type of filter I recommend reading this excellent paper.  If you just want some simple code then read on.

The interesting parts are lines 76 to 91. First we read all the scaled data from the device. Adjust the gyroscope data by the offset. The offset is the value of the gyroscope reading when it's not moving and is taken from the very first reading. This is fine for simple testing but ideally a true offset value should be determined by calibrating the sensor.
Now calculate the gyroscope delta, this is how much the sensor has rotated since the last sample was taken and then add it to a running total This gives us a rotation angle just from reading from the gyroscope (the green line in the graph above)
Next read the rotation values from the accelerometer just like we did in the previous post Now the complementary filter is used to combine the data.
We take the previous readings (last_x, last_y) and add in the gyroscope data then scale this by K, then add in the accelerometer data scaled by K1 and this value is our new angle. The coefficients K and K1 should add up to 1, in this case they are 0.98 and 0.02 respectively. You can change the values of K and K1 to suit your application as described in the previously linked article. The time intervals for the loop needs to be reasonably accurate for this to work well and the sample rate should be 100Hz or higher.

If you run the code and direct the output to a file
    sudo ./ > plot.dat
you can then generate gnuplot diagrams similar to those above, save the following to a file gnuplot-command.plg
    set terminal wxt persist size 800,600 background '#000000' # enhanced font 'Consolas,10' 

    set style line 99 linecolor rgb "#ffffff" linetype 0 linewidth 2
    set key top right textcolor linestyle 99 
    set grid linestyle 99
    set border linestyle 99

    set xlabel "time (s)" textcolor linestyle 99
    set ylabel "degrees" textcolor linestyle 99

    set yrange [-180:180]

    plot filename using 1:2 title "Accelerometer X" with line linewidth 2 , \
      filename using 1:3 title "Gyroscope X" with line linewidth 2 , \
      filename using 1:4 title "Filter X" with line linewidth 2
    plot filename using 1:5 title "Accelerometer Y" with line linewidth 2 , \
      filename using 1:6 title "Gyroscope Y" with line linewidth 2 , \
      filename using 1:7 title "Filter Y" with line linewidth 2 
then to generate a graph
    gnuplot -e "filename='plot.dat'" gnuplot-command.plg

In the next post I show how to hook up a HMC5883L Compass module and incorporate it into the code so we can get a true bearing.  Well that is when I get a new one as I seem to have fried mine.