LSM9DS1 Madgwick fusion code, what did I do wrong here..?

young990729 · March 19, 2024, 5:05pm

Hi guys
To do the deadreckoning task with my imu, which is lsm9ds1 (NWU) from adafruit
I wrote the code about sensor fusion to get the correct orientation of my imu
and tried plotting the movement in python to do the deadreckoning
HOWERER, even after I put my imu on the desk and got the x, y, and z acceleration and quaternions via csv to put all in Python, after I plotted it, the dot, which is the position point, i guess, moved to the positive z-axis.
So, I think my orientation code is not perfect...
I did calibration for acc, gyro and magnetometer (using motioncal software)
I attach the code below so anyone gives the hint.. It would be really helpful to me
Thanks!!

#include <Wire.h>
#include <Arduino.h>
#include <Adafruit_LSM9DS1.h>
#include <math.h>
#define betaDef 0.6f


float declination = 9.0;

float pitch, yaw, roll;
float invSqrt(float x);

float deltat = 0.0f;        // integration interval for both filter schemes
uint32_t lastUpdate = 0; // used to calculate integration interval
uint32_t Now = 0;        // used to calculate integration interval
float freq = 0.0f;
// Timing 

const unsigned long interval = 1000000 / 70; // 
unsigned long lastUpdateTime = 0; // 마지막 업데이트 시간
unsigned long startTime = 0; // 데이터 로깅을 시작한 시점
bool loggingStarted = false; // 데이터 로깅 시작 여부

Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

//--------calibration values
float AccErrorX =  -0.12;
float AccErrorY =  0.03;
float AccErrorZ =  0.02;
float gyroErrorX = -0.22;
float gyroErrorY = 0.04;
float gyroErrorZ = -0.02;

//---------container for the acc,mag and gyro
float ax1, ay1, az1, gx1, gy1, gz1, mx1, my1, mz1, mx2, my2, mz2;


//quaternion container
volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;


void setupSensor()
{
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
// 2.) Set the magnetometer sensitivity
lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);

// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}



//------ vector math
float vector_dot(float a[3], float b[3])
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}


void vector_normalize(float a[3])
{
float mag = sqrt(vector_dot(a, a));
a[0] /= mag;
a[1] /= mag;
a[2] /= mag;
}


void setup()
{

Serial.begin(115200);
while (!Serial) {
 delay(1); // will pause Zero, Leonardo, etc until serial console opens
}
Serial.println("LSM9DS1 data read demo");
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
 Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
 while (1);
}
Serial.println("Found LSM9DS1 9DOF");
// helper to just set the default scaling we want
setupSensor();
}



