search

kriswiner / MPU9250

Arduino sketches for MPU9250 9DoF with AHRS sensor fusion
1.03k stars 472 forks source link

Rotating the Euler angles to get proper roll, pitch, and yaw angles #398

Open rwboedhoe opened 4 years ago

rwboedhoe commented 4 years ago

Hi Kriswiner,

I apologize in advance. A similar question has been asked by skrick in issue #141. I have read your answer. However, I am not able to solve the problem with the information that was provided.

I want to calculate the roll, pitch and yaw angle for any arbitrary orientation. I am using the MPU9250 in combination with the Arduino where I use Madgwick's filter, a quaternion based filter that combines magnetometer accelerometer and gyroscope readings, to calculate the orientation of the sensor.

Skrick had a similar question. You told him that he can calculate the bias at any given position. I assume that to calculate the bias for any giving position of the sensor you would need a flat surface to mount the sensor so each axis can be aligned with the direction of the gravity acceleration and the poles in this new position. However, I do not have this flat surface that I can rotate to any given position.

I thought that the conjugate of the quaternion, that is used to determine the orientation, can rotate the angles back to zero. However, the new pitch, roll and yaw angles after this rotation are incorrect. Rotating around the pitch angle results in a rotation in the pitch and yaw angle.

My questions to you are:

  1. Should I also redefine the NED?
  2. Is the initial idea of rotating the quaternion correct?
  3. If the answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position.

Thanks a lot! Your Github page helped so many people getting the MPU9250 working correctly. I highly appreciate your work.

Rwboedhoe

kriswiner commented 4 years ago

OK, there are several ingredients to getting accurate absolute orientation estimation (meaning yaw, pitch, and roll for arbitrary orientation). These include choice of sensor suite, calibration, fusion method, sensor configuration, and fusion rate among others. The MPU9250 is a pretty good sensor. In order to calibrate the accel and gyro, the methods in the github sketches are adequate for achieving ~4 degree rms heading accuracy. In this case, the sensor is best left flat and motionless to collect accel and gyro data from which offset bias can be calculated and stored in the MCU EEPROM or sketch for subsequent use. In other words, this only has to be done once, but well. There are more sophisticated methods we use that do involve calculating the offset bias at each of 24 separate orientations (using something we call a Cali-cube) which allows us to routinely get 1 degree rms heading accuracy.

For mag calibration you can use this https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration method, but freescale also has a more sophisticated method that I would recommend for best results. This is the hardest bust most important calibration, and in many cases also has to be done only once.

The sensors need to run at an adequate sample rate, 200 Hz for accel/gyro minimum but I would recommend 500 Hz or 1000 Hz for most applications. The mag can run at 100 Hz. Then the sensor fusion has to be done on an MCU capable of itering the fusion solution 10 - 20 times per new gyro data set. This typically means using an STM32L4 or Teensy 3.6. Or you can use a motion co-processor like the EM7180 and then just stream the quaternions to the AVR MCU.

The sensor data need to be fed into the Madgwick or Mahony filter in a consistent way, usually NED but could be ENU.

If you do this you should be able to get ~4 degree rms heading accuracy for all orientations except where the solution is degenerate, i.e., at the poles.

On Wed, Oct 30, 2019 at 6:24 AM rwboedhoe notifications@github.com wrote:

Hi Kriswiner,

I apologize in advance. A similar question has been asked by skrick in issue #141 https://github.com/kriswiner/MPU9250/issues/141. I have read your answer. However, I am not able to solve the problem with the information that was provided.

I want to calculate the roll, pitch and yaw angle for any arbitrary orientation. I am using the MPU9250 in combination with the Arduino where I use Madgwick's filter, a quaternion based filter that combines magnetometer accelerometer and gyroscope readings, to calculate the orientation of the sensor.

