particle_filter.R

## particle filter implementation by isobe

particle_filter <- function(x0,y,f_noise,f_like,N,M=1) {
  tmax <- nrow(y)
  D <- length(x0) # == ncol(y)
  
  do_noise <- function(x) {
    x1 <- c()
    for (i in 1:N) {
      for (j in 1:M) {
        v <- f_noise(D)
        print(x[i,]+v)
        x1<-rbind(x1,x[i,]+v)
      }
    }
    return(x1)
  }
  
  do_like <- function(x,t) {
    apply(x,1,function(xi) f_like(xi,y[t,]))
  }

  resampling <- function(x,w) {
    # input dim(x) = (N*M,D)
    # --> output dim = (D,N)
    wsum <- c()
    for (i in 1:(N*M)) {
      wsum <- c(wsum,sum(w[1:i]))
    }
    total <- sum(w)
    pos <- (1:N) * total/N
    r <- runif(1,0,total) # roulette
    pos <- (pos+r) %% total
    ret <- c()
    for (i in 1:N) {
      j <- which(wsum>=pos[i])[1]
      ret <- cbind(ret,x[j,])
    }
    return(ret)
  }
  
  xx <- list(matrix(x0,D,N))
  
  for (t in 1:tmax) {
    x <- xx[[t]]         # --> dim(x)=(D,N)
    x <- do_noise(t(x)) # --> dim(x)=(N*M,D)
    w <- do_like(x,t) # --> length(w)=N*M
    xx[[t+1]] <- resampling(x,w) # --> dim(x)=(D,N)
  }
  
  return(xx)
}


##### test program  #####

test <- function() {
  x0 <- c(0,0)
  
  y <- rbind(c(4,4),
             c(8,6),
             c(6,-1),
             c(-2,-5),
             c(-8,-9),
             c(-6,0),
             c(-7,3),
             c(-3,6),
             c(0,4))
  
  f_like = function(xt,yt) {
    return(exp(-sum((yt-xt)**2)))
  }
  
  f_noise = function(D) {
    rnorm(D,0,3)
  }
  xx <- particle_filter(x0,y,f_noise=f_noise,f_like=f_like,N=1000)
  
  par(mfrow=c(3,3))
  
  xlim = c(-10,10)
  ylim = c(-10,10)
  for (t in 1:nrow(y)) {
    x <- xx[[t+1]]
    plot(y[1:t,1],y[1:t,2],type="b",col="3",xlab="",ylab="",xlim=xlim,ylim=ylim)
    par(new=T)
    plot(x[1,],x[2,],col="2",xlab=paste("t =",t),ylab="",xlim=xlim,ylim=ylim)
  }
}

test()