void loop(){

  if (Serial.available()) {
      char rx_char = Serial.read();
      if (rx_char == 'g') {
        loggingStarted = true;
        startTime = micros();
      }
    }

  if (loggingStarted && (micros() - lastUpdateTime >= interval)){
    lastUpdateTime = micros();
    double elapsedTime = (lastUpdateTime - startTime) / 1000000.0;



    lsm.read();
    sensors_event_t a, m, g, temp;
    lsm.getEvent(&a, &m, &g, &temp);

    static float gxyz[3], axyz[3], mxyz[3];

    //acceleration value calculation in g's
    ax1 = (a.acceleration.x - AccErrorX)*(1/9.807);//m/s^2 into g
    ay1 = (a.acceleration.y - AccErrorY)*(1/9.807);//g
    az1 = (a.acceleration.z + 0.02)*(1/9.807);//g
    axyz[0] = ax1;
    axyz[1] = ay1;
    axyz[2] = az1;


    vector_normalize(axyz);

    //gyro reading
    gx1 = (g.gyro.x) - gyroErrorX;
    gy1 = (g.gyro.y) - gyroErrorY;
    gz1 = (g.gyro.z) - gyroErrorZ; //radian

    gxyz[0] = gx1;
    gxyz[1] = gy1;
    gxyz[2] = gz1;

      //magnetometer reading milligauss unit
      //Magnetometer calibration part is the most important to 
      //get the clean output

      mx1 = (m.magnetic.x + 4.85); //Hard iron offset calibrated
      my1 = (m.magnetic.y - 6.82);
      mz1 = (m.magnetic.z + 11.88);


      float corrected_V[] = {mx1, my1, mz1};
      //Soft iron offset calibrated
      float softIron_cali[3][3] = {
      {0.991, 0.029, 0.005},
      {0.029, 0.961, -0.001},
      {0.005, -0.001, 1.044}
      };


      float result_V[3]; // To store the result of the matrix multiplication


      // Perform matrix multiplication
      result_V[0] = corrected_V[0] * softIron_cali[0][0] + corrected_V[1] * softIron_cali[0][1] + corrected_V[2] * softIron_cali[0][2];
      result_V[1] = corrected_V[0] * softIron_cali[1][0] + corrected_V[1] * softIron_cali[1][1] + corrected_V[2] * softIron_cali[1][2];
      result_V[2] = corrected_V[0] * softIron_cali[2][0] + corrected_V[1] * softIron_cali[2][1] + corrected_V[2] * softIron_cali[2][2];
      mx2 = result_V[0];//*10;//uT into milli gauss
      my2 = result_V[1];//*10;
      mz2 = result_V[2];//*10;


      mxyz[0] = mx2;
      mxyz[1] = my2;
      mxyz[2] = mz2;



      vector_normalize(mxyz);


      axyz[0] = -axyz[0];
      gxyz[0] = -gxyz[0];
      /*
      ax1 = -ax1;
      gx1 = -gx1;
      */



      Now = micros();
      deltat = (Now - lastUpdate) * 1.0e-6;
      freq = 1/deltat;
      lastUpdate = Now;
      MadgwickAHRSupdate(gxyz[0], gxyz[1], gxyz[2], axyz[0], axyz[1], axyz[2],mxyz[0], mxyz[1], mxyz[2]);
      roll  = atan2(2.0 * (q0*q1 + q2*q3), 1.0 - 2.0 * (q1*q1 + q2*q2));
      pitch = asin(2.0 * (q0*q2 - q3*q1));
      yaw   = atan2(2.0 * (q0*q3 + q1*q2), 1.0 - 2.0 * (q2*q2 + q3*q3));

      /*
      roll  = atan2(2.0 * (q0*q1 + q2*q3), 1.0 - 2.0 * (q1*q1 + q2*q2));
      pitch = asin(2.0 * (q0*q2 - q3*q1));
      yaw   = atan2(2.0 * (q0*q3 + q1*q2), 1.0 - 2.0 * (q2*q2 + q3*q3));
      */
      // to degrees
      yaw   *= 180.0 / PI;
      pitch *= 180.0 / PI;
      roll *= 180.0 / PI;




      yaw = yaw - declination;
      if (yaw < 0) yaw += 360.0;
      if (yaw >= 360.0) yaw -= 360.0;
      yaw = 360.0 - yaw;
      if (yaw >= 360.0) yaw -= 360.0;
        // 센서 값과 함께 상대 시간을 전송합니다.
        
            Serial.print(elapsedTime,3); Serial.print(",");
            Serial.print(-ax1, 2); Serial.print(",");
            Serial.print(ay1, 2); Serial.print(",");
            Serial.print(az1, 2); Serial.print(",");
      
            Serial.print(q0,2); Serial.print(",");
            Serial.print(q1,2); Serial.print(",");
            Serial.print(q2,2); Serial.print(",");
            Serial.println(q3,2); 
            
        
    
          //Serial.println(yaw, 3);
      /*
      if(Serial.available())
      {
        char rx_char;

        // reads the next byte of incoming serial data and stores it in rx_char(singlechar)
        rx_char = Serial.read();

        if(rx_char == '.'){
          Serial.print(roll, 2);
          Serial.print(", ");
          Serial.print(pitch, 2);
          Serial.print(", ");
          Serial.println(yaw, 2);
        
          if(rx_char == 'z'){
            yaw = 0;
          }
        }
      }*/
  }
}


