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 The Balance Filter by Shane Colton.  If you just want some simple code then read on.

The interesting parts are lines 76 to 91.
#!/usr/bin/python

import smbus
import math
import time

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

gyro_scale = 131.0
accel_scale = 16384.0

address = 0x68  # This is the address value read via the i2cdetect command

def read_all():
    raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
    raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)

    gyro_scaled_x = twos_compliment((raw_gyro_data[0] << 8) + raw_gyro_data[1]) / gyro_scale
    gyro_scaled_y = twos_compliment((raw_gyro_data[2] << 8) + raw_gyro_data[3]) / gyro_scale
    gyro_scaled_z = twos_compliment((raw_gyro_data[4] << 8) + raw_gyro_data[5]) / gyro_scale

    accel_scaled_x = twos_compliment((raw_accel_data[0] << 8) + raw_accel_data[1]) / accel_scale
    accel_scaled_y = twos_compliment((raw_accel_data[2] << 8) + raw_accel_data[3]) / accel_scale
    accel_scaled_z = twos_compliment((raw_accel_data[4] << 8) + raw_accel_data[5]) / accel_scale

    return (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z)
    
def twos_compliment(val):
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a, b):
    return math.sqrt((a * a) + (b * b))


def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

bus = smbus.SMBus(0)  # or bus = smbus.SMBus(1) for Revision 2 boards

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

now = time.time()

K = 0.98
K1 = 1 - K

time_diff = 0.01

(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()

last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

gyro_offset_x = gyro_scaled_x 
gyro_offset_y = gyro_scaled_y

gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y

print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (last_x), gyro_total_x, (last_x), (last_y), gyro_total_y, (last_y))

for i in range(0, int(3.0 / time_diff)):
    time.sleep(time_diff - 0.005) 
    
    (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
    
    gyro_scaled_x -= gyro_offset_x
    gyro_scaled_y -= gyro_offset_y
    
    gyro_x_delta = (gyro_scaled_x * time_diff)
    gyro_y_delta = (gyro_scaled_y * time_diff)

    gyro_total_x += gyro_x_delta
    gyro_total_y += gyro_y_delta

    rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
    rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

    last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
    last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
    
    print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (rotation_x), (gyro_total_x), (last_x), (rotation_y), (gyro_total_y), (last_y))
First we read all the scaled data from the device.
    (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
Adjust the gyroscope data by the offset.
    gyro_scaled_x -= gyro_offset_x
    gyro_scaled_y -= gyro_offset_y
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
    gyro_x_delta = (gyro_scaled_x * time_diff)
    gyro_y_delta = (gyro_scaled_y * time_diff)

    gyro_total_x += gyro_x_delta
    gyro_total_y += gyro_y_delta
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 
    rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
    rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
Now the complementary filter is used to combine the data.
    last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
    last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
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 ./filter-test.py > 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.

No comments:

Post a comment

Temperature logging with a DS18B20 and a Raspberry Pi

I wanted to do some temperature logging so I hooked up a DS18B20 temperature sensor to a Raspberry Pi. About the DS18B20 Dallas DS18B...