Go Down

### Topic: problem getting Quaternions from Gyro (Read 321 times)previous topic - next topic

#### FBW921

##### Jul 30, 2020, 06:18 pmLast Edit: Jul 30, 2020, 06:20 pm by FBW921
Hello everyone,

I have been trying to calculate quaternions from a 3-axis gyroscope using nothing else. I am aware of drift and the fact that the orientation will be relative.

The problem is that when I convert these quaternions to euler angles, I get close to 67 deg for a pitch of actually 90 deg. But, if I do not induce any pitch and do roll and yaw rotations separately they give the correct roll and yaw angles. Secondly, all the angles get significantly off if I rotate the sensor in a figure 8, making it totally useless if it encounters any complex motions. So where exactly am I going wrong in my code?

I am using ngo duong's method of calculating quaternions from gyro readings, he has summarized his method in this 1:27 sec youtube video, https://www.youtube.com/watch?v=VNIszCO4azs

But I have attached the relevant formulas below for your convenience. NOTE: I believe there is a typo in part 1 of ngo duong's formulas, in the multiply quaternions formula, in the first line, there is a +q3p3, but I think it should be -q3p3. I think this because he seems to be referencing the Hamilton product between quaternions.

Equipment: I use the arduino uno with the BNO055 imu.

The code:

