function [ z H ] = measurement( x )
%MEASUREMENT Measurement Model:
%   Measure the robot's distance from several landmarks.

% landmarks:
l = [0, -1; -3, -2; 0, 6; -3, 6]';

nl = size(l,2);
nx = size(x,3);

z = zeros(nl,1,nx);
H = zeros(nl,3);

pos = x(1:2,:,:);
for i = 1:nl
    diff = pos - repmat(l(:,i),1,1,size(pos,3));
    z(i,:,:) = sqrt(sum(diff.*diff,1));
end % for

% EKF case
if size(x,3) == 1
    for i = 1:nl
        diff = pos - l(:,i);
        H(i,:) = [ diff(1)/(z(i,1))^(1/2), diff(2)/(z(i,1))^(1/2), 0;];
    end % for
end 

end

