function out=ModelRobot(call,xprevious,dt,control,covrobot)

%ModelRobot    Function that implements the transition probability model of the robot
%
% out=ModelRobot(call,xprevious,dt,control,covrobot)
% call: value used to define the behaviour of the function
% xprevious: state(x,y,theta) associated with the previous time
% dt: time interval
% control: control signal : u(1): linear velocity, u(2): angular velocity
% covrobot: covariance (error) in the control signals
% out: returned value, dependent on call
%   If call==0, it returns the new state, including the random noise
%               associated with the control signals
%   If call==2, it returns the new state, without considering the error in the control
%               signal
%

% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 8: Monte Carlo II
% Autores: Eva Besada-Portas, Sergey M. Plis


N=size(xprevious,1); %The function works matricially, and so multiple states can be provided
switch(call)
    case 0, %sample
        noise=gsamp([0 0],covrobot,N);

        vel=control(1);
        out(:,3)=AngNorm(xprevious(:,3)+dt*(control(2)+noise(:,2))); %theta_t=theta{t-1}+dt*control(2)+noise_theta;
        vel=vel+noise(:,1); %vel=vel+noise;
        out(:,1)=xprevious(:,1)+dt*vel.*cos(out(:,3));%x_t=x{t-1}+dt*vel)*cos(theta_t);
        out(:,2)=xprevious(:,2)+dt*vel.*sin(out(:,3));%x_t=x{t-1}+dt*vel*cos(theta_t)+noise_x;
        
    case 2, %sample without noise
        vel=control(1);
        out(:,3)=AngNorm(xprevious(:,3)+dt*control(2)); 
        out(:,1)=xprevious(:,1)+dt*vel*cos(out(:,3));
        out(:,2)=xprevious(:,2)+dt*vel*sin(out(:,3));
    otherwise,
        error('Not other options');
end
