% This program gives an example on how to call the funtions DfDlyDopplerLoc.m, 
% CRLB.m and CRLBdelonly.m for estimating position and velocity of target,
% and calculating CRLBs.
%
% It will reproduce the Fig.1 and Fig.2 in the paper below.
%
% Liu Yang, Le Yang and K. C. Ho, "Moving target localization in multistatic
% sonar by differential delays and Doppler shifts," IEEE Signal Process. 
% Lett., vol. 23, no. 9, pp. 1160-1164, Sep. 2016.
% 
% Liu Yang, Le Yang and K. C. Ho   09-2016
% 


%% Program initialization.
clear all;                             % Clean the workspace
%close all;                             % Close all figures
%clc;                                   % Clear the command window
tic;

%% Scenario.
M = 3;                                 % Number of transmitters
N = 5;                                 % Number of reveicers
L = 10000;                             % Number of ensemble runs
c = 1500;                              % Signal propogation speed (m/s)
fc = [10;12;14]*1e3;                   % Carrier frequencies (kHz*1e3)
sigma_tau = 0.01:0.005:0.06;           % Standard deviation of differencial delays (s)
sigma_f = sqrt(1e5)*sigma_tau;         % Standard deviation of Doppler shifts (Hz)
leg = length(sigma_tau);

uo = [200;0];                          % Target position
uodot = [20;10];                       % Target velocity
xt = [500,-300,-1000];                                                                         
yt = [500,300,-1000];                                                          
xs = [-300,300,-1000,600,-600];                                                     
ys = [1000,-100,300,-1000,-700];
T = [xt;yt];                           % Transmitter positions
S = [xs;ys];                           % Receiver positions

% Initialize random number generator
seed = 2;
randn('state',seed);                  
Ave1 = 0; Ave2 = 0;           
for MCIdx = 1:L
    Ave1 = Ave1 + randn(M*N,1);
    Ave2 = Ave2 + randn(M*N,1);
end
Ave1 = Ave1/L; Ave2 = Ave2/L;

%% Generate differential delays, Doppler shifts.
tauo = zeros(M*N,1);                   % Differential delays
fo = zeros(M*N,1);                     % Doppler shifts
unit = @(x,y) (x-y)/norm(x-y);         % the unit vector from y to x.
for i = 1:M
    for j = 1:N
        tauo((i-1)*N+j,1) = (norm(uo-T(:,i))+norm(uo-S(:,j))-norm(T(:,i)-S(:,j)))/c;
        fo((i-1)*N+j,1) = fc(i)*(unit(uo,T(:,i))'*uodot+unit(uo,S(:,j))'*uodot)/c;
    end
end

%% Simulations
fprintf('Simulation in progress \n');
for nseIdx = 1:leg
    disp(['noise level=',num2str(sigma_tau(nseIdx))]);
    Qtau = sigma_tau(nseIdx)^2*eye(M*N);        % Covariance of measurements
    Qf = sigma_f(nseIdx)^2*eye(M*N);
    phi = zeros(4,L);                           % Creat matrix for each ensemble run

    randn('state',seed);
    for MCIdx = 1:L
        tau = tauo + (randn(M*N,1)-Ave1)*sigma_tau(nseIdx); % Generate measurements 
        f = fo + (randn(M*N,1)-Ave2)*sigma_f(nseIdx);
%         phi(:,MCIdx) = LocTwoStg(Qtau,Qf,c,fc,t,s,tau,f);   % Two-stage method
        phi(:,MCIdx) = MultiStaticDfDlyDopplerLoc(tau,f,T,S,c,fc,Qtau,Qf);   % Two-stage method
    end 
    % CRLB using differential delays and Doppler shifts
%%    [CRLBu(nseIdx),CRLBudot(nseIdx)] = CRLB(Qtau,Qf,c,fc,t,s,uo,uodot);
    [CRLBu(nseIdx),CRLBudot(nseIdx)] = MultiStaticDfDlyDopplerLocCRLB(uo,uodot,T,S,c,fc,Qtau,Qf);
    % CRLB using only differential delays
%%    crlbu(nseIdx) = CRLBdelonly(Qtau,c,t,s,uo);
    crlbu(nseIdx) = MultiStaticDfDlyLocCRLB(uo,T,S,c,Qtau);
    
    phi = real(phi);
    results = sum(phi,2)./L;
    MSEu(nseIdx) = sum(sum((phi(1:2,:)-uo*ones(1,L)).^2)')/L;
    MSEudot(nseIdx) = sum(sum((phi(3:4,:)-uodot*ones(1,L)).^2)')/L;
    BIASu(nseIdx) = sum((results(1:2)-uo).^2);
    BIASudot(nseIdx) = sum((results(3:4)-uodot).^2);
end

%% Plot the results
% Fig.1 
figure(1);hold on;grid on;box on;
plot(sigma_tau,10*log10(CRLBu),'r-');
plot(sigma_tau,10*log10(MSEu),'bx-');
plot(sigma_tau,10*log10(BIASu),'ko-.');
plot(sigma_tau,10*log10(crlbu),'r--');
axis([0.01 0.06 -20 35]); 
xlabel('\sigma_\tau(s)');
ylabel('10log(MSE or Bias^2)');
legend('CRLB','MSE, Proposed Method','Bias, Proposed Method','CRLB, Using Only Differential Delay Times','location','southeast');

% Fig.2
figure(2);hold on;grid on;box on;
plot(sigma_tau,10*log10(CRLBudot),'r-');
plot(sigma_tau,10*log10(MSEudot),'bx-');
plot(sigma_tau,10*log10(BIASudot),'ko-.');
xlabel('\sigma_\tau(s)');
ylabel('10log(MSE or Bias^2)');
axis([0.01 0.06 -50 5]); 
legend('CRLB','MSE, Proposed Method','Bias, Proposed Method','location','southeast');
toc;

