Using the LSM9DS1 version 2 lib with madgwick Gives wrong Roll,Pitch and Yaw

rasmusklitgaard · October 7, 2021, 10:41am

Hey

I am trying to get a rotation vector out of my Arduino namo 33 ble.

I am Using the lib LSM9DS1 version 2 lib from git here

And i Have implemented the mahony and madgwick lib from
here

as many have done before.

but my problem is the when i am trying to output my rotation vector in yaw and have done a rotation of 90 degrees it only output around 38 - 48 degrees.

I dont know if it was my calibration that is off or what was wrong.
the filter and calculator class have been testet before with a wiimote controller and did work fine with that up untill the wiimote got stuck in the accelerometer.
even the madgwick lib seems not to work proberly or is it my sensor?

very much apreciate any suggestions since this is my a little part of my bachelor projeckt and should only have been a small part of the project and not a timeconsuming issue with getting the right orientation.

THX

Rasmus

**my code. **
Sketch:

#include <Arduino_LSM9DS1.h>
#include <TimeLib.h>
#include <MadgwickAHRS.h>
#include <SensorFusion.h>
#include <Filter.h>
#include <calc.h>
// for timestamp
#define TIMEHEADER "T"
#define TIMEREQUEST 7
unsigned long t = 0;
unsigned long mi = 0;
unsigned long last = 0;

// for sensor fusion
SF fusion;
float deltat = 0;

Madgwick mad;

Filter f;
Filter f2;
calc calculator;

void setup() {
  Serial.begin(9600);
  f.setSamplePeriod(0.0042);
  f.setkP(8);
  f.setkI(0.000);
  f.setBeta(0.001);
  f2.setSamplePeriod(0.0042);
  f2.setkP(8);
  f2.setkI(0.000);
  f2.setBeta(0.01);
  mad.begin(238);

  while (!Serial);
  Serial.println("Started");
  setSyncProvider(requestSync);
  Serial.println("Waiting for sync message");
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  // /*******************    For an improved accuracy run the DIY_Calibration_Accelerometer sketch first.     ****************
  //********************         Copy/Replace the lines below by the code output of the program              ****************/
  // Accelerometer code
   IMU.setAccelFS(3);
   IMU.setAccelODR(4);
   IMU.setAccelOffset(-0.004423, -0.021684, -0.035664);
   IMU.setAccelSlope (1.004703, 0.999619, 1.009910);
  ///***********************************************************************************************************************************
  //*******  FS  Full Scale         range 0:±2g | 1:±24g | 2: ±4g | 3: ±8g  (default=2)                                           ******
  //*******  ODR Output Data Rate   range 0:off | 1:10Hz | 2:50Hz | 3:119Hz | 4:238Hz | 5:476Hz, (default=3)(not working 6:952Hz) ******
  //************************************************************************************************************************************/
  IMU.accelUnit =  GRAVITY;   // or  METERPERSECOND2
  //
  ///*******   The gyroscope needs to be calibrated. Offset controls drift and Slope scales the measured rotation angle  *********
  //*****************   Copy/Replace the lines below by the output of the DIY_Calibration_Gyroscope sketch   ********************/
  // Gyroscope code
   IMU.setGyroFS(3);
   IMU.setGyroODR(4);
   IMU.setGyroOffset (0.225098, 0.559692, -2.038330);
   IMU.setGyroSlope (1.179980, 1.130254, 1.146827);


  //
  //
  //
  ///*****************************************************************************************************************************
  //*********  FS  Full Scale       setting 0: ±245°/s | 1: ±500°/s | 2: ±1000°/s | 3: ±2000°/s       ****************************
  //*********  ODR Output Data Rate setting 0:off | 1:10Hz | 2:50Hz | 3:119Hz | 4:238Hz | 5:476Hz, (not working 6:952Hz)   *******
  //*****************************************************************************************************************************/
  IMU.gyroUnit= DEGREEPERSECOND; //   DEGREEPERSECOND  RADIANSPERSECOND  REVSPERMINUTE  REVSPERSECOND
  //
  //   /*****************   For a proper functioning of the magnetometer it needs to be calibrated            ********************
  //*****************   Replace the lines below by the output of the DIY_Calibration_Magnetometer sketch   ********************/
   // Magnetometer code
   IMU.setMagnetFS(3);
   IMU.setMagnetODR(7);
   IMU.setMagnetOffset(2.399902, 14.291992, -2.126465);
   IMU.setMagnetSlope (0.995480, 1.149142, 1.180199);
  ///******************************************************************************************************************************
  //****  FS  Full Scale        range (0=±400 | 1=±800 | 2=±1200 | 3=±1600  (µT)                                              *****
  //****  ODR Output Data Rate  range (6,7,8)=(40,80,400)Hz | not available on all chips (0..5): (0.625,1.25,2.5,5.0,10,20)Hz *****
  //*******************************************************************************************************************************/
  IMU.magnetUnit = MICROTESLA;  //   GAUSS   MICROTESLA   NANOTESLA

}

