Looking for a better method to do Quaternion differentiaton

Question

I have a quaternion (4x1) and an angular velocity vector(3x1) 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.

Answer 2

Simple conversion from "exponential map" to quaternion. (exponential map equal to angular velocity multiplied by deltaTime). Result quaternion is delta rotation for passed deltaTime and angular velocity "w".

Vector3 em = w*deltaTime; // exponential map
{
Quaternion q;
Vector3 ha = em/2.0; // vector of half angle

double l = ha.norm();
if(l > 0)
{
    double ss = sin(l)/l;
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss);
}else{
    // if l is too small, its norm can be equal 0 but norm_inf greater 0
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z());
}

Answer 3

There is a metod with very good trade-off between speed and accuracy how to increment a quaterniom representing rotational state ( i.e. integrate differential equation of rotational motion) by small vector increment of angle dphi ( which is vector angular velocity omega mulptipliad by time step dt ).

The Exact (and slow) method of rotation of quaternion by vector:

void rotate_quaternion_by_vector_vec ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;
  double norm = Math.sqrt( r2 );

  double halfAngle = norm * 0.5d;
  double sa = Math.sin( halfAngle )/norm; // we normalize it here to save multiplications
  double ca = Math.cos( halfAngle );
  x*=sa; y*=sa; z*=sa;  

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;
}

The problem is that you have to compute slow functions like cos, sin, sqrt. Instead you can obtain considerable speed gain and reasonable accuracy for small angles ( which is the case if the time step of your simulation is reasonable small ) by approximating sin and cos by taylor expansion expressed just using norm^2 instead of norm.

Like this Fast method of rotation of quaternion by vector:

void rotate_quaternion_by_vector_Fast ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;

  // derived from second order taylor expansion
  // often this is accuracy is sufficient
  final double c3 = 1.0d/(6 * 2*2*2 )      ; // evaulated in compile time
  final double c2 = 1.0d/(2 * 2*2)         ; // evaulated in compile time
  double sa =    0.5d - c3*r2              ; 
  double ca =    1    - c2*r2              ; 

  x*=sa;
  y*=sa;
  z*=sa;

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

}

you can increase accuracy by doing half o angle which 5 more multiplications:

  final double c3 = 1.0d/( 6.0 *4*4*4  ) ; // evaulated in compile time
  final double c2 = 1.0d/( 2.0 *4*4    ) ; // evaulated in compile time
  double sa_ =    0.25d - c3*r2          ;  
  double ca_ =    1     - c2*r2          ;  
  double ca  = ca_*ca_ - sa_*sa_*r2      ;
  double sa  = 2*ca_*sa_                 ;

or even more accurate by an other spliting angle to halfs:

  final double c3 = 1.0d/( 6 *8*8*8 ); // evaulated in compile time
  final double c2 = 1.0d/( 2 *8*8   ); // evaulated in compile time
  double sa = (  0.125d - c3*r2 )      ;
  double ca =    1      - c2*r2        ;
  double ca_ = ca*ca - sa*sa*r2;
  double sa_ = 2*ca*sa;
         ca = ca_*ca_ - sa_*sa_*r2;
         sa = 2*ca_*sa_;

---

NOTE: If you use more sophisticated integration scheme than just verlet (like Runge-Kutta ) you would probably need a differential of the quaternion, rather than the new ( updated ) quaternion.

this is possible to see in the code here

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

it could be interpreted as multiplication of the old (not updated ) quaternion by ca ( cosine of half angle ) which is approximativelly ca ~ 1 for small angles and adding the rest (some cross interactions). So the differential simply:

  dq[0] =  x*qw + y*qz - z*qy + (1-ca)*qx;
  dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy;
  dq[2] =  x*qy - y*qx + z*qw + (1-ca)*qz;
  dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw;

where term ( 1 - ca ) ~ 0 for small angles and could be sometimes neglected ( basically it just renormalize the quternion).