function phi = MultiStaticDfDlyLocDoppler(tau,f,T,S,c,fc,Qtau,Qf)
% phi = MultiStaticDfDlyLocDoppler(tau,f,T,S,c,fc,Qtau,Qf)
%
% This function realizes the two-stage closed-form algorithm for estimating
% the unknown states (position and velocity) of a moving target using 
% differential delay and Doppler shift measurements.
%
% Input parameter list:
% tau  : ((NumTx)(NumRx) x 1), Differential delay measurements.
% f    : ((NumTx)(NumRx) x 1), Doppler shift measurements.
% T    : (2 x NumTx), Transmitter positions.
% S    : (2 x NumRx), Receiver positions.
% c    : Signal propogation speed (m/s).
% fc   : Carrier frequency vector (Hz).
% Qtau : Covairance matrix of the differential delay measuremetns.
% Qf   : Covairance matrix of the Doppler shift measurements.
% Output
% phi  : Estimated position phi(1:2) and velocity phi(3:4).
%
% Reference: 
% 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
%

cal_num = 3;                              % Number of iterations in stage 1
M = length(T(1,:));                       % Numbers of transmitters
N = length(S(1,:));                       % Numbers of receivers
Qm = blkdiag(Qtau,Qf);                    % Covariance of measurements

%Stage 1
B1 = eye(2*M*N) + [zeros(M*N,2*M*N);eye(M*N) zeros(M*N)];
G1Tau = zeros(M*N,4+2*M);
G1F = zeros(M*N,4+2*M);
h1Tau = zeros(M*N,1);
h1F = zeros(M*N,1);
for i = 1:M
    for j = 1:N
        G1Tau((i-1)*N+j,:) = 2*[T(:,i)'-S(:,j)' zeros(1,i-1) c*tau((i-1)*N+j)+norm(T(:,i)-S(:,j)) zeros(1,2*M-i+2)];
        G1F((i-1)*N+j,:) = 2*[zeros(1,i+1) c*f((i-1)*N+j) zeros(1,M-i) fc(i)*(T(:,i)'-S(:,j)') zeros(1,i-1) fc(i)*(c*tau((i-1)*N+j)+norm(T(:,i)-S(:,j))) zeros(1,M-i)];
        h1Tau((i-1)*N+j,:) = 2*c*tau((i-1)*N+j)*norm(T(:,i)-S(:,j))+2*T(:,i)'*(T(:,i)-S(:,j))+c^2*tau((i-1)*N+j)^2;
        h1F((i-1)*N+j,:) = 2*c^2*tau((i-1)*N+j)*f((i-1)*N+j)+2*c*f((i-1)*N+j)*norm(T(:,i)-S(:,j));
    end
end
G1 = [G1Tau;G1F];
h1 = [h1Tau;h1F];

for k = 1:cal_num       % iterating stage1 to improve the weighting matrix W1
    W1 = inv(B1*Qm*B1');
    phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
    
    % Update B1
    r = sqrt(sum((ones(N,1)*phi1(1:2)'-S').^2,2));
    b1 = 2*c*kron(eye(M),diag(r));
    rdot = (ones(N,1)*phi1(1:2)'-S')*phi1(M+3:M+4)./r;
    b2 = 2*c*kron(diag(fc),diag(rdot));
    B1 = [b1 zeros(M*N);
        b2 b1];
end
    
%Stage 2   
B2 = [zeros(M,2) 2*diag(phi1(3:M+2)) zeros(M,M+2)
        zeros(M,2) diag(phi1(M+5:end)) zeros(M,2) diag(phi1(3:M+2))
        eye(2) zeros(2,2*M+2)
        zeros(2,M+2) eye(2) zeros(2,M)];
h2alpha = phi1(3:M+2).^2-phi1(1:2)'*phi1(1:2)*ones(M,1)+2*T'*phi1(1:2)-diag(T'*T);
h2beta = phi1(3:M+2).*phi1(M+5:2*M+4)-phi1(1:2)'*phi1(M+3:M+4)*ones(M,1)+T'*phi1(M+3:M+4);
h2 = [h2alpha;h2beta;zeros(4,1)]; 
G2 = -[2*(ones(M,1)*phi1(1:2)'-T'),zeros(M,2);ones(M,1)*phi1(M+3:M+4)',ones(M,1)*phi1(1:2)'-T';eye(4)];
W2 = inv(B2')*G1'*W1*G1*inv(B2);                                       
phi2 = inv(G2'*W2*G2)*G2'*W2*h2;  
    
% Final estimates
phi(1:2) = phi1(1:2) - phi2(1:2);
phi(3:4) = phi1(M+3:M+4) - phi2(3:4);
    
phi = phi';
