Exercise 3, Problem 7

The function to calculate the next pose implemented in MATLAB has the following form:

function newpose = kinupdate(pose,robotpar,ts,wheelspeed)
th = pose(3);

Rinv = [cos(th) -sin(th) 0;

        sin(th) cos(th)  0;

         0        0      1];

d = robotpar(1);
rR = robotpar(2);
rL = robotpar(3);
phiR = wheelspeed(1);
phiL = wheelspeed(2);

M = [(rR*phiR+rL*phiL)/2; 
             0; 
     (rR*phiR-rL*phiL)/d];

newpose = pose' + Rinv*M*ts;

end

Where:
th = angle theta
d = distance between the wheels
rR and rL = the diameters of the right and left wheels
ts = sample time

phiR and phiL = the angular velocities of the left and right wheels