11

I've got an initial quaternion , q0. And I get angular velocity measurments, I integrate the the velocities so I got 3 angles at 50Hz or so. How can make a quaternion based on the 3 angles? I can't just make 3 quaternions, can I?

So to make it clear.

Q.new=Q.new*Q.update(alfa,beta,gamma)

Q.new represents my current orientation in a quaternion, I want to update it by multiplying with a Q.update quaternion. How can I make the Q.update with the angles?

Thanks!

Mike Pugh
  • 6,787
  • 2
  • 27
  • 25
user3598726
  • 951
  • 2
  • 11
  • 27
  • The sad news is: It is *a lot* more complicated than that. See for example [Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration](http://infolab.ulsan.ac.kr/research/orientation/paper.pdf). By the way, how are you going to deal with the gyro drift? – Ali May 06 '14 at 21:13
  • This question does not appear to be about programming within the scope defined in the help center. – Ali May 06 '14 at 21:14
  • Sorry guys. I just started working as a software developer 2 weeks ago and this is my first question here(the funny thing is i am a mechanical engineer :D). You are the best! About the drift? I Don't know yet. – user3598726 May 07 '14 at 18:25

5 Answers5

12

Forgive me thread necromancing, but the answers all seem to complicated and some, like me, may prefer this more 'convenient' approach:

Say omega=(alpha,beta,gamma) is the measured angular velocity vector of the gyros. Then we rotate

theta = ||omega||*dt; //length of angular velocity vector

many units (deg or rad dependent on gyro) around

v = omega / ( ||omega|| ); // normalized orientation of angular velocity vector

Thus, we can construct the rotation quaternion as:

Q.update = (cos(theta/2),v_x * sin(theta/2), v_y * sin(theta/2), v_z * sin(theta/2));

All that is left now is to rotate our current rotation by Q.update. This is trivial:

Q.new = multiply_quaternions(Q.update,Q.new); 
// note that Q.update * Q.new != Q.new * Q.update for quaternions

Done. Quaternions are beautiful, aren't they?

Some slides on gyros and quaternions that may be useful: http://stanford.edu/class/ee267/lectures/lecture10.pdf

Obromios
  • 15,408
  • 15
  • 72
  • 127
FirefoxMetzger
  • 2,880
  • 1
  • 18
  • 32
  • 2
    Why are you using Q.new = multiply_quaternions(Q.update,Q.new) and not Q.new = multiply_quaternions(Q.new,Q.update)? On page 26 of the script you linked they multiply by Q.update on the right hand side. – jrichner Jan 29 '19 at 14:59
  • This is a great solution, but what if the omega does not change in time? Then the normalization would be infinity. – anishtain4 Jun 28 '19 at 17:35
6

I presume you are integrating euler angles because you like to make your life difficult. First and foremost the gyro does not directly integrate into your euler angles. If you are asking this question I'm going to assume you also don't know how to properly find rate of change euler angles from your gyro measurements. You need a transformation matrix for that to work. I highly recommend picking up a copy of Farrell's "Aided navagition" On page 57 he explains how to calculate the transformation matrix to change your gyro rates into Euler rates. But why bother when you can get your rate of change quaternion directly from the quaternion and your gyro data:

rate of change quaternion  = qdot
quaternion = q
gyro quaternion = w = [0,gyrox,gyroy,gyroz]

so

qdot = 0.5 *  q Ⓧ w

where Ⓧ represents the quaternion product. Be careful with your frames here. The gyro represents the angular rate of sensor frame with respect to the inertial frame represented in the sensor frame. That means your quaternion needs to represent a similar rotation from the sensor to the inertial frame. In this case it should represent a rotation from the inertial frame to the gyro frame. If we disregard things like the rotation of the earth the preceding equation is valid.

Be aware about "Aided Navigation." In my opinion his treatment of quaternions is very confusing.

Michael Baker
  • 61
  • 1
  • 2
  • What about the order of roll,pitch,yaw quaternion multiplication because roll x pitch is not equal to pitch x roll in quaternion multiplication – user818117 Jul 10 '17 at 19:18
  • Can you please elaborate? Why gyro data can be used with 0 as rotation quaternion and what is the 0.5 in your `qdot = 0.5 * q Ⓧ w` means? should we scalar multiply the quaternion q? Where is the time scale gone? – Ariksu Oct 04 '17 at 22:36
  • 1
    Concerning the use of 0...The so-called gyro quaternion defined in this answer represents a vector in 3d space and as such it has a "scalar" part equal to zero. That is necessary to allow a rotation-quaternion (with non-zero scalar part) to effect a rotation on the vector when using quaternion multiplication. Key info here is that quaternions can represent both vectors(with three basis components) and rotations (which is a angle turn about a direction). These are two entirely different things. Admittedly it is confusion though. – J.E.Tkaczyk Feb 07 '18 at 20:18
  • Concerning the roll x pitch is not the same as pitch x roll. This is resolved by understanding that the three angles (gyroX * dt, gyroY *dt, and gyroZ * dt) are not Euler angles. These are the components of the rotation vector. That is they are rotations that occur about the x, y and z axes simultaneously as with the turn about the rotation axis. There are multiple Euler angle representations of this same rotation depending on what convention is used. Better to stick to the axis/angle representation which is closely related to quaternions – J.E.Tkaczyk Feb 07 '18 at 20:25
  • Concerning the factor of 0.5. I believe this relates to the fact that the rotation angle g about an axis with components (x,y,z) corresponds to the quaternion: [cos g/2, sin g/2 x (x,y,z)]. Note the half angles. However, this answer does not build the quaternions correctly. See better answer below. It is also described here [quaternion rotation](https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation) – J.E.Tkaczyk Feb 07 '18 at 20:47
5

You can just integrate your angular velocity to get angular position (as Euler angles), convert the Euler angles to Quaternion, then multiply the Quaternion to accumulate the orientation.

Suppose your input is given by a 3D vector of angular velocity: omega = (alpha, beta, gamma), given by degrees per second. To get the Euler angles, E, in degrees, multiply omega by the change in time, which we can call dt. This results to:

Vector3D omega = new Vector3D(alpha, beta, gamma);
Vector3D E = omega * dt;

You can get dt by subtracting the current time by the time of your previous update. After getting the 3D Euler angles from gyroscope data, convert it into a Quaternion (w,x,y,z) through this equation (from Wikipedia):

float w = cos(E.x/2) * cos(E.y/2) * cos(E.z/2) + sin(E.x/2) * sin(E.y/2) * sin(E.z/2);
float x = sin(E.x/2) * cos(E.y/2) * cos(E.z/2) - cos(E.x/2) * sin(E.y/2) * sin(E.z/2);
float y = cos(E.x/2) * sin(E.y/2) * cos(E.z/2) + sin(E.x/2) * cos(E.y/2) * sin(E.z/2);
float z = cos(E.x/2) * cos(E.y/2) * sin(E.z/2) - sin(E.x/2) * sin(E.y/2) * cos(E.z/2);

Quaternion q = new Quaternion(w, x, y, z);

Just copy-paste the two code snippets above into your Q.update() method then return the Quaternion. If you want to know how the equation works, just check the Wiki link and read through it.

Enrico Tiongson
  • 400
  • 4
  • 5
  • 2
    I'm pretty sure this is wrong. The first part is close. The integration of the angular velocity gives you the change in angular position which is a rotation from the starting orientation. The rotation vector has components x,y and z, but, significantly, these are not Euler angles. The rotation occurs about the rotation axis and simultaneously about the three axes. Another answer here gives the fairly close relationship of x,y,z to the quaternion. Euler angles describe rotation as a sequential set of three rotations and there are multiple conventions for the sequential order. – J.E.Tkaczyk Feb 07 '18 at 20:09
  • 1
    Integrating angular velocites won't give you euler angles. The gyro data give change rates in the body reference system, which is different to euler angles. – user3207838 May 03 '19 at 08:35
  • This is wrong as `E.x=pi/2` gives the same rotation as `Ex=pi/2+2*pi` – anishtain4 Jun 28 '19 at 17:55
0

You could convert these angles to a single quaternion and then perform the operation you described, or you could either convert each one of them to an axis-angle pair and then to a quaternion, and then multiply the three quaternions together. Refer to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles and http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 to more details.

edu_
  • 850
  • 2
  • 8
  • 16
  • -1 As I say above in the comment, it is *a lot* more complicated than that. Please delete this answer. – Ali May 06 '14 at 21:15
  • I assumed he already had the rotation (alpha, beta, and gamma) with respect to the last orientation, and just wanted to update the current orientation by multiplying the two quaternions. He originally asked the following: "How can make a quaternion based on the 3 angles? I can't just make 3 quaternions, can I?". For this question, I don't see why my answer should receive a -1. However if you interpreted his question as: "How can I obtain orientation from a gyro", then of course, it is a lot more complicated, and other factors such as gyro drift should be considered before integrating anything. – edu_ May 08 '14 at 01:39
  • Yes, it seems to me that what he ultimately wants; it looks like he is trying to track the orientation. [From his comment](http://stackoverflow.com/questions/23503151/how-to-update-quaternion-based-on-3d-gyro-data#comment36086555_23503151) it is obvious that he does not know about the gyro drift and how to deal with it. Telling him how to do the update is not help but harm as you help him go further on the wrong the track. The sooner he realizes the difficulties, the better, and that's what I consider as actual help. – Ali May 08 '14 at 10:57
  • Agreee. What happened was that I answered his original question before the posted the comment. Anyways, if he has an IMU (i.e. a gyroscope together with an accelerometer), I suggest he looks into Madgwick's orientation filter. There are lots of resources in http://www.x-io.co.uk/ – edu_ May 08 '14 at 14:16
  • I know about the drift, I correct it with an accelerometer using complementary filter :D i figured everything out, ill post the code when its ready – user3598726 May 09 '14 at 11:28
0

1) The update itself is simple. Just create quaternion from "axis angle"representation from angular velocity.

BWT, common way to represent "angular velocity" is vector of 3 scalars. Vector is parallel to axis of momental rotation in LOCAL object frame. So your equation will look: Q.new=Q.update(alfa,beta,gamma)*Q.new. http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions

And take into account the time of integration (simplest approximation, Euler integration) Q.new = Q.fromVector(angularVelocity * deltaTime)*Q.new

Also NOTE, angularVelocity * deltaTime produce "exponential map" rotation, that easy can be converted into quaternion.

For precise integration , more complicated approximation should be used. But you must know exact meaning of your measurements (time of measurement, noise and more).

2) The function update is not clear from your question. What is (alfa,beta,gamma) parameters? If this is a momental rotation velocity about 3 orthogonal axis , than you can build angular velocity from this simply. Just be sure in proper units (radians per second).

3) The way to get useful integration of accelerometers data is too complicated for short answer. Every hardware should be handled with own custom properties. Also it should fuse data from linear acceleration to avoid drift.

minorlogic
  • 1,872
  • 1
  • 17
  • 24
  • As i said , too few information. I can guess it can look like : update(alfa,beta,gamma) { Vector3 r = Vector3(alfa,beta,gamma) * deltaTimeSec; double l = r.length(); Quaternion rQ = (0.5*r.x*sin(l)/l, 0.5*r.y*sin(l)/l), 0.5*r.z*sin(l)/l, 0.5*cos(l) ); return rQ; } – minorlogic May 08 '14 at 07:56