calculating orientation and location using Mahony quaternions

[Admin]

I've been reading as much as I can but the information refuses to enter my brain. Time for me to ask for help . . .

I have an IMU spitting out 9 DoF of data. Given that, I'm trying to figure out the x/y/z and rotation coordinates over time using an Axon Mote microcontroller.

It appears what I need to use is the Mahony algorithm. It takes in that data and spits out four quaternions. But I'm not entirely sure what to do with those quaternions that I get.

1)
Given a series of calculated quaternions over time (using MahonyAHRS.c), how do I calculate the final x/y/z coordinates? And how do I determine the angular orientation? C example code would be sufficient.

2)
Second, looking at the top of his code, I see:

volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame

How do I calculate this so as to get my own initial constants? Is it just 180 degrees if I see 1, and 0 degrees if I see 0? Which quaternion for which axis of rotation?

3)
I also see this:

#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain

Am I correct to assume Mahony uses PI to control the error of the quaternions? Any tips on determining these beyond the usual guesswork/experimentation with my IMU?

[jwatte]

I'm not familiar with the Mahony algorithm, but I am familiar with quaternions.

Unit Quaternions are a representation of a three-dimensional orientation. A unit vector multiplied by a quaternion will be taken from the "basis" orientation to the orientation represented by the quaternion.

From your questions, it sounds like you're not actually clear what an orientation transform is, though. An orientation transform can be thought of as a typical X/Y/Z vector "tripod" (red/green/blue arrows in most 3d software.) The default "world" or "base" orientation typically has X to the right, Y up, and Z out of the screen (but there are others -- like X east, Y north, and Z up in a geocentric coordinate space, for example.)
The tripod representing a quaternion would then have its "X" "Y" and "Z" arrows pointing in the direction they would be transformed to, if you put in the unit X, Y or Z vectors from the base orientation. For unit quaternions, this wlll be a rigid, non-scaling, non-shearing, non-translating transform -- similar to a 3x3 rotation matrix where all the rows (and columns) are unit length and orthogonal.

Quaternions are represented as a vector (xyz) and a scalar (w) and the two unit quaternions that represent "identity" (no change) have the vector 0,0,0, and the scalar 1 or -1. (Yes, quaternions cover the parameter space of 3D rotations twice, just to make things extra exciting :-)
I would assume that the quaternion (1,0,0,0) in your example represents identity -- no change relative to world. However, because the code doesn't say whether it puts w first or last (both are accepted conventions,) it's hard to tell. It could also be the 180-degree rotation around the x axis, represented by (x,y,z)==(1,0,0) and w == 1.

Note that a "180 degree rotation" is not well defined -- there exists an infinite number of rotations that rotate 180 degrees, and each of them will make the transformed input end up in a different orientation! For example, consider rotating 180 degreers around the X axis, versus the Y axis, versus the Z axis. The end results are very different.

A unit quaternion should have length 1, so sqrt(x*x+y*y+z*z+w*w) should be 1. Using this, you can calculate the quaternion representing a rotation of R around unit-length axis A as:
w = cos(R / 2)
xyz = A * scale (where scale makes the whole quaternion length 1 -- I'm too lazy to look up the function, but it's on Wikipedia and Mathworld and all over the web, really, and even pretty easy to derive yourself :-)

Finally, when you say "what is my x, y, z," what do you mean? X, Y, Z are typically used alone for translation, and quaternions are typically not used with translation at all. And when you say your "angular orientation," what angles do you mean? A quaternion is already a representation of orientation. Do you mean "heading, pitch, roll"? If so, relative to what coordinate frame? X axis as forward? Note that heading/pitch/roll relative to a fixed world reference behaves very unintuitively when you're not mainly pointing "forward."

[Admin]

Finally, when you say "what is my x, y, z," what do you mean? X, Y, Z are typically used alone for translation, and quaternions are typically not used with translation at all. And when you say your "angular orientation," what angles do you mean? A quaternion is already a representation of orientation. Do you mean "heading, pitch, roll"? If so, relative to what coordinate frame? X axis as forward? Note that heading/pitch/roll relative to a fixed world reference behaves very unintuitively when you're not mainly pointing "forward."

I'm still going through your response, but wanted to clarify my question.

