Cálculo de los ángulos de Euler. Pero obtiene un cambio (delta) en cambio y no un valor constante

Lo que he hecho ahora:

Estoy usando una IMU (MPU6000) para calcular los ángulos de Euler de un objeto y para esto estoy usando Madgwick algoritmo AHRS. La MPU da una aceleración debido a la gravedad (a) sobre el eje y la velocidad angular (g) sobre el eje y alimento estos valores para calcular los cuaterniones que se convierten a los ángulos de Euler utilizando esta función que se encuentra en Wikipedia .

Entonces, ¿qué está mal?

1. Estoy haciendo que los ángulos de Euler sean de giro de inclinación: 0.001518, 0.001626, 0.003990. Esto no está en grados o luminosidad. ¿Cuál es la dimensión aquí?
   Me estoy alimentando, el acelerómetro lee sobre cada eje (por lo tanto, m / s ^ 2) y, después de calcular el ángulo de rotación desde el giro, el valor theta (en grados) sobre cada eje.

   MadgwickAHRSupdateIMU((float)tx, (float)ty, (float)tz,(float)ax, (float)ay, (float)az);
   

   donde tx, ty, tz son el ángulo y ax, ay, az están en G.
   tx = gx * timestep; donde gx es la lectura cruda del giroscopio. Y el paso del tiempo es el tiempo entre las lecturas del giro.