How to get relative rotation matrix from two consecutive orientations of MPU6050

I need to find the relative rotation matrix between two orientations of MPU6050. My approach (which fails) uses the MPU6050 quaternion from jrowbergs code. Let quaternion at orientation 1 is q1 & quaternion at orientation 2 is q2.

I find the relative qualternion q =q2*q1_inverse.
Then I convert the relative quaternion to a rotation matrix and also a axis-angle representation.

However on examining the rotation matrix & the axis vector, I find that they are highly incorrect.

Kindly suggest how I can get the rotation matrix from the MPU6050. Or do I need to use the MPU9250?

Not enough information. Please read and follow the instructions in the "How to use this forum" post.

Note that the MPU6050 lacks a magnetometer, so it cannot produce a stable 3D, North referenced orientation.

I need the relative orientation between two orienations. I dont need absolute orienation/north aligned orientation.

I use jrowbergs code to get the quaternions from the MPU6050

I have the c++ code using eigen which implements the quaternion inverse and rotation matrix to axis angle. Kindly mention if any other details are needed. I tried two approaches

first approach : directly find relative rotation using quaternions and converto to rotation matrix
second approach: convert individual quaternions to rotation matrices and find the relative rotation matrix using the same

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
using namespace std;
int main()
{

Quaternionf q0(0.51,-0.05,0.00,-0.86);
Quaternionf q1(0.36,-0.62,-0.34,-0.61);

Quaternionf q0_inv=q0.inverse();
Quaternionf q=q1*q0_inv;
q.normalize();
std::cout << "quaternion="<<q.w() <<","<<q.x() <<","<< q.y() <<","<< q.z() << std::endl; 

Matrix3f M = q.toRotationMatrix();

Matrix3f M0 = q0.toRotationMatrix();
Matrix3f M1 = q1.toRotationMatrix();

Matrix3f N = M1*M0.inverse();

std::cout << "rotation matrix using quaternions="<< M << std::endl;
std::cout << "rotation matrix using matrix multiplication of rotation matrices from quaternions="<< N << std::endl;

AngleAxisf newAngleAxis(M);

cout << newAngleAxis.angle() << "\n" << newAngleAxis.axis() << "\n\n";

}

The MPU6050 cannot produce a stable 3D orientations, period. If represented by the usual Eulerian system, the yaw angle will always drift.

Please provide details showing that the two orientations you have make sense by themselves, and show the results of attempting to deduce the relative transformation.

Explain why you think this result is incorrect.

I use a MPU 9250 to get Quaternions. I do not calibrate the magnometer, I do not need absolute North just lines of force crossings.

///* https://github.com/bolderflight/MPU9250 */
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
float deltat = 0.0f;                             // integration interval for both filter schemes
#define Kp 7.50f
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 1.7f
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[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 = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;        // use reciprocal for division
  ax *= norm;
  ay *= norm;
  az *= norm;
  // Normalise magnetometer measurement
  norm = sqrt(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;        // use reciprocal for division
  mx *= norm;
  my *= norm;
  mz *= norm;
  // Reference direction of Earth's magnetic field
  hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
  hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
  bx = sqrt((hx * hx) + (hy * hy));
  bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
  // Estimated direction of gravity and magnetic field
  vx = 2.0f * (q2q4 - q1q3);
  vy = 2.0f * (q1q2 + q3q4);
  vz = q1q1 - q2q2 - q3q3 + q4q4;
  wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
  wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
  wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - 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.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
  }
  else
  {
    eInt[0] = 0.0f;     // prevent integral wind up
    eInt[1] = 0.0f;
    eInt[2] = 0.0f;
  }
  // Apply feedback terms
  gx = gx + Kp * ex + Ki * eInt[0];
  gy = gy + Kp * ey + Ki * eInt[1];
  gz = gz + Kp * ez + Ki * eInt[2];
  // Integrate rate of change of quaternion
  pa = q2;
  pb = q3;
  pc = q4;
  q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
  q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
  q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
  q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
} // void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)

---> see next post

I then calculate Euler angles from the quaternions:

