Mayitzin / ahrs

Attitude and Heading Reference Systems in Python

Home Page:https://ahrs.readthedocs.io/

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Unable to obtain angles in Madgwick

EmreKabadayi opened this issue · comments

Hello. I'm new to sensor fusion and have been trying to use Madgwick filter with accelerometer and gyroscope for two weeks. Unfortunately had no luck with it so far. Despite me moving the IMU module, the angle values I get from q.to_angles() stay the same. Does anyone know this issue? Part of the code that is related to ahrs is given below. I use an MPU6050.
Things to note:

  • Gyroscope values are in Rad/s
  • Frequency was adjusted
  • Accelerometer data on the plot was divided by 100 to fit it in
madgwick= Madgwick(frequency = 10)
arraysize = 50
arr_acc = np.array([accel_xout,accel_yout,accel_zout],dtype = float)
q1 = acc2q(arr_acc)

for t in range(0,arraysize):
    gyro_xrad = gyro_xout * 0.017453292519943
    gyro_yrad = gyro_yout * 0.017453292519943
    gyro_zrad = gyro_zout * 0.017453292519943
    acc_x[t] = accel_xout / 100
    arr_acc = np.array([accel_xout,accel_yout,accel_zout],dtype = float)
    arr_gyr = np.array([gyro_xout,gyro_yout,gyro_zout], dtype = float)
    q1 = madgwick.updateIMU(q1, arr_gyr, arr_acc)
    Q = Quaternion(q1)
    angles = Q.to_angles()
    roll[t] = angles[0]
    pitch[t] = angles[1]
    yaw[t] = angles[2]
    time.sleep(0.1)
plt.plot(x_axis, acc_x, label ="accelerometer x")
plt.plot(x_axis, roll, label ="roll")
plt.plot(x_axis, pitch, label ="pitch")
plt.plot(x_axis, yaw, label ="yaw")
plt.legend()
plt.show()

github_plot_2