function R=rotate(theta)
% rotate

R=[cos(theta) -1*sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];