Code: [Select]
`#include <Wire.h>#include <Adafruit_Sensor.h>#include <Adafruit_BNO055.h>#include <utility/imumaths.h>#include <math.h>float elapsedTime, currentTime, previousTime;float pitchy, rolly;float x, y, z;float q0, q1, q2, q3;float qDot1, qDot2, qDot3, qDot4;float recipNorm;float roll2, pitch2, yaw2;float nxa,nxb,nxc,nya,nyb,nyc,nza,nzb,nzc;float qx1,qx2,qx3,qx4,qy1,qy2,qy3,qy4,qz1,qz2,qz3,qz4;;/* This driver reads raw data from the BNO055   Connections   ===========   Connect SCL to analog 5   Connect SDA to analog 4   Connect VDD to 3.3V DC   Connect GROUND to common ground   History   =======   2015/MAR/03  - First release (KTOWN)*//* Set the delay between fresh samples */#define BNO055_SAMPLERATE_DELAY_MS (1)// Check I2C device address and correct line below (by default address is 0x29 or 0x28)//                                   id, addressAdafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);/**************************************************************************//*    Arduino setup function (automatically called at startup)*//**************************************************************************/void setup(void){  Serial.begin(38400);  Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");  /* Initialise the sensor */  if(!bno.begin(bno.OPERATION_MODE_ACCGYRO))  {    /* There was a problem detecting the BNO055 ... check your connections */    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");    while(1);  }  delay(1000);  /* Display the current temperature */  int8_t temp = bno.getTemp();  Serial.print("Current Temperature: ");  Serial.print(temp);  Serial.println(" C");  Serial.println("");  bno.setExtCrystalUse(true); q0 = 1.0;q1 = 0.0;q2 = 0.0;q3 = 0.0;}/**************************************************************************//*    Arduino loop function, called once 'setup' is complete (your own code    should go here)*//**************************************************************************/void loop(void){  previousTime = currentTime;        // Previous time is stored before the actual time read  currentTime = millis();            // Current time actual time read  elapsedTime = (currentTime - previousTime)/1000; // in seconds imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);x = ((gyro.x())*(PI/180));y = ((gyro.y())*(PI/180));z = ((gyro.z())*(PI/180));nxa = (pow(q0, 2))+(pow(q1, 2))-(pow(q2, 2))-(pow(q3, 2));nxb = 2*((q1*q2) + (q0*q3));nxc = 2*((q1*q3) + (q0*q2));nya = 2*((q1*q2) - (q0*q3));nyb = (pow(q0, 2))+(pow(q1, 2))-(pow(q2, 2))-(pow(q3, 2));nyc = 2*((q1*q0) + (q3*q2));nza = 2*((q1*q3) + (q0*q2));nzb = 2*((q2*q3) - (q0*q1));nzc = (pow(q0, 2))-(pow(q1, 2))-(pow(q2, 2))+(pow(q3, 2));qx1 = cos((x*elapsedTime)/2);qx2 = nxa* sin((x*elapsedTime)/2);qx3 = nxb* sin((x*elapsedTime)/2);qx4 = nxc* sin((x*elapsedTime)/2);qy1 = cos((y*elapsedTime)/2);qy2 = nya* sin((y*elapsedTime)/2);qy3 = nyb* sin((y*elapsedTime)/2);qy4 = nyc* sin((y*elapsedTime)/2);qz1 = cos((z*elapsedTime)/2);qz2 = nza* sin((z*elapsedTime)/2);qz3 = nzb* sin((z*elapsedTime)/2);qz4 = nzc* sin((z*elapsedTime)/2);//----------------------------------------------------------------//now for the hamilton product part qn+1 = qxqyqzqn//----------------------------------------------------------------//** = hamilton product of two sets of quaternions//qx**qyfloat q01, q11,q21,q31;q01 = hamprod1(qx1,qx2,qx3,qx4,qy1,qy2,qy3,qy4);q11 = hamprod2(qx1,qx2,qx3,qx4,qy1,qy2,qy3,qy4);q21 = hamprod3(qx1,qx2,qx3,qx4,qy1,qy2,qy3,qy4);q31 = hamprod4(qx1,qx2,qx3,qx4,qy1,qy2,qy3,qy4);//(qx**qy)**(qz)float q02, q12,q22,q32;q02 = hamprod1(q01,q11,q21,q31,qz1,qz2,qz3,qz4);q12 = hamprod2(q01,q11,q21,q31,qz1,qz2,qz3,qz4);q22 = hamprod3(q01,q11,q21,q31,qz1,qz2,qz3,qz4);q32 = hamprod4(q01,q11,q21,q31,qz1,qz2,qz3,qz4);//((qx**qy)**(qz))**(old quaternions)q0 = hamprod1(q02,q12,q22,q32,q0,q1,q2,q3);q1 = hamprod2(q02,q12,q22,q32,q0,q1,q2,q3);q2 = hamprod3(q02,q12,q22,q32,q0,q1,q2,q3);q3 = hamprod4(q02,q12,q22,q32,q0,q1,q2,q3);roll2 =(180/PI)* atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);pitch2 =(180/PI)* asinf(-2.0f * (q1*q3 - q0*q2));yaw2 =(180/PI)* atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);Serial.print(pitch2);Serial.print(" , ");Serial.print(roll2);Serial.print(" , ");Serial.println(yaw2);   delay(BNO055_SAMPLERATE_DELAY_MS);}//tester functionfloat test(float a, float b){  float result;  result =a*b;  return result;  }float hamprod1(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr1;  pr1 = a1*b1-a2*b2-a3*b3-a4*b4;  return pr1;  }float hamprod2(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr2;  pr2 = a1*b2+a2*b1+a3*b4-a4*b3;  return pr2;  }float hamprod3(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr3;  pr3 = a1*b3-a2*b4+a3*b1+a4*b2;  return pr3;  }float hamprod4(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr4;  pr4 = a1*b4+a2*b3-a3*b2+a4*b1;  return pr4;  }`

#### FBW921

#1
##### Jul 30, 2020, 09:17 pmLast Edit: Jul 30, 2020, 09:19 pm by FBW921
moderator note: this is not a duplicate post as this version of getting quaternions by stanford university is completely different from ngo duong's method.

Hi everyone,

I'm trying to integrate gyro values to get quaternions using the STANFORD UNIVERSITY METHOD. Attached is a short summary of how to do it by metzger. The lecture pdf that explains it in more detail can be found at:

http://stanford.edu/class/ee267/lectures/lecture10.pdf

In my code I get the quaternions and then convert them to euler angles using the method provided on wikipedia.

The problem is that if the imu is rotated in a figure 8 pattern the angles go way off. But, the algorithm gives pretty darn accurate angles if the sensor is rotated about each of its axis separately.

Can anyone spot any problems in my code? And please if you have your own way of calculating quaternions from gyro values I would be pleased to see it.

equipment: arduino uno, BNO055 imu.

