function [u,v,k]=TmLocObjSsrMtnEft_QN(uInit,vInit,t,vs,m,Qt0,c,err)
% [u,v,k]=TmLocObjSsrMtnEft_QN(uInit,vInit,t,vs,m,Qt0,c,err)
%
% MLE for Time Delay 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:        (M x 1), time delay measurements.
% Qt0:      (M x M), covariance matrix structure of measurement noise.
% 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,dtTduv] = m2gradT(setak,t,vs,m,Qt0,c);

%% initialize Bk
Bk=zeros(2*K);
% method 1 : approximate it using finite-difference method
for i=1:2*K
    dsetaki=zeros(2*K,1);
    dsetaki(i)=ds;
    Bk(:,i) = (m2gradT(setak+dsetaki,t,vs,m,Qt0,c)-djduvk)/ds;
end
if rcond(Bk)<=1e-19
    % method 2 : approximate it using the matrix in Gauss-Newton
    Bk=(dtTduv*inv(Qt0)*dtTduv');
end

Hk=inv(Bk);

%% apply Qusi-Newton iterations
for k=1:MAXk,
    % inverse update
    dsetak=-Hk*djduvk;
    [step] = backtrT(1,setak,dsetak,t,vs,m,Qt0,c);
    dsetak=step*dsetak;
    setak1=setak+dsetak;
    [djduvk1] = m2gradT(setak1,t,vs,m,Qt0,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(djduvk1)<=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,dtTduv] = m2gradT(setak,t,vs,m,Qt0,c)
% [djduv,dtTduv] = m2gradR(setak,t,vs,m,Qt0,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 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:        (M x 1), time delay measurements.
% Qt0:      (M x M), covariance matrix structure of measurement noise.
% c:         signal propagation speed.
%
% Output:
% djduv:    (2Dim x M), gradient for Quasi-Newton iteration update.
% dtTduv:   (2Dim x M), 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
%

warning off;

[K,M]=size(t);       % number of sensors
uk=setak(1:K);
vk=setak(K+1:2*K);
rk=zeros(M,1);   % time measurements in the k-th step
rk1=zeros(M,1);    % 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);
dtTduv=zeros(2*K,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);
    
    rk(i)=B/A;
    s(:,i)=t(:,i)+rk(i)*vs(:,i)/c;
    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;
    
    dtTduv(1:K,i)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)+pukims(:,i))/c;
    dtTduv(K+1:2*K,i)=rk1(i)/c*dtTduv(1:K,i);
end
djduv=-dtTduv*inv(Qt0)*(m-rk/c);

% ------------------------------------------------------------------------------
function [stepf] = backtrT(step0,xk,dk,t,vs,m,Qt0,c)
% [stepf] = backtrT(step0,xk,dk,t,vs,m,Qt0,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 = eps; % Backtracking Line Search equation %  1e-10
%gamma = 1e-10;
delta = 0.5;  % parameter adjusting the step size
rhok  = 1e-8;

% positive direction (+)alpha
step = step0;
% Fk=JcostR_v1(xk,t,vs,m,Qt0,c);
Fk=JcostT(xk,t,vs,m,Qt0,c);

% Fk1=JcostR_v1(xk+step.*dk,t,vs,m,Qt0,c);
Fk1=JcostT(xk+step.*dk,t,vs,m,Qt0,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=JcostR_v1(xk+step.*dk,t,vs,m,Qt0,c);
        Fk1=JcostT(xk+step.*dk,t,vs,m,Qt0,c);
    end
end

step1 = step;
F1  = Fk1-(Fk-gamma*step1^2*(norm(dk))^2);

% negative direction (-)alpha
step = step0;
% Fk1=JcostR_v1(xk-step.*dk,t,vs,m,Qt0,c);
Fk1=JcostT(xk-step.*dk,t,vs,m,Qt0,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=JcostR_v1(xk-step.*dk,t,vs,m,Qt0,c);
        Fk1=JcostT(xk-step.*dk,t,vs,m,Qt0,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 [Jt] = JcostT(seta,t,vs,m,Qt0,c)
%
% Cost function obtained from Likelihood function using time delay
% 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:        (M x 1), time delay measurements.
% Qt0:       M x M), covariance matrix structure of measurement noise.
% c:         signal propagation speed.
%
% Output:
% Jt:       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);
rk=zeros(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)
ukimt=zeros(K,M);

uki=zeros(K,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);
    
    rk(i)=B/A;
end

Jt=(m-rk/c)'*inv(Qt0)*(m-rk/c);

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

