My Error to calculate quaternion rate with Python

1

Happy new year!!!

I am calculate quaternion rate with Python.But there are problems and I dont know why.I think there must be something wrong with IMU update.
My calculated and measured values just like this picture.The calculated value is a solid line and the measured value is a dashed line.

QQ截图20190101211003.png458×879 40.1 KB

My IMU update code just like this. Before the calculation, I used a low-pass filter with Acc and Gyro.

def update(self, gyro, acc):
    gyro = np.matrix(gyro)
    acc = np.matrix(acc)
    q_matrix = np.dot(self.q.T,self.q)
    acc = acc / np.sqrt(np.sum(np.multiply(acc,acc)))
    
    
    vx = 2*(q_matrix[1,3] - q_matrix[0,2])
    vy = 2*(q_matrix[0,1] + q_matrix[2,3])
    vz = q_matrix[0,0] - q_matrix[1,1] - q_matrix[2,2] + q_matrix[3,3]
    
    
    ax = acc[0,0]
    ay = acc[0,1]
    az = acc[0,2]
    ex = ay * vz - az * vy
    ey = az * vx - ax * vz
    ez = ax * vy - ay * vx
    
    
    self.exInt += ex * self.Ki
    self.eyInt += ey * self.Ki
    self.ezInt += ez * self.Ki
    
    
    gx = gyro[0,0]
    gy = gyro[0,1]
    gz = gyro[0,2]
    gx += self.Kp * ex + self.exInt
    gy += self.Kp * ey + self.eyInt
    gz += self.Kp * ez + self.ezInt
    
    
    q0t = self.q[0,0]
    q1t = self.q[0,1]
    q2t = self.q[0,2]
    q3t = self.q[0,3]
    self.q[0,0] = q0t + (-q1t*gx - q2t*gy -q3t*gz) * self.halfT
    self.q[0,1] = q1t + (q0t*gx + q2t*gz -q3t*gy) * self.halfT
    self.q[0,2] = q2t + (q0t*gy - q1t*gz +q3t*gx) * self.halfT
    self.q[0,3] = q3t + (q0t*gz + q1t*gy -q2t*gx) * self.halfT
    self.q = self.q / np.sqrt(np.sum(np.multiply(self.q, self.q)))
    
    q0 = self.q[0,0]
    q1 = self.q[0,1]
    q2 = self.q[0,2]
    q3 = self.q[0,3]
    
    roll = math.atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2))
    pitch = math.asin(2*(q0*q2 - q3*q1))
    yaw = math.atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3))
2

my Kp, Ki,halfT just like this:

    self.Kp = 2
    self.Ki = 0.005
    self.halfT = 1/(2*sample_freq)
3

Is there anyone who can help me?thank you

4

This is hard to wrap my head around it. It would help if you describe what exactly you’re trying to do. So what are the inputs, what are you trying to calculate, what are you expecting, and what are you actually seeing?

5

I am happy to see you replay.And I’m sorry I didn’t describe the problem properly.I want create Euler angles from the quaternion.My update function is calculate quaternion from Gyro and Acc data.This sensor data from the PX4 log.So:

1、The input of update is Acc and Gyro data from PX4 log，and the output is Euler angles
2、In the Picture,the dashed line is Euler angles read from PX4 log,the solid line is my calculate

Now I’ve solved half the problem.I find that the acceleration is in the opposite direction which from PX4 log.
When I fixed the problem, it was much better,but not good enough，just like this picture.

20190102185535.png468×874 48.7 KB

pitch Angle and Roll angle are still have some small errors, but Yaw angle have big error.

6

I suspect the margin of Yaw error is very large because there is offset gyro in Z axis.So I minus the params of “CAL_GYRO0_ZOFF” read from QGC.But still have the problem.

7

Aha. So if you want to estimate the yaw angle, you also need to take the mag into consideration, otherwise your estimate will drift over time. Most likely that’s the error you’re seeing for yaw.