function [u,v,k]=TmDpLocObjSsrMtnEft_QN(uInit,vInit,t,vs,m,Q0,fc,c,err)
% [u,v,k]=TmDpLocObjSsrMtnEft_QN(uInit,vInit,t,vs,m,Q0,fc,c,err)
%
% MLE for Time Delay and Doppler shift localization by Quasi-Newton
% iterations (Broyden's method).
%
% Input:
% uInit:    (Dim x 1), initial guess of object position.
% vInit:    (Dim x 1), initial guess of object velocity.
% t:        (Dim x M), sensor positions when emitting signals, M = num. of sensors.
% vs:       (Dim x M), sensor velocities.
% m:        (2M x 1), time delay and Doppler shift measurements.
% Q0:       (2M x 2M), covariance matrix structure of measurement noise.
% fc:       (M x 1), sensor carrier frequencies.
% c:        signal propagation speed.
% err:      residual error to terminate iterations.
%
% Output:
% u:        (Dim x 1), object position estimate.
% v:        (Dim x 1), object velocity estimate.
% k:        number of iterations taken.
%
% 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
MAXk=200;               % maximum number of iterations

setak=[uInit;vInit];
ds=1e-4;
djduvk=zeros(2*K,1);
step=1;
[djduvk,G] = m2gradTF(setak,t,vs,m,Q0,fc,c);

%% initialize Bk
% method 2 : approximate it using the matrix in Gauss-Newton
Bk=(G*inv(Q0)*G');
if rcond(Bk)<=1e-17
    for i=1:2*K
        dsetaki=zeros(2*K,1);
        dsetaki(i)=ds;
        Bk(:,i) = (m2gradTF(setak+dsetaki,t,vs,m,Q0,fc,c)-djduvk)/ds;
    end
end

Hk=inv(Bk);

%% apply Quasi-Newton iterations
for k=1:MAXk
    % inverse update
    dsetak=-Hk*djduvk;
    [step] = backtrTF(1,setak,dsetak,t,vs,m,Q0,fc,c);
    
    dsetak=step*dsetak;
    setak1=setak+dsetak;
    [djduvk1] = m2gradTF(setak1,t,vs,m,Q0,fc,c);
    y=djduvk1-djduvk;
    
    Hk=(eye(2*K)-dsetak*y'/(y'*dsetak))*Hk*(eye(2*K)-y*dsetak'/(y'*dsetak))+dsetak*dsetak'/(y'*dsetak);  % BFGS inverse update
    if  norm(y)<=err
        u=setak1(1:K);
        v=setak1(K+1:2*K);
        break;
    end
    setak=setak1;
    djduvk=djduvk1;
end

%% extreme condition
if exist('u')==0 % if the method cannot converge within the threshold
    u=setak1(1:K);
    v=setak1(K+1:2*K);
end

% --------------------------------------------------------------------------------
% --------------------------------------------------------------------------------
function [djduv,dmTduv] = m2gradTF(setak,t,vs,m,Q0,fc,c)
% [djduv,dmTduv] = m2gradTF(setak,t,vs,m,Q0,fc,c)
%
% Computes the gradient of cost function with respect to uk and vk using
% the measurements together with the uk and vk, for time delay and Doppler
% shift localization by Quasi-Newton MLE.
%
% Input:
% setak:    [uk',vk']', object location.
% t:        (Dim x M), sensor positions when emitting signals, M = num. of sensors.
% vs:       (Dim x M), sensor velocities.
% m:        (2M x 1), time delay and Doppler shift measurements.
% Q0:       (M x M), covariance matrix structure of measurement noise.
% fc:       (M x 1), sensor carrier frequencies.
% c:         signal propagation speed.
%
% Output:
% djduv:    (2Dim x 2M), gradient for Quasi-Newton iteration update.
% dmTduv:   (2Dim x 2M), gradient of measurements w.r.t. object location.
%
% 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
%

