mkmeasure(X , Xobs , R , ind_pos , ind_mes);

function Y = mkmeasure(X , Xobs , R , ind_pos , ind_mes);

%
% Generate  measurement (bearing and eventually range) given X (state of the M sources over time),
%          Xobs (state of the maeouverer) and R the covariance matrix of the assumed Gaussian noise process
%
% Y = mkmeasure(X , Xobs , R);
%
%
% Inputs
% ------
%
% X            (nx x 1 x M x T) state sources
%
% Xobs         (nx x 1 x T)     state maneouverer
%
% R            covariance noise process. R can be a (nz x nz) or (nz x nz x M) or (nz x nz x M x T)
%
% Optionnal input
%
% Y = mkmeasure(X , Xobs , R , ind_pos , ind_mes);
%
% ind_pos     x and y index in the state vector (default ind_pos = [1 , 2])
%
% ind_mes     bearing and range index in the measurement vector (default ind_pos = [1 , 2])
%
%
% Ouput
% -----
%
% Y           (nz x 1 x M x T)
%
%
%***************************************************************************%%
%  Auteur Sbastien PARIS ([e-mail]), Septembre 2002                   %
%***************************************************************************%%


if (nargin < 4)
   
   ind_pos = [1 , 2];
   
end

if (nargin < 5)
   
   ind_mes = [1 , 2];
   
end



[nx , a , M , T] = size(X);

[nz , s , q , v] = size(R);


Y                = zeros(nz , 1 , M , T);

OM               = ones(M , 1);

Xobservateur     = permute(Xobs(: , : , : , OM) , [1 2 4 3]);

N                = ndtimes(permute(ndchol(R) , [2 1 3 4]) , randn(nz , 1 , M , T));

tpy              = X(ind_pos(2) , : , : , :) - Xobservateur(ind_pos(2) , : , : , :);

tpx              = X(ind_pos(1) , : , : , :) - Xobservateur(ind_pos(1) , : , : , :);

Y(ind_mes(1) , : , : , :) = atan2(tpy , tpx)          +  N(1 , : , : , :);  % angle

if (nz == 2)

  Y(ind_mes(2) , : , : , :) = sqrt((tpx.^2  +  tpy.^2)) + N(2 , : , : , :);  % range

end