Skrick had a similar question. You told him that he can calculate the bias at any given position. I assume that to calculate the bias for any giving position of the sensor you would need a flat surface to mount the sensor so each axis can be aligned with the direction of the gravity acceleration and the poles in this new position. However, I do not have this flat surface that I can rotate to any given position.

I thought that the conjugate of the quaternion, that is used to determine the orientation, can rotate the angles back to zero. However, the new pitch, roll and yaw angles after this rotation are incorrect. Rotating around the pitch angle results in a rotation in the pitch and yaw angle.

My questions to you are:

  1. Should I also redefine the NED?
  2. Is the initial idea of rotating the quaternion correct?
  3. If the answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position.

Thanks a lot! Your Github page helped so many people getting the MPU9250 working correctly. I highly appreciate your work.

Rwboedhoe

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/398?email_source=notifications&email_token=ABTDLKVJI4RLFPF4K4AUC6DQRGDJNA5CNFSM4JGZH722YY3PNVWWK3TUL52HS4DFUVEXG43VMWVGG33NNVSW45C7NFSM4HVNGB4Q, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKTNRALH6IMZHO2AB2LQRGDJNANCNFSM4JGZH72Q .

rwboedhoe commented 4 years ago

Thanks for your answer, however, I don't know if we misunderstood each other. I am already able to get the yaw, pitch and roll angles. I looked at all the ingredients you mentioned and did all the steps to get the correct angle estimate. Now, however, I want to calculate the roll, pitch and yaw angle for any arbitrary orientation of the earth frame.

I would like to be able to redefine the orientation of the earth frame, where the earth frame will be defined by the direction of the gravity and the poles.

Some background information why I want to be able to change the direction of the earth frame:

I am developing a device that can measure the range of motion of the hip joint. To do this the device will be placed on the lateral part of the leg. I want that the Euler angles are equal to zero at the initial start of the measurement. Because the device will be placed on both legs and multiple persons will wear it, where not everyone has a similar leg bodybuild, I need to be able to redefine the earth frame.

I thought that the conjugate of the quaternion, that is used to determine the rotation, can rotate the angles back to zero. At any given orientation, described by a quaternion, the conjugate quaternion can be determined.

The new orientation that results in Euler angles that are equal to zero, because of the multiplication of this quaternion with it's conjugate, would define the new earth frame.

All quaternions, describing the orientation of the sensor, would be multiplied with this conjugate quaternion.

I hoped by doing this that I would be able to define the new earth frame for any given orientation. However, the new pitch, roll and yaw angles are incorrect. A rotation around the pitch axis results in a rotation in the pitch and yaw axes.

My questions to you are:

  1. Should I redefine the NED?
  2. Is my initial idea of rotating the quaternion correct?
  3. If your answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position?
kriswiner commented 4 years ago

Yes, I thought your question was how to get accurate absolute orientation estimation, sounds like you have done this already. Assuming you have an accurate absolute orientation estimation engine, then it sounds like you are asking me how to manipulate quaternions for a particular result. I still don't understand what you are after, and I am no expert on quaternion manipulations so I have included my colleague Greg on the thread who might be able to help better.

An accurate absolute orientation estimation engine provides Euler angles relative to a fixed world coordinate system such that YPR = 0,0,0 means the device parallel to magnetic North in Yaw, andwith pitch and roll perpendicular to Gravity. This is absolute, and all accurate absolute orientation engines will provide the same answer whether worn by different people or by the same person at different times, etc. So what I would do in your case as far as I understand it is to simply define a convenient reference (if parallel to True North and perpendicular to gravity is not) and measure deltas from it.

An accurate absolute orientation estimation engine provides this estimation in terms of quaternions as well, where the same reference is at Q = 1, 0, 0, 0 and it is true you can manipulate quaternions to get relative orientation wrt some other reference, but the mechanics is more complicated and less intuitive (at least to me) and the result has to be the same. If this is what you are after, perhaps Greg can help you, I cannot.

