MPU6050 rotation problem using MahonyAHRS

Hi, guys. Recently, I started the IMU rotation recently, but now I’am trapped by the IMU’ rotation in z-axis(gravity direction) after I’ve checked every impossible place caused that problem, the implement is as follows:

  1. MPU6050 board, 100hz;
  2. the Mahony algorithm of 9-axisdata2quaternion and the formular of quaternion2agle in “MATLAB code“ which x-io provided;
    www.x-io.co.uk/open-source-imu-and-ahrs-algorithms
  3. the unit of input data of the algorithm is the same as the data which “MATLAB code“ provided;
  4. Ki=0 & Kp=10, which is a reference when the sample frequency is 100hz.

The problem is as follows:

  1. when rotate IMU in x-axis from -90° to 90°, the angle in z-axis change from -40° to 40°, but in the data which “MATLAB code“ provided, the angle in z-axis is almost constant;
  2. when rotate IMU in z-axis, the rotation angle calculated is unstable. Sometime, it can be fast calculated but show less or larger angle than real value; sometimes, it took time to converge.

I’ve seen the raw data, the wave shape of accelerometer and gyroscope in x-, y, and z-axis is same as the data which “MATLAB code“ provided. But the wave shape of magnetometer have some different place compared the data which “MATLAB code“ provided. I don’t know if the difference caused the problem.
I don’t know how put the picture of the data in this topic so I just put it into the attachment. The upper figure show magnetometer data of the IMU which I used, the nether figure show the magnetometer data which “MATLAB code“ provided.
If I lost something to describe this problem, please tell me. And I really hope someone can help me to fix it.‍

![)}7%QFOEU}({2GQK%4I%`N.png|795x432