0
$\begingroup$

The Beta value is set according to the original report, which specifies for IMU B-0.033. and the sensor data acquisition and filter update is periodic at 200Hz.

The angular velocity values from the gyroscope are converted to rads/s and fed to the filter. the resulting quaternion values are used to calculate Euler angles using the below conversion algorithm.

// Euler Angles

// roll (x-axis rotation)

    double sinr_cosp = 2 * (q0 * q1 + q2 * q3);
    double cosr_cosp = 1 - 2 * (q0 * q1 + q2 * q3);
    ROLL = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)

    double sinp = 2 * (q0 * q2 - q3 * q1);
    if (abs(sinp) >= 1)
        PITCH = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        PITCH = asin(sinp);

// yaw (z-axis rotation)

    double siny_cosp = 2 * (q0 * q3 + q1 * q2);
    double cosy_cosp = 1 - 2 * (q2 * q2 + q3 * q3);
    YAW = atan2(siny_cosp, cosy_cosp);

//conversion to degrees

    Eu_Roll = ROLL*(180/M_PI); 
    Eu_Pitch = PITCH*(180/M_PI);
    Eu_Yaw = YAW*(180/M_PI);

Find the attached image of the Roll Pitch and Yaw drifting. [1]: https://i.stack.imgur.com/jguNN.png

$\endgroup$
5
  • $\begingroup$ Do you just use the gyroscope or also an accelerometer or magnetometer? And did you calibrate those sensors? $\endgroup$ – fibonatic Jan 20 at 0:47
  • $\begingroup$ 3 axis Acc and 3 axis Gyro data is used, No magnetometer. The gyro is calibrated for bias correction, 200 samples are averaged and subtracted from the sensor data. the gyro data after calibration looks to be around zero, at rest. $\endgroup$ – Dammu Jan 20 at 10:20
  • $\begingroup$ Acc data is not calibrated, as the filter normalises the vectors, also the raw Az data read around 10 at rest,Ax&Ay around 0. so I figured it is fine. $\endgroup$ – Dammu Jan 20 at 10:28
  • $\begingroup$ Are you writing this yourself or are you using Madgwick's own implementations? You've posted code for the angle conversions but not the Madgwick filter itself. $\endgroup$ – Chuck Jan 20 at 11:34
  • $\begingroup$ I'm using Madgwick's own implementation. $\endgroup$ – Dammu Jan 20 at 11:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.