Filtering the Signal

The MPU-6050 accelerometer produces an accurate measure of the acceleration but is prone to noise. Trying to control the T-Bot using this signal alone will result in a very agitated robot. On the other hand, the integrated gyroscope signal is very smooth but drifts as errors accumulate. The T-Bot will simply fall over if you try this alone. We need to combine the signals. There are some very sophisticated ways of doing this. The most common way is to use a linear quadratic estimater or Kalman Filter. This is a very elegant filter and was used in the navigation system for the Apollo mission to the Moon.

An explanation of the Kalman filter can be found here and an implementation of the Kalman filter by Kristian Lauszus for the Arduino can be found here. However, the T-Bot uses a much simpler filter which, in this case, is just as effective. You can use the python code below the compare the two (CombinationFilter.py).

We will assume you already know how to run a python script. If not, there are numerous YouTube tutorials available. If you would like to use an integrated development environment (IDE), you can try Spyder or Geany. This works very well on the Raspberry Pi.

Setup Python Code

Import libraries. You will need to have Numpy and Matplotlib installed on your system.

import numpy as np
import matplotlib.pyplot as plt
plt.ion()

Load the data extracted from the T-bot (See Extracting Data From T-Bot tutorial) You can skip extracting the data if you want to use the T-Bot_FilteredData.dat file included with the downloaded software.

v1 = np.loadtxt('T-Bot_FilteredData.dat')
v1[np.where(v1[:,0]<0.001),0]=0.013

The Simple Combination Filter only has one parameter to set.

################   Simple Combination Filter    ################
'''
This code serves as a guide to show how the filters work. 
A filter_weighting of 0 zero will be equivalent to using
the gyro only. A value of 1 will be equivalent to using the accelerometer  
only. You can use serial_GetData.py to collect the data from the T-BOT
to see how effective your filter is.
'''

angle = 0
filter_weighting = 0.04

def getAngleCFilter(pitch, gyro_rate, dt):
    global angle
    angle += gyro_rate * dt
    angle += filter_weighting * (pitch - angle)
    return angle

The Kalman has five parameters to set.

####################   Kalman Filter    ########################
''' 
Based on Arduino code written by Kristian Lauszus 2012
                 TKJ Electronics.
'''

# User parameters
bias = 0
R_measure = 0.15 # measurement noise
Q_angle = 0.1 # process noise 
Q_bias = 0.3 # 
R_measure = 1 # measurement noise


# create arrays

P = np.zeros((2,2))
K = np.zeros(2)


def getAngle(pitch, gyrorate, dt):
    global bias
    global angle
    global P
    rate = gyrorate - bias;
    angle += dt * rate;
    # Update estimation error covariance - Project the error covariance ahead
    P[0,0] += dt * (dt*P[1,1] - P[0,1] - P[1,0] + Q_angle)
    P[0,1] -= dt * P[1,1]
    P[1,0] -= dt * P[1,1]
    P[1,1] += Q_bias * dt

    # Calculate Kalman gain - Compute the Kalman gain
    S = P[0,0] + R_measure
    K[0] = P[0,0] / S
    K[1] = P[1,0] / S
    y = pitch - angle

    #Calculate angle and bias - Update estimate with measurement zk (newAngle)
    angle += K[0] * y
    bias += K[1] * y

    #Calculate estimation error covariance - Update the error covariance
    P[0,0] -= K[0] * P[0,0]
    P[0,1] -= K[0] * P[0,1]
    P[1,0] -= K[1] * P[0,0]
    P[1,1] -= K[1] * P[0,1]
    return angle

Now we can calculate the filtered angles using the Simple Combination Filter and the Kalman Filter.

################## Get filterd angle  ##########################

# Simple combination filter
angleCF = np.array([getAngleCFilter(v1[x,1],v1[x,2],v1[x,0]) for x in range(v1.shape[0])])

# Kalman Filter
angle = 0 # reset global value back to 0
angleKF = np.array([getAngle(v1[x,1],v1[x,2],v1[x,0]) for x in range(v1.shape[0])])

# Integrated Gyro Signal (Not Filtered)
gyroangle = np.cumsum(v1[:,2]*v1[:,0])

# integrated dt to give t for the x axis
t = np.cumsum(v1[:,0])

Now we can plot the data!

###############   Plot the data  ########################

plt.figure(figsize=(10, 8))

ax = plt.subplot(211)
plt.title('On-board Processing')
ax.plot(t, v1[:,1], c=(91/255.,111/255.,189/255.),label = 'Measured Pitch')
ax.plot(t, v1[:,4], c=(56/255.,192/255.,255/255.),label = 'Unfiltered Gyro Angle from T-Bot')
ax.plot(t, v1[:,3], c=(255/255.,0/255.,0/255.),label = 'Filtered Angle by T-Bot')
ax.legend(loc = 'best',prop={ 'size': 8})
plt.xlabel('t (s)')
plt.ylabel('angle (deg)')
plt.axis('tight')
ax = plt.subplot(212)
plt.title('From Python Code')
ax.plot(t, v1[:,1],  c=(91/255.,111/255.,189/255.),label = 'Measured Pitch')
ax.plot(t, gyroangle, c=(56/255.,192/255.,255/255.),label = 'Unfiltered Gyro Angle')
ax.plot(t, angleKF, 'g',label = 'Kalman Filter',linewidth=2)
ax.plot(t, angleCF, 'r--',label = 'Combination Filter',linewidth=2)

ax.legend(loc = 'best',prop={ 'size': 8})
plt.xlabel('t (s)')
plt.ylabel('angle (deg)')
plt.axis('tight')
plt.subplots_adjust(hspace=0.3)
plt.show()