Problem 8, Exercise 2.2

By use and extension of the previously created function, we obtain:


function [ projectedLine lineCov ] = projectToLaser( worldLine,poseIn, covIn)
%[projectedLine, lineCov] = PROJECTTOLASER(worldLine,poseIn,covIn)
%Project a word line to the laser scanner frame given the
%world line, the robot pose and robot pose covariance. Note that the laser
%scanner pose in the robot frame is read globally
%   worldLine: The line in world coordinates
%   poseIn: The robot pose
%   covIn: The robot pose covariance
%
%   projectedLine: The line parameters in the laser scanner frame
%   lineCov: The covariance of the line parameters

%% Constants
global lsrRelPose % The laser scanner pose in the robot frame is read globally



%% Calculation

[h(1), h(2)]=world2robot(poseIn, worldLine);
[projectedLine(1), projectedLine(2)]=world2robot(lsrRelPose, h);


lineCov = zeros(2,2)+covIn;
end

The results are shown below. We will note that the last three pictures also show the extracted lines from the data and are thus also a solution for the last problem in this exercise.

Results with 0 uncertainty:
Line track:

Circular track:

Box track:

After adding some uncertainties we notice that the extracted lines do not perfectly fit the world:

Line track:

Circle track:

Box Track:

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

Exercise 8, problem 1.2

After introducing the code to calculate the covariance

delTh=(delSr-delSl)/odoB;
delS=(delSr+delSl)/2;
nablapf=[1 0 -delS*sin(poseIn(3)+delTh/2); 0 1 delS*cos(poseIn(3)+delTh/2); 0 0 1];

nablau=[(1/2)*cos(poseIn(3)+delTh/2)-delS/(2*odoB)*sin(poseIn(3)+delTh/2) (1/2)*cos(poseIn(3)+delTh/2)+(delS/(2*odoB))*sin(poseIn(3)+delTh/2);
        (1/2)*sin(poseIn(3)+delTh/2)+delS/(2*odoB)*cos(poseIn(3)+delTh/2) (1/2)*sin(poseIn(3)+delTh/2)-delS/(2*odoB)*cos(poseIn(3)+delTh/2);          1/odoB                                                                       -1/odoB];

sigmaU=[kR*abs(delS)    0;
            0       kL*abs(delS)];

covOut1 =nablapf*covIn*nablapf';
covOut2 =nablau*sigmaU*nablau';
covOut  =covOut1+covOut2;

The following figures were obtained:


- for a linear path:

 - for a circular path:

 - for a square path:

Exercise 8, problem 1.1

To calculate the predicted pose, the following formula was inserted:

poseOut = poseIn + [((delSr+delSl)/2)*cos(poseIn(3)+(delSr-delSl)/(2*odoB));((delSr+delSl)/2)*sin(poseIn(3)+(delSr-delSl)/(2*odoB));(delSr-delSl)/odoB];

After inserting the code in positionPrediction.m, we got the following results:

- for a circular path:

 - for a linear path:

 - for a square path: