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()