% Performance of joint and sequential estimations for transceiver based
% localization by calling rountines:
%   XcvrTOALoc_GN_MLE: joint estimation
%   XcvrTOALoc_GN_Seq: sequential estimation
%
% Clarification: both joint and sequential estimations require receiver synchronization.
%
% Reference:
% K. C. Ho, "Localization through transceivers in unknown constant velocity
% trajectories," IEEE Trans. Signal Process., vol. 70, pp. 3011-3028, 2022.
%
% Note: number of ensemble runs in line (48) is set to 20 only to reduce
% computation time.
%
% K. C. Ho     08-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear;
warning off;

% ############### selection #############################
L=2;    % 2-D or 3-D
M=1;    % number of transceivers
N=2;    % number of sensors

% L=3;    % 2-D or 3-D
% M=2;    % number of transceivers
% N=3;    % number of sensors

PfmVsNse=1; % 1:PerformanceVsNoise, 0:PerformanceVsDuration
OfstUnkn=1;   % time offsets fully unknown?
% #######################################################

% ========== default settings for simulation (Should Not Be Changed) ==========
K=50;           % number of time samples (duration)
NsePwrLgAll=[-140+40*(L-2):5*2:20]; % noise power in log-scale
nsePwrFxLg=-60;
if (L==2) OfstBias=(0.01)*(1-OfstUnkn) ; end;
if (L==3) OfstBias=(0.1)*(1-OfstUnkn) ; end;
MLE_GN=1; SeqEst=1;

NumEnsemble=20; % number of ensemble runs
% =============================================================================

if (PfmVsNse)
    KAll=K;
else
    KAll=[10:10:K];
    NsePwrLgAll=nsePwrFxLg;
end;

