IMU calibration and drift


I have setup the Nano 33 BLE to read the IMU data and send it over Bluetooth.

The issue I am having is with drift over time. I assume this is coming from the device being uncalibrated. Can anyone shed any light on how you could have some kind of calibration function that can then be used to offset the IMU data that is sent to the Madgwick filter?

Been struggling with this for weeks and would really appreciate the help.

I am connecting this device via web bluetooth so I can stream the data to a browser and applying the transformations to a 3d model. Unfortunately I need accurate data that doesnt drift so I can then take samples for a ML model.

Hope this code helps anyone interested in setting up the basic BLE and madgwick filters


#include <ArduinoBLE.h>
#include <Arduino_LSM9DS1.h>
#include <MadgwickAHRS.h>

// Madgwick
Madgwick filter;
// sensor's sample rate is fixed at 119 Hz:
const float sensorRate = 119;

float roll, pitch, heading;

// BLE Service
BLEService imuService("917649A0-D98E-11E5-9EEC-0002A5D5C51B"); // Custom UUID

// BLE Characteristic
BLECharacteristic imuCharacteristic("917649A1-D98E-11E5-9EEC-0002A5D5C51B", BLERead | BLENotify, 12);

long previousMillis = 0;  // last timechecked, in ms
unsigned long micros_per_reading, micros_previous;

void setup() {
  Serial.begin(115200);    // initialize serial communication

  pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected

  // begin initialization
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);

  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1);

  // Setup bluetooth

  // start advertising
  Serial.println("Bluetooth device active, waiting for connections...");

  // start the filter to run at the sample rate:


  Serial.print("Accelerometer sample rate = ");
  Serial.println(" Hz");
  Serial.println("Acceleration in G's");
  Serial.print("Gyroscope sample rate = ");
  Serial.println(" Hz");
  Serial.println("Gyroscope in degrees/second");
  Serial.print("Magnetic field sample rate = ");
  Serial.println(" uT");
  Serial.println("Magnetic Field in uT");

  micros_per_reading = 1000000 / 119;
  micros_previous = micros();

// send IMU data
void sendSensorData() {

  float ax, ay, az; // Acceleration
  float gx, gy, gz; // Gyroscope
  float mx, my, mz; // Magnometer

  // read orientation x, y and z eulers
  IMU.readAcceleration(ax, ay, az);
  IMU.readGyroscope(gx, gy, gz);
  IMU.readMagneticField(mx, my, mz);

  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); //for all 3
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();
  Serial.print("Orientation: ");
  Serial.print(" ");
  Serial.print(" ");

  // Send 3x eulers over bluetooth as 1x byte array
  float data[3];
  data[0] = heading;
  data[1] = pitch;
  data[2] = roll;
  imuCharacteristic.setValue((byte *) &data, 12);


void loop() {
  // wait for a BLE central
  BLEDevice central = BLE.central();

  // if a BLE central is connected to the peripheral:
  if (central) {
    Serial.print("Connected to central: ");
    // print the central's BT address:
    // turn on the LED to indicate the connection:
    digitalWrite(LED_BUILTIN, HIGH);

    // while the central is connected:
    while (central.connected()) {
      unsigned long micros_now;
      micros_now = micros();

      if (micros_now - micros_previous >= micros_per_reading) {
        if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() && IMU.magneticFieldAvailable()) { // XX
          micros_previous = micros_previous + micros_per_reading;
    // when the central disconnects, turn off the LED:
    digitalWrite(LED_BUILTIN, LOW);
    Serial.print("Disconnected from central: ");

do you have any updates on this?
I'm stuck with the same issue


I am getting some help with writing the calibration function at runtime.

I will post the code on github once its done.

I could imagine a lot of people would require it.


I was able to use Paul Stoffregen’s MotionCal GitHub - PaulStoffregen/MotionCal: Motion Sensor Calibration Tool to calibrate this magnetometer, seemingly with good results. Had to make some small mods and get the axes and units right of course.

I calibrated the gyro by averaging values reported with the device sitting still.

The accelerometer errors seemed small enough that I could not improve them by calibrating.

LSM9DS1/LSM9DS1_MS5611_BasicAHRS_t3.ino at master · kriswiner/LSM9DS1 · GitHub this code also offers some routines to calibrate the magnetometer. It looks like good code but I have not tried to run it.