void loop() {

  float x, y, z;
  float gx, gy, gz, mx, my, mz;

  if (IMU.magnetAvailable())                   // alias IMU.magneticFieldAvailable in library version 1.01
  { IMU.readMagnet(mx, my, mz);                   // alias IMU.readMagneticField in library version 1.01

  }
  if (IMU.gyroAvailable())                     // alias IMU.gyroscopeAvailable
  {
    IMU.readGyro(gx, gy, gz);                     // alias IMU.readGyroscope
  }
  if (IMU.accelAvailable())                   // alias IMU.accelerationAvailable in library version 1.01
  { IMU.readAccel(x, y, z);                  // alias IMU.readAcceleration  in library version 1.01
  }
  // for time stamp
  last = mi;
  mi = millis();
  if (Serial.available()) {
    t = processSyncMessage();
  } else {
    t += (mi - last);
  }
  float gx1 = calculator.deg2Rad(gx);
  float gy1 = calculator.deg2Rad(gy);
  float gz1 = calculator.deg2Rad(gz);
  f.UpdateMahony(gx1, gy1, gz1, x, y, z, mx, my, mz);
  f2.UpdateMahony(gx1, gy1, gz1, x, y, z);
  mad.updateIMU(gx, gy, gz, x, y, z);
  //f.sensorFusion(gx,gy,gz,x,y,z);
  //deltat = fusion.deltatUpdate();
  if (t > 1357041600) {
    //fusion.MadgwickUpdate(gx ,gy,gz,x,y,z,mx,my,mz,deltat);
    fusion.MahonyUpdate(calculator.deg2Rad(gx),calculator.deg2Rad(gy),calculator.deg2Rad(gz),x,y,z,deltat);

    float q1[4];
    float q[4];
    f.getQuaternion(q);
    //  q[0] = f.getq0();
    //    q[1] = f.getq1();
    //      q[2] = f.getq2();
    //        q[3] = f.getq3();
    f2.getQuaternion(q1);
    float rotV[3];
    float rotV1[3];
    calculator.quaternionToRotVec(q, rotV);
    calculator.quaternionToRotVec(q1, rotV1);

    Serial.print('[');
    //    Serial.print(IMU.getAccelODR());
    //    Serial.print(';');
    //        Serial.print(IMU.getGyroODR());
    //    Serial.print(';');
    //        Serial.print(IMU.getMagnetODR());
    //    Serial.print(';');
    //    Serial.print('\t');
    Serial.print(t);
    Serial.print(';');
    Serial.print(mad.getRoll());
    Serial.print(';');
    Serial.print(mad.getPitch());
    Serial.print(';');
    Serial.print(mad.getYaw());
    Serial.print(';');
    Serial.print(fusion.getRoll());
    Serial.print(';');
    Serial.print(fusion.getPitch());
    Serial.print(';');
    Serial.print(fusion.getYaw());
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV[0]));
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV[1]));
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV[2]));
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV1[0]));
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV1[1]));
    Serial.print(';');
    Serial.print(calculator.rad2Deg(rotV1[2]));
    Serial.print(';');
    Serial.print(x);
    Serial.print(';');
    Serial.print(y);
    Serial.print(';');
    Serial.print(z);
    Serial.print(';');
    Serial.print(gx);
    Serial.print(';');
    Serial.print(gy);
    Serial.print(';');
    Serial.print(gz);
    Serial.print(';');
    Serial.print(q[0]);
    Serial.print(';');
    Serial.print(q[1]);
    Serial.print(';');
    Serial.print(q[2]);
    Serial.print(';');
    Serial.print(q[3]);
    Serial.print(';');
    Serial.print(q1[0]);
    Serial.print(';');
    Serial.print(q1[1]);
    Serial.print(';');
    Serial.print(q1[2]);
    Serial.print(';');
    Serial.print(q1[3]);
    Serial.println(']');
  }
}

