Exercise 8, Problem 2.1

The function implements the required transformation:


function [angle r]=world2robot(pos, lin)
%input arguments:
%pose is a vector of x, y and theta respectively,
%representing the robot pose while line is a vector
%of the radius and angle in world coordinates

x=pos(1);
y=pos(2);
theta=pos(3);
wr=lin(2);
wangle=lin(1);
angle=wangle-theta;
r=wr - sqrt(x^2 +y^2)*cos(wangle - atan2(y,x));
end