27 replies to this topic

[Korken]

Hi all! This is my first real post on this forum.

I have for a while now, while studying at University (towards Master in Engineering Physics and Electrical Engineering), been developing different kinds of AHRS-filters with some success and I'd firstly like to talk a little about one before I go to my real question.

My first real/good AHRS that I developed was for me to really learn and use different filtering techniques but also I had an idea.
Many filter I had looked at used a DCMs as base for their filters. These were nice to look at and I understood exactly what was going on all the time. But most often some type of non-linearity or design choise made the filters very large (9-13 states) and after examining where most computing power goes into a filter (which is, for linear Kalman, the NxN inverse) I wanted to make much simpler filters without the need of extremely large state-vectors.
The idea I got then was to use two Kalman filters in cascade. One that estimates the attitude (the 3rd row of the DCM) and one the estimates the heading.
This effectively made the filter go from a one 12 state filter to two 6 state filters. The inverse of a 12x12 matrix was about 10k floating ops and the inverse of two 6x6 matrices was about 1.2k floating ops, effectively making the filter about 8-9 times faster with the same precision.

But now to my real question. The last year I've been studying Quaternions at my spare time and I believe I can use those for making even better filters.
But I'm stuck. I have grave problems when trying understanding how use them when I have three rotations.
What I mean is that I have a rotation around X, Y and Z, but the rotation quaternion wants a vector to rotate about and an angle.
How do I get the net vector and net rotation? Because I don't want to make 3 multiplications.
Is there a way to do this?
Or am I going at this the wrong way? Because I want to calculate my prediction using quaternions resulting in a quaternion. Instead of using DCMs for the prediction and states.
Perhaps that someone could help me understand how to make predictions using quaternions?

I saw in a video about OP that it used some sort of quaternion based filter so I hope someone here can help me with this and thanks for a great forum!
If I was unclear about something just ask!

Regards
Emil Fresk
Luleå University of Technology
Sweden

Edited by Korken, 08 July 2012 - 02:25 PM.

[Kenn Sebesta]

Check out Representing Attitude - Euler Angles, Unit Quaternions, and Rotation Vectors. It will answer many of your questions.

You're right, you should avoid switching back to DCM for the updates, once you're already working in quats.

[bluehash]

Looks like you forgot the link on that Kenn.. Link.

[Kenn Sebesta]

I just copy/pasted the title from my papers directory. So I wouldn't say "forgot", per se, I would say "too lazy."

[Korken]

Thanks you, I will start reading that document at once!

[peabody124]

One thing I would recommend - with quaternions give up the idea they should be "intuitive", that's a dream. Just think in terms of operations - you want ot rotate a 3D vector based on a quaternion rotation? That's fine, there is an operation for that. They are a representation and htey define a set of operations and that is enough.

e.g:

What I mean is that I have a rotation around X, Y and Z, but the rotation quaternion wants a vector to rotate about and an angle.
How do I get the net vector and net rotation? Because I don't want to make 3 multiplications.
Is there a way to do this?

Three of the components define that direction so it's trivial to get. More importantly, I'd question why you want to get that vector directly, but rather you want to do your prediction state directly on quaternions. In the compllimentary filter code, for example, you'll see based on hte gyros (in body referenced frame) it's fairly simple to compute the delta quaternion that results in and to integrate that.

[Korken]

peabody124:
Thanks for your response!
No, I do not want to use ordinary 3D vectors anymore. I want to cross the bridge and go full quaternion so that my filter works only in the quaternion region and uses the associated math.
If I want other types representations I would have to translate the quaternion to a DCM or Euler angles.
For as I understand I can design my regulators so they can use quaternions and regulate on these, so there is no reason to go back one step to DCM or Euler.

Basically I want to shed the skin of simple and go for better performance. In a way to put it.

Edited by Korken, 08 July 2012 - 08:29 PM.

[peabody124]

Some things are still going to be 3D vectors, eg. your sensors in the body referenced farme. There are still efficient rotations based on quaternions to use o these.

[Korken]

Yes, ofcurse! I don't expect to get away without any transformations.
I'm just not that sure of how everything works in the quaternion world and I don't have anyone to ask at the university so prepare for many questions.

[Jurek]

I don't have anyone to ask at the university

how true that is . . . i also went to the university hoping that there will be people to talk to. Seems that this is extremely rare tough . . . .

[peabody124]

Honestly there are two wikipedia pages on quaternions and spatial rotations that are quite good. Then the rest is googling for the particular transform you want. As I said earlier, IMO the trick is to not assume there is something to "get" and treat it as an abstract operator you just look up and use.

