function [x,x0,x1] = TmLocObjMtnEft_CFS(m,t,Qt,c)
% [x,x0,x1] = TmLocObjMtnEft_CFS(m,t,Qt,c)
%
% Closed-from solution for Special Case: standing still sensor
% time delay localization.
%
% Input:
% m:    (M x 1), time delay and Doppler measurements, M = num. of sensors.
% t:    (Dim x M), location of sensor.
% Qt:   (M x M), covariance matrix of measurement noise.
% c:    signal propagation speed.
%
% Output:
% x:    (2Dim x 1), x=[u,v]' final solution.
% x1:   (2Dim+3 x 1), x1=[u,v,a1,a2,a3]' solution from first stage.
%
% The program can be used for 2-D(Dim=2) or 3-D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang and X. Shen, "Localization of a moving object
% with sensors in motion by time delays and Doppler shifts," IEEE Trans.
% Signal Process., vol. 68, pp. 5824-5841, 2020.
%
% Tianyi Jia and K. C. Ho   11-08-2020
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

warning off;

[K,M]=size(t);          % K=localization dimension, M=number of sensors

% -- closed form solution --

%% step 1: solve x1
h0=zeros(M,1);
G0=zeros(M,2*K+3);
B0=eye(M);

for i=1:M,
    G0(i,1:K)=8*t(:,i)';
    G0(i,K+1:2*K)=4*m(i)*t(:,i)';
    G0(i,2*K+1)=-4;
    G0(i,2*K+2)=-m(i)^2;
    G0(i,2*K+3)=-4*m(i);
    h0(i)=4*t(:,i)'*t(:,i)-c^2*m(i)^2;
end

W0=inv(B0*Qt*B0');%inv(eye(M-1)+ones(M-1));%%eye(M-1);

x0=(G0'*W0*G0)\G0'*W0*h0;    % Matrix can be close to singular

u0=x0(1:K);
v0=x0(K+1:2*K);
a2=x0(2*K+2);
a3=x0(2*K+3);

for i=1:M
    %% construct from a and b
    B0(i,i)=-2*c^2*m(i)+4*a3-4*t(:,i)'*v0+2*m(i)*a2;
end
W0=inv(B0*Qt*B0');
x1=(G0'*W0*G0)\G0'*W0*h0;   % Matrix is close to singular

%% step 2: solve x2. method a
u1=x1(1:K);
v1=x1(K+1:2*K);

G1=zeros(2*K+3,2*K);
h1=zeros(2*K+3,1);
B1=zeros(2*K+3,2*K+3);

G1(1:2*K,1:2*K)=eye(2*K);
G1(2*K+1,1:K)=ones(1,K);
G1(2*K+2,1:K)=ones(1,K);
G1(2*K+2,K+1:2*K)=2*ones(1,K);
G1(2*K+3,K+1:2*K)=ones(1,K);

h1(1:K)=u1.*u1;
h1(K+1:2*K)=u1.*v1;
h1(2*K+1)=(x1(2*K+1));
h1(2*K+2)=(u1+v1)'*(u1+v1)-(x1(2*K+2));
h1(2*K+3)=x1(2*K+3);

B1(1:K,1:K)=2*diag(u1);
B1(K+1:2*K,1:K)=diag(v1);
B1(K+1:2*K,K+1:2*K)=diag(u1);
B1(2*K+1,2*K+1)=1;
B1(2*K+2,1:K)=2*(u1+v1)';
B1(2*K+2,K+1:2*K)=2*(u1+v1)';
B1(2*K+2,2*K+2)=-1;
B1(2*K+3,2*K+3)=1;

W1=inv(B1')*G0'*W0*G0*inv(B1);

x2=(G1'*W1*G1)\G1'*W1*h1;
x=zeros(2*K,1);
x(1:K)=sign(x1(1:K)).*sqrt(abs(x2(1:K)));
x(K+1:2*K)=(x2(K+1:2*K))./(x(1:K));

end

