How to update a quaternion based on 3D gyro data?

Question

I have an initial quaternion, q0. And I get angular speed measurements, I integrate speeds, so I got 3 angles at 50 Hz or so. How to make a quaternion based on three angles? I can't just make three quaternions, can I?

So, to be clear.

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

Q.new represents my current orientation in quaternion, I want to update it by multiplying Q.update quaternion. How can I do Q.update with corners?

Thanks!

Answer 1

You can simply integrate the angular speed to get the angular position (like Euler angles), convert the Euler angles to Quaternion, and then multiply the Quaternion to copy the orientation.

Suppose your input is given by the three-dimensional angular velocity vector: omega = (alpha, beta, gamma), given in degrees per second. To get Euler angles, E, in degrees, multiply omega by the change in time we can call dt. This leads to:

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

You can get dt by subtracting the current time at the time of your previous update. After obtaining the three-dimensional Euler angles from the gyroscope data, convert it to Quaternion (w, x, y, z) through this equation (from Wikipedia ):

 float w = cos(Ex/2) * cos(Ey/2) * cos(Ez/2) + sin(Ex/2) * sin(Ey/2) * sin(Ez/2); float x = sin(Ex/2) * cos(Ey/2) * cos(Ez/2) - cos(Ex/2) * sin(Ey/2) * sin(Ez/2); float y = cos(Ex/2) * sin(Ey/2) * cos(Ez/2) + sin(Ex/2) * cos(Ey/2) * sin(Ez/2); float z = cos(Ex/2) * cos(Ey/2) * sin(Ez/2) - sin(Ex/2) * sin(Ey/2) * cos(Ez/2); Quaternion q = new Quaternion(w, x, y, z); 

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

Answer 2

I assume that you integrate Euler angles because you like to make your life difficult. First of all, the gyroscope does not directly integrate into your Euler angles. If you ask this question, I’m going to assume that you also don’t know how to correctly find the rate of change of Euler angles from your gyroscope measurements. For this you need a transformation matrix. I highly recommend building a copy of Farrell "Aided navagition". On page 57, he explains how to calculate the transformation matrix in order to change the gyroscope speed to Euler speed. But why bother when you can get your quadrantion of speed change 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 Ⓧ is the product of quaternions. Be careful with your framework here. The gyroscope represents the speed of the angular frame of the sensor relative to the inertial frame presented in the frame of the sensor. This means that your quaternion should represent a similar rotation from the sensor to the inertial system. In this case, it should represent a rotation from the inertial frame to the gyroscope. If we neglect such things as the rotation of the Earth, then the previous equation holds.

Remember the "help navigation". In my opinion, his treatment of quaternions is very confusing.

Answer 3

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

Say, omega = (alpha, beta, gamma) is the measured velocity vector of gyroscopes. Then we travel

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

many units (degrees or rad depending on the gyro) around

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

Thus, we can construct a quaternion of rotation as:

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

All that remains now is the rotation of our current rotation on 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 gyroscopes and quaternions that may be useful: http://stanford.edu/class/ee267/lectures/lecture10.pdf

Answer 4

You can convert these angles into one quaternion, and then perform the described operation, or you can convert each of them into a pair with an axial angle, and then into a quaternion, and then multiply the three quaternions together. See http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles and http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 for more details.

Answer 5

1) The update itself is simple. Just create a quaternion from the axis-angle view from the angular speed.

BWT, a general way of representing "angular velocity" is the vector of three scalars. The vector is parallel to the axis of rotation in the LOCAL object. So your equation will look like: Q.new = Q.update (alfa, beta, gamma) * Q.new. http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions

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

Also NOTE, angularVelocity * deltaTime rotates the "exponential map", which can easily be converted to a quaternion.

For accurate integration, a more complex approximation should be used. But you must know the exact value of your measurements (measurement time, noise, etc.).

2) Function update is not clear from your question. What are (alpha, beta, gamma) parameters? If this is an instantaneous rotation speed of about 3 orthogonal axes, you can simply build the angular speed. Just make sure in the correct units (radians per second).

3) The way to obtain a useful integration of accelerometer data is too complex for a short answer. Each equipment must be processed with its own custom properties. In addition, it must fuse data from linear acceleration to avoid drift.