unsigned long processSyncMessage() {
  unsigned long pctime;
  const unsigned long DEFAULT_TIME = 1357041600; // Jan 1 2013

  if (Serial.find(TIMEHEADER)) {
    pctime = Serial.parseInt();
    if ( pctime >= DEFAULT_TIME) { // check the integer is a valid time (greater than Jan 1 2013)
      setTime(pctime); // Sync Arduino clock to the time received on the serial port
    }
  }
  return pctime;
}

time_t requestSync()
{
  Serial.write(TIMEREQUEST);
  return 0; // the time will be sent later in response to serial mesg
}

Filter.cpp

#include "Filter.h"
#include <math.h>

Filter::Filter() :sPeriod{ 0 }, kP{ 1 }, kI{ 0 } {
	quarternion[0] = 1;
	quarternion[1] = 0;
	quarternion[2] = 0;
	quarternion[3] = 0;
	quarternionM[0] = 1;
	quarternionM[1] = 0;
	quarternionM[2] = 0;
	quarternionM[3] = 0;
	intError[0] = 0;
	intError[1] = 0; 
	intError[2] = 0; 
	angleX = 0;
	angleY = 0;
	angleZ = 0;
	Beta = 0;
};

Filter::Filter(float samplePeriod) :sPeriod{ samplePeriod }, kP{ 1 }, kI{ 0 } {
	quarternion[0] = 1;
	quarternion[1] = 0;
	quarternion[2] = 0;
	quarternion[3] = 0;
	quarternionM[0] = 1;
	quarternionM[1] = 0;
	quarternionM[2] = 0;
	quarternionM[3] = 0;

	intError[0] = 0;
	intError[1] = 0;
	intError[2] = 0; 
	angleX = 0;
	angleY = 0;
	angleZ = 0;
	Beta = 0;
};

Filter::Filter(float samplePeriod, float proportionalGain) :sPeriod{ samplePeriod }, kP{ proportionalGain }, kI{ 0 }{
	quarternion[0] = 1;
	quarternion[1] = 0;
	quarternion[2] = 0;
	quarternion[3] = 0;
	quarternionM[0] = 1;
	quarternionM[1] = 0;
	quarternionM[2] = 0;
	quarternionM[3] = 0;

	intError[0] = 0;
	intError[1] = 0;
	intError[2] = 0;
	angleX = 0;
	angleY = 0;
	angleZ = 0;
	Beta = 0;
};

Filter::Filter(float samplePeriod, float proportionalGain, float integralGain) : sPeriod{ samplePeriod }, kP{ proportionalGain }, kI{ integralGain } {
	quarternion[0] = 1;
	quarternion[1] = 0;
	quarternion[2] = 0;
	quarternion[3] = 0;
	quarternionM[0] = 1;
	quarternionM[1] = 0;
	quarternionM[2] = 0;
	quarternionM[3] = 0;

	intError[0] = 0;
	intError[1] = 0;
	intError[2] = 0;
	angleX = 0;
	angleY = 0;
	angleZ = 0;
	Beta = 0;
};

Filter::~Filter() {};

void Filter::UpdateMadwick(float gx, float gy, float gz, float ax, float ay, float az) {
	float q1 = quarternionM[0], q2 = quarternionM[1], q3 = quarternionM[2], q4 = quarternionM[3];   // short name local variable for readability
	float norm;
	float s1, s2, s3, s4;
	float qDot1, qDot2, qDot3, qDot4;

	// Auxiliary variables to avoid repeated arithmetic
	float _2q1 = 2.0f * q1;
	float _2q2 = 2.0f * q2;
	float _2q3 = 2.0f * q3;
	float _2q4 = 2.0f * q4;
	float _4q1 = 4.0f * q1;
	float _4q2 = 4.0f * q2;
	float _4q3 = 4.0f * q3;
	float _8q2 = 8.0f * q2;
	float _8q3 = 8.0f * q3;
	float q1q1 = q1 * q1;
	float q2q2 = q2 * q2;
	float q3q3 = q3 * q3;
	float q4q4 = q4 * q4;
	
	// Normalise accelerometer measurement
	norm = (float)sqrt(ax * ax + ay * ay + az * az);
	if (norm == 0.0f) return; // handle NaN
	norm = 1 / norm;        // use reciprocal for division
	ax *= norm;
	ay *= norm;
	az *= norm;

	// Gradient decent algorithm corrective step
	s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
	s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
	s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
	s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
	norm = 1.0f / (float)sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
	s1 *= norm;
	s2 *= norm;
	s3 *= norm;
	s4 *= norm;

	// Compute rate of change of quaternion
	qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
	qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
	qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
	qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;

	// Integrate to yield quaternion
	q1 += qDot1 * sPeriod;
	q2 += qDot2 * sPeriod;
	q3 += qDot3 * sPeriod;
	q4 += qDot4 * sPeriod;
	norm = 1.0f / (float)sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
	quarternionM[0] = q1 * norm;
	quarternionM[1] = q2 * norm;
	quarternionM[2] = q3 * norm;
	quarternionM[3] = q4 * norm;
};


