Chapter 10: Madgwick AHRS Attitude Estimation

10.1 Key Points

• Quaternion attitude representation
• Accelerometer and gyroscope fusion
• Madgwick 6-DOF AHRS update flow
• Converting quaternion to Euler angles

10.2 Course Content

The gyroscope provides angular velocity but drifts over time. The accelerometer gives a gravity reference but is noisy during motion. Madgwick AHRS fuses both sensors to estimate roll, pitch, and yaw-related attitude information.

10.3 Basic Learning

Why Use Quaternions

Euler angles are easy to read but suffer from singularities. Quaternions avoid gimbal lock and are efficient for continuous attitude updates.

A quaternion contains four components:

q = [q0, q1, q2, q3]

AHRS Pipeline

1. Read accelerometer and gyroscope data.
2. Normalize the accelerometer vector.
3. Convert gyroscope units to radians per second.
4. Run the Madgwick update step.
5. Normalize the quaternion.
6. Convert the quaternion to roll, pitch, and yaw for display or control.

10.4 Program Study

Typical update loop:

qmi8658_data_t imu;
if (qmi8658_read(&imu)) {
    madgwick_update_imu(&ahrs,
                        imu.gx * DEG_TO_RAD,
                        imu.gy * DEG_TO_RAD,
                        imu.gz * DEG_TO_RAD,
                        imu.ax, imu.ay, imu.az,
                        dt);
}

Quaternion to Euler conversion:

roll  = atan2f(2.0f * (q0*q1 + q2*q3),
               1.0f - 2.0f * (q1*q1 + q2*q2));
pitch = asinf(2.0f * (q0*q2 - q3*q1));
yaw   = atan2f(2.0f * (q0*q3 + q1*q2),
               1.0f - 2.0f * (q2*q2 + q3*q3));

10.5 Summary

This chapter introduced IMU fusion with the Madgwick algorithm. The result is a stable attitude estimate that can be used for logging, control, or future robot behavior extensions.

---