Note: I am aware the BNO055 has its own fusion algorithm.

Code: [Select]
`#include <Wire.h>#include <Adafruit_Sensor.h>#include <Adafruit_BNO055.h>#include <utility/imumaths.h>#include <math.h>float elapsedTime, currentTime, previousTime;float pitchy, rolly;float x, y, z;float q0, q1, q2, q3, q0p, q1p, q2p, q3p;float qDot1, qDot2, qDot3, qDot4;float recipNorm;float roll2, pitch2, yaw2;float theta,omega;float vx, vy, vz;float qup0,qup1,qup2,qup3;;/* This driver reads raw data from the BNO055   Connections   ===========   Connect SCL to analog 5   Connect SDA to analog 4   Connect VDD to 3.3V DC   Connect GROUND to common ground   History   =======   2015/MAR/03  - First release (KTOWN)*//* Set the delay between fresh samples */#define BNO055_SAMPLERATE_DELAY_MS (1)// Check I2C device address and correct line below (by default address is 0x29 or 0x28)//                                   id, addressAdafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);/**************************************************************************//*    Arduino setup function (automatically called at startup)*//**************************************************************************/void setup(void){  Serial.begin(38400);  Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");  /* Initialise the sensor */  if(!bno.begin(bno.OPERATION_MODE_ACCGYRO))  {    /* There was a problem detecting the BNO055 ... check your connections */    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");    while(1);  }  delay(1000);  /* Display the current temperature */  int8_t temp = bno.getTemp();  Serial.print("Current Temperature: ");  Serial.print(temp);  Serial.println(" C");  Serial.println("");  bno.setExtCrystalUse(true);  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");q0 = 1.0;q1 = 0.0;q2 = 0.0;q3 = 0.0;}/**************************************************************************//*    Arduino loop function, called once 'setup' is complete (your own code    should go here)*//**************************************************************************/void loop(void){  previousTime = currentTime;        // Previous time is stored before the actual time read  currentTime = millis();            // Current time actual time read  elapsedTime = (currentTime - previousTime)/1000; // in seconds    imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);x = ((gyro.x())*(PI/180));y = ((gyro.y())*(PI/180));z = ((gyro.z())*(PI/180));if (x == 0) {x = .00001;}//velocity vector magnitude = omegaomega = sqrt((pow(x,2))+(pow(y,2))+(pow(z,2)));theta = (omega)*elapsedTime;vx = x/omega;vy = y/omega;vz = z/omega;qup0 = cos(theta/2);qup1 = vx*sin(theta/2);qup2 = vy*sin(theta/2);qup3 = vz*sin(theta/2);q0p = hamprod1(qup0,qup1,qup2,qup3,q0,q1,q2,q3);q1p = hamprod2(qup0,qup1,qup2,qup3,q0,q1,q2,q3);q2p = hamprod3(qup0,qup1,qup2,qup3,q0,q1,q2,q3);q3p = hamprod4(qup0,qup1,qup2,qup3,q0,q1,q2,q3);q0 = q0p;q1 = q1p;q2 = q2p;q3 = q3p;roll2 =(180/PI)* atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);pitch2 =(180/PI)* asinf(-2.0f * (q1*q3 - q0*q2));yaw2 =(180/PI)* atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);Serial.print(pitch2);Serial.print(" , ");Serial.print(roll2);Serial.print(" , ");Serial.println(yaw2);    delay(BNO055_SAMPLERATE_DELAY_MS);}float hamprod1(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr1;  pr1 = a1*b1-a2*b2-a3*b3-a4*b4;  return pr1;  }float hamprod2(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr2;  pr2 = a1*b2+a2*b1+a3*b4-a4*b3;  return pr2;  }float hamprod3(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr3;  pr3 = a1*b3-a2*b4+a3*b1+a4*b2;  return pr3;  }float hamprod4(float a1, float a2, float a3, float a4, float b1, float b2, float b3, float b4){  float pr4;  pr4 = a1*b4+a2*b3-a3*b2+a4*b1;  return pr4;  }`

#### jremington

