[Math] Extracting Euler angles from quaternion close to singularity

quaternionsrotationssingularity

I've written some code to convert a quaternion to Euler angles, assuming a ZYX Euler sequence. Following the conventions detailed in this paper https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770019231.pdf 

m11=qin(:,1).^2+qin(:,2).^2-qin(:,3).^2-qin(:,4).^2;
m12=2.*(qin(:,2).*qin(:,3)-qin(:,1).*qin(:,4)); 
m13=2.*(qin(:,2).*qin(:,4)+qin(:,1).*qin(:,3));   

m21=2.*(qin(:,2).*qin(:,3)+qin(:,1).*qin(:,4));
m22=qin(:,1).^2-qin(:,2).^2+qin(:,3).^2-qin(:,4).^2;   

m31=2.*(qin(:,2).*qin(:,4)-qin(:,1).*qin(:,3));
m32=2.*(qin(:,3).*qin(:,4)+qin(:,1).*qin(:,2));
m33=qin(:,1).^2-qin(:,2).^2-qin(:,3).^2+qin(:,4).^2;

rz=atan2(m21,m11);    
ry=zeros(length(m31),1);
idx=m31.^2<1;
ry(idx)=atan(-m31(idx)./sqrt(1-m31(idx).^2));
ry(~idx)=-atan2(m31(~idx),0);

rx=atan(m32./m33);

However, when I implement the above code on a microprocessor the roll angle (rx) becomes erroneous at ~ 85 degrees. I believe this is a common issue known as 'gimbal lock', resulting from singularities due to a rotation in pitch being close to 90 degrees (I believe this occurs close to 85 degrees due to the single precision values used by the micro).

I've been researching how the problem is typically handled and have found the following paper https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf. This paper champions the benefits of this methodology when using single precision operations, over other techniques of handling the problem, such as the methodology detailed by Ken Shoemake. The author handles the conversion close to singularities by calculating the first two angles and then calculating the third angle using the other angles.

I've tried to apply the same methodology to extract a ZYX Euler sequence.

Z rotation matrix:

\begin{bmatrix}
c_{3} & s_{3} & 0\\
-s_{3} & c_{3} & 0\\
0 & 0 & 1
\end{bmatrix}

Y rotation matrix:

\begin{bmatrix}
c_{2} & 0 & -s_{2}\\
0 & 1 & 0\\
s_{2} & 0 & c_{2}
\end{bmatrix}

Multiplying the two together I get

\begin{bmatrix}
c_{3}c_{2} & s_{3} & -s_{2}c_{3}\\
-s_{3}c_{2} & c_{3} & s_{2}s_{2}\\
s_{2} & 0 & c_{2}
\end{bmatrix}

Taking the transpose of the above I get:

\begin{bmatrix}
c_{3}c_{2} & -s_{3}c_{2} & s_{2}\\
s_{3} & c_{3} & 0\\
-s_{2}c_{3} & s_{2}s_{2} & c_{2}
\end{bmatrix}

Multiplying this with an arbitrary rotation matrix I get:

\begin{bmatrix}
c_{3}c_{2}m_{11}-s_{3}c_{2}m_{21}+s_{2}m_{31} & c_{3}c_{2}m_{01}-s_{3}c_{2}m_{22}+s_{2}m_{32} & c_{3}c_{2}m_{13}-s_{3}c_{2}m_{23}+s_{2}m_{31}\\
s_{3}m_{11}+c_{3}m_{21} & s_{3}m_{12}+c_{3}m_{22}& s_{3}m_{13}+c_{3}m_{22}\\
-s_{2}c_{3}m_{11}+s_{2}s_{2}m_{21}+c_{2}m_{31} & -s_{2}c_{3}m_{12}+s_{2}s_{2}m_{22}+c_{2}m_{32} & -s_{2}c_{3}m_{13}+s_{2}s_{2}m_{23}+c_{2}m_{33}
\end{bmatrix}

The final rotation is in this instance about the X axis:

X rotation matrix:

\begin{bmatrix}
1 & 0 & 0\\
0 & c_{1} & s_{1}\\
0 & -s_{1} & c_{1}
\end{bmatrix}

From this I get the following code:

rz=atan2(m21,m11);
c2=sqrt((m11.^2)+(m21.^2));
ry=atan2(-m31,c2);
s3=sin(rz);
c3=cos(rz);        
rx=atan2(s3.*m13+c3.*m12, c3.*m22+s3.*m23);

Unfortunately this does not seem to give the same result as the first set of equations. Could someone point out my mistake, or a better alternative method of doing this? Are there any issues with this method? Ideally some code examples would be appreciated.

Best Answer

I went through a paper and wrote this code which works at north and south pole singularities.

    const double radToDeg = (180.0 / Math.PI);
    const double degToRad = (Math.PI / 180.0);

    public static Vector3D ToEulerRad(Quaternion rotation)
    {
        double sqw = rotation.Real * rotation.Real;
        double sqx = rotation.ImagX * rotation.ImagX;
        double sqy = rotation.ImagY * rotation.ImagY;
        double sqz = rotation.ImagZ * rotation.ImagZ;
        double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
        double test = rotation.ImagX * rotation.Real - rotation.ImagY * rotation.ImagZ;

        if (test > 0.4995f * unit)
        { // singularity at north pole
            double Y = 2f * Math.Atan2(rotation.ImagY, rotation.ImagX);
            double X = Math.PI / 2;
            double Z = 0;
            Vector3D v = new Vector3D(X, Y, Z);
            return NormalizeAngles(radToDeg * v);
        }
        if (test < -0.4995f * unit)
        { // singularity at south pole
            double Y = -2f * Math.Atan2(rotation.ImagY, rotation.ImagX);
            double X = -Math.PI / 2;
            double Z = 0;
            Vector3D v = new Vector3D(X, Y, Z);
            return NormalizeAngles(radToDeg * v);
        }
        Quaternion q = new Quaternion(rotation.ImagY,rotation.Real, rotation.ImagZ, rotation.ImagX);
        double y = System.Math.Atan2(2f * q.ImagX * q.Real + 2f * q.ImagY * q.ImagZ, 1 - 2f * (q.ImagZ * q.ImagZ + q.Real * q.Real));     // Yaw
        double x = System.Math.Asin(2f * (q.ImagX * q.ImagZ - q.Real * q.ImagY));                             // Pitch
        double z = System.Math.Atan2(2f * q.ImagX * q.ImagY + 2f * q.ImagZ * q.Real, 1 - 2f * (q.ImagY * q.ImagY + q.ImagZ * q.ImagZ));      // Roll
        return NormalizeAngles( radToDeg * new Vector3D(x,y,z));
    }
    private static Vector3D NormalizeAngles(Vector3D angles)
    {
        double X = NormalizeAngle(angles.X);
        double Y = NormalizeAngle(angles.Y);
        double Z = NormalizeAngle(angles.Z);
        return new Vector3D(X, Y, Z);
    }
    private static double NormalizeAngle(double angle)
    {
        while (angle > 360)
        {
            angle -= 360;
        }
        while (angle < 0)
        {
            angle += 360;
        }
        return angle;
    }

    public static Vector3D ToEulerAngles(Quaternion rotation)
    {
        return radToDeg * ToEulerRad(rotation);
    }
Related Question