Question

I have a quaternion (4×1) and an angular velocity vector(3×1) and I call a function to calculates the differential quaternion as explained in this web. The code looks like this:

    float wx = w.at<float>(0);
float wy = w.at<float>(1);
float wz = w.at<float>(2);
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0);
float qy = q.at<float>(1);
float qz = q.at<float>(2);

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy);    // qdiffx
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz);    // qdiffy
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx);    // qdiffz
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz);   // qdiffw

So now I have the differential quaternion stored in q and then I update the quaternion by simply adding this differential quaternion.

Is this method suitable for predicting movement of rigid objects or is there a better method to predict quaternion with angular velocity? This works but I am not getting the expected results.

Answer 1

There are a couple things that might be going on. You don’t mention re-normalizing that quaternion. Bad things will definitely happen if you’re not doing that. You also don’t say that you multiply the delta-quaternion components by the amount of time that has passed dt before you add them to the original quaternion. If your angular velocity is in radians per second, but you’re only stepping forward by a fraction of a second, you’ll step too far. However, even so, since you’re stepping through a discrete amount of time and trying to pretend that it’s infinitesimal, weird things are going to happen, particularly if your timestep or angular velocity is large.

The physics engine, ODE, provides the option to update a body’s rotation from its angular velocity as though it were taking an infinitesimal step or to update using a finite sized step. The finite step is much more accurate, but involves some trig. functions and so is a little bit slower. The relevant ODE source code can be seen here, lines 300-321, with code finding the delta-quaternion here, line 310.

float wMag = sqrt(wx*wx + wy*wy + wz*wz);
float theta = 0.5f*wMag*dt;
q[0] = cos(theta);  // Scalar component
float s = sinc(theta)*0.5f*dt;
q[1] = wx * s; 
q[2] = wy * s;
q[3] = wz * s;

Where sinc(x) is:

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667);
else return sin(x)/x;

This allows you to avoid the divide-by-zero problem and is still very precise.

The quaternion q is then pre-multiplied onto the existing quaternion representation of the body’s orientation. Then, re-normalize.

---

Edit–Where this formula comes from:

Consider initial quaternion q0 and final quaternion q1 which results after rotating with angular velocity w for dt amount of time. All we’re doing here is changing the angular velocity vector into a quaternion and then rotating the first orientation by that quaternion. Both quaternions and angular velocities are variations on the axis-angle representation. A body that is rotated from its canonical orientation by theta around unit axis [x,y,z] will have the following quaternion representation of its orientation: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z].
A body that is rotating theta/s around unit axis [x,y,z] will have angular velocity w=[theta*x theta*y theta*z]. So, in order to decide how much rotation will happen over dt seconds, we first extract the magnitude of the angular velocity: theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2). Then we find the actual angle by multiplying by dt (and simultaneously divide by 2 for convenience in turning this into a quaternion). Since we need to normalize the axis [x y z], we’re also dividing by theta. That’s where the sinc(theta) part comes from. (since theta has an extra 0.5*dt in it from being the magnitude, we multiply that back out). The sinc(x) function is just using the Taylor series approximation of the function when x is small because it’s numerically stable and more accurate to do so. The ability to use this handy function is why we didn’t just divide by the actual magnitude wMag. Bodies that are not rotating very fast will have very small angular velocities. Since we expect this to be pretty common, we need a stable solution. What we end up with is a quaternion that represents a single step time step dt of rotation.