Exercise 10, Problem 1.1

Calculation of targetpose in odometry coordinates.

function out = trans(transform,targetPose)
% out <-> odoTargetPose (notation)
% odoTargetPose = TRANS(transform,targetPose)
% Transform a given point in world coordinates (targetPose) to odometry
% coordinates, using the origo of the odometry coordinates in world
% coordinates (transform).
%calculation of targetpose in ordinary coordinates
    t=transform; %for shorter name
    tMatrix=[cos(t(3)) -sin(t(3))  0; sin(t(3))  cos(t(3))  0; 0 0 1];
    temp = tMatrix*targetPose ;
    out = temp + t;
    out(3) = normalizeAngle(out(3));
end