Assume my robot starts at position 0,0,0 (x,y,z coordinates) and has orientation a,a,a (heading, pitch, roll). It moves around a bunch, gets blown by the wind, goes up and down, etc., for a few minutes. Let's say I calculated the quaternions every 0.01s. Given that list of quaternions, how do I calculate the current x/y/z coordinates, and a,a,a?

[Admin]

Ok, I've now carefully read through your post . . .

I actually did quite a lot of reading of quaternions, but I don't really understand how to convert it to 'normal' rotational degrees from the horizontal.

The Mahony algorithm, to me, is a magic black box that takes IMU data as an input and then outputs the calculated quaternions.

I then just need to figure out how to relate that to robot rotation + translation so I can control the thrusters appropriately. But that's where I'm stuck . . . I'm sure there is code out there I can just copy/paste, but can't seem to find it . . .

[jwatte]

Let's say I calculated the quaternions every 0.01s. Given that list of quaternions, how do I calculate the current x/y/z coordinates, and a,a,a?

Given that unit quaternions only represent orientations, you cannot calculate x/y/z from that. You need to also integrate the forces. The quaternions (orientation of drone in world space) plus forces (acceleration of drone in world space) allow you to translate the forces to world space and then integrate to arrive at world-space position. If you have a quaternion Q that represent orientation (transform from world to drone space) then multiplying the force vector by Q' (conjugate) takes that vector from drone space to world space.

Also, again, "heading pitch roll" in world space is almost entirely useless. Heading is useful for knowing which compass direction "forward" points, but the pitch and roll meanings change as heading changes. Perhaps you want Tate-Bryant orientation order? (Each of heading, pitch, and roll are expressed relative to the one before it.) Even so, it's only useful for display, not typically for actual math. Turning the quaternion into a 3x3 rotation matrix and plotting the basis vectors would be a better visualization in my experience.

The varaible here that I know nothing about is the Mahony quaternions. There are ways you can use multiple non-unit quaternions to represent movement; perhaps that's what they are doing?

[Admin]

Roll, pitch, and yaw are useful for stability. I was using a complimentary filter which linearizes and averages to get roll/pitch/yaw estimates, but the math ended up getting very complicated once I added in translation.

Let's say a quadrotor lifts off the ground and experiences acceleration(s) for 10s. Its' rotation oscillates because of winds. Given the history of orientations and accelerations, what is the final robot altitude in meters? If the robot had perfect unchanging orientation, translation is simply:
1/2 * accel * dt^2.

A pressure sensor or GPS has insufficient resolution for flying inside.

You can inspect the Mahony code here:
https://code.google.com/p/moveonpc/source/browse/external/MahonyAHRS/MahonyAHRS.c?name=mater
Mahony has published papers on it too, but I'm no mathematician . . .


ps - I've written up code that compiles so when I get home tonight I'll just see how quaternions are affected in real time when I rotate the sensor by hand. Maybe it'll be a no brainer once it leaves incomprehendable  mathematic notation and enters the comprehendable real world.

[jwatte]

Roll, pitch, and yaw are useful for stability

Roll, pitch, and yaw only make sense as relative to the vehicle. Unfortunately, if you convert your pose quaternion to RPY, you get RPY of the vehicle relative to the identity pose. As you deviate from the "straight ahead" heading, your pitch and yaw will become interrelated, and likely not represent what you think they represent.

Much better is a 3x3 rotation matrix that gives you the vectors that X, Y, and Z vehicle directions point in identity world space. You can then dot your own X with world up to see whether you're pitching down or up, and dot your own Y with world up to see whether you're canting left or right. (Assuming local X forward, Y left, Z up -- adjust for whatever convention you use.)

Similarly, if you can get your local X, Y and Z vectors in world space (which is what a rotation matrix is) then you can simply multiply the force vector you get from the acceleration sensor by that, multiply by time, and integrate into position. In column-vector-on-right systems, the first column of the pose rotation matrix P will be the world direction of your X vector, and thus P * acceleration A turns into the math of A.x scaling column 1 of P, A.y scaling column 2 of P, and A.z scaling column 3 of P.

[Admin]

Attached is the quaternions calculated after moving my IMU up 2 feet, then moving it back down 2 feet.

