####### #SIR Filter for Bearings-only-Tracking # input y: observation # par: vector of parameters (obs sd, noise sd) # prior: vector of prior parameters (prior mn, prior sd) # assume indpend priors; (x,z) then (xdot,zdot) # N: Number of particles # SIR: T - SIR filter; F - SIS filter # PATH: T - store paths; F- Filter only # ####### SIRfilter=function(N,y,par,prior,SIR=T,PATH=F,xlim=c(-10,10),ylim=c(-10,10),Nplot=0) { n=length(y) ##number of observations if(Nplot<=0) Nplot=N ##(1)simulate particles particles=matrix(0,nrow=N,ncol=4) ## particles are (x,z) and (xdot,zdot) for(j in 1:4) particles[,j]=rnorm(N,prior[j],prior[j+4]) plot(0,0,pch=2,col=2,cex=2,xlim=xlim,ylim=ylim,xlab="",ylab="") points(particles[1:Nplot,1],particles[1:Nplot,2]) ##(2) calculate weights y=atan(x/z)+eps; eps~N(0,par[1]^2) bearings=atan2(particles[,2],particles[,1]) ##atan2 gives bearings btwn[-pi,pi] ##care required for bearings ~ -pi; obs ~ pi and vice-versa. error=pmin(abs(bearings-y[1]),abs(bearings-y[1]+2*pi),abs(bearings-y[1]+2*pi)) w=dnorm(error,0,par[1]) plot(0,0,pch=2,col=2,cex=2,xlim=xlim,ylim=ylim,xlab="",ylab="") if(y[1]>0){ lines(c(0,100),c(0,100*tan(y[1])),col=2,lwd=2) }else{ lines(c(0,-100),c(0,-100*tan(y[1])),col=2,lwd=2) } points(particles[1:Nplot,1],particles[1:Nplot,2]) plot(0,0,pch=2,col=2,cex=2,xlim=xlim,ylim=ylim,xlab="",ylab="") if((y[1]> -pi/2 && y[1]< pi/2) ){ lines(c(0,100),c(0,100*tan(y[1])),col=2,lwd=2) }else{ lines(c(0,-100),c(0,-100*tan(y[1])),col=2,lwd=2) } for(j in 1:Nplot) points(particles[j,1],particles[j,2],lwd=sqrt(as.integer(5*N*w[j]/sum(w)))) if(PATH){ paths=array(0,c(N,4,n)) paths[,,1]=particles } ##(3) log of estimate of likelihood logl=log(mean(w)) w=w/sum(w) ##normalise to avoid numerical instability for(i in 2:n){##LOOP ##(1) Resample if SIR; otherwise propagate weights# if(SIR){ plot(0,0,pch=2,col=2,cex=2,xlim=xlim,ylim=ylim,xlab="",ylab="") index=sample(1:N,prob=w,size=N,rep=T) if(PATH){ paths=paths[index,,] particles=particles[index,] if(i==2){ for(j in 1:Nplot) points(particles[j,1]+rnorm(1,0,0.01),particles[j,2]+rnorm(1,0,0.01)) }else{ for(j in 1:Nplot){ lines(paths[j,1,1:(i-1)],paths[j,2,1:(i-1)]) points(particles[j,1]+rnorm(1,0,0.01),particles[j,2]+rnorm(1,0,0.01)) } } }else{ particles=particles[index,] for(j in 1:Nplot) points(particles[j,1]+rnorm(1,0,0.01),particles[j,2]+rnorm(1,0,0.01)) } } ##(2) Propagate particles #xdot=xdot+rnorm(1,0,par[2]); x=x+xdot; ditto for z new.particles=particles new.particles[,3:4]=new.particles[,3:4]+rnorm(2*N,0,par[2]) new.particles[,1:2]=new.particles[,1:2]+new.particles[,3:4] if(SIR){ for(j in 1:Nplot){ lines(c(particles[j,1],new.particles[j,1]),c(particles[j,2],new.particles[j,2]),col=3,lwd=1) } points(new.particles[1:Nplot,1],new.particles[1:Nplot,2],col=3) }else{ for(j in 1:Nplot){ if(5*N*w[j]/sum(w)>1){ lines(c(particles[j,1],new.particles[j,1]),c(particles[j,2],new.particles[j,2]),col=3,lwd=1) points(new.particles[j,1],new.particles[j,2],col=3) } } } particles=new.particles if(PATH) paths[,,i]=particles ##(3) Weight bearings=atan2(particles[,2],particles[,1]) ##atan2 gives bearings btwn[-pi,pi] ##care required for bearings ~ -pi; obs ~ pi and vice-versa. error=pmin(abs(bearings-y[i]),abs(bearings-y[i]+2*pi),abs(bearings-y[i]+2*pi)) if(SIR){ w=dnorm(error,0,par[1]) }else{ w=w*dnorm(error,0,par[1]) logl=logl+log(sum(w)) w=w/sum(w) } plot(0,0,pch=2,col=2,cex=2,xlim=xlim,ylim=ylim,xlab="",ylab="") if((y[i]> -pi/2 && y[i]< pi/2) ){ lines(c(0,100),c(0,100*tan(y[i])),col=2,lwd=2) }else{ lines(c(0,-100),c(0,-100*tan(y[i])),col=2,lwd=2) } if(!PATH){ for(j in 1:Nplot) points(particles[j,1],particles[j,2],lwd=sqrt(as.integer(5*N*w[j]/sum(w)))) }else{ for(j in 1:Nplot){ lines(paths[j,1,1:(i)],paths[j,2,1:(i)],lwd=sqrt(as.integer(N*w[j]/sum(w)))) points(particles[j,1],particles[j,2],lwd=sqrt(as.integer(5*N*w[j]/sum(w))))} } ##(4) update log of estimate of likelihood if(SIR) logl=logl+log(mean(w)) } } ##simulate BOT data of length n BOTsim=function(n,par,prior){ y=rep(0,n) x=rnorm(4,prior[1:4],prior[5:8]) y[1]=atan2(x[2],x[1]) for(i in 2:n){ x[3:4]=x[3:4]+rnorm(2,0,par[2]) x[1:2]=x[1:2]+x[3:4] y[i]=atan2(x[2],x[1]) } return(y) } par=c(0.1,0.25) prior=c(4,5,-0.8,-0.8,0.5,0.5,0.05,0.05) n=15 x=matrix(c(1+(1:n-n/2)^2/n,6-0.8*1:n),byrow=F,ncol=2) y=atan2(x[,2]+rnorm(n,0,0.25),x[,1]+rnorm(n,0,0.25)) #y=BOTsim(n,par,prior) xlim=c(-1,5) ylim=c(5-n-2,8) N=200 par(ask=F) pdf("SIR_BOT.pdf") SIRfilter(N,y,par,prior,SIR=T,PATH=F,xlim=xlim,ylim=ylim,Nplot=100) SIRfilter(N,y,par,prior,SIR=T,PATH=T,xlim=xlim,ylim=ylim,Nplot=200) SIRfilter(N,y,par,prior,SIR=F,PATH=F,xlim=xlim,ylim=ylim,Nplot=N) dev.off()