Particle filter assignment.

particle.r

# particle.r
# George Lesica
# CSCI 555 - FA 2012
# Homework 7.5
# Solution to problem 2

samp <- function(S) {
  return(sample(S$X, length(S$X), replace=T, prob=S$W))
}

filter <- function(S, u, z) {
  # Args:
  #   P: Particles (X and W attributes for positions and weights)
  #   u: Movement function (gets x)
  #   z: Sensor function (gets x)
  Xp <- sapply(samp(S), u)
  Wp <- sapply(Xp, z)
  Wp <- Wp / sum(Wp)
  
  Sp <- list(X=Xp, W=Wp)
  return(Sp)
}

# Simulation parameters
n <- 10000
xMin <- 0
xMax <- 4

bins <- 40

move <- function(x) {
  return((x + runif(1, 0.5, 1.0)) %% xMax)
}

door <- function(x) {
  if (x < 1) {
    return(7/8)
  } else {
    return(1/8)
  }
}

nodoor <- function(x) {
  if (x < 1) {
    return(1/8)
  } else {
    return(7/8)
  }
}

plotParticles <- function(X, title="Particles") {
  hist(
    X,
    breaks=bins,
    col="lightblue",
    xlab="Particle Location",
    ylab="Count",
    main=title
  )
}

simulate <- function(n, animate=FALSE) {
  S <- list(X=runif(n, xMin, xMax), W=rep(1/n, n))
  S1 <- filter(S, move, door)
  S2 <- filter(S1, move, door)
  S3 <- filter(S2, move, nodoor)
  
  if (! animate) {
    orig <- par(mfrow=c(2,2))
  }
  
  plotParticles(samp(S), "Time Step 0")
  plotParticles(samp(S1), "Time Step 1")
  plotParticles(samp(S2), "Time Step 2")
  plotParticles(samp(S3), "Time Step 3")
  
  if (! animate) {
    par(orig)
  }
}

#saveGIF(simulate(100, animate=T), interval=1.0, outdir=getwd())
#saveGIF(simulate(1000, animate=T), interval=1.0, outdir=getwd())
#saveGIF(simulate(10000, animate=T), interval=1.0, outdir=getwd())

simulate(100)
simulate(1000)
simulate(10000)