function theta=DSF2DLocMtTm_CFS(f,S,fo,c,T,Qn,Qs,Qfo,idc)
% theta=DSF2DLocMtTm_CFS(f,S,fo,c,T,Qn,Qs,Qfo,idc)
%
% Closed-form solution for the object location (position and velocity)
% in 2-D by using frequency measurements at multiple successive instants.
%
% Input:
%   f:      (M x 1), frequency measurements, M = number of sensors
%   S:      (2 x M), sensor positions
%   fo:     scalar, available center frequency that is not accurate
%   c:      scalar, signal propagation speed
%   T:      time interval between two consecutive measurements
%   Qn:     (MN x MN), measurement noise covariance matrix, N = number of instants
%   Qs:     (2M x 2M), sensor position error covariance matrix
%   Qfo:    scalar, variance of available center frequency
%   idc:    scalar, indicator for weighting matrix generation
%           idc =0, identity weighting matrix
%           idc =1, for measurement noise only
%           idc=2,  for measurement noise and sensor pos. errors
%           idc=3,  for measurement noise and source freq. errors
%           otherwise,    for measurement noise, sensor position and source freq. errors
%
% Output:
%   theta:  (4 x 1), object location ([position; velocity]) estimate
%
% Reference:
% M. Ahmed, K. C. Ho, and G. Wang, "Localization of a moving source by
% frequency measurements," IEEE Trans. Signal Process., vol. 68,
% pp. 4839-4854, 2020.
%
% Musaab Ahmed, K. C. Ho	08-18-2021
% Musaab Ahmed, K. C. Ho	11-26-2021, changed symbols for weighting matrix
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

M=length(S);
N=length(f)/M;

d=c*(1-f/fo);
d2=d.^2;

%=====================================
%      stage 1
%=====================================

h1=zeros(M*N,1);
A1=zeros(N*M,16);

for k=1:M*N
    i=mod(k+M-1,M)+1;
    n=fix((k-1)/M);
    h1(k,1)=d2(k)*(S(:,i)'*S(:,i));
    A1(k,:)=[2*d2(k)*S(:,i)' 2*n*T*d2(k)*S(:,i)' -d2(k) -2*n*T*d2(k) ...
        S(1,i)^2-n^2*T^2*d2(k) S(2,i)^2-n^2*T^2*d2(k) 2*S(1,i)*S(2,i) ...
        -2*S(:,i)' -2*n*T*S(:,i)' 1 n^2*T^2 2*n*T];
end
iW1=eye(N*M);

% -- initial solution without weighting matrix --
psi1=(A1'/iW1*A1)\A1'/iW1*h1;

% -- generating weighting matrix --
if (idc==1)
    iW1=eye(length(f));
else
    u_all=psi1(1:2)*ones(1,N)+psi1(3:4)*(0:N-1)*T;
    r=sqrt(sum((kron(u_all,ones(1,M))-repmat(S,1,N)).^2,1));
    B1=2*c/fo*diag((d.*r'.^2));
    Ds1=fo/c*psi1(3)./r-(f'-fo)./r.^2.*(kron(u_all(1,:),ones(1,M))-repmat(S(1,:),1,N));
    Ds2=fo/c*psi1(4)./r-(f'-fo)./r.^2.*(kron(u_all(2,:),ones(1,M))-repmat(S(2,:),1,N));
    for m=0:N-1
        Dsa=Ds1(m*M+1:M+m*M);Dsa=diag(Dsa);
        Dsb=Ds2(m*M+1:M+m*M);Dsb=diag(Dsb);
        Dsc= Dsb(:,[1;1]*(1:size(Dsb,2)));Dsc(:,1:2:end) = Dsa;
        Ds(m*M+1:M+m*M,:)=Dsc;
    end
    Dfo=-f/fo;
    
    switch idc,
        case 1,
            Qs=0; Qfo=0;
        case 2,
            Qfo=0;
        case 3,
            Qs=0;
    end;
    iW1=B1*(Ds*Qs*Ds'+Dfo*Qfo*Dfo'+Qn)*B1';
    iW1=iW1/max(iW1(:));
end;

% -- first stage solution --
psi1=(A1'/iW1*A1)\(A1'/iW1*h1);

%=====================================
%      stage 2
%=====================================

h2=psi1;

A2=zeros(16,4);
A2(1:4,1:4)=eye(4);
A2(5,1:2)=psi1(1:2)';
A2(6,1:2)=psi1(3:4)';
A2(7,3)=psi1(3);
A2(8,4)=psi1(4);
A2(9,3:4)=0.5*[psi1(4) psi1(3)];
A2(10,1:2)=[psi1(7) psi1(9)];
A2(11,1:2)=[psi1(9) psi1(8)];
A2(12,3:4)=[psi1(7) psi1(9)];
A2(13,3:4)=[psi1(9) psi1(8)];
A2(14,1:2)=psi1(10:11)';
A2(15,3:4)=psi1(12:13)';
A2(16,1:2)=psi1(12:13)';

theta=psi1(1:4);

for l=1:5
    
    % -- weighting matrix --
    B2=eye(16);
    B2(5,1:2)=-theta(1:2)';
    B2(6,3:4)=-theta(1:2)';
    B2(7,3)=-theta(3);
    B2(8,4)=-theta(4);
    B2(9,3:4)=-0.5*[theta(4) theta(3)];
    B2(10,7:9)=-[theta(1) 0 theta(2)];
    B2(11,8:9)=-[theta(2) theta(1)];
    B2(12,7:9)=-[theta(3) 0 theta(4)];
    B2(13,8:9)=-[theta(4) theta(3)];
    B2(14,10:11)=-theta(1:2)';
    B2(15,12:13)=-theta(3:4)';
    B2(16,12:13)=-theta(1:2)';
    iW2=B2/(A1'/iW1*A1)*B2';iW2=iW2/max(iW2(:));
    
    % -- final solution --
    theta=(A2'/iW2*A2)\(A2'/iW2*h2);
    
end

% --------------------------------------------------------------