void Filter::UpdateMahony(float gx, float gy, float gz, float ax, float ay, float az) {
	float q1 = quarternion[0] , q2 = quarternion[1]  , q3 = quarternion[2] , q4 = quarternion[3];   // short name local variable for readability
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;
	float pa, pb, pc;

	// Normalise accelerometer measurement
	norm = (float)sqrt((ax * ax) + (ay * ay) + (az * az));
	if (norm == 0) return; // handle NaN
	norm = 1 / norm;        // use reciprocal for division
	ax *= norm;
	ay *= norm;
	az *= norm;
	//printf("ax : %f ay : %f az : %f\n", ax, ay, az);
	// Estimated direction of gravity
	vx = 2.0 * ((q2 * q4 ) - (q1 * q3));
	vy = 2.0 * ((q1 * q2 ) + (q3 * q4));
	vz = (q1 * q1 ) - (q2 * q2 ) - (q3 * q3 ) + (q4 * q4);
	//printf("vx : %f vy : %f vz : %f\n", vx, vy, vz);
	// Error is cross product between estimated direction and measured direction of gravity
	ex = ((ay * vz ) - (az * vy));
	ey = ((az * vx ) - (ax * vz));
	ez = ((ax * vy ) - (ay * vx));
	if (kI > 0)
	{
		intError[0] += ex;      // accumulate integral error
		intError[1] += ey;
		intError[2] += ez;
	}
	else
	{
		intError[0] = 0.0f;     // prevent integral wind up
		intError[1] = 0.0f;
		intError[2] = 0.0f;
	}

	// Apply feedback terms
	gx = gx + (kP * ex ) + (kI * intError[0]);
	gy = gy + (kP * ey ) + (kI * intError[1]);
	gz = gz + (kP * ez ) + (kI * intError[2]);
	//printf("gx : %f gy : %f gz : %f\n", gx, gy, gz);

	// Integrate rate of change of quaternion
	pa = q2;
	pb = q3;
	pc = q4;
	q1 = q1 + (((-q2 * gx) - (q3 * gy ) - (q4 * gz)) * (0.5 * sPeriod));
	q2 = pa + (((q1 * gx) + (pb * gz) - (pc * gy)) * (0.5 * sPeriod));
	q3 = pb + (((q1 * gy) - (pa * gz) + (pc * gx)) * (0.5 * sPeriod));
	q4 = pc + (((q1 * gz) + (pa * gy) - (pb * gx)) * (0.5 * sPeriod));

	// Normalise quaternion
	norm = (float)sqrt((q1 * q1) + (q2 * q2) + (q3 * q3) + (q4 * q4));
	norm = 1.0f / norm;
	quarternion[0] = q1 * norm;
	quarternion[1] = q2 * norm;
	quarternion[2] = q3 * norm;
	quarternion[3] = q4 * norm;
    //printf("Quaternion q1: %f q2: %f q3: %f q4: %f\n", quarternion[0], quarternion[1], quarternion[2], quarternion[3]);

};