void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float hx, hy;
	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;


	// Rate of change of quaternion from gyroscope
	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Normalise magnetometer measurement
		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
		mx *= recipNorm;
		my *= recipNorm;
		mz *= recipNorm;

		// Auxiliary variables to avoid repeated arithmetic
		_2q0mx = 2.0f * q0 * mx;
		_2q0my = 2.0f * q0 * my;
		_2q0mz = 2.0f * q0 * mz;
		_2q1mx = 2.0f * q1 * mx;
		_2q0 = 2.0f * q0;
		_2q1 = 2.0f * q1;
		_2q2 = 2.0f * q2;
		_2q3 = 2.0f * q3;
		_2q0q2 = 2.0f * q0 * q2;
		_2q2q3 = 2.0f * q2 * q3;
		q0q0 = q0 * q0;
		q0q1 = q0 * q1;
		q0q2 = q0 * q2;
		q0q3 = q0 * q3;
		q1q1 = q1 * q1;
		q1q2 = q1 * q2;
		q1q3 = q1 * q3;
		q2q2 = q2 * q2;
		q2q3 = q2 * q3;
		q3q3 = q3 * q3;

		// Reference direction of Earth's magnetic field
		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
		_2bx = sqrt(hx * hx + hy * hy);
		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
		_4bx = 2.0f * _2bx;
		_4bz = 2.0f * _2bz;

		// Gradient decent algorithm corrective step
		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		// Apply feedback step
		qDot1 -= beta * s0;
		qDot2 -= beta * s1;
		qDot3 -= beta * s2;
		qDot4 -= beta * s3;
	}


	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / freq);
	q1 += qDot2 * (1.0f / freq);
	q2 += qDot3 * (1.0f / freq);
	q3 += qDot4 * (1.0f / freq);
	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}



float invSqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as R
from matplotlib import animation

# 데이터 로드
data = np.genfromtxt("/Users/isuyeong/Desktop/deadrec/q4.csv", delimiter=",", skip_header=1)
timestamp = data[:, 0]

# 쿼터니언 및 가속도 데이터 추출
start_index = np.where(timestamp >= 0)[0][0]
acceleration_g = data[start_index:, 1:4]  # X, Y, Z 가속도 (g 단위)
quaternions = data[start_index:, 4:8]    # 쿼터니언 데이터 (w, x, y, z)

# delta_time 계산
delta_time = np.diff(timestamp[start_index:], prepend=timestamp[start_index])

# 중력 가속도 제거 및 지구 좌표계로 변환, 로우패스 필터 적용
acceleration_earth = np.zeros_like(acceleration_g)
for i in range(len(delta_time)):
    # 가속도 데이터를 m/s² 단위로 변환
    acc_m_s2 = acceleration_g[i] * 9.81
    # 로우패스 필터 적용: 0.1m/s² 미만 가속도를 0으로 설정
    acc_m_s2[abs(acc_m_s2) < 0.1] = 0.0
    rotation = R.from_quat(quaternions[i])
    # 중력 가속도 벡터를 센서 프레임에서 지구 프레임으로 변환하여 제거
    gravity = rotation.apply([0, 0, 9.81])
    acceleration_earth[i] = rotation.apply(acc_m_s2) - gravity

# 속도 및 위치 적분
velocity = np.zeros_like(acceleration_earth)
position = np.zeros_like(velocity)
for i in range(1, len(delta_time)):
    velocity[i] = velocity[i-1] + acceleration_earth[i] * delta_time[i]
    position[i] = position[i-1] + velocity[i] * delta_time[i]

# 위치를 미터에서 센티미터로 변환
position *= 100  # 1m = 100cm

# 애니메이션 생성
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111, projection='3d')
line, = ax.plot([], [], [], 'r-', lw=2)

def init():
    ax.set_xlim([np.min(position[:,0]), np.max(position[:,0])])
    ax.set_ylim([np.min(position[:,1]), np.max(position[:,1])])
    ax.set_zlim([np.min(position[:,2]), np.max(position[:,2])])
    ax.set_xlabel('X (cm)')
    ax.set_ylabel('Y (cm)')
    ax.set_zlabel('Z (cm)') 
    return line,

def update(frame):
    line.set_data(position[:frame, 0:2].T)
    line.set_3d_properties(position[:frame, 2])
    ax.set_title("Time: {:.3f} s".format(timestamp[start_index + frame]))
    return line,

ani = animation.FuncAnimation(fig, update, frames=len(position), init_func=init, interval=20, blit=False)

plt.show()

