function [SeqCRLB,Aux] = XcvrTOALocSeqCRLB(uo,t0o,vo,s,K,Q,OfstUnkn,OfstBias)
% function [SeqCRLB,Aux] = XcvrTOALocSeqCRLB(uo,t0o,vo,s,K,Q,OfstUnkn,OfstBias)
%
% CRLB (theoretical performance to be exact) for distant object localization
% using moving transceivers, sequential estimation
%
% Input parameter list:
% uo: (dim x 1), object location
% t0o:((M dim) x M), initial positions of M transceivers
% vo: ((M dim) x M), velocities of M transceivers
% s:  (Dim x N), N sensor positions
% K:  K = num. of time instants
% Q:  (KMN x KMN), covariance matrix of distance measurements
% OfstUnkn:  flag, 1 if measurement offsets known and 0 if offsets unknown
% OfstBias:  residual bias offsets when OfstUnkn=0
%
% Output parameter list:
% SeqCRLB:  CRLB matrix,
%           ((L(2M+1)+M) x (L(2M+1)+M)) when OfstUnkn=1,
%           ((L(2M+1)) x (L(2M+1))) when OfstUnkn=0
% Aux:  auxiliary values related to bias when OfstUnkn=1
%
% 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
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[L,M]=size(t0o); N=size(s,2);

k=0:K-1;

for i=1:M,
    tio=repmat(t0o(:,i),1,K)+vo(:,i)*k;
    liuo(:,i)=sqrt(sum((tio-repmat(uo,1,K)).^2));
    rhoiuo=-(tio-repmat(uo,1,K))./repmat(liuo(:,i)',L,1);
    
    Gi=[];
    Hi=[];
    for j=1:N,
        
        lijo(:,i,j)=sqrt(sum((tio-repmat(s(:,j),1,K)).^2));
        rhoijo=(tio-repmat(s(:,j),1,K))./repmat(lijo(:,i,j)',L,1);
        if (OfstUnkn)
            Gij=[rhoiuo; (-rhoiuo+rhoijo); repmat(k,L,1).*(-rhoiuo+rhoijo); ones(1,length(rhoiuo))]';
        else
            Gij=[rhoiuo; (-rhoiuo+rhoijo); repmat(k,L,1).*(-rhoiuo+rhoijo)]';
        end;
        Gi=[Gi; Gij];
    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;

% -- rearrange G and H with all time offsets at the end --
if (OfstUnkn)
    tpG=G;
    for i=1:M,
        G(:,L+(2*L)*(i-1)+1:L+(2*L)*(i))=tpG(:,L+(2*L+1)*(i-1)+1:L+(2*L+1)*(i-1)+2*L);
        G(:,end-M+i)=tpG(:,L+(2*L+1)*(i-1)+2*L+1);
    end;
end;

% -- sequential estimation, CRLB for t --
Ec=eye(K);
E=[Ec,-Ec];
% -----------
if (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;

if (OfstUnkn)
    G1=E*G(:,L+1:end-M);
else
    G1=E*G(:,L+1:end);
end;
Q1=E*Q*E';

SeqFIM=(G1'*inv(Q1)*G1);
SeqCRLB=inv(SeqFIM);

% -- expected performance for object location estimate --
R=G(:,L+1:end-M*OfstUnkn)*inv(G1'*inv(Q1)*G1)*G1'*inv(Q1)*E;
Qu=(eye(size(G,1))-R)*Q*(eye(size(G,1))-R)';
W=inv(Q);

if (OfstUnkn)
    COVu=inv([G(:,1:L),G(:,end-M+1:end)]'*W*[G(:,1:L),G(:,end-M+1:end)])*[G(:,1:L),G(:,end-M+1:end)]'*W*Qu ...
        *W*[G(:,1:L),G(:,end-M+1:end)]*inv([G(:,1:L),G(:,end-M+1:end)]'*W*[G(:,1:L),G(:,end-M+1:end)]);
else
    COVu=inv([G(:,1:L)]'*W*[G(:,1:L)])*[G(:,1:L)]'*W*Qu ...
        *W*[G(:,1:L)]*inv([G(:,1:L)]'*W*[G(:,1:L)]);
end;

Aux.COVu=COVu;

if (~OfstUnkn)
    BIASu=inv([G(:,1:L)]'*W*[G(:,1:L)])*[G(:,1:L)]'*W*ones(M*N*K,1)*OfstBias;
    Aux.BIASu=BIASu;
end;

Aux.Qu=Qu;

