alphabetamontecarlorun.m

Yaakov Bar-Shalom, X.-Rong Li,Thiagalingam Kirubarajan - Estimation with Applications to Tracking an

%%% DynaEst 3.032 02/18/2001
% Copyright (c) 2000 Yaakov Bar-Shalom
%
% AlphaBetaMonteCarloRun  alpha beta Kalman filter -- Monte Carlo runs
% it's availabe only when SimulationFlag is 1,2,3
% Case 1 : No ExternalTruth, External Z : SimulationFlag = 1, 2 and no generation
% Case 2 : Exist ExternalTruth, No ExternalZ : SimulationFlag =1,2 and generation of ground truth or SimulationFlag = 3
% Case 3 : Extist ExternalTruth, ExternalZ : SimulationFlag =1,2 and generation truth and measurement or SimulationFlag =3 and generation measurement
% Case 4 : No ExternalTruth, ExternalZ : Simulation Flag = 4 <---Impossible with this file,see KFMCRUNFromMeasurement file

% True Track = Truth
% Mesurement = Mesurement
% Estimation = Estimation

% average xt(truth) = longxt
% average v(process noise) = longv
% average w(measurement noise) = longw
% average z(measurement) = longz
% average xp(predicted estimate) = longxp 
% average zp(predicted measurement) = lognzp 
% average nu(inovation) = longnu
% average x(estimate) = longx
% average xe(error) = longxe
% W = FilterGain 
% P = Covariance

% if do not exist T, generate T
if exist('ExternalT','var') == 0
    if nmt == 1 
        ExternalT = Tmin+(Tmax-Tmin)*rand(1,nrun);
    else
        ExternalT = ones(1,nrun)*Tmulti ;
    end
end
nxactual = nx ;

% End of Step 1
longxt=zeros(nx,kmax);
longx=zeros(nxf,kmax);longxp=longx;longsqP1=longx ;
longxe=zeros(nxactual,kmax);longnee=longxe;longRMS1=longxe;
longz=zeros(nzf,kmax);longzp=longz;longnun=longz; longnu=longz;
longv=zeros(nvf,kmax);
longw=zeros(nwf,kmax);
% longmodePr=zeros(nmf,kmax);
longnees=zeros(1,kmax); longnis=longnees;
LongP=zeros(nxf*nxf,kmax);LongPP=LongP;
LongMSE=zeros(nxactual*nxactual, kmax);LongRMS2=LongMSE;LongsqP2=LongMSE;LongRMS3=LongMSE;LongsqP3=LongMSE;
LongS=zeros(nzf*nzf,kmax);
LongW=zeros(nxf*nzf,kmax);
[c,maxsize]=computer;

if nx*nx*nx*kmax<=maxsize
    LLongRMS3=zeros(nx*nx*nx,kmax);LLongsqP3=LLongRMS3;
else
    errordlg('RMS3 and sqP3 are truncated.','error');
    LLongRMS3=zeros(nx*nx*nx,round(maxsize/(nx^3)));LLongsqP3=LLongRMS3;
end

Ft = zeros(nx,nx); Ff = zeros(nxf,nxf);
Gt = zeros(nx,nv); Gf = zeros(nxf,nvf);
Ht = zeros(nz,nx); Hf = zeros(nzf,nxf);
It = zeros(nz,nw); If = zeros(nzf,nwf);
Qt = zeros(nv,nv); Qf = zeros(nvf,nvf);
Rt = zeros(nw,nw); Rf = zeros(nwf,nwf);

Truth = zeros(nrun,nx,kmax);
Measurement = zeros(nrun,nz,kmax);
Estimation = zeros(nrun,nxf,kmax);
FilterGain = zeros(nxf,nzf,kmax) ;
Covariance = zeros(nxf,nxf,kmax) ;
Pvariance = Covariance;
Svariance = zeros(nzf,nzf,kmax);
colordef none;

% Monte Carlo runs:
Hf_wait = waitbar(0,'Doing Monte Carlo runs. Please wait...');

existtruth = exist('ExternalTruth','var') ;
existZ = exist('ExternalZ','var') ;

