Exercise 4, Problem 4

The script presented in the previous problem has been tested on the robot and the position and angle values recorded by the robot were stored in a file. We have implemented the following program in MATLAB:

[x,y,th] = textread('log.txt','%f %f %f');
hold on;
i=1;
while i <= length(x)
    a=[x(i) x(i)+0.5*cos(th(i))];
    b=[y(i) y(i)+0.5*sin(th(i))];
    line(a,b)
    i=i+1;
end  
plot(x,y,'Color','red','LineWidth',3);

The program takes the values stored in the file and plots the trajectory (the red line) and the corresponding orientation of the robot at each point (the blue lines).