#2
##### Jul 30, 2020, 10:23 pmLast Edit: Jul 30, 2020, 10:24 pm by jremington
I suggest to study the Mahony filter, as coded by Sebastian Madgwick. The gyro term integration (Euler method) works as advertised provided that the axial conventions are observed and right handed, and that the gyro scale factor and interval timing are correct.

To dissociate the accelerometer (feedback) term from the gyro term, just set the acceleration vector to zero.

Code: [Select]
`//// MPU-6050 Mahony AHRS  // last update 7/9/2020 sjr//#include "Wire.h"// AD0 low = 0x68 (default for Sparkfun module)// AD0 high = 0x69int MPU_addr = 0x68;// vvvvvvvvvvvvvvvvvv  VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv//These are the previously determined offsets and scale factors for accelerometer and gyro for// a particular example of an MPU-6050. They are not correct for other examples.//The AHRS will NOT work well or at all if these are not correctfloat A_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyzfloat G_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest#define gscale ((250./32768.0)*(PI/180.0))  //gyro default 250 LSB per d/s -> rad/s// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^// GLOBALLY DECLARED, required for Mahony filter// vector to hold quaternionfloat q[4] = {1.0, 0.0, 0.0, 0.0};// Free parameters in the Mahony filter and fusion scheme,// Kp for proportional feedback, Ki for integralfloat Kp = 30.0;float Ki = 0.0;// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.// with MPU-6050, some instability observed at Kp=100 Now set to 30.// char s[60]; //snprintf buffer, if needed// globals for AHRS loop timingunsigned long now_ms, last_ms = 0; //millis() timers// print intervalunsigned long print_ms = 200; //print angles every "print_ms" millisecondsfloat yaw, pitch, roll; //Euler angle outputvoid setup() {  Wire.begin();  Serial.begin(9600);  Serial.println("starting");  // initialize sensor  // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g  Wire.beginTransmission(MPU_addr);  Wire.write(0x6B);  // PWR_MGMT_1 register  Wire.write(0);     // set to zero (wakes up the MPU-6050)  Wire.endTransmission(true);}// AHRS loopvoid loop(){  static int i = 0;  static float deltat = 0;  //loop time in seconds  static unsigned long now = 0, last = 0; //micros() timers  //raw data  int16_t ax, ay, az;  int16_t gx, gy, gz;  int16_t Tmp; //temperature  //scaled data as vector  float Axyz[3];  float Gxyz[3];  Wire.beginTransmission(MPU_addr);  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)  Wire.endTransmission(false);  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers  int t = Wire.read() << 8;  ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)  t = Wire.read() << 8;  ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)  t = Wire.read() << 8;  az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)  t = Wire.read() << 8;  Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)  t = Wire.read() << 8;  gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)  t = Wire.read() << 8;  gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)  t = Wire.read() << 8;  gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)  Axyz[0] = (float) ax;  Axyz[1] = (float) ay;  Axyz[2] = (float) az;  //apply offsets and scale factors from Magneto  for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3];  Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s  Gxyz[1] = ((float) gy - G_off[1]) * gscale;  Gxyz[2] = ((float) gz - G_off[2]) * gscale;  //  snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);  //  Serial.println(s);  now = micros();  deltat = (now - last) * 1.0e-6; //seconds since last update  last = now;  Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);  // Compute Tait-Bryan angles.  // In this coordinate system, the positive z-axis is down toward Earth.  // Yaw is the angle between Sensor x-axis and Earth magnetic North  // (or true North if corrected for local declination, looking down on the sensor  // positive yaw is counterclockwise, which is not conventional for NED navigation.  // Pitch is angle between sensor x-axis and Earth ground plane, toward the  // Earth is positive, up toward the sky is negative. Roll is angle between  // sensor y-axis and Earth ground plane, y-axis up is positive roll. These  // arise from the definition of the homogeneous rotation matrix constructed  // from quaternions. Tait-Bryan angles as well as Euler angles are  // non-commutative; that is, the get the correct orientation the rotations  // must be applied in the correct order which for this configuration is yaw,  // pitch, and then roll.  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles  // which has additional links.  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));  //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.  yaw   = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));  // to degrees  yaw   *= 180.0 / PI;  if (yaw < 0) yaw += 360.0; //compass circle  pitch *= 180.0 / PI;  roll *= 180.0 / PI;  now_ms = millis(); //time to print?  if (now_ms - last_ms >= print_ms) {    last_ms = now_ms;    // print angles for serial plotter...    //  Serial.print("ypr ");    Serial.print(yaw, 0);    Serial.print(", ");    Serial.print(pitch, 0);    Serial.print(", ");    Serial.println(roll, 0);  }}//--------------------------------------------------------------------------------------------------// Mahony scheme uses proportional and integral filtering on// the error between estimated reference vector (gravity) and measured one.// Madgwick's implementation of Mayhony's AHRS algorithm.// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms//// Date      Author      Notes// 29/09/2011 SOH Madgwick    Initial release// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load// last update 07/09/2020 SJR minor edits//--------------------------------------------------------------------------------------------------// IMU algorithm updatevoid Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {  float recipNorm;  float vx, vy, vz;  float ex, ey, ez;  //error terms  float qa, qb, qc;  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms  float tmp;  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  tmp = ax * ax + ay * ay + az * az;  if (tmp > 0.0)  {    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)    recipNorm = 1.0 / sqrt(tmp);    ax *= recipNorm;    ay *= recipNorm;    az *= recipNorm;    // Estimated direction of gravity in the body frame (factor of two divided out)    vx = q[1] * q[3] - q[0] * q[2];    vy = q[0] * q[1] + q[2] * q[3];    vz = q[0] * q[0] - 0.5f + q[3] * q[3];    // Error is cross product between estimated and measured direction of gravity in body frame    // (half the actual magnitude)    ex = (ay * vz - az * vy);    ey = (az * vx - ax * vz);    ez = (ax * vy - ay * vx);    // Compute and apply to gyro term the integral feedback, if enabled    if (Ki > 0.0f) {      ix += Ki * ex * deltat;  // integral error scaled by Ki      iy += Ki * ey * deltat;      iz += Ki * ez * deltat;      gx += ix;  // apply integral feedback      gy += iy;      gz += iz;    }    // Apply proportional feedback to gyro term    gx += Kp * ex;    gy += Kp * ey;    gz += Kp * ez;  }  // Integrate rate of change of quaternion, q cross gyro term  deltat = 0.5 * deltat;  gx *= deltat;   // pre-multiply common factors  gy *= deltat;  gz *= deltat;  qa = q[0];  qb = q[1];  qc = q[2];  q[0] += (-qb * gx - qc * gy - q[3] * gz);  q[1] += (qa * gx + qc * gz - q[3] * gy);  q[2] += (qa * gy - qb * gz + q[3] * gx);  q[3] += (qa * gz + qb * gy - qc * gx);  // renormalise quaternion  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);  q[0] = q[0] * recipNorm;  q[1] = q[1] * recipNorm;  q[2] = q[2] * recipNorm;  q[3] = q[3] * recipNorm;}`

