function [theta,cost] = XcvrTOALoc_GN_Seq(d,s,K,Q,OfstUnkn,theta0)
% function [theta,cost] = XcvrTOALoc_GN_Seq(d,s,K,Q,OfstUnkn,theta0)
%
% Sequential estimation using Gauss-Newton iterative implementation
%
% Input parameter list:
% d:  (KMN x 1), distance (time delay) measurement vector
%      M = num. of transceivers, N = num. of sensors
% s:  (Dim x N), sensor positions
% K:  K = num. of time instants
% Q:  (KMN x KMN), covariance matrix of distance measurements
% OfstUnkn:  flag, 1 if time offsets known and 0 if offsets unknown
% theta0:    initial solution guess
%
% Output parameter list:
% theta:    solution
% cost:     minimum optimization cost value
%
% The code can work with 2-D(Dim=2) or 3-D(Dim=3) localization.
%
% Reference:
% K. C. Ho, "Localization through transceivers in unknown constant velocity
% trajectories," IEEE Trans. Signal Process., vol. 70, pp. 3011-3028, 2022.
%
% K. C. Ho     08-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

theta=theta0*1e3; cost=1e9;

[L,N]=size(s);
M=(length(theta0)-L)/(2*L+1*OfstUnkn);

tiParaLgth=2*L;

k=0:K-1;

% -- set E --
if (N==2)
    Ec=eye(K);
    E=[Ec,-Ec];
    % -----------
elseif (N ==3)
    E=[-eye(K),eye(K),zeros(K); -eye(K),zeros(K),eye(K)];
elseif (N==4)
    E=[-eye(K),eye(K),zeros(K),zeros(K); -eye(K),zeros(K),eye(K),zeros(K); -eye(K),zeros(K),zeros(K),eye(K)];
elseif (N==5)
    E=[-eye(K),eye(K),zeros(K),zeros(K),zeros(K); -eye(K),zeros(K),eye(K),zeros(K),zeros(K); ...
        -eye(K),zeros(K),zeros(K),eye(K),zeros(K); -eye(K),zeros(K),zeros(K),zeros(K),eye(K)];
else
    E=zeros((N-1)*K,N*K);
    for ii=1:N-1,
        E((ii-1)*K+1:ii*K,:)=[-eye(K),zeros(K,(ii-1)*K),eye(K),zeros(K,((N-1)-ii)*K)];
    end;
end;

Eo=E;
for i=2:M,
    E=blkdiag(E,Eo);
end;
% -----------

tOnly=0;
MxItn=100;
W=inv(Q);
theta0org=theta0;

for itn=1:MxItn,
    dHat=zeros(K,M,N);
    u=theta0(1:L);
    for i=1:M,
        ti=repmat(theta0(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L),1,K)+theta0(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L)*k;
        liu(:,i)=sqrt(sum((ti-repmat(u,1,K)).^2));
        rhoiu=-(ti-repmat(u,1,K))./(repmat(liu(:,i)',L,1)+eps);
        if (OfstUnkn)
            delta(i)=theta0(end-M+i);
        else
            delta(i)=0;
        end;
        
        Gi=[];
        for j=1:N,
            lij(:,i,j)=sqrt(sum((ti-repmat(s(:,j),1,K)).^2));
            rhoij=(ti-repmat(s(:,j),1,K))./(repmat(lij(:,i,j)',L,1)+eps);
            Gij=[rhoiu; (-rhoiu+rhoij); repmat(k,L,1).*(-rhoiu+rhoij); ones(1,length(rhoiu))]';
            Gi=[Gi; Gij];
            dHat(:,i,j)=liu(:,i)+lij(:,i,j)+delta(i)*OfstUnkn;
        end;
        
        if (i>1)
            G=[G, zeros((i-1)*N*K,size(Gi,2)-L); Gi(:,1:L), zeros(N*K,size(G,2)-L), Gi(:,L+1:end)];
        else
            G=Gi;
        end;
    end;
    dHat=permute(dHat,[1 3 2]);
    
    % -- rearrange G and H with all time offsets at the end --
    tpG=G;
    for i=1:M,
        G(:,L+tiParaLgth*(i-1)+1:L+tiParaLgth*(i))=tpG(:,L+(tiParaLgth+1)*(i-1)+1:L+(tiParaLgth+1)*(i-1)+2*L);
        G(:,end-M+i)=tpG(:,L+(2*L+1)*(i-1)+2*L+1);
    end;
    % --------------------------------------------------------
    
    % -- estimate t0 and v --
    theta0org=theta0;
    G1=E*G(:,L+1:end-M);
    W1=inv(E*Q*E');
    dx1=(G1'*W1*G1)\G1'*W1*E*(d-dHat(:));
    %     dx1=inv(G1'*W1*G1)*G1'*W1*E*(d-dHat(:));
    if (OfstUnkn)
        dx=[zeros(L,1);dx1;zeros(M,1)];
    else
        dx=[zeros(L,1);dx1];
    end;
    if (norm(dx)>1e-10)
        theta0=theta0+0.125*dx;
    else
        break;
    end;
    
end;    % itn

theta01=theta0;
if (~tOnly)
    theta0=theta0org;
end;

% -- estimate u and delta --
for itn=1:MxItn,
    dHat=zeros(K,M,N);
    u=theta0(1:L);
    for i=1:M,
        ti=repmat(theta0(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L),1,K)+theta0(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L)*k;
        liu(:,i)=sqrt(sum((ti-repmat(u,1,K)).^2));
        rhoiu=-(ti-repmat(u,1,K))./(repmat(liu(:,i)',L,1)+eps);
        if (OfstUnkn)
            delta(i)=theta0(end-M+i);
        else
            delta(i)=0;
        end;
        Gi=[];
        for j=1:N,
            lij(:,i,j)=sqrt(sum((ti-repmat(s(:,j),1,K)).^2));
            if (OfstUnkn)
                Gij=[rhoiu; ones(1,length(rhoiu))]';
            else
                Gij=[rhoiu]';
            end;
            Gi=[Gi; Gij];
            dHat(:,i,j)=liu(:,i)+lij(:,i,j)+delta(i)*OfstUnkn;
        end;
        if (i>1)
            G=[G, zeros(size(G,1),1); Gi(:,1:L), zeros(size(Gi,1),size(G,2)-L), Gi(:,end)];
        else
            G=Gi;
        end;
    end;
    dHat=permute(dHat,[1 3 2]);
    
    dx2=(G'*W*G)\G'*W*(d-dHat(:));
    %     dx2=inv(G'*W*G)*G'*W*(d-dHat(:));
    if (OfstUnkn)
        dx=[dx2(1:L);dx1*0;dx2(end-M+1:end)];
    else
        dx=[dx2(1:L);dx1*0];
    end;
    if (norm(dx)>1e-10 )
        theta0=theta0+0.125*dx;
    else
        break;
    end;
end;

% ----- divergence in t, u should be bad -----
if (sum((theta0(1:L)-theta0org(1:L)).^2)==0)
    theta0(1:L)=theta0(1:L)+1e10;
end;

theta0(L+1:end-M)=theta01(L+1:end-M);
theta=theta0;

cost=(d-dHat(:))'*W*(d-dHat(:));
