#### Selected R commands and output used in Chapter 8 #### See the text for further information. #### ## Implementation (I) ## observations in yy. state in xx. estimation in mu mm <- 10000 tt <- 100 xx <- 1:mm logw <- rep(0,mm) mu <- 1:tt for(t in 1:tt){ Line A1 xx <- rnorm(mm)*ss1 Line A2 logw <- logw+(xx*yy[t]-xx**2/2)/ss2 Line A3 logw <- logw-max(logw) Line A4 w <- exp(logw) mu[t] <- xx*w/sum(w) } #### ## Implementation (II) ## observations in yy. states in xx. estimation in mu mm <- 10000 tt <- 100 xx <- 1:mm w <- rep(1,mm) mu <- 1:tt for(t in 1:tt){ Line B1 for(j in 1:mm){ Line B2 xx[j] <- rnorm(1)*ss1 Line B3 w[j] <- w[j]/sqrt{2*pi}*exp(-(xx-yy[t])**2/2/ss2) Line B4 } mu[t] <- xx*w/sum(w) } #### ############## ### function of generic SMC (not full information) ### with delay weighting method and estimating function funH() ### with Likelihood ############## SMC <- function(Sstep,nobs,yy,mm,par, xx.init, xdim, ydim, resample.sch,delay=0,funH=identity){ #--------------------------------------------------------------- xxd <- array(dim=c(xdim,mm,delay+1)) # setting delay buffer delay.front = 1 loglike=0 xx <- xx.init if(xdim==1) xx=matrix(xx,nrow=1,ncol=mm) xxd[,,1] <- funH(xx) xhat <- array(dim=c(xdim,nobs,delay+1)) logww <- rep(0,mm) for(i in 1:nobs){ if(ydim==1){ yyy <- yy[i] }else{ yyy <- yy[,i] } #------------ call single step propagation step <- Sstep(mm,xx,logww,yyy,par,xdim,ydim) xx <- step$xx logww <- step$logww-max(step$logww) loglike=loglike+max(step$logww) ww <- exp(logww) sumww=sum(ww) ww <- ww/sumww #------------ managing delay buffer xxd[,,delay.front] <- funH(xx) delay.front <- delay.front%%(delay+1) + 1 order <- circular2ordinal(delay.front, delay+1) xhat[,i,] <- tensor(xxd,ww,2,1)[,order] #------------ resampling if(resample.sch[i]==1){ r.index <- sample.int(mm,size=mm,replace=T,prob=ww) xx[,] <- xx[,r.index] xxd[,,] <- xxd[,r.index,] logww <- logww*0 loglike=loglike+log(sumww) } } if(resample.sch[nobs]==0){ ww <- exp(logww) sumww=sum(ww) loglike=loglike+log(sumww) } #------------ rearranging delay output if(delay > 0){ for(dd in 1:delay){ xhat[,1:(nobs-dd),dd+1] <- xhat[,(dd+1):nobs,dd+1] xhat[,(nobs-dd+1):nobs,dd+1] <- NA } } return(list(xhat=xhat,loglike=loglike)) } #-------------------------end function ------------------------- #### ################## ### function of generic SMC (full information) ### no R-B ### with delay weighting method and estimating function funH() ################## SMC.Full <- function(Sstep.Full,nobs,yy,mm,par,xx.init,xdim, ydim, resample.sch,delay=0,funH=identity){...} #--------------------------------------------------------------- #### ################ ### function of generic SMC (full information) ### with R-B estimation. No delay ################ SMC.Full.RB=function(Sstep.Full.RB,nobs,yy,mm,par,xx.init,xdim, ydim,resample.sch){...} #--------------------------------------------------------------- #### ################## ### function of generic SMC Smoother ### with estimating function funH() ################## SMC.Smooth=function(Sstep,Sstep.Smooth,nobs,yy,mm,par, xx.init,xdim,ydim,resample.sch,funH=identity){...} #--------------------------------------------------------------- #### ################ ### function of generic MKF (full information) ### with R-B estimation. No delay ################ MKF.Full.RB <- function(MKFstep.Full.RB,nobs,yy,mm,par,II.init, mu.init,SS.init,xdim,ydim,resample.sch){...} #--------------------------------------------------------------- #### ################## ### Residual resample step ################## resampling.res <- function(mm,ww){ mj <- floor(mm*ww) index <- rep.int(1,mj[1]) for(j in 2:mm) index <- c(index,rep.int(j,mj[j])) ww2 <- mm*ww-mj if(sum(ww2)>0){ ww2 <- ww2/sum(ww2) mres <- mm-sum(mj) indexrex <- sample.int(mm,size=mres,replace=T,prob=ww2) index <- c(index,indexrex) } return(list(index=index)) } #-------------------------end function ------------------------- ################# ### Stratified resample step ################# resampling.str=function(mm,ww){ u <- runif(1) cusumw <- cumsum(ww) mj <- floor(mm*cusumw-u)+1 mj[mj<0] <- 0 index <- rep(1,mj[1]) for(j in 2:mm) index <- c(index,rep.int(j,mj[j]-mj[j-1])) return(list(index=index)) } #-------------------------end function ------------------------- #### > mm=8*10000 > ww=c(0,0,4,1,1,1,1,10) > ww=rep(ww,10000) > ww=ww/sum(ww) > system.time(for(i in 1:10) resampling.res(mm,ww)) user system elapsed 43.59 0.17 44.09 > system.time(for(i in 1:10) resampling.str(mm,ww)) user system elapsed 70.93 0.55 71.73 > system.time(for(i in 1:10) sample.int(mm,size=mm,replace=T,prob=ww)) user system elapsed 0.01 0.00 0.02 #### ################# ### Function of propagation step: Clutter. partial state proposal. Sstep.Clutter=function(mm,xx,logww,yyy,par,xdim,ydim){ #--------------------------------------------------------------- #------- state equation propagation ee <- rnorm(mm)*par$ssw xx[1,] <- xx[1,]+xx[2,]+0.5*ee xx[2,] <- xx[2,]+ee uu <- 1:mm #------- for each MC sample. Vectorized on all yyy observations for(j in 1:mm){ temp <- sum(exp(-(yyy-xx[1,j])**2/2/par$ssv**2)) uu[j] <- temp/par$ssv/sqrt(2*pi)*par$pd/par$nyy+(1-par$pd)/par$yr } #------- update weight logww <- logww+log(uu)-max(log(uu)) return(list(xx=xx,logww=logww)) } #-------------------------end function ------------------------- ################ ### Function of Clutter Propagation step Full Information. Sstep.Clutter.Full <- function(mm,xx,logww,yyy,par,xdim,ydim, resample.sch){ #--------------------------------------------------------------- #------- passing the parameters ssw2 <- par$ssw**2; ssv2 <- par$ssv**2 nyy <- par$nyy; pd <- par$pd; yr <- par$yr #------- setting needed temp variables mu.i <- matrix(nrow=nyy,ncol=mm) cc.i <- matrix(nrow=nyy,ncol=mm) CC <- 1:mm tau2 <- 1/(4/ssw2+1/ssv2); tau <- sqrt(tau2) aa <- 0.5*(-log(pi)+log(tau2)-log(ssw2)-log(ssv2)) for(jj in 1:mm){ mu.i[,jj] <- (4*(xx[1,jj]+xx[2,jj])/ssw2+yyy/ssv2)*tau2 temp <- -2*(xx[1,jj]+xx[2,jj])**2/ssw2-yyy**2/2/ssv2 cc.i[,jj] <- temp +mu.i[,jj]**2/2/tau2+aa cc.i[,jj] <- exp(cc.i[,jj])*pd/nyy CC[jj] <- sum(cc.i[,jj])+(1-pd)/yr #------- updating weight logww[jj] <- logww[jj]+log(CC[jj]) } #-------- resampling if(resample.sch==1){ logpp <- logww-max(logww) pp <- exp(logpp) pp <- pp/sum(pp) r.index <- sample.int(mm,size=mm,replace=T,prob=pp) xx[,] <- xx[,r.index] mu.i[,] <- mu.i[,r.index] cc.i[,] <- cc.i[,r.index] CC <- CC[r.index] } #-------- generating new samples for(jj in 1:mm){ ind <- sample.int(nyy+1,1,prob=c((1-pd)/yr,cc.i[,jj])/CC[jj]) if(ind == 1){ ee <- rnorm(1) xx[1,jj] <- xx[1,jj]+xx[2,jj]+0.5*ee*ssw xx[2,jj] <- xx[2,jj]+ee*ssw } if(ind >1){ xx[2,jj] <- -2*xx[1,jj]-xx[2,jj] xx[1,jj] <- rnorm(1)*tau+mu.i[ind-1,jj] xx[2,jj] <- xx[2,jj]+2*xx[1,jj] } } return(list(xx=xx,logww=logww,r.index=r.index)) } #----------------------end function ----------------------------- ################ ### Function of Clutter propagation step Full Information ### with R-B estimation of mean. Sstep.Clutter.Full.RB=function(mm,xx,logww,yyy,par,xdim,ydim, resample.sch){...} ################ ### Function KF for tracking in clutter with known indicator I_t. clutterKF <- function(nobs,ssw,ssv,yy,ii){...} ################ ### Function of simulating target moving in clutter. simuTargetClutter <- function(nobs,pd,ssw,ssv,xx0,ss0,nyy, yrange){...} #--------------------------------------------------------------- #### library("NTS") ################# ### simulation of one data set #--------------------------------------------------------------- set.seed(1) #------ parameter nobs <- 100; pd <- 0.95; ssw <- 0.1; ssv <- 0.5; xx0 <- 0; ss0 <- 0.1; nyy <- 50; yrange <- c(-80,80); xdim <- 2; ydim <- nyy; #------ simulation simu <- simuTargetClutter(nobs,pd,ssw,ssv,xx0,ss0,nyy,yrange) plot(1,1,type='n',xlim=c(0,nobs+1),ylim=yrange,xlab='time',ylab='signal') for(i in 1:nyy) points(1:nobs,simu$yy[i,],cex=0.4) #fig_clutterSignal.pdf ########## ### KF with known indicators. #--------------------------------------------------------------- outKF <- clutterKF(nobs,ssw,ssv,simu$yy,simu$ii) xhat.kf <- outKF$xhat ######### ### Check Resampling Schedule. 50 SMC runs. Same data #--------------------------------------------------------------- set.seed(1) mm <- 10000 sch <- c(1,2,5,10,20,100) nk <- 50 par <- list(ssw=ssw,ssv=ssv,nyy=nyy,pd=pd,yr=yr) xhat.r <- array(dim=c(nobs,6,nk)) for(m in 1:6){ resample.sch <- rep.int(c(rep(0,sch[m]-1),1),nobs/sch[m]) for(kk in 1:nk){ xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out <- SMC(Sstep.Clutter,nobs,simu$yy,mm,par, xx.init,xdim,ydim,resample.sch) xhat.r[,m,kk] <- out$xhat[1,,1] } } tstep <- c(10,30,50,70,90); res.r <- array(dim=c(nobs,6,nk)) for(m in 1:6){ for(ii in 1:nk) res.r[,m,ii] <- xhat.r[,m,ii]-simu$xx } myboxplot(res.r,tstep,xlab='time') #fig_clutterR1.pdf myboxplot(res.r[,1:5,],tstep,xlab='time') #fig_clutterR2.pdf ########## ### Check MC sample size: resample every time step: ### 50 SMC runs. Same data #--------------------------------------------------------------- set.seed(1) par <- list(ssw=ssw,ssv=ssv,nyy=nyy,pd=pd,yr=yr) resample.sch <- rep.int(1,nobs) nk <- 50 xhat.m <- array(dim=c(nobs,4,nk)) mall <- c(1000,4000,16000,64000) for(m in 1:4){ for(kk in 1:nk){ mm <- mall[m] xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out <- SMC(Sstep.Clutter,nobs,simu$yy,mm,par, xx.init,xdim,ydim,resample.sch) xhat.m[,m,kk] <- out$xhat[1,,1] } } tstep <- c(10,30,50,70,90); res.m <- array(dim=c(nobs,4,nk)) for(m in 1:4){ for(ii in 1:nk) res.m[,m,ii] <- xhat.m[,m,ii]-simu$xx } myboxplot(res.m,tstep,xlab='time') #fig_clutterM.pdf ################## ### Check Delay. resample every step. 50 SMC runs. Same data #--------------------------------------------------------------- set.seed(1) mm <- 10000; nk <- 50; resample.sch <- rep.int(1,nobs) delay <- 8 xhat.d <- array(dim=c(nobs,delay+1,nk)) for(kk in 1:nk){ xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out <- SMC(Sstep.Clutter,nobs,simu$yy,mm,par, xx.init,xdim,ydim,resample.sch,delay=delay) xhat.d[,,kk] <- out$xhat[1,,] } res1.d <- array(dim=c(nobs-8,8,nk)) res2.d <- array(dim=c(nobs-8,8,nk)) tt <- 1:(nobs-delay) for(m in 1:delay){ for(ii in 1:nk){ res1.d[tt,m,ii] <- xhat.d[tt,m,ii]-xhat.kf[tt] res2.d[tt,m,ii] <- xhat.d[tt,m,ii]-simu$xx[tt] } } myboxplot(res1.d,tstep,xlab='time') #fig_clutterD1.pdf myboxplot(res2.d,tstep,xlab='time') #fig_clutterD2.pdf ################ ###---- check with different samples nyy=50, ssv=0.1 ################ nk <- 50 nobs <- 100; pd <- 0.95; ssw <- 0.1; ssv <- 0.1; xx0 <- 0; ss0 <- 0.1; nyy <- 50; yrange <- c(-80,80); xdim <- 2; ydim <- nyy; yr <- yrange[2]-yrange[1] par <- list(ssw=ssw,ssv=ssv,nyy=nyy,pd=pd,yr=yr) #--- simulation for repeated use Yy <- array(dim=c(nyy,nobs,nk)) Xx <- matrix(ncol=nk,nrow=nobs) Ii <- matrix(ncol=nk,nrow=nobs) kk <- 0 seed.k <- 1 while(kk < nk){ seed.k <- seed.k+1 set.seed(seed.k) kk <- kk+1 simu <- simuTargetClutter(nobs,pd,ssw,ssv,xx0,ss0,nyy,yrange) if(max(abs(simu$yy))>80){ kk <- kk-1 } else{ Xx[,kk] <- simu$xx; Yy[,,kk] <- simu$yy; Ii[,kk] <- simu$ii } } #--- KF with known indicator Xxhat.kf <- matrix(ncol=nk,nrow=nobs) for(kk in 1:nk){ outKF <- clutterKF(nobs,ssw,ssv,Yy[,,kk],Ii[,kk]) Xxhat.kf[,kk] <- outKF$xhat } ###---- ----- different delay resample.sch <- rep.int(1,nobs) delay <- 8 mm <- 10000; Xxhat.d <- array(dim=c(nobs,delay+1,nk)) set.seed(1) for(kk in 1:nk){ xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out <- SMC(Sstep.Clutter,nobs,Yy[,,kk],mm,par, xx.init,xdim,ydim,resample.sch,delay=delay) Xxhat.d[,,kk] <- out$xhat[1,,] } tstep <- c(10,30,50,70,90); Xres1.d <- array(dim=c(nobs-8,9,nk)) Xres2.d <- array(dim=c(nobs-8,9,nk)) tt <- 1:(nobs-8) for(m in 1:9){ for(ii in 1:nk){ Xres1.d[tt,m,ii] <- Xxhat.d[tt,m,ii]-Xxhat.kf[tt,ii] Xres2.d[tt,m,ii] <- Xxhat.d[tt,m,ii]-Xx[tt,ii] } } myboxplot(Xres1.d,tstep,xlab='time',ylim=c(-0.3,0.3)) #fig_clutterXD2.pdf myboxplot(Xres2.d,tstep,xlab='time',ylim=c(-0.3,0.3)) #fig_clutterXD4.pdf #---- compare Partial and Full --- resample.sch <- rep.int(1,nobs) delay <- 0; mm <- 10000; Xxhat.full <- array(dim=c(nobs,1,nk)) Xxhat.p <- array(dim=c(nobs,1,nk)) Xres.com2 <- array(dim=c(nobs,2,nk)) set.seed(1) for(kk in 1:nk){ xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out1 <- SMC(Sstep.Clutter,nobs,Yy[,,kk],mm,par, xx.init,xdim,ydim,resample.sch) Xxhat.p[,1,kk] <- out1$xhat[1,,1] out2 <- SMC.Full(Sstep.Clutter.Full,nobs,Yy[,,kk],mm,par, xx.init,xdim,ydim,resample.sch) Xxhat.full[,1,kk] <- out2$xhat[1,,1] } tstep <- c(10,30,50,70,90); for(ii in 1:nk){ Xres.com2[,1,ii] <- Xxhat.p[,1,ii]-Xx[,ii] Xres.com2[,2,ii] <- Xxhat.full[,1,ii]-Xx[,ii] } myboxplot(Xres.com2,tstep,xlab='time') #fig_clutterXCom3.pdf myboxplot(Xres.com2,tstep,xlab='time',ylim=c(-0.5,0.5)) #fig_clutterXCom4.pdf ################# #Check with different samples nyy=10, ssv=0.02,ssw=0.15 ################# ##--- generate samples for repeated use nobs <- 100; pd <- 0.95; ssw <- 0.15; ssv <- 0.02; xx0 <- 0; ss0 <- 0.1; nyy <- 10; yrange <- c(-80,80); xdim <- 2; ydim <- nyy; nk <- 50 yr <- yrange[2]-yrange[1] par <- list(ssw=ssw,ssv=ssv,nyy=nyy,pd=pd,yr=yr) Yy10 <- array(dim=c(nyy,nobs,nk)) Xx10 <- matrix(ncol=nk,nrow=nobs) Ii10 <- matrix(ncol=nk,nrow=nobs) kk <- 0 seed.k <- 1 while(kk < nk){ seed.k <- seed.k+1 set.seed(seed.k) kk <- kk+1 simu <- simuTargetClutter(nobs,pd,ssw,ssv,xx0,ss0,nyy,yrange) if(max(abs(simu$yy))>80){ kk <- kk-1 } else{ Xx10[,kk] <- simu$xx; Yy10[,,kk] <- simu$yy; Ii10[,kk] <- simu$ii } } ## KF with known indicators Xxhat.kf10 <- matrix(ncol=nk,nrow=nobs) for(kk in 1:nk){ outKF <- clutterKF(nobs,ssw,ssv,Yy10[,,kk],Ii10[,kk]) Xxhat.kf10[,kk] <- outKF$xhat } ###---- compare Partial and Full --- mm=2000 resample.sch <- rep.int(1,nobs) delay <- 0; mm <- 2000; nk <- 50 Xxhat.p10 <- array(dim=c(nobs,1,nk)) Xxhat.RB10 <- array(dim=c(nobs,2,nk)) Xres.com1.10 <- array(dim=c(nobs,3,nk)) Xres.com2.10 <- array(dim=c(nobs,4,nk)) set.seed(1) for(kk in 1:nk){ xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out1 <- SMC(Sstep.Clutter,nobs,Yy10[,,kk],mm,par, xx.init,xdim,ydim,resample.sch) Xxhat.p10[,1,kk] <- out1$xhat[1,,1] } set.seed(1) for(kk in 1:nk){ cat('\n','iteration',kk);flush.console() xx.init <- matrix(nrow=2,ncol=mm) xx.init[1,] <- yrange[1]+runif(mm)*yr xx.init[2,] <- rep(0.1,mm) out3 <- SMC.Full.RB(Sstep.Clutter.Full.RB,nobs,Yy10[,,kk],mm,par, xx.init,xdim,ydim,resample.sch) Xxhat.RB10[,1,kk] <- out3$xhatRB[1,] Xxhat.RB10[,2,kk] <- out3$xhat[1,] } tstep <- c(10,30,50,70,90); for(ii in 1:nk){ Xres.com1.10[,1,ii] <- Xxhat.p10[,1,ii]-Xxhat.kf10[,ii] Xres.com1.10[,2,ii] <- Xxhat.RB10[,2,ii]-Xxhat.kf10[,ii] Xres.com1.10[,3,ii] <- Xxhat.RB10[,1,ii]-Xxhat.kf10[,ii] Xres.com2.10[,1,ii] <- Xxhat.p10[,1,ii]-Xx10[,ii] Xres.com2.10[,2,ii] <- Xxhat.RB10[,2,ii]-Xx10[,ii] Xres.com2.10[,3,ii] <- Xxhat.RB10[,1,ii]-Xx10[,ii] Xres.com2.10[,4,ii] <- Xxhat.kf10[,ii]-Xx10[,ii] } myboxplot(Xres.com1.10,tstep,xlab='time') #fig_clutterXcom10_1.pdf myboxplot(Xres.com1.10,tstep,xlab='time',ylim=c(-0.01,0.01)) #fig_clutterXcom10_2.pdf myboxplot(Xres.com2.10,tstep,xlab='time') #fig_clutterXcom10_3.pdf myboxplot(Xres.com2.10,tstep,xlab='time',ylim=c(-0.1,0.1)) #fig_clutterXCom10_4.pdf #### ################ #Function of single step propagation: passive sonar. # proposal used: state equation Sstep.Sonar=function(mm,xx,logww,yy,par,xdim=1,ydim=1){...} #################### #Function of simple step smooth step: passive sonar Sstep.Smooth.Sonar=function(mm,xxt,xxt1,ww,vv,par){...} ################### #Function to simulate the data simPassiveSonar=function(nobs,q,r,start,seed=20){...} #--------------------------------------------------------------- #### #parameters s2 <- 20 #second sonar location at (s2,0) q <- c(0.03,0.03) r <- c(0.02,0.02) nobs <- 200 start <- c(10,10,0.01,0.01) H <- c(1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1) H <- matrix(H,ncol=4,nrow=4,byrow=T) W <- c(0.5*q[1], 0, 0, 0.5*q[2], q[1],0, 0,q[2]) W <- matrix(W,ncol=2,nrow=4,byrow=T) V <- diag(r) mu0 <- start SS0 <- diag(c(1,1,1,1))*0.01 #simulation simu_out <- simPassiveSonar(nobs,q,r,start,seed=20) yy <- simu_out$yy tt <- 100:200 plot(simu_out$xx[1,tt],simu_out$xx[2,tt],xlab='x',ylab='y',xlim=c(25.8,34)) #--------------------------------------- #Run delayed filtering. Maximum delay 20 #--------------------------------------- mm <- 100000 set.seed(1) resample.sch <- rep(1,nobs) xdim <- 4;ydim <- 2; xx.init <- mu0+SS0%*%matrix(rnorm(mm*4),nrow=4,ncol=mm) par <- list(H=H,W=W,V=V,s2=s2) delay <- 20 out <- SMC(Sstep.Sonar,nobs,yy,mm,par,xx.init,xdim,ydim,resample.sch,delay) tt <- 100:200 plot(simu_out$xx[1,tt],simu_out$xx[2,tt],xlab='x',ylab='y',xlim=c(25.8,34)) for(dd in c(1,11,21)){ tt <- 100:(200-dd) lines(out$xhat[1,tt,dd],out$xhat[2,tt,dd],lty=22-dd,lwd=2) } legend(26.1,-12,legend=c("delay 0","delay 10","delay 20"),lty=c(21,11,1)) ## PassiveSonar.Fig1.pdf plot(simu_out$xx[1,]-out$xhat[1,,1],ylab='x error',type='n') lines(simu_out$xx[1,]-out$xhat[1,,1],lty=21,lwd=1) lines(simu_out$xx[1,1:189]-out$xhat[1,1:189,11],lty=2,lwd=2) lines(simu_out$xx[1,1:179]-out$xhat[1,1:179,21],lty=1,lwd=2) abline(h=0) legend(0,2,legend=c("delay 0","delay 10","delay 20"),lty=c(21,2,1)) ## PassiveSonar.Fig2.pdf plot(simu_out$xx[2,]-out$xhat[2,,1],ylab='y error',type='n') lines(simu_out$xx[2,]-out$xhat[2,,1],lty=21,lwd=1) lines(simu_out$xx[2,1:189]-out$xhat[2,1:189,11],lty=2,lwd=2) lines(simu_out$xx[2,1:179]-out$xhat[2,1:179,21],lty=1,lwd=2) abline(h=0) legend(-2,-0.7,legend=c("delay 0","delay 10","delay 20"),lty=c(21,2,1)) ## PassiveSonar.Fig3.pdf #--------------------------------------- #Run smoothing #--------------------------------------- set.seed(1) mm <- 5000 par <- list(H=H,W=W,V=V,s2=s2) xx.init <- mu0+SS0%*%matrix(rnorm(mm*4),nrow=4,ncol=mm) out.s5K <- SMC.Smooth(Sstep.Sonar,Sstep.Smooth.Sonar,nobs,yy,mm,par, xx.init,xdim,ydim,resample.sch) tt <- 100:200 plot(simu_out$xx[1,tt],simu_out$xx[2,tt],xlab='x',ylab='y',xlim=c(25.8,34)) lines(out.s5K$xhat[1,tt],out.s5K$xhat[2,tt],lty=1,lwd=2) ##-- from Example 6.4.1 -- EKF smoothing results lines(muTall[1,tt],muTall[2,tt],lty=2,lwd=2) legend(26.1,-12,legend=c("SMC Smoother","EKF Smoother"),lty=c(1,2),lwd=2) ## PassiveSonar.Fig4.pdf #### ####### #Function of singple step propagation using state equation: SV Sstep.SV=function(mm,xx,logww,yyy,par,xdim,dim){...} ####### #Function of SMC implementation for parallel processing wrap.SMC=function(par.natural,setseed=T,resample=T){...} #--------------------------------------------------------------- #### library(tensor) ## Read in data y <- matrix(scan("SPY0717.dat"),ncol=2,byrow=T) yy <- y[,1] yy <- yy-mean(yy) yy <- yy[1:250] ## Define search grid and prepare for parallel run ## range.mu <- seq(-5, -4.4, length=20) range.phi <- seq(0.9, 1.0, length=20) range.sigma <- seq(0.2, 0.5, length=20) range <- list(range.mu, range.phi, range.sigma) nobs <- length(yy) mm <- 20000 grid <- expand.grid(range.mu, range.phi, range.sigma) names(grid) <- c("mu", "phi", "sigma") list.par <- as.list(data.frame(t(grid))) ## Run grid search parallel ### library(parallel) RNGkind("L'Ecuyer-CMRG") ### Use same seed for all computation set.seed(1) lll <- mcmapply(wrap.SMC, list.par, MoreArgs=list(setseed=T), mc.cores=detectCores(), mc.set.seed=T) lll.array <- array(lll, dim=c(20,20,20)) lll.max <- max(lll.array) max.index <- which(lll.array==lll.max, arr.ind=T) ### Plot pdf('Loglike_grid.pdf', width=12, height=6) par(mfrow=c(1,3)) plot(lll.array[,max.index[2],max.index[3]]~range.mu, xlab="mu", ylab="log likelihood", main="likelihood for mu", type='l',cex.lab=1.2) plot(lll.array[max.index[1],,max.index[3]]~range.phi, xlab="phi", ylab="log likelihood", main="likelihood for phi", type='l',,cex.lab=1.2, ylim=c(1538,1542.5)) plot(lll.array[max.index[1],max.index[2],]~range.sigma, xlab="sigma", ylab="log likelihood", main="likelihood for sigma", type='l',,cex.lab=1.2) dev.off() ## MLE: iterative grid search ## ### Define search box LU.mu <- c(-5,-4.4) LU.phi <- c(0.7,1.0) LU.sigma <- c(0.2,0.5) LU <- list(LU.mu, LU.phi, LU.sigma) ### Initial point par.current <- c(-4.7, 0.9, 0.35) grid.current <- matrix(rep(par.current, 10), ncol=3, byrow=T) names(grid.current) <- c('mu','phi','sigma') ### Accuracy epsilon <- c(0.01, 0.005, 0.005) within.epsilon <- 0 while(within.epsilon<3){ within.epsilon <- 0 for(i in 1:3){ sequence <- seq(LU[[i]][1], LU[[i]][2], length=10) grid.current[,i] <- sequence set.seed(1) lll.tmp <- mcmapply(wrap.SMC, as.list(data.frame(t(grid.current))), MoreArgs=list(setseed=T), mc.cores=detectCores(), mc.set.seed=T) par.current[i] <- sequence[which.max(lll.tmp)] grid.current[,i] <- par.current[i] if (LU[[i]][2]-LU[[i]][1] <= epsilon[i]){ within.epsilon <- within.epsilon + 1 } else { # Shrink the interval by half LU[[i]] <- 0.5*LU[[i]] + 0.5*par.current[i] } } } mle <- par.current lll.mle <- vector('list', 3) ## Local variability around MLE without resampling ### n.grid <- 30 range.local.mu <- seq(mle[1]-0.07, mle[1]+0.07, length=n.grid) range.local.phi <- seq(mle[2]-0.05, mle[2]+0.05, length=n.grid) range.local.sigma <- seq(mle[3]-0.1, mle[3]+0.1, length=n.grid) range.local <- list(range.local.mu, range.local.phi, range.local.sigma) lll.local <- vector('list', 3) for(i in 1:3){ grid.tmp <- matrix(rep(mle, n.grid), ncol=3, byrow=T) grid.tmp[,i] <- range.local[[i]] ### Use same seed for all computation set.seed(1) lll.tmp <- mcmapply(wrap.SMC, as.list(data.frame(t(grid.tmp))), MoreArgs=list(setseed=T, resample=F), mc.cores=detectCores(), mc.set.seed=T) ### Use different seeds for all computation set.seed(1) lll2.tmp <- mcmapply(wrap.SMC, as.list(data.frame(t(grid.tmp))), MoreArgs=list(setseed=F, resample=F), mc.cores=detectCores(), mc.set.seed=F) lll.local[[i]] <- list(lll.tmp, lll2.tmp) } ### Plot pdf('Variability.pdf', width=12, height=6) par(mfrow=c(1,3)) for(i in 1:3){ lll.tmp <- lll.local[[i]][[1]] lll2.tmp <- lll.local[[i]][[2]] m <- min(lll.tmp,lll2.tmp) M <- max(lll.tmp,lll2.tmp) plot(range.local[[i]],lll.tmp, xlab=c("mu","phi","sigma")[i], ylab="log likelihood", type='l', ylim=c(2*m-M, 2*M-m), cex.lab=1.2) lines(range.local[[i]], lll2.tmp, lty=2) legend('bottomright', lty=c(1,2), legend=c('Same seed','Different Seeds')) } dev.off() ## Posterior Mean, Smoothing and filtering ### ww <- exp(lll-max(lll)) ww <- ww/sum(ww) ###Posterior Mean par.hat <- ww%*%as.matrix(grid) ###Filtering and Smoothing par <- natural2ar(par.hat) resample.sch <- rep(c(0,0,0,0,1), length=nobs) set.seed(1) xx.init <- par.hat[1] + par.hat[3]*rnorm(mm) out <- SMC(Sstep.SV, nobs, yy, mm, par, xx.init, 1, 1, resample.sch, delay=nobs-1) out.filtering <- out$xhat[1,,1] out.smoothing <- diag(out$xhat[1,, nobs:1]) ### Plot pdf('Filtering.pdf', width=10, height=8) par(mfrow=c(3,1)) par(mar=c(0,6,4,1)) plot(yy, xaxt='n', ylab='Y', type='l', main='Filtering') par(mar=c(6,6,0,1)) plot(out.filtering,xlab='Time',ylab='xhat',type='l') plot(out.smoothing, xlab='Time', ylab='xhat', type='l') dev.off() #### ############## ### function of Mixture Kalman Filter step for fading channels ############## MKFstep.fading=function(mm,II,mu,SS,logww,yyy,par,xdim,ydim,resample){ HH <- par$HH; WW <- par$WW; GG1 <- par$GG1; GG2 <- par$GG2; VV <- par$VV; prob1 <- 1:mm; prob2 <- 1:mm; CC <- 1:mm mu.i <- array(dim=c(xdim,2,mm)); SS.i <- array(dim=c(xdim,xdim,2,mm)); xxRB <- matrix(nrow=xdim,ncol=mm) IpRB <- 1:mm for(jj in 1:mm){ out1 <- KFoneLike(mu[,jj],SS[,,jj],yyy,HH,GG1,WW,VV) out2 <- KFoneLike(mu[,jj],SS[,,jj],yyy,HH,GG2,WW,VV) prob1[jj] <- exp(-out1$like) prob2[jj] <- exp(-out2$like) CC[jj] <- prob1[jj]+prob2[jj] mu.i[,1,jj] <- out1$mu mu.i[,2,jj] <- out2$mu SS.i[,,1,jj] <- out1$SS SS.i[,,2,jj] <- out2$SS xxRB[,jj] <- (prob1[jj]*mu.i[,1,jj]+prob2[jj]*mu.i[,2,jj])/CC[jj] IpRB[jj] <- prob1[jj]/CC[jj]*(2-II[jj])+(1-prob1[jj]/CC[jj])*(II[jj]-1) logww[jj] <- logww[jj]+log(CC[jj]) } ww <- exp(logww-max(logww)) ww <- ww/sum(ww) xhatRB <- sum(xxRB*ww) IphatRB <- sum(IpRB*ww) if(resample==1){ r.index <- sample.int(mm,size=mm,replace=T,prob=ww) mu.i[,,] <- mu.i[,,r.index] SS.i[,,,] <- SS.i[,,,r.index] prob1 <- prob1[r.index] CC <- CC[r.index] II <- II[r.index] logww <- logww*0 } II.new <- (runif(mm) > prob1/CC)+1 for(jj in 1:mm){ mu[,jj] <- mu.i[,II.new[jj],jj] SS[,,jj] <- SS.i[,,II.new[jj],jj] } xhat <- sum(mu*ww) Iphat <- sum(((2-II)*(2-II.new)+(II-1)*(II.new-1))*ww) return(list(mu=mu,SS=SS,II=II.new,logww=logww,xhat=xhat, xhatRB=xhatRB,Iphat=Iphat,IphatRB=IphatRB)) } ################# ### function of SIS step: Fading ################# SISstep.fading=function(mm,xx,logww,yyy,par,xdim2,ydim){ HH <- par$HH; WW <- par$WW; VV <- par$VV; GG <- par$GG xxx <- xx[1:4,] xxx <- HH%*%xxx+WW%*%matrix(rnorm(mm),nrow=1,ncol=mm) alpha <- GG%*%xxx pp1 <- 1/(1+exp(-2*yyy*alpha/VV**2)) temp1 <- -(yyy-alpha)**2/2/VV**2 temp2 <- -(yyy+alpha)**2/2/VV**2 temp.max <- apply(cbind(temp1,temp2),1,max) temp1 <- temp1-temp.max temp2 <- temp2-temp.max loguu <- temp.max+log(exp(temp1)+exp(temp2)) logww <- logww+loguu logww <- as.vector(logww)-max(logww) II.new <- (runif(mm) > pp1)+1 xx[6,] <- (2-xx[5,])*(2-II.new)+(xx[5,]-1)*(II.new-1) xx[5,] <- II.new xx[1:4,] <- xxx return(list(xx=xx,logww=logww)) } #### ### Fading channel analysis HH <- matrix(c(2.37409, -1.92936, 0.53028,0, 1,0,0,0, 0,1,0,0, 0,0,1,0),ncol=4,byrow=T) WW <- matrix(c(1,0,0,0),nrow=4) GG <- matrix(0.01*c(0.89409,2.68227,2.68227,0.89409),nrow=1) VV <- 1.3**15*0.001 par <- list(HH=HH,WW=WW,GG=GG,VV=VV) set.seed(1) par(mfrow=c(1,2)) simu <- simu_fading(200,par) plot(simu$alpha,type='l',ylim=range(simu$yy),ylab='alpha',xlab='time') abline(h=0,lty=2) plot(simu$yy,type='l',ylab='y',xlab='time') #fig_fading0 #------- get SNR and root of AR polynomial SNR0=10*log(var(simu$alpha)/VV**2)/log(10) print(SNR0) abs(polyroot(c(1,-2.37409, +1.92936, -0.53028))) #------- MKF setup GG1 <- GG; GG2 <- -GG xdim <- 4; ydim <- 1 nobs <- 10050 mm <- 100; resample.sch <- rep(1,nobs) kk <- 20 SNR <- 1:kk; BER1 <- 1:kk; BER0 <- 1:kk; BER.known <- 1:kk tt <- 50:(nobs-1) for(i in 1:kk){ VV <- 1.3**i*0.001 par <- list(HH=HH,WW=WW,GG=GG,VV=VV) par2 <- list(HH=HH,WW=WW,GG1=GG1,GG2=GG2,VV=VV) set.seed(1) simu <- simu_fading(nobs,par) mu.init <- matrix(0,nrow=4,ncol=mm) SS0 <- diag(c(1,1,1,1)) for(n0 in 1:20){ SS0 <- HH%*%SS0%*%t(HH)+WW%*%t(WW) } SS.init <- aperm(array(rep(SS0,mm),c(4,4,mm)),c(1,2,3)) II.init <- floor(runif(mm)+0.5)+1 out <- MKF.Full.RB(MKFstep.fading,nobs,simu$yy,mm,par2,II.init, mu.init,SS.init,xdim,ydim,resample.sch) SNR[i] <- 10*log(var(simu$yy)/VV**2-1)/log(10) Strue <- simu$ss[2:nobs]*simu$ss[1:(nobs-1)] Shat <- rep(-1,(nobs-1)) Shat[out$IphatRB[2:nobs]>0.5] <- 1 BER1[i] <- sum(abs(Strue[tt]-Shat[tt]))/(nobs-50)/2 Shat0 <- sign(simu$yy[1:(nobs-1)]*simu$yy[2:nobs]) BER0[i] <- sum(abs(Strue[tt]-Shat0[tt]))/(nobs-50)/2 S.known <- sign(simu$yy*simu$alpha) Shat.known <- S.known[1:(nobs-1)]*S.known[2:nobs] BER.known[i] <- sum(abs(Strue[tt]-Shat.known[tt]))/(nobs-50)/2 } plot(SNR,BER.known,panel.first=grid(abline(v=c(15,20,25,30,35), h=0.01*c(1,3,5,7,9,11),lty=2)),log='y',xlim=c(14,35),yaxt="n", ylim=c(0.007,0.12),type='b',ylab='BER',xlab='SNR(dB)',pch=1, lwd=2,cex=1.5) axis(2,0.01*c(1,3,5,7,9,11)) lines(SNR,BER1,type='b',pch=2,lwd=2,cex=1.5) lines(SNR,BER0,type='b',pch=3,lwd=2,cex=1.5) legend(15,0.015,pch=c(1,2,3),cex=1.5, legend=c('known channel bound','MKF','differential')) ## comparison of MKF and SMC with partial state equation proposal ## get processing time VV <- 1.3**15*0.001 par <- list(HH=HH,WW=WW,GG=GG,VV=VV) par2 <- list(HH=HH,WW=WW,GG1=GG1,GG2=GG2,VV=VV) nobs <- 10050 set.seed(2) simu <- simu_fading(nobs,par) time.M=NULL mm.all <- c(5,10,20,50,100,300) kk <- 6 BER.M <- 1:kk GG1 <- GG GG2 <- -GG xdim <- 4; ydim <- 1 for (i in 1:kk){ ptm <- proc.time() mm <- mm.all[i] mu.init <- matrix(0,nrow=4,ncol=mm) SS0 <- diag(c(1,1,1,1)) for(n0 in 1:20){ SS0 <- HH%*%SS0%*%t(HH)+WW%*%t(WW) } SS.init <- aperm(array(rep(SS0,mm),c(4,4,mm)),c(1,2,3)) II.init <- floor(runif(mm)+0.5)+1 print(c(i));flush.console() out <- MKF.Full.RB(MKFstep.fading,nobs,simu$yy,mm,par2,II.init, mu.init,SS.init,xdim,ydim,resample.sch) Strue <- simu$ss[2:nobs]*simu$ss[1:(nobs-1)] Shat <- rep(-1,(nobs-1)) Shat[out$IphatRB[2:nobs]>0.5] <- 1 BER.M[i] <- sum(abs(Strue[tt]-Shat[tt]))/(nobs-50)/2 time.M=cbind(time.M,proc.time()-ptm) } BER.M; time.M time.S=NULL mm.all <- c(100,200,500,1000,4000,8000) delay <- 0 kk <- 6 xdim2 <- 6 BER.S <- 1:kk for (i in 1:kk){ ptm <- proc.time() mm <- mm.all[i] SS0 <- diag(c(1,1,1,1)) for(n0 in 1:20){ SS0 <- HH%*%SS0%*%t(HH)+WW%*%t(WW) } xx.init <- matrix(nrow=xdim2,ncol=mm) xx.init[1:4,] <- sqrt(SS0)%*%matrix(rnorm(4*mm),nrow=4,ncol=mm) xx.init[5,] <- sample.int(2,mm,0.5) xx.init[6,] <- rep(1,mm) print(c(i));flush.console() out <- SMC(SISstep.fading,nobs,simu$yy,mm,par,xx.init, xdim2,ydim,resample.sch) Strue <- simu$ss[2:nobs]*simu$ss[1:(nobs-1)] Shat <- rep(-1,(nobs-1)) Shat[out$xhat[6,2:nobs,1]>0.5] <- 1 BER.S[i] <- sum(abs(Strue[tt]-Shat[tt]))/(nobs-50)/2 time.S=cbind(time.S,proc.time()-ptm) } BER.S; time.S ####