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