Finding the best method for Quatterion differentiaton

Question

I have a speed vector of quaternion (4x1) and angular (3x1), and I call a function to calculate the differential quaternion, as described in this web . The code is as follows:

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 the motion of rigid objects or is there a better way to predict a quaternion at angular speed? This works, but I do not get the expected results.

Answer 1

There are a couple of things that can happen. You are not talking about normalizing this quaternion. Bad things are bound to happen if you do not. You also do not say that you multiply the components of the delta quaternion by the amount of time elapsed dt before adding them to the original quaternion. If your angular speed is in radians per second, but you only stepped forward for a split second, you went too far. However, even in this case, as you go through a certain time and try to pretend that it is infinitely small, strange things will happen, especially if your timestep or angular speed is great.

The physics engine, ODE, provides the ability to update the rotation of a body from its angular speed, as if it took an infinitesimal step or was updated using a step of finite size. The final step is much more accurate, but includes some triggers. functions and therefore a little slower. The corresponding ODE source code can be seen here, lines 300-321 , with the delta quaternion search code 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) :

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

This avoids the problem of division by zero and is still very accurate.

The quaternion q then pre-multiplied by the existing quaternion representation of the orientation of the body. Then, re-normalize.

---

Edit - where does this formula come from:

Consider the initial quaternion q0 and the final quaternion q1 , which occurs after rotation with angular speed w for dt amount of time. All we do here is change the angular velocity vector into a quaternion, and then rotate the first orientation to that quaternion. Both quaternions and angular velocities are variations in axial representation. A body that is rotated from its canonical orientation on theta around the 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 rotating theta/s around the axis [x,y,z] will have a speed angular w=[theta*x theta*y theta*z] . So, to determine how much rotation there will be in dt seconds, we first extract 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 at the same time divide by 2 for the convenience of turning this into a quaternion). Since we need to normalize the [xyz] axis, we also divide by theta . Where the part of sinc(theta) comes from. (since theta has outside 0.5*dt in it of magnitude, we multiply this back). The sinc(x) function simply uses the Taylor series approximation of the function when x small, because it is numerically stable and more precisely to do it. The ability to use this convenient function is why we did not just share the actual value of wMag . Bodies that do not spin very fast will have a very low angular speed. Since we expect this to be fairly common, we need a stable solution. As a result, we get a quaternion representing the step step of dt by one step.

Answer 2

There is a method with a very good compromise between speed and accuracy , how to increase the quaternion representing the rotational state (i.e. integrate the differential equation of rotational motion), the vector increase in the angle dphi (which is the omega mulptipliad angular velocity vector with the time step dt ).

The exact (and slow) vector quaternion rotation method :

 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 get a significant increase in speed and reasonable accuracy at small angles (which occurs if the time step of your simulation is reasonably small) by approximating sin and cos a Taylor extension, expressed only using norm^2 instead of norm .

Like this Fast vector quaternion rotation method :

 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 making half the angle, which is another 5 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 an even more accurate other half angle:

  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 a more complex integration scheme than just a verlet (for example, Runge-Kutta), you probably need the quaternion differential , and not a new (updated) quaternion.

this can be seen 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 a multiplication of the old (not updated) quaternion by ca (cosine with a half angle), which is approximately ca ~ 1 for small angles and adding the rest (some transverse interactions). Therefore, the differential is simple:

  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 the term ( 1 - ca ) ~ 0 for small angles and can sometimes be neglected (basically it's just a renormalization of quternion).

Answer 3

Simple conversion from an "exponential map" to a quaternion. (exponential map is equal to angular speed times deltaTime). The result of the quaternion is a delta rotation for the past deltaTime and angular speed "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()); }