void Filter::UpdateMahony(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
        {
            float q1 = quarternion[0] , q2 = quarternion[1]  , q3 = quarternion[2] , q4 = quarternion[3];   // short name local variable for readability
            float norm;
            float hx, hy, bx, bz;
            float vx, vy, vz, wx, wy, wz;
            float ex, ey, ez;
            float pa, pb, pc;

            // Auxiliary variables to avoid repeated arithmetic
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = (float)sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0) return; // handle NaN
            norm = 1 / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = (float)sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0) return; // handle NaN
            norm = 1 / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            hx = 2 * mx * (0.5 - q3q3 - q4q4) + 2 * my * (q2q3 - q1q4) + 2 * mz * (q2q4 + q1q3);
            hy = 2 * mx * (q2q3 + q1q4) + 2 * my * (0.5f - q2q2 - q4q4) + 2 * mz * (q3q4 - q1q2);
            bx = (float)sqrt((hx * hx) + (hy * hy));
            bz = 2 * mx * (q2q4 - q1q3) + 2 * my * (q3q4 + q1q2) + 2 * mz * (0.5f - q2q2 - q3q3);

            // Estimated direction of gravity and magnetic field
            vx = 2 * (q2q4 - q1q3);
            vy = 2 * (q1q2 + q3q4);
            vz = q1q1 - q2q2 - q3q3 + q4q4;
            wx = 2 * bx * (0.5 - q3q3 - q4q4) + 2 * bz * (q2q4 - q1q3);
            wy = 2 * bx * (q2q3 - q1q4) + 2 * bz * (q1q2 + q3q4);
            wz = 2 * bx * (q1q3 + q2q4) + 2 * bz * (0.5 - q2q2 - q3q3);

            // Error is cross product between estimated direction and measured direction of gravity
            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
            if (kI > 0)
            {
                intError[0] += ex;      // accumulate integral error
                intError[1] += ey;
                intError[2] += ez;
            }
            else
            {
                intError[0] = 0.0;     // prevent integral wind up
                intError[1] = 0.0;
                intError[2] = 0.0;
            }

            // Apply feedback terms
            gx = gx + kP * ex + kI * intError[0];
            gy = gy + kP * ey + kI * intError[1];
            gz = gz + kP * ez + kI * intError[2];

            // Integrate rate of change of quaternion
            pa = q2;
            pb = q3;
            pc = q4;
            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5 * sPeriod);
            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5 * sPeriod);
            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5 * sPeriod);
            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5 * sPeriod);

            // Normalise quaternion
            norm = (float)sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
            norm = 1.0 / norm;
            quarternion[0] = q1 * norm;
            quarternion[1] = q2 * norm;
            quarternion[2] = q3 * norm;
            quarternion[3] = q4 * norm;
        }


void Filter::updateIMU(float ax, float ay, float az, float gx, float gy, float gz,float T) {
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;
	float filtCoef = 0.75f, filtCoef2 = 0.75f;
	float ax0, ay0, az0, gx0, gy0, gz0;
	float q0 = quarternion[0];
	float q1 = quarternion[1];
	float q2 = quarternion[2];
	float q3 = quarternion[3];
    float kP = 5.0f;
	float Ki = 0.0005;
	float halfT = 0.5f;


	ax0 = ax;
	ay0 = ay;
	az0 = az;
	gx0 = gx;
	gy0 = gy;
	gz0 = gz;

	// normalise the measurements
	norm = (float)sqrt(ax * ax + ay * ay + az * az);
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;

	// estimated direction of gravity
	vx = 2 * (q1 * q3 - q0 * q2);
	vy = 2 * (q0 * q1 + q2 * q3);
	vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	//printf("estimated direction of gravity vx = %f vy = %f vz = %f",vx,vy,vz);

	// error is sum of cross product between reference direction of field and direction measured by sensor
	ex = (ay * vz - az * vy);
	ey = (az * vx - ax * vz);
	ez = (ax * vy - ay * vx);

	// integral error scaled integral gain
	exInt = filtCoef * exInt + (1 - filtCoef) * ex;
	eyInt = filtCoef * eyInt + (1 - filtCoef) * ey;
	ezInt = filtCoef * ezInt + (1 - filtCoef) * ez;

	// adjusted gyroscope measurements
    gx = filtCoef2 * gx + (1 - filtCoef2) * (kP * ex + Ki * exInt) / T;
    gy = filtCoef2 * gy + (1 - filtCoef2) * (kP * ey + Ki * eyInt) / T;
    gz = filtCoef2 * gz + (1 - filtCoef2) * (kP * ez + Ki * ezInt) / T;

	// integrate quaternion rate and normalise
	q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT * T;
	q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT * T;
	q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT * T;
	q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT * T;

	// normalise quaternion
	norm = (float)sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
    //printf("q0 = %f, q1 = %f, q2 = %f, q3 = %f\n", q0, q1, q2, q3);

	quarternion[0] = q0;
	quarternion[1] = q1;
	quarternion[2] = q2;
	quarternion[3] = q3;
	

};