Time (s), accx, accy, accz, q0, q1, q2, q3
0.000,-0.01,-0.00,1.00,0.05,-0.68,-0.59,-0.43
0.014,-0.01,-0.00,1.01,0.05,-0.68,-0.59,-0.43
0.029,-0.00,-0.00,1.00,0.05,-0.68,-0.59,-0.43
0.043,-0.01,0.00,1.02,0.05,-0.68,-0.59,-0.43
0.057,-0.01,0.00,1.01,0.05,-0.67,-0.59,-0.43
0.071,-0.01,0.00,1.02,0.05,-0.67,-0.59,-0.43
0.086,-0.00,-0.00,1.01,0.05,-0.67,-0.59,-0.43
0.100,-0.01,-0.00,1.01,0.05,-0.67,-0.59,-0.44
0.114,-0.01,0.00,1.01,0.05,-0.67,-0.59,-0.44
0.129,-0.01,-0.00,1.01,0.05,-0.67,-0.59,-0.44
0.143,-0.00,-0.01,1.01,0.05,-0.67,-0.59,-0.44
0.157,0.00,-0.00,1.01,0.05,-0.67,-0.59,-0.44
0.172,-0.01,0.00,1.01,0.05,-0.67,-0.59,-0.44
0.186,-0.00,0.00,1.01,0.05,-0.67,-0.59,-0.44
0.200,-0.01,-0.00,1.01,0.05,-0.67,-0.59,-0.44
0.214,-0.01,-0.00,1.01,0.05,-0.67,-0.59,-0.45
0.229,-0.00,-0.01,1.01,0.05,-0.67,-0.59,-0.45
0.243,-0.01,-0.00,1.01,0.05,-0.67,-0.59,-0.45
0.257,-0.01,-0.00,1.01,0.05,-0.66,-0.59,-0.45
0.272,-0.01,0.00,1.01,0.06,-0.66,-0.59,-0.45
0.286,-0.01,-0.00,1.01,0.06,-0.66,-0.59,-0.45```

jremington · March 20, 2024, 1:12am

The numerical integration is unlikely to work well or at all if you calculate Euler angles and print data every update cycle. Update the quaternion as rapidly as possible, and calculate Euler angles and/or print very infrequently.

Your code wastes a lot of time scaling the magnetometer and accelerometer data, then normalizes it once in the main loop, and once again in the Madgwick function.

Are the gyro values in units of radians/sec?

I don't understand your description of what you think is wrong with the orientation.

young990729 · March 20, 2024, 5:13am

OH first of all thanks for what you mentioned above

The unit of the gyro values is radian/sec
As you mentioned, I think I have to reduce the steps of the calculation
The thing that I can't understand is not the orientation. If my orientation code is okay, my question is how to use the quaternions or acceleration data to infer the position in 3D.
Currently, I use the quaternions to set the orientation in python and the acceleration data to track the position(csv =>python which is postprocessing). But even when I use the stationery data from imu to track position in python, when I plot the data the data looks like I move my imu to the sky.
What you mentioned above is I have to update the quaternion as fast as possible to do good numerical integration and get the position of my IMU in 3D?

jremington · March 20, 2024, 2:16pm

Not possible with consumer grade IMUs. The data are too noisy, and the orientation estimate is too inaccurate. For an explanation of the problem see Using Accelerometers to Estimate Position and Velocity | CH Robotics

young990729 · March 20, 2024, 3:39pm

So, you mean I should upgrade the IMU to get accurate data and also for the dead reckoning?
OHHH okay really appreciate the solutions you suggest!

jremington · March 20, 2024, 4:28pm

The last time I looked, IMUs capable of reasonably accurate dead reckoning cost roughly in the vicinity of USD 50,000 and up.

system · September 16, 2024, 4:28pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Arduino Nano 33 BLE orientation computation using LSM9DS1 and Madgwick Filter Nano 33 BLE	7	170	August 28, 2025
Roll Pitch Yaw steady output Programming	13	265	October 3, 2025
Using the LSM9DS1 version 2 lib with madgwick Gives wrong Roll,Pitch and Yaw Sensors	5	1744	April 9, 2022
Zero value euler angles from quaternions Sensors	10	135	July 8, 2025
BLE Sense Rev2 IMU & Magnetometer Sensor Fusion - values drift, what is the correct axis orientation for BMI270/BMM150? Nano 33 BLE Sense	7	1371	February 9, 2025

LSM9DS1 Madgwick fusion code, what did I do wrong here..?

Related topics