Closed form integration first-time derivative of a quaternion

[gsilano]

Hi everybody! This conversation follows what discussed here: robotology/whole-body-controllers#110.

Just to summarize, the problem I'm facing concerns the integration of the first-time derivative of a quaternion. From what I've seen, implementing the solution described in [1] there is a strange behavior in single-axis rotations. Roughly speaking, for a single-axis rotation, i.e., only one angular velocity is applied per axis, along x, y, and z, I get rotation around yaw, -pitch, and -roll angles instead of roll, pitch, and yaw angles.

I wonder why this happens. Also, I think that having this solution can help in any scenario, not just the one closely related to the iCub ecosystem.

The MATLAB test code is reported in robotology/whole-body-controllers#110.

[1] https://www.ashwinnarayan.com/post/how-to-integrate-quaternions

[traversaro]

Thanks @gsilano ! I guess this could be of interest of the members of @robotology/iit-dynamic-interaction-control that works in the simulation of dynamical system that evolve over the 3D rotation group (SO(3)), that is sometimes parametrized by using unit quaternions.

[S-Dafarra]

Hi @gsilano, to be honest I would not consider [1] as a reliable source of information. I believe that the formula presented in the "Integration" section assumes the matrix A to be constant, but this is not the case, as it depends on the quaternion itself.

To the best of my knowledge, there is no closed-form integration for the quaternion. In most of the cases, the integration of a quaternion resorts to numerical methods that treat it as a R^4 vector. The "dynamics" of the quaternion is then modified to include some "stabilization" terms to keep the quaternion unitary: see for example https://core.ac.uk/download/pdf/191492036.pdf

Related works:

• Integrating Rotations Using Nonunit Quaternions
• Geometric Integration of Quaternions
• Quaternions And Dynamics

[diegoferigo]

Another great source for quaternion math is Quaternion kinematics for the error-state Kalman filter, particularly 4.6 Time-integration of rotation rates (thanks @prashanthr05 for the reference).

[gsilano]

Thanks for the support and help. I followed the implementation in A novel Quaternion integration approach for describing the behavior of non-spherical particles, and developed the related MATLAB code. The results I get once again are weird, and they leave me perplexed. I can't isolate the source of the problem, i.e., the implementation or the math (or even worst, both). Perhaps, you can get more than me.

Let me explain what the attached files contain. MATLAB_SCRIPTS.zip

1. I assumed an angular rotation on a single-axis, first along x, then y, and finally z (each analysis is separated by sections in the "test_quaternions.m" MATLAB script). Also, I assumed an angular velocity omega of 0.1 rad/s and an initial value of the roll, pitch, and yaw angles equal to zero (this is encoded in q_0).

Then, I implemented the numerical integration solution provided in A novel Quaternion integration approach for describing the behavior of non-spherical particles, eqs (46-50), and How to Integrate Quaternions taking care to understand what it happens when (i) changing the axes convention (lines 40-42); (ii) changing the MATLAB function to retrieve EULER angles from QUATERNIONS and vice versa (I implemented the function eulerToQuat and quatToEuler), (iii) applying a rotation to the Euler angles obtained by using [3].

As a result, the following implementation resulted to be reasonable:

FROM EULER TO QUATERNION --> MY FUNCTION
NUMERICAL METHOD FROM A novel Quaternion integration approach for describing the behavior of non-spherical particles
FROM QUATERNION TO EULER --> MATLAB FUNCTION WITH XYZ CONVENTION

2. Supposing a HUGE mistake in the previous tests, I ran others (from line 167 onwards). Assuming a non-zero initial angle (q_0) with an angular velocity still applied on one single-axis per time. Also, a non-zero initial angle (q_0) with speeds applied on all axes (lines 216-224).

FROM EULER TO QUATERNION --> MATLAB FUNCTION
NUMERICAL METHOD FROM A novel Quaternion integration approach for describing the behavior of non-spherical particles
FROM QUATERNION TO EULER --> MATLAB FUNCTION

The obtained results are wrong.

3. I ran the same tests as 2, but with

FROM EULER TO QUATERNION --> MY FUNCTION
NUMERICAL METHOD FROM A novel Quaternion integration approach for describing the behavior of non-spherical particles
FROM QUATERNION TO EULER --> MATLAB FUNCTION

The obtained results are wrong.

4. I ran the same tests as 2, but with

CHANGING THE SIGNS TO ROLL AND PITCH ANGLES, YAW IS UNCHANGED
FROM EULER TO QUATERNION --> MY FUNCTION
NUMERICAL METHOD FROM A novel Quaternion integration approach for describing the behavior of non-spherical particles
FROM QUATERNION TO EULER --> MATLAB FUNCTION

The results are right.

5. I ran the same tests as 4, but with a lower time interval. This resulted in a lower numerical error as expected.

[gsilano]

Just to summarize, in the end, I fixed the problem by looking at my model from a different point of view. Either way, I think the problem (i.e., how to find in a closed or numerically form the integration of the first time derivative of a quaternion) is still interesting and has raised a lot of questions about the strange behavior observed in MATLAB. It could be due to some implementation issues or even misunderstandings. However, I hope that my questions, your answers, and my solution/implementation proposal can be a starting point for others as well. Thanks again for your help.