#### ballscrewbob

#3
##### Jul 31, 2020, 12:55 am
@FBW921

There was a report for this and I agree with the report that it "IS" part of the same thing.

TOPIC MERGED.

Could you take a few moments to Learn How To Use The Forum.
Other general help and troubleshooting advice can be found here.
It will help you get the best out of the forum.

It may not be the answer you were looking for but its the one I am giving based on either experience, educated guess, google (who would have thunk it ! ) or the fact that you gave nothing to go with in the first place so I used my wonky crystal ball.

#### FBW921

#4
##### Aug 03, 2020, 09:14 pm
Hey JRemington, thank you for sharing with me the Mahony Filter. And also thank you for answering in such a direct manner.

Unfortunately this is already a method that I have tried. Out of all the methods I've tried the Mahony filter has yielded the best results for getting quaternions using the gyroscope only.

I think I will likely continue with the mahony filter and attempt to implement some refinement of my own, I will also likely retry to find an easy-to-understand kalman filter example.

Have a good day J.

#### FBW921

#5
##### Aug 07, 2020, 09:24 pm
Correction: I have not used the Mahony filter, I have used the Madgwick filter. I had assumed they were the same thing. I may choose to look into the mahony filter if the madgwick cannot perform sufficiently. Thanks J.

Go Up