I'm working on a Python script which reads the data from the MPU6050 IMU and returns the angles using sensor fusion algorithms: Kalman and Complementary filter. Here is the implementation:
Class MPU6050 reads the data from the sensor, processes it. Class Kalman is the implementation of the Kalman filter. The problem is the next: None of the Kalman, neither the Complementary filter returns appropriate angle values from the Y angle. The filters work fine on the X angle, but the Y angle values make no sense. See the graphs below. I've checked the code million times, but still can't figure out where the problem is.
class MPU6050():
def __init__(self):
self.bus = smbus.SMBus(1)
self.address = 0x68
self.gyro_scale = 131.072 # 65535 / full scale range (2*250deg/s)
self.accel_scale = 16384.0 #65535 / full scale range (2*2g)
self.iterations = 2000
self.data_list = array('B', [0,0,0,0,0,0,0,0,0,0,0,0,0,0])
self.result_list = array('h', [0,0,0,0,0,0,0])
self.gyro_x_angle = 0.0
self.gyro_y_angle = 0.0
self.gyro_z_angle = 0.0
self.kalman_x = Kalman()
self.kalman_y = Kalman()
def init_sensor()...
def calculate_angles(self):
dt = 0.01
comp_y = 0.0
comp_x = 0.0
print("Reading data...")
while True:
self.read_sensor_raw()
gyro_x_scaled = (self.result_list[4] / self.gyro_scale)
gyro_y_scaled = (self.result_list[5] / self.gyro_scale)
gyro_z_scaled = (self.result_list[6] / self.gyro_scale)
acc_x_scaled = (self.result_list[0] / self.accel_scale)
acc_y_scaled = (self.result_list[1] / self.accel_scale)
acc_z_scaled = (self.result_list[2] / self.accel_scale)
acc_x_angle = math.degrees(math.atan2(acc_y_scaled, self.dist(acc_x_scaled,acc_z_scaled)))
acc_y_angle = math.degrees(math.atan2(acc_x_scaled, self.dist(acc_y_scaled,acc_z_scaled)))
comp_x = 0.95 * (comp_x + (gyro_x_scaled * dt)) + 0.05 * acc_x_angle
comp_y = 0.95 * (comp_y + (gyro_y_scaled * dt)) + 0.05 * acc_y_angle
kalman_y_angle = self.kalman_y.filter(acc_y_angle, gyro_y_scaled, dt)
kalman_x_angle = self.kalman_x.filter(acc_x_angle, gyro_x_scaled, dt)
self.gyro_x_angle += gyro_x_scaled * dt
self.gyro_y_angle -= gyro_y_scaled * dt
self.gyro_z_angle -= gyro_z_scaled * dt
time.sleep(dt)
def read_sensor_raw(self):
self.data_list = self.bus.read_i2c_block_data(self.address, 0x3B, 14)
for i in range(0, 14, 2):
if(self.data_list[i] > 127):
self.data_list[i] -= 256
self.result_list[int(i/2)] = (self.data_list[i] << 8) + self.data_list[i+1]
def dist(self, a,b):
return math.sqrt((a*a)+(b*b))
class Kalman():
def __init__(self):
self.Q_angle = float(0.001)
self.Q_bias = float(0.003)
self.R_measure = float(0.03)
self.angle = float(0.0)
self.bias = float(0.0)
self.rate = float(0.0)
self.P00 = float(0.0)
self.P01 = float(0.0)
self.P10 = float(0.0)
self.P11 = float(0.0)
def filter(self, angle, rate, dt):
self.rate = rate - self.bias
self.angle += dt * self.rate
self.P00 += dt * (dt * self.P11 - self.P01 - self.P10 + self.Q_angle)
self.P01 -= dt * self.P11
self.P10 -= dt * self.P11
self.P11 += self.Q_bias * dt
S = float(self.P00 + self.R_measure)
K0 = float(0.0)
K1 = float(0.0)
K0 = self.P00 / S
K1 = self.P10 / S
y = float(angle - self.angle)
self.angle += K0 * y
self.bias += K1 * y
P00_temp = self.P00
P01_temp = self.P01
self.P00 -= K0 * P00_temp
self.P01 -= K0 * P01_temp
self.P10 -= K1 * P00_temp
self.P11 -= K1 * P01_temp
return self.angle
EDIT: I've added some information based on @Chuck's answer:
self.result_list[3]
contains the temperature- In my opinion the compl. filter is implemented correctly:
gyro_x_scaled
andgyro_y_scaled
are angular velocities, but they are multiplied bydt
, so they give angle.acc_?_scaled
are accelerations, butacc_x_angle
andacc_x_angle
are angles. Check my comment, where the Complementary filter tutorial is. - Yes, there was something missing in the Kalman filer, I've corrected it.
- I totally agree with you,
sleep(dt)
is not the best solution. I've measured how much time the calculation takes, and it is about 0.003 seconds. The Y angle filters return incorrect values, even ifsleep(0.007)
orsleep(calculatedTimeDifference)
is used.
The Y angle filters still return incorrect values.
gyro_x_scaled * dt
is an angle andacc_x_angle
is also an angle $\endgroup$ – Alex Aug 9 '15 at 9:12