Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
## 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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.