Quatrotate Function from Scratch in Matlab and Python

Admin
0

Codes presented in this article is useful for you want to create a quatrotate function (found in MATLAB) by scratch. 

In brief, quatrotate perform a rotation to a vector V (x, y, z) by using the quaternion Q (q0, q1,q2, q3). Don't forget to verify the results as presented in section 3 of this article (end part).

Quatrotate works by creating Direct Cosine Matrices (DCM) as shown in figure below, and afterwards multiplies the DCM and transposed vector to produce rotated V vector.

1. MATLAB IMPLEMENTATION


function Rotated_Vector = rotateVectorByQ(Q,V)

qn = Q;
% create DCM (Direct Cosine Matrices)
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;


Rotated_Vector = (dcm*V')';


2. PYTHON IMPLEMENTATION


def rotateVectorByQ(Q, V):

    # Equivalen to quatrotate MATLAB
    # 1x4 Q
    # 1x3 V

    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]

    q11 = (q0*q0)+(q1*q1)-(q2*q2)-(q3*q3)
    q12 = 2*((q1*q2)+(q0*q3))
    q13 = 2*((q1*q3)-(q0*q2))
    q21 = 2*((q1*q2)-q0*q3)
    q22 = (q0*q0)-(q1*q1)+(q2*q2)-(q3*q3)
    q23 = 2*((q2*q3)+(q0*q1))
    q31 = 2*((q1*q3)+(q0*q2))
    q32 = 2*((q2*q3)-(q0*q1))
    q33 = (q0*q0)-(q1*q1)-(q2*q2)+(q3*q3)


    DCM = np.array([[q11, q12, q13],
      [q21, q22, q23],
      [q31, q32, q33]]);

    V = np.matmul(DCM,V.transpose())
    return V

3. VERIFICATION
Please noted that :
Q = 1x4 vector
V = 1x3 vector

The above codes have been tested using values as follow :

Q = [0.784896612167358, -0.362114369869232, 0.475989699363708, 0.162001579999924]
V = [0.212127346533177, 0.151271140560544, 0.965463117310188]

Resulted rotated_V should be :

Rotated_V = [-0.743479512404512, -0.423328449157434, 0.517717749096532]


ENJOY !

Post a Comment

0Comments
* Please Don't Spam Here. All the Comments are Reviewed by Admin.
Post a Comment (0)

Disclaimer : Content provided on this page is for general informational purposes only. We make no representation or warranty of any kind, express or implied, regarding the accuracy, adequacy, validity, reliability, availability or completeness of any information.

#buttons=(Accept !) #days=(20)

Our website uses cookies to enhance your experience. Learn More
Accept !
To Top