MATLAB: What's wrong with my quatrotate script?

Question

My version of MATLAB doesn't have the quatrotate function included, so I wrote my own using the equation MathWorks provide here. Trouble is, I don't get the same answers they get in their example in my function, or when I hand calculate it.

Under their example if I input the following I should get an n vector [-1 1 1]:

q = [1 0 1 0]; r = [1 1 1]; n = quatrotate(q, r)

n =

-1.0000 1.0000 1.0000

In my function, and by hand, I get:

[-3 1 1]

What am I missing here? The more I search the more confused I get. As far as I can tell the answer should be [-3 1 1].

Here is the function I wrote:

function [n] = quatrotate(q,r)
%Rotate a given acceleration vector by a given quaternion
%
%Inputs:
% q:      A matrix containing a set of quaternion vectors of the 
%         form q = [w,x,y,z]
% r:      A matrix containing a set of linear acceleration vectors 
%         of the form r= [i,j,k] (also known as [x,y,z])
% 
% Outputs:
% n:      The solved matrix containing the rotated vector of each linear 
%         acceleration component
% 

%This assumes that the quaternion is normalised (sqw + sqx + sqy + sqz =1), 
%if not it should be normalised before doing the conversion. 
%To normalise divide qx, qy, qz and qw by n where n=sqrt(qx2 + qy2 + qz2 + qw2)


 for k = 1:size(q,1)
     rot=[(1-2.*q(k,3).^2-2.*q(k,4).^2) 2.*(q(k,2).*q(k,3)+q(k,1).*q(k,4))...
          2.*(q(k,2).*q(k,4)-q(k,1).*q(k,3));2.*(q(k,2).*q(k,3)-q(k,1).*q(k,4))...
          (1-2.*q(k,2).^2-2.*q(k,4).^2) 2.*(q(k,3).*q(k,4)+q(k,1).*q(k,2));...
         2.*(q(k,2).*q(k,4)+q(k,1).*q(k,3)) 2.*(q(k,3).*q(k,4)-q(k,1).*q(k,2))...
          (1-2.*q(k,2).^2-2.*q(k,3).^2)];
     n(k,:) = rot*r(k,:)';
 end

Thanks in advance!

Answer 1

Well first of all, in order for quatrotate to work, you need to use a unit quaternion (i.e. length of 1).

Second of all, I see that you are using the matrix provided by the MATLAB page. I've recently derived that matrix myself and found that the MATLAB page has the wrong matrix.

According to this (page 45), rotating a vector by a quaternion is

p' = p + 2w(v × p)+2(v × (v × p))

Where,

• p' is the output vector after rotation,
• p is the starting vector to be rotated,
• w is the first coefficient in the quaternion,
• v is the vector of [second coefficent, third coefficient, fourth coefficient] of the quaternion,
• × is the cross product operand.

I encourage you to go to the link and derive the matrix yourself. You will see that the matrix provided on the MATLAB page has the wrong additions and subtractions.

This is what's said on the MATLAB page:

Here's my derivation (here the quaternion is [q0, q1, q2, q3] and the vector is [x, y, z]):

First row:

Second row:

Third row:

Here you can see that the signs are incorrect on MATLAB's website. I've emailed them about the error and is waiting to hear back.

Answer 2

1. first of all you need to calculate the modulus of the given Quaternion q:

   for index = size(q,1):-1:1
       mod(index,:) = norm(q(index,:),2);
   end
   

2. Then normalize it:

   qn = q./(mod* ones(1,4));
   

3. Now calculate the Direct Cosine Matrix using these formulae:

    dcm = zeros(3,3,size(qn,1));
    dcm(1,1,:) = qn(:,1).^2 + qn(:,2).^2 - qn(:,3).^2 - qn(:,4).^2;
    dcm(1,2,:) = 2.*(qn(:,2).*qn(:,3) + qn(:,1).*qn(:,4));
    dcm(1,3,:) = 2.*(qn(:,2).*qn(:,4) - qn(:,1).*qn(:,3));
    dcm(2,1,:) = 2.*(qn(:,2).*qn(:,3) - qn(:,1).*qn(:,4));
    dcm(2,2,:) = qn(:,1).^2 - qn(:,2).^2 + qn(:,3).^2 - qn(:,4).^2;
    dcm(2,3,:) = 2.*(qn(:,3).*qn(:,4) + qn(:,1).*qn(:,2));
    dcm(3,1,:) = 2.*(qn(:,2).*qn(:,4) + qn(:,1).*qn(:,3));
    dcm(3,2,:) = 2.*(qn(:,3).*qn(:,4) - qn(:,1).*qn(:,2));
    dcm(3,3,:) = qn(:,1).^2 - qn(:,2).^2 - qn(:,3).^2 + qn(:,4).^2;

4. According to MATLAB documents, the rotation of a vector r by the calculated dcm can be found as follows:

   if ( size(q,1) == 1 ) 
       % Q is 1-by-4
       qout = (dcm*r')';
   elseif (size(r,1) == 1) 
       % R is 1-by-3
       for i = size(q,1):-1:1
           qout(i,:) = (dcm(:,:,i)*r')';
       end
   else
       % Q is M-by-4 and R is M-by-3
       for i = size(q,1):-1:1
           qout(i,:) = (dcm(:,:,i)*r(i,:)')';
       end
   end