On Thu, Oct 31, 2019 at 5:28 AM rwboedhoe notifications@github.com wrote:

Thanks for your answer, however, I don't know if we misunderstood each other. I am already able to get the yaw, pitch and roll angles. I looked at all the ingredients you mentioned and did all the steps to get the correct angle estimate. Now, however, I want to calculate the roll, pitch and yaw angle for any arbitrary orientation of the earth frame.

I would like to be able to redefine the orientation of the earth frame, where the earth frame will be defined by the direction of the gravity and the poles.

Some background information why I want to be able to change the direction of the earth frame:

I am developing a device that can measure the range of motion of the hip joint. To do this the device will be placed on the lateral part of the leg. I want that the Euler angles are equal to zero at the initial start of the measurement. Because the device will be placed on both legs and multiple persons will wear it, where not everyone has a similar leg bodybuild, I need to be able to redefine the earth frame.

I thought that the conjugate of the quaternion, that is used to determine the rotation, can rotate the angles back to zero. At any given orientation, described by a quaternion, the conjugate quaternion can be determined.

The new orientation that results in Euler angles that are equal to zero, because of the multiplication of this quaternion with it's conjugate, would define the new earth frame.

All quaternions, describing the orientation of the sensor, would be multiplied with this conjugate quaternion.

I hoped by doing this that I would be able to define the new earth frame for any given orientation. However, the new pitch, roll and yaw angles are incorrect. A rotation around the pitch axis results in a rotation in the pitch and yaw axes.

My questions to you are:

  1. Should I redefine the NED?
  2. Is my initial idea of rotating the quaternion correct?
  3. If your answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position?

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/398?email_source=notifications&email_token=ABTDLKWK4IIBFHWTOSXZ3FLQRLFPHA5CNFSM4JGZH722YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOECXSKDA#issuecomment-548349196, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKQB4XXWINPP5MT4F3TQRLFPHANCNFSM4JGZH72Q .

kriswiner commented 4 years ago

Hi,

I think a few definitions are in order here. For any AHRS system:

  1. Pitch angle reference: Earth’s gravitational vector
  2. Roll angle reference: Earth’s gravitational vector
  3. Yaw (Heading) angle reference: Earth’s geomagnetic field

So by definition, the quaternions generated are referenced with respect to “Up” and “North” and deliver Euler angles referenced with respect to “Up”. So there is no reorientation of “the earth frame”… That’s just not possible.

What I think you want to do is determine difference quaternions to generate difference angles; that is the initial absolute orientation is for the hip in the “Initial” or “Relaxed” state and the final absolute orientation is for the hip in the “final” state of interest. The AHRS oientations are vectors; the hip angle vector is the vector between the initial and final states. If the initial state isn’t (0, 0, 0), simply subtract the initial state vector from the final state vector…

Direct subtraction can be done with the Euler angles to generate difference angles; difference quaternions can be calculated as well but the transformations are more complicated than simple subtraction…

Best,

Greg

