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.
nkjt :
The site you link as your source defines rotation matrix R as:\n\nR = Z*Y*X;\n\n\nWhere X, Y, Z are the three individual rotation matrices.\n\nHowever, in your code you are doing this:\n\nrotationM(:,:,count) = Y(:,:,count)*Z(:,:,count);\nrotationM(:,:,count) = X(:,:,count)*rotationM(:,:,count);\n\n\nWhich is equivalent to:\n\nR = X*Y*Z;\n\n\nThese 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.",
2014-08-07T16:23:12
Jommy :
The class HPF (High Precision Floats) by John D'Errico supports floats with an arbitrary number of decimals. You can also look at the function vpa of the Symbolic Math Toolbox.",
2014-08-07T13:39:31