void Filter::sensorFusion(float gx, float gy, float gz, float ax, float ay, float az) {
    angleX = 0.98 * (angleX + gx * sPeriod) + 0.02 * ax;
    angleY = 0.98 * (angleY + gy * sPeriod) + 0.02 * ay;
    angleZ = 0.98 * (angleZ + gz * sPeriod) + 0.02 * az;

};
/*
float Filter::kalman(float U) {
	K = (P * H) / (H * P * H + R);

	uHat = uHat + K * (U - H * uHat);

	P = (1 - K * H) * P + Q;

	return uHat;
};
*/
void Filter::setSamplePeriod(float samplePeriod) {
	this->sPeriod = samplePeriod;
};

void Filter::setkP(float proportionalGain) {
	this->kP = proportionalGain;
};

void Filter::setkI(float integralGain) {
	this->kI = integralGain;
};

void Filter::setBeta(float beta) {
	this->Beta = beta;
};

void Filter::setQuaternion(float Quarternion[4]) {
	quarternion[0] = Quarternion[0];
  quarternion[1] = Quarternion[1];
  quarternion[2] = Quarternion[2];
  quarternion[3] = Quarternion[3];
};

void Filter::setIntegralError(float intergralError[3]) {
	intError[0] = intergralError[0];
  intError[1] = intergralError[1];
  intError[2] = intergralError[2];
};

float Filter::getSamplePeriod() {
	return sPeriod;
};

float Filter::getProportionalGain() {
	return kP;
};

float Filter::getIntegralGain() {
	return kI;
};

float Filter::getBeta() {
	return Beta;
};

void Filter::getQuaternion(float quat[4]) {
	quat[0] = quarternion[0];
  quat[1] = quarternion[1];
  quat[2] = quarternion[2];
  quat[3] = quarternion[3];
};

void Filter::getQuaternionMad(float quatMad[4]) {
	quatMad[0] = quarternionM[0];
  quatMad[1] = quarternionM[1];
  quatMad[2] = quarternionM[2];
  quatMad[3] = quarternionM[3];
};

void Filter::getIntegralError(float intErr[3]) {
	intErr[0] = intError[0];
  intErr[1] = intError[1];
  intErr[2] = intError[2];
};

float Filter::getAx() {
	return angleX;
};
float Filter::getAy() {
	return angleY;
};
float Filter::getAz() {
	return angleZ;
};

void Filter::updateQuaternionOff(float quaternionOffSet[4]) {
	quarternion[0] += quaternionOffSet[0];
	quarternion[1] += quaternionOffSet[1];
	quarternion[2] += quaternionOffSet[2];
	quarternion[3] += quaternionOffSet[3];
};

calc.cpp

#include "calc.h"
#define _USE_MATH_DEFINES
calc::calc() {};
calc::~calc() {};

void calc::eulToQuaternion(float roll, float pitch, float yaw, float quat[4]) {
    float cy = cos(yaw * 0.5);
    float sy = sin(yaw * 0.5);
    float cp = cos(pitch * 0.5);
    float sp = sin(pitch * 0.5);
    float cr = cos(roll * 0.5);
    float sr = sin(roll * 0.5);

    
    quat[0] = cr * cp * cy + sr * sp * sy;
    quat[1] = sr * cp * cy - cr * sp * sy;
    quat[2] = cr * sp * cy + sr * cp * sy;
    quat[3] = cr * cp * sy - sr * sp * cy;

    
};

void calc::quaternionToEul(float quaternion[4],float eul[3]) {
    
    //double pi = 2 * asin(1.0);

    // roll (x-axis rotation)
    float sinr_cosp = 2 * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
    float cosr_cosp = 1 - 2 * (quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2]);
    eul[0] = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (quaternion[0] * quaternion[2] - quaternion[3] * quaternion[1]);
    if (std::abs(sinp) >= 1)
      
        eul[1] = copysign(pi / 2, sinp); // use 90 degrees if out of range
    else
        eul[1] = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (quaternion[0] * quaternion[3] + quaternion[1] * quaternion[2]);
    double cosy_cosp = 1 - 2 * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]);
    eul[2] = atan2(siny_cosp, cosy_cosp);

};

void calc::quaternionToRotVec(float quaternion[4],float rotVec[3]) {
    float q0 = quaternion[0];
    float q1 = quaternion[1];
    float q2 = quaternion[2];
    float q3 = quaternion[3];
    float norm = (float)sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;
    
    float theata = 2 * acos(q0);
    float angle = theata / sin(theata / 2);
    rotVec[0] = angle * q1;
    rotVec[1] = angle * q2;
    rotVec[2] = angle * q3;


};

