Measuring angle position with IMU on Arduino Nano 33 IoT not working properly

Hi, I am using Arduino Nano 33 IoT’s IMU to measure the angle position(roll, yaw, pitch). I am using MadgwickAHRS.h library and following code for angle measurement:

#include <Arduino_LSM6DS3.h>
#include <MadgwickAHRS.h>
// initialize a Madgwick filter:
Madgwick filter;
// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;
void setup() {
  // attempt to start the IMU:
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU");
    // stop here if you can't access the IMU:
    while (true);
  // start the filter to run at the sample rate:
void loop() {
  // values for acceleration & rotation:
  float xAcc, yAcc, zAcc;
  float xGyro, yGyro, zGyro;
  // values for orientation:
  float roll, pitch, heading;
  // check if the IMU is ready to read:
  if (IMU.accelerationAvailable() &&
      IMU.gyroscopeAvailable()) {
    // read accelerometer & gyrometer:
    IMU.readAcceleration(xAcc, yAcc, zAcc);
    IMU.readGyroscope(xGyro, yGyro, zGyro);
    // update the filter, which computes orientation:
    filter.updateIMU(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc);
    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(" ");
    Serial.print(" ");
  if (pitch > 20 && pitch < 50) {
    digitalWrite(LED_BUILTIN, HIGH);
    digitalWrite(LED_BUILTIN, LOW);

(The code is from this website :

I’m trying to figure out which angle those three values(heading, pitch, roll) are representing. It seems to be changing inconsistently when I rotate the board. Can someone explain to me how this code is working, and what those three values mean?


Just glancing through that code suggests to me that whoever put it together really does not understand the concepts, and it was never tested properly. It won't ever work well as it stands. Did you notice that the authors don't bother to tell you how the angles are defined? I doubt they know.

First, you can't measure the yaw angle, using just accelerometer and rate gyro data. The best you can do is estimate a relative yaw angle from the gyro data, starting from wherever the sensor was pointed when it was booted up.

Second, AHRS functions don't work well if they do not run fast. The gyro integration step introduces large errors if the time step is large, and in this case, it will be large because the program prints a bunch useless stuff every time through the loop, slowly, at 9600 Baud. This may not be a problem with a USB connection to a PC, but it IS a problem if you print through the UART serial port.

I suggest to modify the program to print angles only every elapsed second, or maybe every 50th pass through the loop, using at least 115200 Baud: i.e. Serial.begin(115200);

   Serial.print("Orientation: "); //delete this line
    Serial.print(" ");
    Serial.print(" ");

Third, and most importantly, you have to calibrate the gyro or the measurements are meaningless. There is usually a large offset on each axis when the sensor is held still (which you can verify by just examining the raw gyro data) and these MUST be estimated, and subtracted from the raw gyro measurements, in order for the AHRS to work at all.

Finally, if you are interested only in measuring (relatively) stationary tilt and roll angles about the accelerometer x and y axes, the accelerometer data are all you need. This is the calculation, using the raw accelerometer measurements xa, ya and za (float values). Start with the sensor flat, with Z up.

// formula from
  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied

  Serial.print("roll = ");
  Serial.print(", pitch = ");