From: Tlera Corporation tleracorp@gmail.com Sent: Thursday, October 31, 2019 9:33 AM To: kriswiner/MPU9250 reply@reply.github.com Cc: Greg Tomasch poirot@earthlink.net Subject: Re: [kriswiner/MPU9250] Rotating the Euler angles to get proper roll, pitch, and yaw angles (#398)

Yes, I thought your question was how to get accurate absolute orientation estimation, sounds like you have done this already. Assuming you have an accurate absolute orientation estimation engine, then it sounds like you are asking me how to manipulate quaternions for a particular result. I still don't understand what you are after, and I am no expert on quaternion manipulations so I have included my colleague Greg on the thread who might be able to help better.

An accurate absolute orientation estimation engine provides Euler angles relative to a fixed world coordinate system such that YPR = 0,0,0 means the device parallel to magnetic North in Yaw, andwith pitch and roll perpendicular to Gravity. This is absolute, and all accurate absolute orientation engines will provide the same answer whether worn by different people or by the same person at different times, etc. So what I would do in your case as far as I understand it is to simply define a convenient reference (if parallel to True North and perpendicular to gravity is not) and measure deltas from it.

An accurate absolute orientation estimation engine provides this estimation in terms of quaternions as well, where the same reference is at Q = 1, 0, 0, 0 and it is true you can manipulate quaternions to get relative orientation wrt some other reference, but the mechanics is more complicated and less intuitive (at least to me) and the result has to be the same. If this is what you are after, perhaps Greg can help you, I cannot.

On Thu, Oct 31, 2019 at 5:28 AM rwboedhoe <notifications@github.com mailto:notifications@github.com > wrote:

Thanks for your answer, however, I don't know if we misunderstood each other. I am already able to get the yaw, pitch and roll angles. I looked at all the ingredients you mentioned and did all the steps to get the correct angle estimate. Now, however, I want to calculate the roll, pitch and yaw angle for any arbitrary orientation of the earth frame.

I would like to be able to redefine the orientation of the earth frame, where the earth frame will be defined by the direction of the gravity and the poles.

Some background information why I want to be able to change the direction of the earth frame:

I am developing a device that can measure the range of motion of the hip joint. To do this the device will be placed on the lateral part of the leg. I want that the Euler angles are equal to zero at the initial start of the measurement. Because the device will be placed on both legs and multiple persons will wear it, where not everyone has a similar leg bodybuild, I need to be able to redefine the earth frame.

I thought that the conjugate of the quaternion, that is used to determine the rotation, can rotate the angles back to zero. At any given orientation, described by a quaternion, the conjugate quaternion can be determined.

The new orientation that results in Euler angles that are equal to zero, because of the multiplication of this quaternion with it's conjugate, would define the new earth frame.

All quaternions, describing the orientation of the sensor, would be multiplied with this conjugate quaternion.

I hoped by doing this that I would be able to define the new earth frame for any given orientation. However, the new pitch, roll and yaw angles are incorrect. A rotation around the pitch axis results in a rotation in the pitch and yaw axes.

My questions to you are:

  1. Should I redefine the NED?
  2. Is my initial idea of rotating the quaternion correct?
  3. If your answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position?

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/398?email_source=notifications&email_token=ABTDLKWK4IIBFHWTOSXZ3FLQRLFPHA5CNFSM4JGZH722YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOECXSKDA#issuecomment-548349196 , or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKQB4XXWINPP5MT4F3TQRLFPHANCNFSM4JGZH72Q . https://github.com/notifications/beacon/ABTDLKQ6JROCQY2JKBEWJY3QRLFPHA5CNFSM4JGZH722YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOECXSKDA.gif

rwboedhoe commented 4 years ago

Hi Greg,

Thanks for your answer. You are correct that I would like to determine the difference in the quaternions to generate the new Euler angles. However, I would like to use the difference in quaternions instead of the difference in Euler angles. The rotation of the Euler angles can cause a singularity if the second angle of the rotation order reaches 90 degrees. If my initial state for the second rotation is e.g 45 degrees, I would be limited to a maximum of 45 degrees of rotation. For this reason, I prefer to calculate the difference in the quaternions.

I do not know if I am using the correct approach; Rotating the quaternion back to zero via the conjugate, determined at the new reference state, and multiplying every new quaternion by this conjugate does not give me correct angles. Do you know more about how to calculate the difference in the quaternions?

I apologies to ask you the same questions, but I am still confused.

  1. Should I redefine the NED?
  2. Is my initial idea of rotating the quaternion correct?
  3. If your answer to question two is yes, how should I do this?
  4. If the answer to question two is no, do you no an alternative method to rotate the reference plane to any given position?

RwBoedhoe

ahmetipkin commented 3 years ago

This is more than one year later; but I'd like to input here in case someone else bumps to the problem :)

@rwboedhoe your approach is actually correct; you're probably multiplying the quaternion in wrong order tho :)

Theory

Our 'world' coordinate system is constant. It is defined by our orientation definition (NWU, ENU, NED or so) and it doesn't change. What sensor/IMU (MPU9250 for instance) actually does is to give us a 'quaternion' so we can take that world coordinate system and translate it to 'sensor' coordinate system; which depends on our orientation. The sensor finds the 'world' coordinate system by using Magnetometer, Acceleration and Gyro input; by using a Fusion algorithm (like Madgwick or Mahony or such).

Assume that you have such arrangement that your sensors +X coordinate points to North; +Y points to West and +Z points upwards (you can use different of course; as long as you are consistent). Lets call this coordinate system W. Imagine that you have rotated around Z axis for 180 degrees (Yaw; in Euler terms); looking at the 'exact opposite' side. The sensor+fusion will now output you a quaternion that takes your \<i,j,k> world coordinate system and will make it \<-i,-j,k>. Note here that not your 'vector' is modified by quaternion but your 'whole world' rotated! Let's name this quaternion is qb and this orientation Ob.

We can analyze what happens here with a 'sample' vector; without loss of generality. Any vector (v) you have in your 'world' coordinates (say [1,1,0]) will have different coordinates in the second orientation and the equation for this would be v' = qb [v] qbT (i.e. quaternion conjugation operation). In the same manner; any vector (v') in this new orientation is described in world coordinates as v = qbT [v'] qb. (Note that you should convert these vectors into quaternions by adding 0 as 'constant' term, for instance making v into [0,v]; but that's implied here). Since this is valid for all vectors in W, we can write this relationship as *Ob = qb W** in 'closed' form, if you will .