void calc::quatToEul(float quaternion[4],float eul[3]) {
    
    calc c;
    float q0 = quaternion[0];
    float q1 = quaternion[1];
    float q2 = quaternion[2];
    float q3 = quaternion[3];
    double sqw = q0 * q0;
    double sqx = q1 * q1;
    double sqy = q2 * q2;
    double sqz = q3 * q3;
    double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
    double test = q1 * q2 + q3 * q0;
    if (test > 0.499 * unit) { // singularity at north pole
        eul[0] = c.rad2Deg(2 * atan2(q1, q1));
        eul[1] = c.rad2Deg(pi / 2);
        eul[2] = c.rad2Deg(0);
        
    }
    else if (test < -0.499 * unit) { // singularity at south pole
        eul[0] = c.rad2Deg(-2 * atan2(q1, q0));
        eul[1] = c.rad2Deg(-pi / 2);
        eul[2] = c.rad2Deg(0);
        
    }
    else{
    eul[0] = c.rad2Deg(atan2(2 * q2 * q0 - 2 * q1 * q3, sqx - sqy - sqz + sqw));
    eul[1] = c.rad2Deg(asin(2 * test / unit));
    eul[2] = c.rad2Deg(atan2(2 * q1 * q0 - 2 * q2 * q3, -sqx + sqy - sqz + sqw));

    }
};

rotMatrix calc::quaternionToMatrix(float quaternion[4]) {
    float q0 = quaternion[0];
    float q1 = quaternion[1];
    float q2 = quaternion[2];
    float q3 = quaternion[3];
    rotMatrix matrix;

    matrix.XX = 2 * (q0 * q0 + q1 * q1) - 1;
    matrix.XY = 2 * (q1 * q2 - q0 * q3); 
    matrix.XZ = 2 * (q1 * q3 + q0 * q2);

    matrix.YX = 2 * (q1 * q2 + q0 * q3);
    matrix.YY = 2 * (q0 * q0 + q2 * q2) - 1;
    matrix.YZ = 2 * (q2 * q3 - q0 * q1);

    matrix.ZX = 2 * (q1 * q3 - q0 * q2);
    matrix.ZY = 2 * (q2 * q3 + q0 * q1);
    matrix.ZZ = 2 * (q0 * q0 + q3 * q3) - 1;
    
    return matrix;
};




float calc::deg2Rad(float deg) {
    return (deg * pi / 180);
}

float calc::rad2Deg(float rad) {
    return (rad * 180 / pi);
}

MarkT · October 7, 2021, 2:23pm

Did you do this?

BTW all those leading/trailing '*' characters make the sketch unreadable - why?

rasmusklitgaard · October 8, 2021, 6:57am

Hey MarkT

Sorry for those '*' characters. i have changed it now so it might be more readable now.

I have done the calibration on both the accelerometer, the gyro and the mag.

I have tried it multiple times with different settings to see if it was the settings that in my case was off or somthing but same thing happens.

jremington · October 9, 2021, 12:40am

Looks like you forgot to ensure a right handed coordinate system. Note that any axis swaps or inversions should be done AFTER sensor calibration constants are applied.

This library does it correctly: GitHub - jremington/LSM9DS1-AHRS: Mahony 3D fusion filter and tilt compensated compass, with sensor calibration code

rasmusklitgaard · October 11, 2021, 10:20am

Hey jRemington

thanks for the comment, but i think i now is a little more enlightened about what is wrong, and i dont think it is the coordinate system.

I belive that i am trying to print out my data over the serial and the loop time is signifikant slower than the actual sampling time i am trying to use with my filter and therfor the readings is off.

i am currently trying to figure out what my actual sampling rate is.

thx

system · April 9, 2022, 10:20am

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	6	74	July 18, 2025
IMU 9250, yaw problem, Madgwick Sensors	3	7492	May 6, 2021
Madgwick IMU Filter Help Sensors	4	2404	May 6, 2021
LSM9DS1 - Heading values are affected by pitch and roll (after calibration) Sensors	21	5920	June 22, 2021
compass heading changes on tilt of device. Sensors	27	5824	May 6, 2021

Using the LSM9DS1 version 2 lib with madgwick Gives wrong Roll,Pitch and Yaw

Related topics