for k=1:size(zt,1)

            if zt(k,2)>0

                d = -grid_dim/2;

            else

                d = grid_dim/2;

            end

            phi = pi_to_pi(zt(k,2) + x(3));

            if zt(k,1) ~= Z_max

                ppx = [x(1),x(1) + zt(k,1)*cos(phi) + d];

                ppy = [x(2),x(2) + zt(k,1)*sin(phi) + d];

                end_points = [end_points;ppx(2),ppy(2)];

                wm = likelihood_field_range_finder_model(X(j,:)',xsensor,...

                           zt(k,:)',nearest_wall, grid_dim, std_hit,Z_weights,Z_max);

                W(j) = W(j) * wm;

            else

                dist = Z_max + std_hit*randn(1);

                ppx = [x(1),x(1) + dist*cos(phi) + d];

                ppy = [x(2),x(2) + dist*sin(phi) + d];

                missed_points = [missed_points;ppx(2),ppy(2)];

            end

            set(handle_sensor_ray(k),'XData', ppx, 'YData', ppy)

        end

function q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max)

% retorna probabilidad de medida range finder :)

% X col, zt col, xsen col

[n,m] = size(N);

% Robot global position and orientation

theta = X(3);

% Beam global angle

theta_sen = zt(2);

phi = pi_to_pi(theta + theta_sen);

%Tranf matrix in case sensor has relative position respecto to robot's CG

rotS = [cos(theta),-sin(theta);sin(theta),cos(theta)];

% Prob. distros parameters

sigmaR = std_hit;

zhit  = Zw(1);

zrand = Zw(2);

zmax  = Zw(3);

% Actual algo

q = 1;

if zt(1) ~= z_max

    % get global pos of end point of measument

    xz = X(1:2) + rotS*x_sensor + zt(1)*[cos(phi);

                                         sin(phi)];

    xi = floor(xz(1)/dim) + 1;

    yi = floor(xz(2)/dim) + 1;

    % if end point doesn't lay inside map: unknown

    if xi<1 || xi>n || yi<1 || yi>m

        q = 1.0/z_max; % all measurements equally likely, uniform in range [0-zmax]

        return

    end

    dist2 = N(xi,yi);

    gd = gauss_1D(0,sigmaR,dist2);

    q = zhit*gd + zrand/zmax;

end

end

 % Computes the circular cross-correlation between two sequences

 %

 % a,b             the two sequences

 % normalize       if true, normalize in [,]

 %

 function c = circularCrossCorrelation(a,b,normalize)

 for k=:length(a)

     c(k)=a*b';

     b=[b(end),b(:end-)]; % circular shift

 end

 if normalize

     minimum = min(c);

     maximum = max(c);

     c = (c - minimum) / (maximum-minimum);

 end