Assume that now you take another orientation O2. The sensor+fusion will now give you another quaternion, say q2. Here this quaternion (as first one) is rotating your 'initial world system' into sensors orientation just as before, hence and same things all over again.

However; we don't want the rotation for W coordinates; we want it based on Ob orientation. What do we do then? Well; we consider the q2 as a 'complex' operation; as if it's actually a quaternion formed with some quaternion x applied before our quaternion qb; hence basically splitting O2 into 'sub components' :

*q2 = qb x**

We're after that x here; because it describes what rotation operation is done to reach O2 orientation from Ob. And it's very easy to find, by multiplying both equations with qb's inverse from left!

*x = qbT q2**

^^ This is the formula we're after!! ^^

Alternatively, you can consider this operation as such that we rotate O2 with respect to 'inverse of Ob' to find the difference.

Why and how does this even work?

Euler's theorem states that any orientation can be described as a rotation by angle θ around a particular vector v. Since 3D space is a linear space (at least we treat it to be 😄 ), any vector can be dissected into 'sub components' with some arbitrary basis, as long as the 'arbitrary basis' encapsulates the whole space as well (i.e. linear basis). In our case; any new orientation (basis) we form is guaranteed to be so; because we're using linear transformations (quaternions!).

When we convert a quaternion to Euler angles, what we actually do is first apply the given rotation quaternion to basis vectors of the initial (world) coordinate system and get the 'transformed' bases; and then use trigonometry to get the angles between those bases. Hence; if we describe the 'base rotation vector' on some other basis; we get the angles with respect to that basis.

Doing this with quaternions is safer; because Euler angles calculations has singularity cases due to requirement of using arc tangent function to calculate the angles (see Quaternion to Euler angles conversion). If Euler angles are used to do such transformation; one has to handle those singularity cases manually.

TL;DR

If you want to get the some quaternion q2 with respect to some other orientation (yielded by quaternion qb); multiply it with its conjugate (inverse) from left:

*qresult = qbT q2**

If you multiply from right; you'll get weird axes rotations, since instead of rotating the vector with respect to your base; you rotate your base with respect to quaternion then 😄