At no time did I rotate the IMU (well, my complimentary filter said it rotated by +/- 4 degrees, but that's relatively insignificant).

Here is my question: what is the equation to convert those four quaternions to determine the roll, pitch, and yaw. We already know the answer: 0,0,0. It didn't rotate. But how do I calculate that knowing only the quaternions?


note: the first two seconds I didn't move the IMU to get a baseline.

[Webbot]

Hmm - quaternions - always did my head in!

At first sight there are a number of things you should check:-
1. The algorithm. The invSqrt function uses a trick that may only work on certain platforms as it casts between a long and a float - which only works if the floating point format obeys a certain standard. Haven't looked at the datasheets so perhaps you should double check, in a standalone program, that this functions gives reasonable guesstimates as its answer.
2. Not sure what IMU board you are using but you will often find that manufacturers place chips at their convenience - ie the accelerometer chip x,y,z axis may be completely different to that of the gyro chip even tho its on the same board. So if you are reading the values from the chip you may need to flip these values before sending them to the quaternion algorithm.

If you are just interested in roll, pitch and yaw then I did some work on AHRS with the Razor. SF made various revisions of the board using different chips any they all suffer from point 2 above (and each board revision is also different). The AHRS code or DCM (directional cosine matrix - google it for a great paper) has been part of webbotlib for a while. If its of interest then see http://webbot.org.uk/iPoint/49.page as it gives some examples.

One of my 'wants' with WebbotLibStudio is that you can take a chip and specify its x,y,z axes; place it on a board with other chips that have axes; and for it to normalise the axes readings based on a given orientation. Equally if this is a breakout board and you use it in your project then you should be able to say what orientation has been used to mount the breakout board on your robot. Given this info I can always give readings based on a fixed co-ordinate system with regard to the robot - ie could be z=direction of travel, y=right side of robot, x=going from the robot upwards.

[Admin]

I'm using this IMU:
http://www.pololu.com/catalog/product/1268


For #1, I had found this awhile back:
http://www.diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root
The guy says it has 'unpredictable behavior', but his issue doesn't seem to be clearly resolved judging by the thread.


For #2, I'll look into that again. But still not sure what quaternion values I should be getting, so it's hard to identify if I made the #2 mistake . . . 
I'll play around with it . . .

The AHRS code or DCM (directional cosine matrix - google it for a great paper) has been part of webbotlib for a while. If its of interest then see http://webbot.org.uk/iPoint/49.page as it gives some examples.

ok thanks, I'll look through that.


I've worked more on my linearized complimentary filter today, and it's decently good if the IMU doesn't rotate beyond 45 degrees from the horizontal. But calculating translation is tricky . . .

[jwatte]

Here is my question: what is the equation to convert those four quaternions to determine the roll, pitch, and yaw. We already know the answer: 0,0,0. It didn't rotate. But how do I calculate that knowing only the quaternions?

I don't see four quaternions; I see four scalars. Taken together they might form a quaternion (I'd assume the blue q is the "w" part of the quaternion.)

Looking at that quaternion, what you get out is clearly not an identity quaternion in the end -- it looks like it suggests a > 100 degree rotation. Perhaps that's because of error accumulation, or because of something else.

To turn a quaternion into HPR, you can convert quaternion to rotation matrix, and then decompose the matrix.

Or you can transform your "forward" vector (typically, "X") and call atan2() on the y and x you get out, to get heading, and do the same thing with your "side" and "up" vectors for whatever convention you're using. Which, btw, you haven't specified yet!
What vector would you expect to be "forward"? What vector would be "up"?

[Admin]

Still struggling . . . can't seem to get correct results from either DCM or Mahony quaternions. I probably have an axis or an initialization or some other error somewhere. I'll toy around another week before I post again in desperation

That said, I'm pretty sure I understand quaternions now. This image mostly verifies what I'm thinking in my head:
http://browse-code.mrpt.org/stable/doc/design_of_images/quaternion.gif

although I still don't quite understand the identity value and what it does . . .

Also, I found the equations to convert quaternions to euler angles (yay!) here.



Calculating linear translation given any euler angle is super easy . . . a more accurate curve interpolation is for another day . . .

[jwatte]

Does that equation assume q0 == w or q3 == w?

Also, from my experience with 3d graphics and physical simulation, you are making a mistake to want to go "back" to Euler angles or HPR. Work in vector math, and use quaternions or 3x3 matrices for orientation. Basically, anytime you use trigonometric functions in runtime code, you're doing something wrong!

Print/display your "forward" and "up" vectors if you want to understand the orientation for debugging purposes.