[D-Lite]

No, I do not want to use ordinary 3D vectors anymore. I want to cross the bridge and go full quaternion so that my filter works only in the quaternion region and uses the associated math.

You may want to have a look at the work of Seb Madgwick. He did a very nice and efficient quaternion implementation of Mahony's DCM algorithm (enhanced with his own magnetic distortion compensation algorithm).

[RoyLB]

One of Mahoney's papers shows one way to use quaternions in the control loop

http://code.google.c...rs.zip&can=2&q=

(there are many more papers and books I could reference, but I am too lazy to even post the links ).

- Roy

[Korken]

Hi again!

Wonderfull documents and references in the documents! Thanks for the links, very good reading!
One thing I however am unable to quite grasp yet. When, in a (E)KF, you create the H matrix (maps measurements to states), how are you supposed to map the acceleration and magnetic fields to the quaternion state?
I have seen as example in "A DCM Based Orientation Estimation Algorithm with an Inertial Measurement Unit and a Magnetic Compass" they have an quaternion example. They do the mapping as shown in Eq. 25.
And they don't have the reference on the form state = H*measurement, but on the form measurement = H(state)*reference. I'm not familiar with this type off approach.

How I thought it should be done is that you map the magnetic and acceleration vector to quaternions and from there you create the H matrix.
Have I misunderstood how it should be done? Or is there a special way of doing it?

Regards
Emil Fresk

Edited by Korken, 12 July 2012 - 05:29 AM.

[RoyLB]

I think they're just trying to formulate all the estimates in the body frame, so they can compare with the sensors and compute the error, but I have not read it in detail (see above re: lazy). I'm sure there are many ways it can be done. i suspect its just computationally more efficient to compute the DCM once (from the quaternion in the case of that paper) then use it to transform all the measurements than to compute individual quaternions or DCMs for each sensor and calculate the errors in quaternion (or DCM) space.

- Roy

[Korken]

I see, but is that not a bad solution?
Because this method needs to know where the gravity and magnetic vector is in the earth frame.
Would this not make a need to know both for every location where you will fly? Ex if I fly is Sweden the magnetic vector is one thing and if I fly in Africa it's another.
There surely must be a better solution to this. Perhaps map the magnetic vector to the horizontal plane?

Edited by Korken, 13 July 2012 - 09:40 AM.

[RoyLB]

I see, but is that not a bad solution?
Because this method needs to know where the gravity and magnetic vector is in the earth frame.
Would this not make a need to know both for every location where you will fly? Ex if I fly is Sweden the magnetic vector is one thing and if I fly in Africa it's another

Presumably the algorithm would need to perform some inverse transformations in order to initialize itself.

.
There surely must be a better solution to this. Perhaps map the magnetic vector to the horizontal plane?

I thought the Madgwick algorithm did that? This is a better link than the one above:

http://www.x-io.co.u...ahrs_algorithms

- Roy

[Korken]

Yeah I have been looking at that, but there are a few things I don't grasp about it.
If you look at the code then first he rotates the mag-measurement by the in-homogeneous DCM(q):

hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);

All okey so far.

After this he calculates the magnitude of the mag in the plane and vertical:

bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

This is where it starts to get strange. Why doesn't he just map it to the plane? Is it to prevent gimbal lock when pointing straight up?
But still, I can accept this, he wants to know the vertical and horizontal magnitude.

Then he estimates the gravity vector from the 3rd row of the homogeneous DCM(q). That row has 1:1 mapping of the gravity vector, so no problems there.

vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;

But then comes the big problem. What does he do with the mag-readings? He takes the first and last element of every column in the in-homogeneous DCM(q) and sums them. Why does he do that? I have no way of relating that to earths magnetic vector. Could someone please explain what's going on there?

wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

It's as if he had (matlab-ish code): [bx, 0, bz] * DCM(q), but I can't understand why he would want to do that.

I hope you guys understand my problem! Else tell me and I will try to emphasize by problem.

[Korken]

One other thing that popped to mind is that what he could be doing is first DCM(q)*[mx; my; mz] and then doing the inverse transform DMC(q)^-1*[bx; 0; bz].
The equations do come out right then.
But that puzzles me even more! Because if I have a measurement in the body-frame and then right-hand rotates it with the DCM(q) you don't get the earth frame as is seems he has got!
You should have first done DCM(q)^-1*[mx; my; mz] and then DMC(q)*[bx; 0; bz].

There is something I haven't gotten correct here. But what is it?
I can't find my error.

[RobCC]

Hello,
Have you read this ?
http://www.x-io.co.u...rnal_report.pdf
Rob