[K,M]=size(t);       % number of sensors
uk=setak(1:K);
vk=setak(K+1:2*K);

rk1=zeros(1,M);   % r1 is the first part of r
ukmt=uk-t;         % u0-t
L=zeros(4,M);      % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)  L(3,i)=norm(u0i-ti) and L(4,i)=norm(u0i-si)
s=zeros(K,M);      % location of sensor with signal being received
ukimt=zeros(K,M);
ukims=zeros(K,M);
uki=zeros(K,M);
pukimt=zeros(K,M);
pukims=zeros(K,M);
Ai=zeros(1,M);
Bi=zeros(1,M);
Ci=zeros(1,M);
Di=zeros(1,M);
G=zeros(2*K,2*M);  % dsetaTduv
mk=zeros(2*M,1);

for i=1:M
    L(1,i)=norm(ukmt(:,i));    %  L(1,i)=norm(u0-ti)
    A=1-vk'*vk/c^2;
    B=-2*ukmt(:,i)'*vk/c;
    C=-L(1,i)^2;
    rk1(i)=(-B+sqrt(B^2-4*A*C))/(2*A);
    uki(:,i)=uk+rk1(i)/c*vk;
    
    ukimt(:,i)=uki(:,i)-t(:,i);
    L(3,i)=norm(ukimt(:,i));   % L(3,i)=norm(u0i-ti)
    A=1-vs(:,i)'*vs(:,i)/c^2;
    B=2*(L(3,i)-vs(:,i)'*ukimt(:,i)/c);
    
    %% time measurement
    mk(i)=B/A/c;
    s(:,i)=t(:,i)+mk(i)*vs(:,i);
    ukims(:,i)=uki(:,i)-s(:,i);      %u0-s
    L(4,i)=norm(ukims(:,i));   % L(4,i)=norm(u0i-si)
    
    pukimt(:,i)=ukimt(:,i)/L(3,i);
    pukims(:,i)=ukims(:,i)/L(4,i);
    
    Ai(i)=pukims(:,i)'*vk/c;
    Bi(i)=pukimt(:,i)'*vk/c;
    Ci(i)=pukims(:,i)'*vs(:,i)/c;
    Di(i)=pukimt(:,i)'*vs(:,i)/c;
    
    G(1:K,i)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)+pukims(:,i))/c;   % dt/du
    G(K+1:2*K,i)=rk1(i)/c*G(1:K,i);                                         % dt/dv
    
    %% Doppler measurement
    mk(i+M)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)'+pukims(:,i)')*(vk-vs(:,i))*fc(i)/c;
    
    dr1Tdu=1/(1-Bi(i))*pukimt(:,i);
    dAidu=((eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*vk/L(4,i)-ukims(:,i)'*vk*(eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*ukims(:,i)/L(4,i)^3)/c;
    dBidu=((eye(K)+dr1Tdu*vk'/c)*vk/L(3,i)-ukimt(:,i)'*vk*(eye(K)+dr1Tdu*vk'/c)*ukimt(:,i)/L(3,i)^3)/c;
    dCidu=((eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*vs(:,i)/L(4,i)-ukims(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*ukims(:,i)/L(4,i)^3)/c;
    dDidu=((eye(K)+dr1Tdu*vk'/c)*vs(:,i)/L(3,i)-ukimt(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*vk'/c)*ukimt(:,i)/L(3,i)^3)/c;
    
    dAidv=(pukims(:,i)+dAidu*rk1(i))/c;
    dBidv=(pukimt(:,i)+dBidu*rk1(i))/c;
    dCidv=rk1(i)/c*dCidu;
    dDidv=rk1(i)/c*dDidu;
    
    G(1:K,M+i)=(fc(i)/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidu+(1+Ai(i))*(dBidu-dDidu))*(1-Bi(i))*(1+Ci(i)) - ...
        ( -dBidu*(1+Ci(i))+ dCidu*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  fc(i)/((1+Ci(i))^2) * ( (dAidu-dCidu)*(1+Ci(i))-dCidu*(Ai(i)-Ci(i)) ) );  % dfT / du
    G(K+1:2*K,M+i)=(fc(i)/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidv+(1+Ai(i))*(dBidv-dDidv))*(1-Bi(i))*(1+Ci(i)) - ...
        ( -dBidv*(1+Ci(i))+ dCidv*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  fc(i)/((1+Ci(i))^2) * ( (dAidv-dCidv)*(1+Ci(i))-dCidv*(Ai(i)-Ci(i)) ) );  % dfT / dv
end

djduv=-G*inv(Q0)*(m-mk);
dmTduv=G;

% --------------------------------------------------------------------------------
function [stepf] = backtrTF(step0,xk,dk,t,vs,m,Q0,fc,c)
% [stepf] = backtrTF(step0,xk,dk,t,vs,m,Q0,fc,c)
%
%% BACKTRACKING ARMIJO-TYPE
% DESCRIPTION:
% Search method along a coordinate axis in which the search should be conducted
% in both directions of the axis. It should also take into account the fact that
% one direction dk can be assigned such that alpha=0 represents a
% local minimum point of the function g(alpha)=F(xk+alpha*dk), for which
% may not be able to find positive or negative values ??of alpha
% close to 0 for which g(alpha)<g(0). If you do not want to use any
% derivative, numerical "finished" procedures must define can
% discriminate the situation. The model presented is an outline
% Backtracking Armijo-type, based on the condition of acceptability of type "Parabolic".
%
% function [alpha] = backtr(alpha_guess,Xk,dk,F,gamma,delta,rhok)
% INPUT:
%       NOTE: (*) indicates necessary input, the other variables are optional
%       (*) step0        - current steplength (1*1) [>0];
%       (*) xk           - current iterate    (M*1);
%       (*) dk           - search direction   (M*1);
%           gamma        - constant provided by the user (1*1) [>0];
%           delta        - constant provided by the user (1*1) into the range [0,  1];
%           rhok         - constant provided by the user (1*1) into the range [0,  1];
%       (*) Fk or Fk1    - function handle of the objective function (RN->R );
% OUTPUT:
%       stepf - the step size when the condition holds (1*1);
% REVISION:
%       Ennio Condoleo - 21.15 13 Feb 2014
% REFERENCE:
%       http://books.google.it/books?id=wXyLzZahvmsC&pg=PA123&lpg=PA123&dq=ottimizzazione+unidimensionale+senza+derivata&source=bl&ots=p5Be0KpbtX&sig=tLnygv0XZwzQYHoyPPfCZ2-e4C4&hl=it&sa=X&ei=CfL7UqfpFcXRhAfujYHIBA&ved=0CDQQ6AEwAQ#v=onepage&q=ottimizzazione%20unidimensionale%20senza%20derivata&f=false

gamma = 1e-10; % Backtracking Line Search equation
delta = 0.5;  %
rhok  = 1e-8;

% positive direction (+)alpha
step = step0;
Fk=JcostTF(xk,t,vs,m,Q0,fc,c);

Fk1=JcostTF(xk+step.*dk,t,vs,m,Q0,fc,c);
while (Fk1>Fk-gamma*step^2*(norm(dk))^2)
    if (step*norm(dk) < rhok)
        step  = 0;              % <-- failure to search for a value of alpha nonzero
        Fk1=Fk;
    else
        step = step*delta;     % <-- reduction of the steplength
        Fk1=JcostTF(xk+step.*dk,t,vs,m,Q0,fc,c);
    end
end
step1 = step;
F1  = Fk1-(Fk-gamma*step1^2*(norm(dk))^2);

% negative direction (-)alpha
step = step0;
Fk1=JcostTF(xk-step.*dk,t,vs,m,Q0,fc,c);

while (Fk1>Fk-gamma*step^2*(norm(dk))^2)
    if (step*norm(dk) < rhok)
        step   = 0;              % <-- failure to search for a value of alpha nonzero
        Fk1=Fk;
    else
        step = step*delta;      % <-- reduction of the steplength
        Fk1=JcostTF(xk-step.*dk,t,vs,m,Q0,fc,c);
    end
end

step2 = -step;
F2 = Fk1-(Fk-gamma*step2^2*(norm(dk))^2);
% choice of the value of alpha for which it is provided with sufficient reduction
if (F1<F2)
    stepf = step1;
else
    stepf = step2;
end

% --------------------------------------------------------------------------------
function [Jtf] = JcostTF(seta,t,vs,m,Q0,fc,c)
% [Jtf] = JcostTF(seta,t,vs,m,Q0,fc,c)
%
% Cost function obtained from Likelihood function using time delay and
% Doppler shift measurements
%
% Input:
% setak:    [uk',vk']', object location.
% t:        (Dim x M), sensor positions when emitting signals, M = num. of sensors.
% vs:       (Dim x M), sensor velocities.
% m:        (2M x 1), time delay and Doppler shift measurements.
% Q0:       (2M x 2M), covariance matrix structure of measurement noise.
% fc:       (M x 1), sensor carrier frequencies.
% c:         signal propagation speed.
%
% Output:
% Jtf:      Likelihood cost function value.
%
% 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

uk=seta(1:K);
vk=seta(K+1:2*K);
mk=zeros(2*M,1);   % time measurements in the k-th step

%% Creat the time measuremet in k-th step
rk1=zeros(1,M);   % r1 is the first part of r
ukmt=uk-t;         % u0-t
L=zeros(4,M);      % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)  L(3,i)=norm(u0i-ti) and L(4,i)=norm(u0i-si)
s=zeros(K,M);      % location of sensor with signal being received
ukimt=zeros(K,M);
ukims=zeros(K,M);
uki=zeros(K,M);
pukimt=zeros(K,M);
pukims=zeros(K,M);
Ai=zeros(1,M);
Bi=zeros(1,M);
Ci=zeros(1,M);
Di=zeros(1,M);
G=zeros(2*K,2*M);
for i=1:M
    L(1,i)=norm(ukmt(:,i));    %  L(1,i)=norm(u0-ti)
    A=1-vk'*vk/c^2;
    B=-2*ukmt(:,i)'*vk/c;
    C=-L(1,i)^2;
    rk1(i)=(-B+sqrt(B^2-4*A*C))/(2*A);
    uki(:,i)=uk+rk1(i)/c*vk;
    
    ukimt(:,i)=uki(:,i)-t(:,i);
    L(3,i)=norm(ukimt(:,i));   % L(3,i)=norm(u0i-ti)
    A=1-vs(:,i)'*vs(:,i)/c^2;
    B=2*(L(3,i)-vs(:,i)'*ukimt(:,i)/c);
    %% range measurement
    mk(i)=B/A/c;
    s(:,i)=t(:,i)+mk(i)*vs(:,i);
    ukims(:,i)=uki(:,i)-s(:,i);      %u0-s
    L(4,i)=norm(ukims(:,i));   % L(4,i)=norm(u0i-si)
    
    pukimt(:,i)=ukimt(:,i)/L(3,i);
    pukims(:,i)=ukims(:,i)/L(4,i);
    
    Ai(i)=pukims(:,i)'*vk/c;
    Bi(i)=pukimt(:,i)'*vk/c;
    Ci(i)=pukims(:,i)'*vs(:,i)/c;
    Di(i)=pukimt(:,i)'*vs(:,i)/c;
    
    %% Doppler measurement
    mk(i+M)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)'+pukims(:,i)')*(vk-vs(:,i))*fc(i)/c;
end

Jtf=(m-mk)'*inv(Q0)*(m-mk);

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

