Bug in dcm2quat function

I've spent a day understanding an unexpected behaviour of dcm2quat function (Aerospace Toolbox functions set). The function transforms the Direct Cosine Matrix to quaternion and do it very well in most cases. I've noticed a problem while converting a DCM time series: I expected to plot "continous" quaternions' values, but at certain samples discontinuities takes place. I found that this happens when the trace of the DCM becomes negative. The function's implementation shows that there is an if-else statement which handles this checks; investigating the else function I found that there is - only in same cases (e.g. the transpose is not affected) - a sign problem. You could find implementation on MATLAB (through F4 for example).

Probably it's a bit confusing so I'm going to attach some graphs to better show what I mean.

What I expected:

What happens:

In this example, the first "hop" is between samples 1059 and 1060 where the DCM are:

T_1059 =

      0.4218   -0.3220    0.8475
      0.7299   -0.4339   -0.5282
      0.5378    0.8414    0.0520

T_1060 =

      0.4312   -0.2603    0.8639
      0.7289   -0.4637   -0.5036
      0.5317    0.8469   -0.0102

And corrispondent quaternions' values are:

dcm2quat(T_1059) = 0.5099   -0.6715   -0.1518   -0.5158
dcm2quat(T_1060) = -0.4892    0.6901    0.1698    0.5055

Where the former is correct, but the latter should be

Q_1060 = 0.4892   -0.6901   -0.1698   -0.5055

I've double checked on euclideanspace.com and the java applet at the bottom of the page works as expected.

It's not a big throuble because I've notice that the quat2dcm inverse function transforms correctly both those values in the proper DCM, but if quaternions are manipulated in the mainwhile, should happen strange behaviours.

How could be adviced this problem?

[Nicolau Werneck]

Apparently your quaternions only need a sign change so the parameters become continuous, but the represented rotation is still the same. You just need to detect these " jumps" and multiply by -1. In you case, because there is no continuous spinning, you can just make one of the parameters to be always positive.

Answer 1

[Alessandro]

This is the correct dcm2quat function:

function q = dcm2quat_mod( dcm )

if any(~isreal(dcm) || ~isnumeric(dcm))
    error(message('aero:dcm2quat:isNotReal'));
end

if ((size(dcm,1) ~= 3) || (size(dcm,2) ~= 3))
    error(message('aero:dcm2quat:wrongDimension'));
end

for i = size(dcm,3):-1:1

      q(i,4) =  0; 

      tr = trace(dcm(:,:,i));

      if (tr > 0)
          sqtrp1 = sqrt( tr + 1.0 );

          q(i,1) = 0.5*sqtrp1; 
          q(i,2) = (dcm(2, 3, i) - dcm(3, 2, i))/(2.0*sqtrp1);
          q(i,3) = (dcm(3, 1, i) - dcm(1, 3, i))/(2.0*sqtrp1); 
          q(i,4) = (dcm(1, 2, i) - dcm(2, 1, i))/(2.0*sqtrp1); 
      else
          d = diag(dcm(:,:,i));
          if ((d(2) >= d(1)) && (d(2) >= d(3)))
              % max value at dcm(2,2,i)
              if (dcm(3, 1, i) - dcm(1, 3, i) >= 0)
                  sqdip1 = sqrt(d(2) - d(1) - d(3) + 1.0 );
              else
                  sqdip1 = -sqrt(d(2) - d(1) - d(3) + 1.0 );
              end

              q(i,3) = 0.5*sqdip1; 

              if ( sqdip1 ~= 0 )
                  sqdip1 = 0.5/sqdip1;
              end

              q(i,1) = (dcm(3, 1, i) - dcm(1, 3, i))*sqdip1; 
              q(i,2) = (dcm(1, 2, i) + dcm(2, 1, i))*sqdip1; 
              q(i,4) = (dcm(2, 3, i) + dcm(3, 2, i))*sqdip1; 
          elseif (d(3) >= d(1))
              % max value at dcm(3,3,i)
              if (dcm(1, 2, i) - dcm(2, 1, i) >= 0)
                  sqdip1 = sqrt(d(3) - d(1) - d(2) + 1.0 );
              else
                  sqdip1 = -sqrt(d(3) - d(1) - d(2) + 1.0 );
              end

              q(i,4) = 0.5*sqdip1; 

              if ( sqdip1 ~= 0 )
                  sqdip1 = 0.5/sqdip1;
              end

              q(i,1) = (dcm(1, 2, i) - dcm(2, 1, i))*sqdip1;
              q(i,2) = (dcm(3, 1, i) + dcm(1, 3, i))*sqdip1; 
              q(i,3) = (dcm(2, 3, i) + dcm(3, 2, i))*sqdip1; 
          else
              % max value at dcm(1,1,i)
              if (dcm(2, 3, i) - dcm(3, 2, i) >= 0)
                  sqdip1 = sqrt(d(1) - d(2) - d(3) + 1.0 );
              else
                  sqdip1 = -sqrt(d(1) - d(2) - d(3) + 1.0 );
              end

              q(i,2) = 0.5*sqdip1;

              if ( sqdip1 ~= 0 )
                  sqdip1 = 0.5/sqdip1;
              end

              q(i,1) = (dcm(2, 3, i) - dcm(3, 2, i))*sqdip1; 
              q(i,3) = (dcm(1, 2, i) + dcm(2, 1, i))*sqdip1; 
              q(i,4) = (dcm(3, 1, i) + dcm(1, 3, i))*sqdip1; 
          end
      end
  end

