function [x,x1] = TmDpLocObjMtnEft_CFS(m,t,Q,fc,c)
% [x,x1] = TmDpLocObjMtnEft_CFS(m,t,Q,fc,c)
%
% Closed-from solution for Special Case: standing still sensor
% time delay and Doppler shift localization.
%
% Input:
% m:    (2M x 1), time delay and Doppler measurements, M = num. of sensors.
% t:    (Dim x M), location of sensor.
% Q:    (2M x 2M), covariance matrix of measurement noise.
% fc:   (M x 1), sensor carrier frequencies.
% 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 in the 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(2*M,1);
G0=zeros(2*M,2*K+3);
B0=eye(2*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;
    
    G0(i+M,1:K)=zeros(1,K);
    G0(i+M,K+1:2*K)=(4*fc(i)+2*m(i+M))*t(:,i)';
    G0(i+M,2*K+1)=0;
    G0(i+M,2*K+2)=-2*m(i)*fc(i)-m(i)*m(i+M);
    G0(i+M,2*K+3)=-4*fc(i)-2*m(i+M);
    h0(i+M)=-c^2*m(i)*m(i+M);
end

W0=inv(B0*Q*B0');           %inv(eye(M-1)+ones(M-1));%%eye(M-1);
x0=inv(G0'*W0*G0)*G0'*W0*h0;
u0=x0(1:K);
v0=x0(K+1:2*K);
a2=x0(2*K+2);
a3=x0(2*K+3);
k=1;
for i=1:M
    B0(i,i)=-2*c^2*m(i)+4*a3-4*t(:,i)'*v0+2*m(i)*a2;
    
    B0(i+M,i)=-c^2*m(i+M)+(2*fc(i)+m(i+M))*a2;
    B0(i+M,i+M)=-c^2*m(i)+2*a3-2*v0'*t(:,i)+a2*m(i);
end
W0=inv(B0*Q*B0');           % When the noise power is large, this matrix is close to singular
x1=(G0'*W0*G0)\G0'*W0*h0;   % Matrix is close to singular in large noise level

%% step 2: solve x2
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));

u1=x1(1:K);
v1=x1(K+1:2*K);

a2=v1'*v1;
a3=v1'*u1;
k=k+1;

end