for nmc=1:nrun,
    waitbar(nmc/nrun); 
    T = ExternalT(nmc);
    
    % Step 2
    if nmt == 1 
        if ~existtruth
            [Ft,Gt,Qt] = ProcessModel(SystemModelFlag,nx,nv,T,omega,xt0,Ftstr,Gtstr,Qtstr) ;
        end
        [Ht,It,Rt] = ObservationModel(nz,nw,Htstr,Itstr,Rtstr) ;
    else
        mode = 1 ;    
    end
    % End of Step 2
    
    [Ff,Gf,Qf] = ProcessModel(FilterModelFlag,nxf,nvf,T,omegaf,x0,Ffstr,Gfstr,Qfstr) ;
    [Hf,If,Rf] = ObservationModel(nzf,nwf,Hfstr,Ifstr,Rfstr) ;  
    
    % initial condition
    clear xt x P;
    xt = zeros(nx,1) ;
    if ~existtruth 
        xt=xt0; 
    end
    x=x0; P=P0; z=zeros(nz,1);
    
    for k=1:kmax,
        % Step 3
        if nmt > 1 
            if k*T > FromTime(mode)
                PrevSystemModelFlag = SystemModelFlag ;
                [Ftstr, Gtstr,Htstr,Itstr,Qtstr,Rtstr vmt wmt SystemModelFlag omega] = FindMultiModel(k,T,FromTime,ToTime,ModeSystem,nmt) ;
                mode = mode + 1 ;
                if ~existtruth
                    [Ft,Gt,Qt] = ProcessModel(SystemModelFlag,nx,nv,T,omega,xt0,Ftstr,Gtstr,Qtstr) ;
                end
                if ~existZ
                    [Ht,It,Rt] = ObservationModel(nz,nw,Htstr,Itstr,Rtstr) ;
                end
                xt = ModeChanging(PrevSystemModelFlag,SystemModelFlag,xt,omega)
            end
        end
        
        % End of Step 3
        
        % generate true state and measurement at k
        if existtruth
            w = Normal(wmt,Rt);            
            xt(:) = ExternalTruth(nmc,:,k);
            if exist('ExternalZ','var')
                z(:) = ExternalZ(nmc,:,k) ;
            else
                % generate z
                z = Ht*xt + It*w ;
            end
        else 
            [xt,z,v,w] = Xzgen(xt,Ft,Gt,Ht,It,Qt,Rt,vmt,wmt);
        end
        
        % store truth and measurement
        
        Truth(nmc,:,k) = xt;
        Measurement(nmc,:,k) = z;
        
        % lemda = vmf/wmf*T^2;   
        clear W OmigaV OmigaW lemda beta alpha;
        W = zeros(2*ncoor,ncoor);
        OmigaV = sqrt(Qf*ones(ncoor,1));
        OmigaW = sqrt(Rf*ones(ncoor,1));
        lemda = OmigaV./OmigaW*T*T;
        beta = 0.25*(lemda.*lemda+4*lemda-lemda.*sqrt(lemda.*lemda+8*lemda));
        alpha = -0.125*(lemda.*lemda+8*lemda-(lemda+4).*sqrt(lemda.*lemda+8*lemda));
        for i = 1: ncoor
            W((i-1)*2+1,i) = alpha(i);
            W((i-1)*2+2,i) = beta(i)/T;
        end
        
        clear P11 P12 P22;
        P11 = alpha.*OmigaW;
        P12 = beta.*OmigaW/T;
        P22 = beta./(T*T).*(alpha-beta/2).*OmigaW./(ones(ncoor,1)-alpha);
        
        P = zeros(2*ncoor,2*ncoor);
        for i = 1: ncoor
            P((i-1)*2+1,(i-1)*2+1) = P11(i);
            P((i-1)*2+2,(i-1)*2+1) = P12(i);
            P((i-1)*2+1,(i-1)*2+2) = P12(i);
            P((i-1)*2+2,(i-1)*2+2) = P22(i);
        end
        
        [x,xp,zp,nu]=AlphaBetaKalman(x,W,z,vmf,wmf,Ff,Gf,Hf,If);
        
        if nmc == 1 
            FilterGain(:,:,k) = W ;
            Covariance(:,:,k) = P ;
        end
        
        Estimation(nmc,:,k) = x;
        
        longxt(:,k) = longxt(:,k) + xt/nrun;
        longz(:,k)  = longz(:,k)  + z/nrun;
        longv(:,k)  = longv(:,k)  + v/nrun;
        longw(:,k)  = longw(:,k)  + w/nrun;
        longx(:,k)  = longx(:,k)  + x/nrun;
        LongP(:,k)  = LongP(:,k)  + P(:)/nrun;
        %    longmodePr(:,k)  = longmodePr(:,k) + modePr/nrun;
        clear xe;
        xe          = xt-x;
        longxe(:,k) = longxe(:,k) + xe/nrun;
        
        %% will be deleted
        %dummy       = xe*xe';
        %LongMSE(:,k)= LongMSE(:,k) + dummy(:)/nrun;
        % LongPP(:,k) = LongPP(:,k) + PP(:)/nrun; 
        % LongS(:,k)  = LongS(:,k) + S(:)/nrun;   
        %LongW(:,k)  = LongW(:,k) + W(:)/nrun;
        %longRMS1(:,k)= longRMS1(:,k) + xe.*xe/nrun;
        %dummy=[]; for i=1:nx, dummy(i) = P(i,i); end
        %longsqP1(:,k)= longsqP1(:,k) + dummy'/nrun;
        %%
        
        clear RMS2 sqP2;
        %% debug 04-01-01
        if ncoor ~= 1       
            for i=1:nx
                for j=1:nx 
                    RMS2(i,j) = xe(i)^2+xe(j)^2; 
                    sqP2(i,j) = P(i,i)+P(j,j);
                end
            end      
        else
            for i=1:nx
                for j=1:nx 
                    RMS2(i,j) = xe(i)^2 ; 
                    sqP2(i,j) = P(i,j);
                end
            end      
        end

        LongRMS2(:,k)= LongRMS2(:,k) + RMS2(:)/nrun;
        LongsqP2(:,k)= LongsqP2(:,k) + sqP2(:)/nrun;
        
        %clear RMS3 sqP3;
        %    for l=1:nx, 
        %     for i=1:nx, for j=1:nx,
        %      RMS3(i,j) = xe(i)^2+xe(j)^2+xe(l)^2;
        %     sqP3(i,j) = P(i,i)+P(j,j)+P(l,l);
        %  end, end
        %   LongRMS3(:,l) = RMS3(:);
        %  LongsqP3(:,l) = sqP3(:);
        %    end
        %if k<=maxsize/(nx^3)
        % LLongRMS3(:,k)=LLongRMS3(:,k) + LongRMS3(:)/nrun;
        % LLongsqP3(:,k)=LLongsqP3(:,k) + LongsqP3(:)/nrun;
        % end
        
        longxp(:,k) = longxp(:,k) + xp/nrun;
        longzp(:,k) = longzp(:,k) + zp/nrun;
        longnu(:,k) = longnu(:,k) + nu/nrun;
        nees        = xe'*inv(P)*xe;
        longnees(k) = longnees(k) + nees/nrun;
        for i=1:nxactual, longnee(i,k) = longnee(i,k) + xe(i)/sqrt(P(i,i))/nrun; end
        if ncoor ==1 | ncoor == 2 
            S = eye(ncoor,ncoor) ;
            switch nz
            case 1, S(1,1) = OmigaW/(1-alpha) ;
            case 2, S(1,1) = OmigaW(1)/(1-alpha(1)) ;
                S(2,2) = OmigaW(2)/(1-alpha(2)) ;
            end
        end
        nis         = nu'*inv(S)*nu;
        longnis(k)  = longnis(k) + nis/nrun;
        for i=1:nzf, longnun(i,k) = longnun(i,k) + nu(i)/sqrt(S(i,i))/nrun; end    
        
    end
end
longRMS1=sqrt(longRMS1);
longsqP1=sqrt(longsqP1);
LongRMS2 = sqrt(LongRMS2);
LongsqP2 = sqrt(LongsqP2);
LLongRMS3 = sqrt(LLongRMS3);
LLongsqP3 = sqrt(LLongsqP3);

clear existtruth existZ mode ;

close(Hf_wait);

% The End