Complementary filter |

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.datyou 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 2then 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