% ------------------------------------------------------------
for idxK=1:length(KAll),    % loop over time samples
    K=KAll(idxK);
    
    % % -- a configuration, can be changed to others --
    if (L==2)
        uo=[979.1488 965.9494]';
        t0o=[ 106.3387    643.5906   -112.4207
            1018.8873    179.8857    646.6095];
        vo=[ 8.7519   -5.7165    9.8616
            -7.1071    8.9892   -2.9183];
        deltao=[42.9880   43.1079   17.7011]';
        s=[-3.1398   64.6582  -43.5414   79.4562  -46.5494
            4.2393   99.7974  -87.9473  -76.6022   99.1429];
    else
        uo=[1540.8166    -1466.2586   1424.5389]';
        t0o=[ 134.0537      342.8954    1163.3420     925.2801   1303.0074
            -429.0546    -1208.2835    -349.0593   -1108.1499   -287.2958
            836.6749      830.2618     938.2632     407.1742    979.5588];
        vo=[ 9.1619    5.5616    -7.9486   -3.8091    -7.1139
            -5.5203    9.8459    -8.8768    6.2442    -5.7393
            -5.4044    0.9369    -5.4708    7.1180    -9.4663];
        deltao=[16.8458    33.8163    47.7656    2.5009    35.5830]';
        s=[-85.9960    -85.7012    -21.1633    -74.4401   -87.9664
            4.8294     32.2690     20.8777    -60.4010   -83.3461
            55.6334    -13.0265     99.2635     77.5531    17.1666];
    end;
    
    t0o=t0o(:,1:M); vo=vo(:,1:M); deltao=deltao(1:M); s=s(:,1:N);
    
    if (L==2 & N<2)
        fprintf('Number of sensors must be at least 2');
    end;
    if (L==3 & (M<2 | N<3))
        fprintf('Number of transceivers must be at least 2 and sensors must be at least 3');
    end;
    
    fprintf('(M,N): (%d,%d)\n',M,N);
    
    % -- CRLB --
    Q=eye(M*N*K);
    [CRLB,Aux] = XcvrTOALocCRLB(uo,t0o,vo,s,K,Q,OfstUnkn,OfstBias);
    [seqCRLB,seqAux] = XcvrTOALocSeqCRLB(uo,t0o,vo,s,K,Q,OfstUnkn,OfstBias);
    
    k=0:K-1;    % time instants in collecting data
    
    % -- show localization geometries --
    if (L==2)
        figure(1); clf; plot(s(1,:),s(2,:),'o',uo(1),uo(2),'^r'); grid on; hold on;
        for i=1:M,
            tio=repmat(t0o(:,i),1,K)+vo(:,i)*k;
            plot(tio(1,1),tio(2,1),'d',tio(1,:),tio(2,:),'-');
        end;
        xlabel('x (m)'); ylabel('y (m)');
    else
        figure(1); clf;
        plot3(s(1,:),s(2,:),s(3,:),'o',uo(1),uo(2),uo(3),'^r'); grid on; hold on;
        for i=1:M,
            tio=repmat(t0o(:,i),1,K)+vo(:,i)*k;
            plot3(tio(1,1),tio(2,1),tio(3,1),'d',tio(1,:),tio(2,:),tio(3,:),'-');
        end;
        xlabel('x (m)'); ylabel('y (m)'); zlabel('z (m)');
    end;
    
    tiParaLgth=2*L;
    
    % ==================== simulation ====================
    
    % -- true measurement values --
    clear liuo lijo do;
    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);
        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);
            do(:,i,j)=liuo(:,i)+lijo(:,i,j)+deltao(i)*OfstUnkn+OfstBias;
        end;
    end;
    
    % -- noise covariance matrix --
    Qr=sqrtm(Q);
    iQr=inv(Qr);
    iQrHf=inv(sqrtm(Q(1:K,1:K)));
    
    % -- record --
    thetaHatAll=zeros((2*L+1*OfstUnkn)*M+L,NumEnsemble,length(NsePwrLgAll));
    thetaHatAllSeq=zeros((2*L+1*OfstUnkn)*M+L,NumEnsemble,length(NsePwrLgAll));
    thetaHat=zeros((2*L+1*OfstUnkn)*M+L,NumEnsemble);
    thetaHatSeq=zeros((2*L+1*OfstUnkn)*M+L,NumEnsemble);
    
    for nseIdx=1:length(NsePwrLgAll),   % loop over noise power
        fprintf('%d/%d: 10log(NsePwr)=%3.2f\n',nseIdx,length(NsePwrLgAll),NsePwrLgAll(nseIdx));
        nsePwr=10^(NsePwrLgAll(nseIdx)/10);
        Qn=sqrt(nsePwr)*Qr;
        
        rng('default');
        for ii=1:NumEnsemble,   % ensemble runs
            
            % -- measurements --
            doTmp=permute(do,[1,3,2]); % make measurement ordering same as in the paper
            d=doTmp(:)+Qn*randn(length(Qn),1);
            
            % -- initialization --
            if (OfstUnkn)
                theta0Opt=[uo', reshape([t0o(:,1:M);vo(:,1:M)],(2*L)*M,1)',deltao(1:M)']';
            else
                theta0Opt=[uo', reshape([t0o(:,1:M);vo(:,1:M)],(2*L)*M,1)']';
            end;
            %theta0Opt=theta0;
            tmp=randn(L+(2*L+1)*M,1);
            theta0=theta0Opt+diag(sqrt(diag(CRLB)))*tmp(1:length(theta0Opt))*sqrt(nsePwr)*4;    % 12dB higher than CRLB
            if (OfstUnkn)
                seqTheta0=theta0Opt+diag(sqrt([diag(seqAux.COVu(1:L,1:L));diag(seqCRLB);diag(seqAux.COVu(end-M+1:end,end-M+1:end))]))*tmp(1:length(theta0Opt))*sqrt(nsePwr)*4;
            else
                seqTheta0=theta0Opt+diag(sqrt([diag(seqAux.COVu(1:L,1:L));diag(seqCRLB)]))*tmp(1:length(theta0Opt))*sqrt(nsePwr)*4;
            end;
            
            % -- positioning algorithm --
            if (MLE_GN)     % joint estimation
                [thetaHat(:,ii),cost] = XcvrTOALoc_GN_MLE(d,s,K,Q,OfstUnkn,theta0);
            end;
            if (SeqEst)     % sequential estimation
                [thetaHatSeq(:,ii),cost] = XcvrTOALoc_GN_Seq(d,s,K,Q,OfstUnkn,seqTheta0);
            end;
            
        end;
        
        thetaHatAll(:,:,nseIdx)=thetaHat;
        thetaHatAllSeq(:,:,nseIdx)=thetaHatSeq;
        
        % -- jont estimation performance measure --
        uMse(nseIdx)=mean(sum((squeeze(thetaHatAll(1:L,:,nseIdx))-repmat(uo,1,NumEnsemble)).^2));
        tMse(nseIdx)=0; vMse(nseIdx)=0;
        for i=1:M,
            tMse(nseIdx)=tMse(nseIdx)+mean(sum((squeeze(thetaHatAll(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L,:,nseIdx))-repmat(t0o(:,i),1,NumEnsemble)).^2));
            vMse(nseIdx)=vMse(nseIdx)+mean(sum((squeeze(thetaHatAll(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L,:,nseIdx))-repmat(vo(:,i),1,NumEnsemble)).^2));
        end;
        if (OfstUnkn)
            if (M==1)
                oMse(nseIdx)=mean((squeeze(thetaHatAll(end,:,nseIdx))-repmat(deltao,1,NumEnsemble)).^2);
            else
                oMse(nseIdx)=mean(sum((squeeze(thetaHatAll(end-M+1:end,:,nseIdx))-repmat(deltao,1,NumEnsemble)).^2));
            end;
        else
            oMse(nseIdx)=NaN;
        end;
        
        % -- CRLB of various parameter estimates for joint estimation --
        dCRLB=diag(CRLB);
        uCrlb(nseIdx)=nsePwr*sum(dCRLB(1:L));
        tCrlb(nseIdx)=0; vCrlb(nseIdx)=0;
        for i=1:M,
            tCrlb(nseIdx)=tCrlb(nseIdx)+nsePwr*sum(dCRLB(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L));
            vCrlb(nseIdx)=vCrlb(nseIdx)+nsePwr*sum(dCRLB(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L));
        end;
        if (OfstUnkn)
            oCrlb(nseIdx)=nsePwr*sum(dCRLB(end-M+1:end));
        else
            oCrlb(nseIdx)=NaN;
            dBias2=diag(Aux.BIAS*Aux.BIAS');
            uBias(nseIdx)=sqrt(sum(dBias2(1:L)));
            tBias(nseIdx)=0; vBias(nseIdx)=0;
            for i=1:M,
                tBias(nseIdx)=tBias(nseIdx)+sum(dBias2(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L));
                vBias(nseIdx)=vBias(nseIdx)+sum(dBias2(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L));
            end;
            tBias(nseIdx)=sqrt(tBias(nseIdx)); vBias(nseIdx)=sqrt(vBias(nseIdx));
        end;
        
        % -- sequential estimate performance measure --
        if (SeqEst)
            useqMse(nseIdx)=mean(sum((squeeze(thetaHatAllSeq(1:L,:,nseIdx))-repmat(uo,1,NumEnsemble)).^2));
            tseqMse(nseIdx)=0; vseqMse(nseIdx)=0;
            for i=1:M,
                tseqMse(nseIdx)=tseqMse(nseIdx)+mean(sum((squeeze(thetaHatAllSeq(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L,:,nseIdx))-repmat(t0o(:,i),1,NumEnsemble)).^2));
                vseqMse(nseIdx)=vseqMse(nseIdx)+mean(sum((squeeze(thetaHatAllSeq(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L,:,nseIdx))-repmat(vo(:,i),1,NumEnsemble)).^2));
            end;
            if (OfstUnkn)
                if (M==1)
                    oseqMse(nseIdx)=mean((squeeze(thetaHatAllSeq(end,:,nseIdx))-repmat(deltao,1,NumEnsemble)).^2);
                else
                    oseqMse(nseIdx)=mean(sum((squeeze(thetaHatAllSeq(end-M+1:end,:,nseIdx))-repmat(deltao,1,NumEnsemble)).^2));
                end;
            else
                oseqMse(nseIdx)=NaN;
            end;
            
            % -- CRLB of various parameter estimates for sequential estimation --
            dseqCRLB=zeros(size(dCRLB));
            dseqCRLB(L+1:end-M*OfstUnkn)=diag(seqCRLB);
            dCOVu=diag(seqAux.COVu); dseqCRLB(1:L)=dCOVu(1:L);
            if (OfstUnkn)
                dseqCRLB(end-M+1:end)=dCOVu(end-M+1:end);
            end;
            useqCrlb(nseIdx)=nsePwr*sum(dseqCRLB(1:L));
            sseqCrlb(nseIdx)=0; vseqCrlb(nseIdx)=0;
            for i=1:M,
                sseqCrlb(nseIdx)=sseqCrlb(nseIdx)+nsePwr*sum(dseqCRLB(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L));
                vseqCrlb(nseIdx)=vseqCrlb(nseIdx)+nsePwr*sum(dseqCRLB(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L));
            end;
            if (OfstUnkn)
                oseqCrlb(nseIdx)=nsePwr*sum(dseqCRLB(end-M+1:end));
            else
                oseqCrlb(nseIdx)=NaN;
                useqBias(nseIdx)=sqrt(trace(seqAux.BIASu * seqAux.BIASu'));
            end;
        end;    % (SeqEst)
        
    end;
    
    % -- performance measure with respect to duration --
    if (length(KAll)>1)
        uMseK(idxK)=uMse; tMseK(idxK)=tMse; vMseK(idxK)=vMse; oMseK(idxK)=oMse;
        uCrlbK(idxK)=uCrlb; tCrlbK(idxK)=tCrlb; vCrlbK(idxK)=vCrlb; oCrlbK(idxK)=oCrlb;
        useqMseK(idxK)=useqMse; tseqMseK(idxK)=tseqMse; vseqMseK(idxK)=vseqMse; oseqMseK(idxK)=oseqMse;
        useqCrlbK(idxK)=useqCrlb; sseqCrlbK(idxK)=sseqCrlb; vseqCrlbK(idxK)=vseqCrlb; oseqCrlbK(idxK)=oseqCrlb;
        if (~OfstUnkn)
            uBiasK(idxK)=uBias; tBiasK(idxK)=tBias; vBiasK(idxK)=vBias;
            useqBiasK(idxK)=useqBias;
        end;
    end;
    
end;    % idxK

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% -- performance vs NoisePower --
if (length(NsePwrLgAll)>1)
    figNum=4+N+(L-2)*2;
    if (OfstUnkn)
        if (MLE_GN)
            figure(figNum); clf; subplot(411); loglog(10.^(NsePwrLgAll/20),sqrt(uMse),'kx-',10.^(NsePwrLgAll/20),sqrt(uCrlb),'r'); grid on; ylabel('RMSE(u) (m)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e2+1e4]); end;
            subplot(412); loglog(10.^(NsePwrLgAll/20),sqrt(tMse),'kx-',10.^(NsePwrLgAll/20),sqrt(tCrlb),'r'); grid on; ylabel('RMSE(t(0)) (m)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e4]); end;
            subplot(413); loglog(10.^(NsePwrLgAll/20),sqrt(vMse),'kx-',10.^(NsePwrLgAll/20),sqrt(vCrlb),'r'); grid on; ylabel('RMSE(v) (m/s)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-6 1e2]); else ylim([1e-4 1e3]); end;
            subplot(414); loglog(10.^(NsePwrLgAll/20),sqrt(oMse),'kx-',10.^(NsePwrLgAll/20),sqrt(oCrlb),'r'); grid on; ylabel('RMSE(\delta) (m)'); xlabel('Noise Level \sigma (m)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e4]); end;
        end;
        if (SeqEst)
            figure(figNum); hold on; subplot(411); hold on; loglog(10.^(NsePwrLgAll/20),sqrt(useqMse),'ko-',10.^(NsePwrLgAll/20),sqrt(useqCrlb),'m'); grid on; ylabel('RMSE(u) (m)'); %xlabel('10 log(NsePwr)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10++10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e4]); end;
            subplot(412); hold on; loglog(10.^(NsePwrLgAll/20),sqrt(tseqMse),'ko-',10.^(NsePwrLgAll/20),sqrt(sseqCrlb),'m'); grid on; ylabel('RMSE(t(0)) (m)'); %xlabel('10 log(NsePwr)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e4]); end;
            subplot(413); hold on; loglog(10.^(NsePwrLgAll/20),sqrt(vseqMse),'ko-',10.^(NsePwrLgAll/20),sqrt(vseqCrlb),'m'); grid on; ylabel('RMSE(v) (m/s)'); %xlabel('10 log(NsePwr)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-6 1e2]); else ylim([1e-4 1e3]); end;
            subplot(414); hold on; p2=loglog(10.^(NsePwrLgAll/20),sqrt(oseqMse),'ko-',10.^(NsePwrLgAll/20),sqrt(oseqCrlb),'m'); grid on; ylabel('RMSE(\delta) (m)'); xlabel('Noise Level \sigma (m)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-5 1e3]); else ylim([1e-2 1e4]); end;
        end;
        legend('Joint Est','CRLB','Seq Est','Theory','location','southeast','NumColumns',2);
    else
        figNum=8+(L-2)*3;
        if (MLE_GN)
            figure(figNum); clf; subplot(311); loglog(10.^(NsePwrLgAll/20),sqrt(uMse),'ks-',10.^(NsePwrLgAll/20),sqrt(uCrlb+uBias.^2),'r'); grid on; ylabel('RMSE(u) (m)'); %xlabel('10 log(NsePwr)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-3 1e2]); else ylim([1e-2 1e2]); end;
            subplot(312); loglog(10.^(NsePwrLgAll/20),sqrt(tMse),'ks-',10.^(NsePwrLgAll/20),sqrt(tCrlb+tBias.^2),'r'); grid on; ylabel('RMSE(t(0)) (m)'); %xlabel('10 log(NsePwr)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-3 1e2]); else ylim([1e-2 1e2]); end;
            subplot(313); loglog(10.^(NsePwrLgAll/20),sqrt(vMse),'ks-',10.^(NsePwrLgAll/20),sqrt(vCrlb+vBias.^2),'r'); grid on; ylabel('RMSE(v) m/s)');  xlabel('Noise Level \sigma (m)');
            xlim([min(10.^(NsePwrLgAll/20)),max(10.^((NsePwrLgAll-10+10)/20))]);
            if (L==2) ylim([1e-6 1e0]); else ylim([1e-4 1e1]); end;
            subplot(311); title(['OfstBias=',num2str(OfstBias), 'm,  (M,N)=(',num2str(M),',',num2str(N),')' ]);
            legend('Joint Est','CRLB','location','southeast');
        end;
    end;
end;    % (length(NsePwrLgAll)>1)

% ----- performance vs K -----
if (length(KAll)>1)
    
    % -- plot results --
    if (MLE_GN)
        figure(12); clf; subplot(411); semilogy(KAll,sqrt(uMseK),'kx-',KAll,sqrt(uCrlbK),'r'); grid on; ylabel('RMSE(u) (m)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
        subplot(412); semilogy(KAll,sqrt(tMseK),'kx-',KAll,sqrt(tCrlbK),'r'); grid on; ylabel('RMSE(t(0)) (m)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]);  if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
        subplot(413); semilogy(KAll,sqrt(vMseK),'kx-',KAll,sqrt(vCrlbK),'r'); grid on; ylabel('RMSE(v) (m/s)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]);if (L==2) ylim([1e-3 1e0]); else ylim([1e-3 1e2]); end;
        subplot(414); semilogy(KAll,sqrt(oMseK),'kx-',KAll,sqrt(oCrlbK),'r'); grid on; ylabel('RMSE(\delta) (m)'); xlabel('Duration K');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
    end;
    if (SeqEst)
        figure(12); hold on; subplot(411); hold on; semilogy(KAll,sqrt(useqMseK),'ko-',KAll,sqrt(useqCrlbK),'r'); grid on; ylabel('RMSE(u) (m)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
        subplot(412); hold on; semilogy(KAll,sqrt(tseqMseK),'ko-',KAll,sqrt(sseqCrlbK),'r'); grid on; ylabel('RMSE(t(0)) (m)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
        subplot(413); hold on; semilogy(KAll,sqrt(vseqMseK),'ko-',KAll,sqrt(vseqCrlbK),'r'); grid on; ylabel('RMSE(v) (m/s)'); %xlabel('10 log(NsePwr)');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-3 1e0]); else ylim([1e-3 1e2]); end;
        subplot(414); hold on; semilogy(KAll,sqrt(oseqMseK),'ko-',KAll,sqrt(oseqCrlbK),'r'); grid on; ylabel('RMSE(\delta) (m)'); xlabel('Duration K');
        xlim([min(KAll),max(KAll)]); if (L==2) ylim([1e-2 1e2]); else ylim([1e-2 1e4]); end;
    end;
    legend('Joint Est','CRLB','location','southwest');
    
end;    % (length(KAll)>1)




