% This program generates the geolocation results using TDOA and FDOA where 
% the source is on earth
%
% The figure generated from this code corresponds to Fig. 6 with M=3 or M=4 
% in the reference paper.
%
% Reference: K. C. Ho and Y. T. Chan, “Geolocation of a known altitude object from
%  TDOA and FDOA measurements," IEEE Trans. Aerosp. Electon. Eng., vol. 33, no. 3,
%  pp. 770-783, Jul. 1997.
%
% Tianyi Jia, K. C. Ho    02-28-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear;

% ******************************************
M=3;
% ******************************************

% -- simulation setting --
NumEnsembles=200; 
NseLvlDB=-50:10:20;     % noise level in dB
NseLvl=10.^(NseLvlDB/20);   % noise standard deviation
NumNseLvl=length(NseLvl);

e=0;
% e=0.0818191908426214957;

ua=[-75.9 45.35]'/180*pi;   % longitude and latitude of the emitter on earth
r=6367.287;                 % emitter radius from earth center
u0=[r*cos(ua(2))*cos(ua(1)) r*cos(ua(2))*sin(ua(1)) (r*(1-e^2))*sin(ua(2))]';
sa_WE=[-50.0 -47.0 -53.0 -51.5]/180*pi; % satellite latitudes
sa_NS=[2.0 0.0 0.0 3.0]/180*pi; % satellite longitudes
rs=42164;                   % satellite distance from earth center
sa=[sa_WE;sa_NS];

s0=zeros(3,M);              % satellite position
for i=1:M
    s0(1,i)=rs*cos(sa(2,i))*cos(sa(1,i));
    s0(2,i)=rs*cos(sa(2,i))*sin(sa(1,i));
    s0(3,i)=rs*sin(sa(2,i));
end

% satellite speed
sv(1,:)=[-15.48 -30.78 -0.054 -119.62];
sv(2,:)=[-13.0  -28.70 -0.041 -95.15];
sv(3,:)=[-772.04 972.72 -38.60 1920.34];
sv=sv(:,1:M);

% -- create true values of TDOA and FDOA --
rg0=zeros(M,1);  % distance between the emitter and satellite
rgd0=zeros(M,1); % range rate the emitter and satellite
m0=zeros(2*M-2,1); % range diferences (TDOAs) and range rate differences (FDOAs)
rg0(1)=norm(s0(:,1)-u0);
rgd0(1)=(s0(:,1)-u0)'*sv(:,1)/rg0(1);
for i=1:M-1
    rg0(i+1)=norm(s0(:,i+1)-u0);
    m0(i)=(rg0(i+1)-rg0(1)); % d0 is distance
    rgd0(i+1)=(s0(:,i+1)-u0)'*sv(:,i+1)/rg0(i+1);
    m0(i+M-1)=rgd0(i+1)-rgd0(1);
end

randn('state',1);
Noise=randn(2*M-2,NumEnsembles);
R=zeros(4,NumNseLvl);    % R(1,:) is the CRLB, R(3,4,:) are the bias and RMSE

% -- measurement noise covariance matrix --
Q0=zeros(2*M-2);
Q0(1:M-1,1:M-1)=(eye(M-1)+ones(M-1))/2;
Q0(M:2*M-2,M:2*M-2)=Q0(1:M-1,1:M-1);
Lm0=chol(Q0,'lower');      % Q0=Lm0*Lm0'

for i=1:NumNseLvl,
    fprintf('%d/%d..',i,NumNseLvl);
    Q=Q0*(NseLvl(i)^2);
    Lm=Lm0*NseLvl(i);
    sum_ut=zeros(3,3);          % for bias computation
    [CRLB] = GeoTDOAFDOA_CRLB(u0,s0,sv,Q);
    R(1,i)=sqrt(trace(CRLB));
    for j=1:NumEnsembles,
        m=m0+Lm*Noise(:,j);    % creat a noise vector whose covariance matrix is Q
        %%  solution M=4
        [u] = GeoTDOAFDOA_EmitterOnEarth(m,r,s0,sv,Q);
        sum_ut(:,1)=sum_ut(:,1)+u;
        R(4,i)=R(4,i)+norm(u-u0)^2/NumEnsembles;
    end
    R(3,i)=norm(sum_ut(:,1)/NumEnsembles-u0);   % bias
    R(4,i)=sqrt(R(4,i));    % RMSE
end

fprintf('\n');

% -- plot results --
figure;
p=plot(NseLvlDB,20*log10(R(1,:)),'-.r',NseLvlDB,20*log10(R(4,:)),'b^');
if (M==4)
    legend('constrained CRLB M=4','new solution method, M=4','location','southeast');
else
    legend('constrained CRLB M=3','new solution method, M=3','location','southeast');
end;
xlabel('TDOA noise power, 20 log(c\sigma_d (km)) ');
ylabel('location accuracy, 20 log(RMSE (km)) ');
set(p,'LineWidth',1);
grid on