void fGetIMU( void *pvParameters )
{
  const int FIFO_SIZE = 30;
  size_t fifoSize;
  float TourqeAngle_uSec = 11.11f;  //uS per degree, based upon set servo limits of MAXservo and MINservo, 11.11/1 = 0.09000900090009
  int Xservo = iX_Posit90; // may need to adjust to servo mounted level.
  int Yservo = iY_Posit90;
  int StartUpCount = 0;
  const int StartUpCountWaitCount = 1; // discard the first 5 readings of the FIFO
  float axBias = 0.0;
  float ayBias = 0.0;
  float azBias = 0.0;
  float gxBias = 0.0;
  float gyBias = 0.0;
  float gzBias = 0.0;
  boolean DidBias = false;
  unsigned long TimePast = micros();
  unsigned long TimeNow = micros();
  int ClippingValue = 3;
  xSemaphoreGive( sema_GetIMU );
  ////
  for (;;)
  {
    xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
    //    // read the sensor
    if ( IMU.readFifo() > -1 )
    {
      if ( StartUpCount >= StartUpCountWaitCount )
      {
        if ( DidBias )
        {
          float ax[FIFO_SIZE], ay[FIFO_SIZE], az[FIFO_SIZE];
          float gx[FIFO_SIZE], gy[FIFO_SIZE], gz[FIFO_SIZE];
          float mx[FIFO_SIZE], my[FIFO_SIZE], mz[FIFO_SIZE];
          IMU.getFifoAccelX_mss(&fifoSize, ax);
          IMU.getFifoAccelY_mss(&fifoSize, ay);
          IMU.getFifoAccelZ_mss(&fifoSize, az);
          IMU.getFifoGyroX_rads(&fifoSize, gx);
          IMU.getFifoGyroY_rads(&fifoSize, gy);
          IMU.getFifoGyroZ_rads(&fifoSize, gz);
          IMU.getFifoMagX_uT(&fifoSize, mx);
          IMU.getFifoMagY_uT(&fifoSize, my);
          IMU.getFifoMagZ_uT(&fifoSize, mz);
          if ( (fifoSize > 0) && (fifoSize < 11) )
          {
            TimeNow = micros();
            deltat = ((TimeNow - TimePast) / 1000000.0f);
            // read fifo and average readings
            for (size_t i = 1; i < fifoSize; i++)
            {
              ax[0] += ax[i];
              ay[0] += ay[i];
              az[0] += az[i];
              gx[0] += gx[i];
              gy[0] += gy[i];
              gz[0] += gz[i];
            }
            //apply bias to averaged reading
            ax[0] = (ax[0] / fifoSize) + axBias;
            ay[0] = (ay[0] / fifoSize) + ayBias;
            az[0] = (az[0] / fifoSize) + azBias;
            gx[0] = (gx[0] / fifoSize) + gxBias;
            gy[0] = (gy[0] / fifoSize) + gyBias;
            gz[0] = (gz[0] / fifoSize) + gzBias;
            MahonyQuaternionUpdate ( ax[0], ay[0], az[0], gx[0], gy[0], gz[0], mx[0], my[0], mz[0] );
            //            // float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
            float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            float roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            float xMagnitude = roll * TourqeAngle_uSec; // adjust sign for MPU mounting direction.
            float yMagnitude = pitch * TourqeAngle_uSec; // adjust sign for MPU mounting direction.
            Xservo += -(int(xMagnitude)); // adjust servo angle to -tilted angle
            Yservo += -(int(yMagnitude)) ;
            //            // clipping per torque
            if ( abs(x_XY_INFO.iY - Yservo) > ClippingValue )
            {
              if ( x_XY_INFO.iY > Yservo )
              {
                x_XY_INFO.iY = x_XY_INFO.iY - ClippingValue;
              }
              else
              {
                x_XY_INFO.iY = x_XY_INFO.iY + ClippingValue;
              }
            }
            else
            {
              x_XY_INFO.iY = Yservo;
            }
            if ( abs(x_XY_INFO.iX - Xservo) > ClippingValue )
            {
              if ( x_XY_INFO.iX > Xservo )
              {
                x_XY_INFO.iX = x_XY_INFO.iX - ClippingValue;
              }
              else
              {
                x_XY_INFO.iX = x_XY_INFO.iX + ClippingValue;
              }
            }
            else
            {
              x_XY_INFO.iX = Xservo;
            }
            if (  (x_XY_INFO.iX < MAXservo) && (x_XY_INFO.iX > MINservo) )
            {
              if ( xSemaphoreTake( sema_X, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
              {
                //                // if unable to take semaphore sema_XY, than servo torque is in progress and skip sending new torque values to servo torque function
                xQueueOverwrite( xQ_XY_INFO, (void *) &x_XY_INFO );
                xEventGroupSetBits( eg, evtTweakServoX );
              }
            }
            else
            {
              x_XY_INFO.iX = iX_Posit90;
              Xservo = iX_Posit90;
              xQueueOverwrite( xQ_XY_INFO, (void *) &x_XY_INFO );
              xSemaphoreTake( sema_X, xSemaphoreTicksToWait );
              xEventGroupSetBits( eg, evtTweakServoX );
            }
            if ( (x_XY_INFO.iY < MAXservo) && (x_XY_INFO.iY > MINservo) )
            {
              if ( xSemaphoreTake( sema_Y, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
              {
                //// if unable to take semaphore sema_XY, than servo torque is in progress and skip sending new torque values to servo torque function
                xQueueOverwrite( xQ_XY_INFO, (void *) &x_XY_INFO );
                xEventGroupSetBits( eg, evtTweakServoY );
              }
            } //  if (  (x_XY_INFO.iY < MAXservo) && (x_XY_INFO.iY > MINservo) )
            else
            {
              x_XY_INFO.iY = iY_Posit90;
              Yservo = iY_Posit90;
              xQueueOverwrite( xQ_XY_INFO, (void *) &x_XY_INFO );
              xSemaphoreTake( sema_Y, xSemaphoreTicksToWait );// causes a delay during torque
              xEventGroupSetBits( eg, evtTweakServoY );
            }
            // Update data to be saved with the latest values
            TimePast = TimeNow;
          } // if ( fifoSize > 0 )
        } // if ( DidBias )

---->>> see next post

  else
        {
          // MPU calibration, hold still mpu still for this event
          int BIAS_FIFO_SIZE = FIFO_SIZE;
          float ax[BIAS_FIFO_SIZE], ay[BIAS_FIFO_SIZE], az[BIAS_FIFO_SIZE];
          float gx[BIAS_FIFO_SIZE], gy[BIAS_FIFO_SIZE], gz[BIAS_FIFO_SIZE];
          float mx[BIAS_FIFO_SIZE], my[BIAS_FIFO_SIZE], mz[BIAS_FIFO_SIZE];
          // throw out first fifo reading
          IMU.enableFifo(true, true, true, false);
          vTaskDelay( pdMS_TO_TICKS( 200 ) );
          IMU.readFifo();
          IMU.enableFifo(true, true, true, false);
          // There is and interaction between sample size and accuracy of bias.
          // Sample size is determined, with the FIFO read, by the length of time FIFO is allowed to gather data.
          // Sample sizes larger then 200mS are greatly inaccurate.
          vTaskDelay( pdMS_TO_TICKS( 150 ) );
          IMU.readFifo();
          IMU.getFifoAccelX_mss(&fifoSize, ax);
          IMU.getFifoAccelY_mss(&fifoSize, ay);
          IMU.getFifoAccelZ_mss(&fifoSize, az);
          IMU.getFifoGyroX_rads(&fifoSize, gx);
          IMU.getFifoGyroY_rads(&fifoSize, gy);
          IMU.getFifoGyroZ_rads(&fifoSize, gz);
          IMU.getFifoMagX_uT(&fifoSize, mx);
          IMU.getFifoMagY_uT(&fifoSize, my);
          IMU.getFifoMagZ_uT(&fifoSize, mz);
          int iCount = 0;
          for (size_t i = 1; i < fifoSize; i++)
          {
            ax[0] += ax[i];
            ay[0] += ay[i];
            az[0] += az[i];
            gx[0] += gx[i];
            gy[0] += gy[i];
            gz[0] += gz[i];
            ++iCount;
          }
          axBias = -(ax[0] / iCount);
          ayBias = -(ay[0] / iCount);
          azBias = -(az[0] / iCount);
          gxBias = -(gx[0] / iCount);
          gyBias = -(gy[0] / iCount);
          gzBias = -(gz[0] / iCount);
          Serial.print( "calibration values ");
          Serial.print ( iCount );
          Serial.print( ", ax ");
          Serial.print ( axBias, 6 );
          Serial.print( ", ay ");
          Serial.print (ayBias, 6 );
          Serial.print( ", az ");
          Serial.print ( azBias, 6 );
          Serial.print( ", gx ");
          Serial.print ( gxBias, 6 );
          Serial.print( ", gy ");
          Serial.print ( gyBias, 6 );
          Serial.print( ", gz ");
          Serial.print ( gzBias, 6 );
          Serial.println();
          DidBias = true;
          TimePast = micros();
          xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
        }
      } // if ( StartUpCount >= StartUpCountWaitCount )
      else
      {
        ++StartUpCount;
      }
    } //  if ( IMU.readFifo() > -1 )
    IMU.enableFifo(true, true, true, false);
    xSemaphoreGive( sema_GetIMU );
    //    Serial.print(uxTaskGetStackHighWaterMark( NULL ));
    //    Serial.println();
    //    Serial.flush();
  } //  for (;;)
  vTaskDelete( NULL );
}

I use the Euler angles to produce servo counter torque values.

I use the BolderFlightSystem library, which works very well on an ESP32. I read the FIFO buffer and average the buffers readings to get the values to be used in the quaternions calculations.

All of the above code, on an ESP32, takes 260uSeconds - measuring code, to calculate. I read the FIFO every other millisecond.

→ see next post

#define Kp 7.50f
//proportional feedback, Ki for integral
#define Ki 1.7f

The base values for those two values were obtained from some research into the depths of the MPU9250 and then adjusted to what you see above from 100 hour runs per value change. If you leave those values at zero the MahonyQuaternionUpdate method will not figure out self correcting quaternion error factors.

I do not calibrate the magnometer

That is a mistake, whether you need a North reference or not.

As you can easily demonstrate for yourself, an uncalibrated magnetometer will often produce no useful readings at all, and will seriously degrade any orientation information the rest of the sensors might contribute.

Follow this excellent tutorial for the very best procedure.

@ Idahowalker: thanks for your effort for posting such a detailed reply. I am going through IMU9250 code. I need to see if it the quaternions from MPU9250 denote a correct orientation in the physical world (With MPU6050 there is no way to get absolute orientation)

@jremington:

I am describing below how my results seem inappropriate/incorrect.

I place my IMU on a table with the IMU facing up. Then I rotate it along the y axis(along the y line marked on the IMU breakout board) by 90 degrees. The quaternions captured at the initial and the final orientation are q0 & q1. the relative quaternion qr is = q1*q0_inverse.

Quaternionf q0(0.83,0.01,-0.03,0.55);
Quaternionf q1(0.60,-0.38,0.59,0.39);

relative quaternion, qr=0.692051,-0.6586,0.295248,-0.011818

The rotation matrix and the axis angle is created from qr using the eigen library.

Since the rotation is 90 about y axis i expect a rotation matrix of the form
[cos(theta) 0 sin(theta) [0 0 1
0 1 0 OR 0 1 0
-sin(theta) 0 cos(theta)] -1 0 0]

But I get

M=
0.825377 -0.372544 0.424221
-0.405259 0.132212 0.904591
-0.393087 -0.918548 -0.0418515

in axis angle format it should result to angle of 90 deg and axis = (0 1 0)

However I get
angle= 84.3567

axis =
-0.91238
0.409017
-0.0163718

the axis found is predominantly oriented along x axis unlike how i rotated it ie along y axis.

The IMU was well calibrated and offsets applied to the jrowberg code as expected.

I would say that the 84 is a pretty good approximation for 90 degrees.

However, it is easy to become confused about body versus reference coordinate systems. You applied a 90 degree rotation about a body axis, not a reference system axis (which would have some definite relationship to North and Down, defined by the code that produced the quaternion).

The decomposition of the relative quaternion is in the reference axial system.

To convince yourself of this, do the experiment with the IMU in a different starting orientation, say rotated 90 degrees about Z, or X pointing directly North, to see if and how that affects the result. But keep in mind that the MPU6050 cannot produce a North-referenced reference frame.

Some useful tools:
https://www.andre-gaschler.com/rotationconverter/
https://quaternions.online/

Your starting orientation, according to the latter visualization tool, is this:

start.png

You really need a better IMU. And, for a state of the art filter, consider using RTIMUlib for Arduino.

jremington:
That is a mistake, whether you need a North reference or not.

I got my X/Y/Z platform to rotate in a 3D figure 8 in a 1000 steps using the ESP32 PWM ledcWrite and converting uS to ticks. Now I am working on the timing of figure 8 rotation to 1000 measurement steps.

Anyways, thanks for the suggestion.

shome:
I need to find the relative rotation matrix between two orientations of MPU6050. My approach (which fails) uses the MPU6050 quaternion from jrowbergs code. Let quaternion at orientation 1 is q1 & quaternion at orientation 2 is q2.

I find the relative qualternion q =q2*q1_inverse.
Then I convert the relative quaternion to a rotation matrix and also a axis-angle representation.

However on examining the rotation matrix & the axis vector, I find that they are highly incorrect.

Kindly suggest how I can get the rotation matrix from the MPU6050. Or do I need to use the MPU9250?

Without providing the data, we can’t find out where you went wrong. Quaternions work fine.
Remember a 3D rotation or 3D orientation is represented by a unit quarternion. All orientations must
be comparable (ie they are all relative to the same reference frame) before you can combine them.

@ jremington:

You suggested to get a better IMU. I am thinking of using MPU9250 which is a 9 axis IMU(gyro, accelerometer, magnetometer).
What is your opinion about its suitability for finding relative rotations wrt the IMU body frame.

I had realized that the rotation angle was nearly correct however the rotation axis was not implying that the quaternions were not w.t.t IMU body frame but some other frame(probably aligned to gravity).

You suggested to get a better IMU.

The newer IMUs from Pololu have lower noise, higher resolution and higher update rates. See this post for one example.