I've simply added a sign-check in each if-statement. For example, before it was only this:

sqdip1 = sqrt(d(2) - d(1) - d(3) + 1.0 );

And now it's:

if (dcm(3, 1, i) - dcm(1, 3, i) >= 0)
    sqdip1 = sqrt(d(2) - d(1) - d(3) + 1.0 );
else
    sqdip1 = -sqrt(d(2) - d(1) - d(3) + 1.0 );
end

Answer 2

[James Tursa]

My advice is to put in code to force the continuity that you want. That way you don't have to worry about some edge case that isn't to your liking in the dcm2quat converter that you happen to be using, and you don't have to worry about testing your dcm2quat converter for these edge cases. E.g., assume that we are always talking about small angle changes from one dcm to the next (for large angle changes the concept of continuity breaks down and becomes meaningless). Then simply ensure that the largest magnitude component doesn't change sign from one step to the next. So keep track of the previous quaternion used and then do the comparison. E.g.,

quat_last = [0 0 0 0];  % Initialize last quaternion
    :
quat = current dcm2quat conversion result
[~,x] = max(abs(quat_last));  % Pick off index of max abs of last quaternion
if( quat(x)*quat_last(x) < 0 )  % If sign changed ...
    quat = -quat;  % Flip all signs
end
quat_last = quat;  % Save quat for next comparison

Note: There is always going to be some sort of sign ambiguity in any dcm2quat converter that you use. Although quat and -quat represent the same "end result" for a rotation, they can produce quite different results if e.g. fed into a control system (which might end up commanding a 359 deg long way around maneuver instead of the 1 deg short maneuver you wanted). I wouldn't label this sign ambiguity as a "bug" in the converter, though. It is up to the user to check for the desired "continuity" of the results from step to step and adjust the output accordingly.

SIDE NOTE:

I just looked at the doc for dcm2quat and it has an incorrect example in it (at least in R2015a). In particular, this

dcm = [0 1 0; 1 0 0; 0 0 1];
q = dcm2quat(dcm)

q =

      0.7071         0         0         0

The dcm used is an invalid rotation matrix. It has a determinant of -1 (an x-y reflection matrix), when it should have a determinant of +1. As such, when fed into the dcm2quat function it returns an invalid result (not even normalized and no error/warn message). Garbage in garbage out. Probably someone who didn't understand what dcm's or quat's should look like cooked up this example and didn't notice the error. A correct example would have been:

>> dcm = [0 1 0; -1 0 0; 0 0 1]

dcm =

       0     1     0
      -1     0     0
       0     0     1

>> det(dcm)

ans =

       1

>> q = dcm2quat(dcm)

q =

      0.7071         0         0    0.7071

I suppose I should submit a documentation bug report on this ...

FOLLOW-UP: 24-Aug-2016

TMW plans to fix the doc with a different example, and probably change the function to throw an error if the input dcm is not sufficiently close to a proper rotation matrix. See Technical Support Case #02131384.

EDIT

See also this discussion of the MATLAB toolbox quaternion conventions:

https://www.mathworks.com/matlabcentral/answers/465053-rotation-order-of-quatrotate