Welcome to ShenZhenJia Knowledge Sharing Community for programmer and developer-Open, Learning and Share
menu search
person
Welcome To Ask or Share your Answers For Others

Categories

I need to filter a rotation matrix and to do that I need to first convert it to rotation angles, then filter those and convert the answer back to a rotation matrix form. The problem is that chaining those two conversions together even without the filtering in between (i.e. angl2rot(rot2angl(rotationMatrix))) introduces a significant error due to the lack of precision in all the trig functions required to do the conversions.

Is there a way to increase the precision of MatLab in general? Or maybe in particular is it possible to make the trig functions (I use sin(), cos() and atan2()) more precise?

EDIT: There go the implementations of functions responsible for converting from rotation matrices to Euler angles and vice versa:

function [ rotationM ] = eul2rot( a, b, c )
%EUL2ROT This function converts euler rotation angles to a 3x3 rotaton
%matrix

    X = zeros(3,3,size(a,1));
    Y = zeros(3,3,size(a,1));
    Z = zeros(3,3,size(a,1));
    rotationM = zeros(3,3,size(a,1));

    for count = 1:size(a,1)

        X(:,:,count) = [1 0 0; ...
            0 cos(a(count)) -sin(a(count)); ...
            0 sin(a(count)) cos(a(count))];

        Y(:,:,count) = [cos(b(count)) 0 sin(b(count)); ...
            0 1 0; ...
            -sin(b(count)) 0 cos(b(count))];

        Z(:,:,count) = [cos(c(count)) -sin(c(count)) 0; ...
            sin(c(count)) cos(c(count)) 0; ...
            0 0 1];

        rotationM(:,:,count) = Y(:,:,count)*Z(:,:,count);
        rotationM(:,:,count) = X(:,:,count)*rotationM(:,:,count);

    end

end

function [ th ] = rot2eul( rot )
%ROT2EUL Converts a 3x3 rotation matrix into a vector of three euler angles
        th = [atan2(rot(3,2,:),rot(3,3,:)); ...
            atan2(-rot(3,1,:),sqrt(rot(3,2,:).^2+rot(3,3,:).^2)); ...
            atan2(rot(2,1,:),rot(1,1,:))];
end

They use the equations found here.

See Question&Answers more detail:os

与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
thumb_up_alt 0 like thumb_down_alt 0 dislike
160 views
Welcome To Ask or Share your Answers For Others

1 Answer

The site you link as your source defines rotation matrix R as:

R = Z*Y*X;

Where X, Y, Z are the three individual rotation matrices.

However, in your code you are doing this:

rotationM(:,:,count) = Y(:,:,count)*Z(:,:,count);
rotationM(:,:,count) = X(:,:,count)*rotationM(:,:,count);

Which is equivalent to:

R = X*Y*Z;

These two are not equivalent, because matrix multiplication is not commutative. Changing the order is of multiplication is equivalent to changing the order in which the rotations are performed (and rotations in 3D are also not commutative), giving you a different result than you were expecting.


与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
thumb_up_alt 0 like thumb_down_alt 0 dislike
Welcome to ShenZhenJia Knowledge Sharing Community for programmer and developer-Open, Learning and Share
...