Calculating quaternions from gyro + accelerometer data

Question

I have gyroscope + accelerometer data at each time period T.

Using C++, I want to calculate the rotation of the object at each time - it can rotate on its axes. I've read that it is convenient to represent the rotation of the system in terms of quaternions (not in Euler angles).

How can I transform from angular velocity (from gyroscope) to the quaternions representation? I think in order to do it I need to solve the differential equation using numerical methods.

Answer 1

I'm not sure which language you're looking for, but the C++ Boost library has a working Quaternion class (quaternion.hpp). I've used this library to create a simple rotation class for computing the results or rotating points about arbitrary vectors with very little difficulty.

UPDATE: Based on your comment, I don't think you necessarily need to use a quaternion library to figure out your angular position at a given time, given either a constant angular velocity or angular acceleration. All you need to do is to figure out what that angle is, and then use the quaternion class to compute the position of vectors when rotated about your rotation vector by the angle you've computed.

Given a constant angular acceleration α, an initial angular velocity ω(t₀) and initial angular position θ(t₀) in the range [0, 2π) the angular position at some time t > t₀, θ(t) is given by:

θ(t) = [θ(t0) + ω(t0)*(t-t0) + α*(t-t0)2/2] mod 2π

Here the mod 2π operation is simply the residual when subtracting n2π where n is the integer required to ensure the residual is in the range [0, 2π). For a constant angular velocity (i.e. α=0) the last term drops out.

That said, all you really need to do is to keep track of the angle over some interval of time under constant acceleration (or determine the average acceleration over that time if it's not constant) and update the angle. You then apply the resulting rotation about your rotation vector to the quaternion you're using to accumulate your rotations. This can be easily implemented as a C++ class.

Still, if you're looking for an open source tool to do this, I expect that any of the game physics modeling libraries will be more than adequate. A couple of open source ones are Bullet and the Open Dynamics Library.

Answer 2

Would you be talking about a Slerp? (Spherical Linear Interpolation)

See Jonathan Blow's article "Understanding Slerp, Then Not Using It" which has example source in C++...

http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/

Answer 3

Each sample from your gyroscopes represents a small rotation:

rot_x = angV_x * timestep
rot_y = angV_y * timestep
rot_z = angV_z * timestep

If the resulting rotations are small, you can convert this directly into a quaternion by taking half the rotation angle:

// for small rotations, quick & dirty quaternion is sufficient
// (note: all angles *must* be in radians!)
float k = timestep * 0.5;
quaternion raw_delta_Q(1.0, angV_x*k, angV_y*k, angV_z*k);  // unnormalized!

// combine rotation for current timestep with orientation state
estimated_orient_Q *= raw_delta_Q;  // multiply by unnormalized delta
estimated_orient_Q *= 1 / norm(estimated_orient_Q);  // then renormalize it!

If your rotations are larger than a few degrees, or if you need maximum accuracy, you will need to pay closer attention to how you get your quaternion.

EDIT: Note that the above code assumes *= is defined to do quaternion multiplication by both quaternions and real scalars. Some form of these functions (as well as the obvious constructor) will be present in any reasonable quaternion library.

Answer 4

What language? E.g. for Python cgkit has a nifty quat module (quaternions initialized from a rotation matrix, not "an angular velocity", but presumably you can build the former from the latter?) and euclid.py has Python source code for a Quaternion class including class methods to build it from rotation matrix and in other ways.