function theta=DSF3DTMA_CFS(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc)
% theta=DSF3DTMA_CFS(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc)
%
% Closed-form solution (CFS) for obtaining the initial object position
% and velocity for TMA.
%
% Input:
% f:      (MN x 1), DSF measurement vector,
%         (M=num. of sensors, N=num. of time measurements).
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% Qn:     (MN x MN), covariance matrix of DSF measurements.
% Qfc:    scalar, variance of center frequency.
% Qs:     (3M x 3M), covariance matrix of sensor positions.
% wghIdc: scalar, weighting matrix indicator for different mix of errors
% wghIdc: scalar, weighting matrix indicator for different mix of errors
%         wghIdc=0   No weighting matrix
%         wghIdc=1   weighting matrix for measurement errors only
%         wghIdc=2   weighting matrix for measurement and sensor pos. errors
%         wghIdc=3   weighting matrix for measurement and source freq. errors
%         otherwise  Full weighting matrix
%
% Output:
% theta:  (6 x 1), solution, [objPos;objVel].
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization
% and motion analysis based on Doppler shifted frequencies," IEEE Trans.
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
%
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

S=S';
M=size(S,1);

r=c*(1-f/fc);
r2=r.^2;

%  -- Stage 1 --
h1=zeros(M*N,1);
A1=zeros(N*M,23);
for k=1:M*N
    i=mod(k+M-1,M)+1;
    n=fix((k-1)/M);
    h1(k,1)=r2(k)*(S(i,:)*S(i,:)');
    A1(k,:)=[2*r2(k)*S(i,:) 2*n*T*r2(k)*S(i,:) -r2(k) -2*n*T*r2(k) -((n*T)^2*r2(k)-S(i,1)^2) -((n*T)^2*r2(k)-S(i,2)^2) -((n*T)^2*r2(k)-S(i,3)^2) 2*S(i,1)*S(i,2) 2*S(i,1)*S(i,3) 2*S(i,2)*S(i,3)  -2*S(i,:) -2*n*T*S(i,:) (n*T)^2 2*n*T 1];
end
iW1=eye(N*M);
phi_1=(A1'/iW1*A1)\A1'/iW1*h1;    % solution without weighting

iW1=DSF3DTMA_Weight(f,fc,S',N,c,T,phi_1(1:3),phi_1(4:6),Qn,Qfc,Qs,wghIdc);

phi_1=(A1'/iW1*A1)\(A1'/iW1*h1);  % stage-1 solution

% -- Stage 2 --
h2=[phi_1(1:7);2*phi_1(8);phi_1(9:11);2*phi_1(12:17);phi_1(18:21);2*phi_1(22);phi_1(23)];
A2=zeros(23,6);
B2=diag([ones(7,1);2;ones(3,1);2*ones(6,1);ones(4,1);2;1]);
A2(1:6,1:6)=eye(6);
A2(7,1:3)=phi_1(1:3)';
A2(8,:)=[phi_1(4:6)' phi_1(1:3)'];
A2(9,4)=phi_1(4);
A2(10,5)=phi_1(5);
A2(11,6)=phi_1(6);
A2(12,4:5)=[phi_1(5) phi_1(4)];
A2(13,4:6)=[phi_1(6) 0 phi_1(4)];
A2(14,5:6)=[phi_1(6) phi_1(5)];
A2(15,1:4)=[phi_1(9) phi_1(12) phi_1(13) phi_1(8)];
A2(16,1:5)=[phi_1(12) phi_1(10) phi_1(14) 0 phi_1(8)];
A2(17,:)=[phi_1(13) phi_1(14) phi_1(11) 0 0 phi_1(8)];
A2(18,4:6)=[phi_1(9) phi_1(12) phi_1(13)];
A2(19,4:6)=[phi_1(12) phi_1(10) phi_1(14)];
A2(20,4:6)=[phi_1(13) phi_1(14) phi_1(11)];
A2(21,4:6)=[phi_1(18) phi_1(19) phi_1(20)];
A2(22,:)=[phi_1(18:20)' phi_1(15:17)'];
A2(23,1:3)=phi_1(15:17)';

phi_2=phi_1;

for l=1:10,
    B2(7,1:3)=-phi_2(1:3)';
    B2(8,1:6)=-[phi_2(4:6)' phi_2(1:3)'];
    B2(9,4)=-phi_2(4);
    B2(10,5)=-phi_2(5);
    B2(11,6)=-phi_2(6);
    B2(12,4:5)=-[phi_2(5) phi_2(4)];
    B2(13,4:6)=-[phi_2(6) 0 phi_2(4)];
    B2(14,5:6)=-[phi_2(6) phi_2(5)];
    B2(15,8:13)=-[phi_2(4) phi_2(1) 0 0 phi_2(2:3)'];
    B2(16,8:14)=-[phi_2(5) 0 phi_2(2) 0 phi_2(1) 0 phi_2(3)];
    B2(17,8:14)=-[phi_2(6) 0 0 phi_2(3) 0 phi_2(1:2)'];
    B2(18,9:13)=-[phi_2(4) 0 0 phi_2(5:6)'];
    B2(19,10:14)=-[phi_2(5) 0 phi_2(4) 0 phi_2(6)];
    B2(20,11:14)=-[phi_2(6) 0 phi_2(4:5)'];
    B2(21,18:20)=-phi_2(4:6)';
    B2(22,15:20)=-[phi_2(4:6)' phi_2(1:3)'];
    B2(23,15:17)=-phi_2(1:3)';

    iW2=B2/(A1'/iW1*A1)*B2';iW2=iW2/max(iW2(:));
    phi_2=(A2'/iW2*A2)\(A2'/iW2*h2);
end

theta=phi_2;

end
