Complementary and Kalman filter don't work for Y angle

Question

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 and gyro_y_scaled are angular velocities, but they are multiplied by dt, so they give angle. acc_?_scaled are accelerations, but acc_x_angle and acc_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 if sleep(0.007) or sleep(calculatedTimeDifference) is used.

The Y angle filters still return incorrect values.

Answer 1

I've found the answer: a minus sign is needed here: gyro_y_scaled = - (self.result_list[5] / self.gyro_scale).

Explanation: gyro_y_scaled is the velocity in rad/sec. If you check the code, especially lines 29-31 on this page, you can see, there is a minus sign before math.degrees(radians), but my implementation has no minus sign before math.degrees(math.atan2(acc_x_scaled, self.dist(acc_y_scaled,acc_z_scaled))). In addition, self.gyro_y_angle -= gyro_y_scaled * dt, there is also minus sign instead of plus. To sum up, the velocity and the angles, mentioned above, had "opposite" values and this is why the filters didn't work.

Answer 2

I've never used Python before in my life, but I'm a Matlab wizard and an engineer, so I'll just go through point-by-point and highlight things that look questionable to me.

1. What is self.result_list[3]? The linear accelerations are 0-2, are you sure you meant to skip 3? Gyro accelerations are 4-6.
2. I see what you're doing using linear accelerations to generate a rotational acceleration, but keep in mind that technique will only generate valid output if there is no true linear acceleration.
3. Your comp_ calculation leaves off a dt at the end. I'm assuming you want that to be angular velocity, but you are multiplying sample time to only the gyro acceleration and not the accel term - you are adding a velocity and an acceleration and using that as one term.
4. Your Kalman filter doesn't update its covariance matrix correctly. The code to which you linked uses tempP00 and tempP01 to update P00, P01, P10, and P11. You do not, so you are changing P00 and P01 first, then using those new values to update P10 and P11. Refer to step 7 on the site you linked.
5. I would caution against using a dedicated sample time variable like your dt in conjuction with sleep. Your loop polls the sensor and does some calculations, all of which requires time. Say that time is 1ms, and your sample time variable is 10ms (0.01s). Because you pause/sleep for 0.01s and THEN do 1ms of communication and processing, your ACTUAL sample time (time between samples) is 11ms, so the sample time you're using for filtering and numeric integration is off by 10%. It's much better to use an interrupt to time sampling than a sleep command. Here it may not be such an issue, but in the future, with longer code or code with logic that has a variable execution time on a per-sample basis, it could have a bigger impact.

As mentioned I'm no Python user so I can't comment regarding your syntax or execution otherwise.