function [solStg2,solStg1]=MSLocMOST_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd)
% [solStg2,solStg1]=MSLocMOST_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd)
%
% This function realizes the algebraic closed-form solution for jointly
% estimating the unknown object and transmitter locations in the MOST scenario 
% using both indirect- and direct-path range and range-rate measurements.
%
% Input parameters:
% r:      (M x 1), indirect-path range measurements.
% d:      (M x 1), direct-path range measurements.
% fr:     (M x 1), indirect-path range-rate measurements.
% fd:     (M x 1), direct-path range-rate measurements.
% s:      (Dim x M), receiver position matrix, M is the number of receivers.
% Qr:     (M x M), covariance matrix of the indirect range measurements.
% Qd:     (M x M), covariance matrix of the direct range measurements.
% Qfr:    (M x M), covariance matrix of the indirect range-rate measurements.
% Qfd:    (M x M), covariance matrix of the direct range-rate measurements.
%
% Output parameters:
% solStg2: (3*Dim+2 x 1), final solution,
%          [objPos;ObjVel;txPos;ofStTm;ofStFq]^T.
% solStg1: (3*Dim+2 x 1), intermediate solution.
%          [objPos;ObjVel;txPos;ofStTm;ofStFq]^T.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Reference:
% Y. Zhang and K. C. Ho, "Multistatic localization in partially dynamic 
% scenario with only sensor positions svailable," IEEE Trans. Aerosp. 
% Electron. Syst., vol. 57, no. 5, pp. 3416-3432, Oct. 2021.
% 
% Yang Zhang and K. C. Ho   10-28-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

[Dim,M]=size(s);    % M=number of receivers
                    % Dim=dimension

% ----- stage 1 -----
for i=1:M
    hr(i,:)=0.5*(r(i)^2-s(:,i)'*s(:,i));
    hfr(i,:)=r(i)*fr(i);
    hd(i,:)=0.5*(d(i)^2-s(:,i)'*s(:,i));
    hfd(i,:)=fd(i);
    Ar(i,:)=[-s(:,i)' zeros(1,Dim) zeros(1,Dim) r(i) 0 r(i) 1 0.5 0 0]; 
    Afr(i,:)=[zeros(1,Dim) -s(:,i)' zeros(1,Dim) fr(i) 0 fr(i) 0 0 r(i) 1]; 
    Ad(i,:)=[zeros(1,Dim) zeros(1,Dim) -s(:,i)' d(i) 0 0 0 0.5 0 0];
    Afd(i,:)=[zeros(1,Dim) zeros(1,Dim) zeros(1,Dim) 0 1 0 0 0 0 0];
end
h1=[hr;hfr;hd;hfd];
A1=[Ar;Afr;Ad;Afd];
Q=blkdiag(Qr,Qfr,Qd,Qfd);
C1=eye(4*M);
% for i=1:M
%     B1(i,i)=d(i);
% end
iW1=C1*Q*C1';
W1=inv(iW1);
phi1=inv(A1'*W1*A1)*A1'*W1*h1;
% % phi1=(A1'/iW1*A1)\(A1'/iW1*h1);

for k=1:1
    for i=1:M
        C1(i,i)=norm(phi1(1:Dim)-s(:,i));
        C1(i+M,i)=((phi1(1:Dim)-s(:,i))/norm(phi1(1:Dim)-s(:,i)))'*phi1(1*Dim+1:2*Dim);
        C1(i+M,i+M)=norm(phi1(1:Dim)-s(:,i));
        C1(i+2*M,i+2*M)=norm(phi1(2*Dim+1:3*Dim)-s(:,i));
    end

    iW1=C1*Q*C1';
    W1=inv(iW1);
    phi1=inv(A1'*W1*A1)*A1'*W1*h1;
% %     phi1=(A1'/iW1*A1)\(A1'/iW1*h1);
end

% ----- stage 2 -----
C21=eye(3*Dim+2);
C22=zeros(3*Dim+2,5);
C23=[(phi1(2*Dim+1:3*Dim)-phi1(1:Dim))',zeros(1,Dim),-(phi1(2*Dim+1:3*Dim)-phi1(1:Dim))',0,0;
    -phi1(2*Dim+1:3*Dim)',zeros(1,Dim),(2*phi1(2*Dim+1:3*Dim)-phi1(1:Dim))',0,0;
    zeros(1,Dim),zeros(1,Dim),-phi1(2*Dim+1:3*Dim)',phi1(3*Dim+1),0;
    -phi1(1*Dim+1:2*Dim)',phi1(2*Dim+1:3*Dim)'-phi1(1:Dim)',phi1(1*Dim+1:2*Dim)',0,0;
    zeros(1,Dim),-phi1(2*Dim+1:3*Dim)',-phi1(1*Dim+1:2*Dim)',0,0];

C24=[2*phi1(3*Dim+3),0,0,0,0;
    2*phi1(3*Dim+1),2,0,0,0;
    0,0,1,0,0;
    2*(phi1(3*Dim+6)-phi1(3*Dim+2)),0,0,2*phi1(3*Dim+3),0;
    2*phi1(3*Dim+2),0,0,2*phi1(3*Dim+1),2];
C2=[C21,C22;C23,C24];
h2=[phi1(1:3*Dim+2);phi1(3*Dim+3)^2;2*phi1(3*Dim+4);phi1(3*Dim+5);2*phi1(3*Dim+3)*phi1(3*Dim+6);2*phi1(3*Dim+7)];

A21=eye(3*Dim+2);
A22=[(phi1(1:Dim)-phi1(2*Dim+1:3*Dim))',zeros(1,Dim),-(phi1(1:Dim)-phi1(2*Dim+1:3*Dim))',0,0;
     phi1(2*Dim+1:3*Dim)',zeros(1,Dim),(phi1(1:Dim)-2*phi1(2*Dim+1:3*Dim))',-2*phi1(3*Dim+3),0;
     zeros(1,Dim),zeros(1,Dim),phi1(2*Dim+1:3*Dim)',-phi1(3*Dim+1),0;
     phi1(1*Dim+1:2*Dim)',phi1(1:Dim)'-phi1(2*Dim+1:3*Dim)',-phi1(1*Dim+1:2*Dim)',0,2*phi1(3*Dim+3);
     zeros(1,Dim),phi1(2*Dim+1:3*Dim)',phi1(1*Dim+1:2*Dim)',-2*phi1(3*Dim+6),-2*phi1(3*Dim+3)];
A2=[A21,A22']';

iW2=C2*inv(A1'*W1*A1)*C2';
W2=inv(iW2);
phi2=inv(A2'*W2*A2)*A2'*W2*h2;
% %phi2=(A2'/iW2*A2)\(A2'/iW2*h2);

solStg1=phi1(1:3*Dim+2);
solStg2=phi2;
