Warning: quaternions numerical integration errors

[gabrielenava]

In the Matlab torque controllers, quaternions are integrated using the Matlab function ode15s. We observed that there's a numerical integration error in the quaternions calculation, whose magnitude is constant in time and depends on the integration tolerances. This error propagates to the rotation matrix, which is used to calculate jacobians, centroidal momentum, mass matrix and forward kinematics. With a relative error tolerance 'RelTol' = 1e-5 and a vector of absolute error tolerances 'AbsTol' = 1e-5, the magnitude of this error is 1e-6. If it is necessary, one can reduce the error reducing the integration tolerances.

[traversaro]

After integration, quaternions are never projected back to the unit hyper-sphere (i.e. normalized)?

[traversaro]

What are the long-term effects of this integration errors?

[naveenoid]

One option could be to try to normalise quaternions both before and after computation of the quaternion derivative within your dynamics function. This might help with the deviation induced during one integrator loop.

On Wed, Mar 9, 2016 at 12:01 PM, Silvio Traversaro <notifications@github.com

wrote:

What are the long-term effects of this integration errors?

— Reply to this email directly or view it on GitHub https://github.com/robotology/mex-wholebodymodel/issues/64#issuecomment-194241817 .

[gabrielenava]

They are not normalized. It seems that there are no significant effects on system's dynamics, I think because the magnitude of this error is not increasing in time.

[gabrielenava]

Actually, the time derivative of quaternions has a correction term, I think to ensure its validity. Maybe this is the reason why the integration error is not increasing

[traversaro]

The magnitude of which error is not increasing? The magnitude of the quaternion error (i.e. distance from the unit hyper-sphere) or the integration error? I assume that you are referring to the quaternion error. I strongly suggest to normalize the quaternion coming out from the integration, even if the correction term is ensuring that the quaternion error is remaining constant. All the quaternion handling functions (such as conversion to rotation matrices or euler angles) assume unit quaternions, and estimating the numerical effect of an error of 1e-6 on the quaternion in the output of this functions is not trivial. Normalizing the quaternion should bring the quaternion error to have a value similar to the machine epsilon, for our case ~1e-16.

[gabrielenava]

I normalize the quaternion after the integration and this fixed the bug. Also, I managed to fix another old bug #39. I'll push the corrections in my branch for now, I'll fix other bugs then I'll be ready for pull request.