kalmanfilter = function(y,m0,c0,phi,V,W){
  n  = length(y)	
  mf = rep(0,n)
  Cf = rep(0,n)
  m  = m0
  C  = C0
  for (t in 1:n){
    a = phi*m
    R = phi^2*C+W
    C = 1/(1/R+1/V)
    m = C*(a/R+y[t]/V)
    Cf[t] = C
    mf[t] = m
  }
  return(list(mf=mf,Cf=Cf))
}

###############################################################
# Simulating some data
###############################################################
set.seed(31415)
n  = 23
W  = 1
V  = 2
x0 = 0
x = rep(0,n)
x[1] = rnorm(1,x0,sqrt(W))
for (t in 2:n)
  x[t] = rnorm(1,x[t-1],sqrt(W))
y = rnorm(n,x,sqrt(V))
y[11] = 20
y[12] = 0
y[13] = 20
plot(y,type="b",xlab="Time")

###############################################################
# Posterior inference
###############################################################

# x0 ~ N(m0,C0)
m0 = 0
C0 = 9

# Exact derivations via Kalman filter
run.exact = kalmanfilter(y,m0,c0,1,V,W)
mf = run.exact$mf
Cf = run.exact$Cf
limf = cbind(mf-qnorm(0.05)*sqrt(Cf),mf,mf+qnorm(0.05)*sqrt(Cf))


par(mfrow=c(2,2))
for (M in c(100,1000,10000,100000)){
# Number of particles
#M = 100

# Bootstrap filter
set.seed(1234)
x = rnorm(M,m0,sqrt(C0))
xs = matrix(0,M,n)
for (t in 1:n){
  # propagation
  x1 = rnorm(M,x,sqrt(W))
  # resampling
  w = dnorm(y[t],x1,sqrt(V))
  x = sample(x1,replace=TRUE,size=M,prob=w)
  xs[,t] = x
}
qx = t(apply(xs,2,quantile,c(0.05,0.5,0.95)))

# Auxiliary particle filter
set.seed(1234)
x = rnorm(M,m0,sqrt(C0))
xs = matrix(0,M,n)
for (t in 1:n){
  # calcular pesos e reamostrar
  w = dnorm(y[t],x,sqrt(V))
  xx = sample(x,size=M,replace=TRUE,prob=w)
  # propagacao
  x1 = rnorm(M,xx,sqrt(W))
  # calculando novos pesos e reamostrando
  w = dnorm(y[t],x1,sqrt(V))/dnorm(y[t],xx,sqrt(V))
  x = sample(x1,replace=TRUE,size=M,prob=w)
  xs[,t] = x
}
qx.apf = t(apply(xs,2,quantile,c(0.05,0.5,0.95)))

# Optimal filter
set.seed(1234)
x = rnorm(M,m0,sqrt(C0))
xs = matrix(0,M,n)
for (t in 1:n){
  # calcular pesos e reamostrar
  w = dnorm(y[t],x,sqrt(V+W))
  xx = sample(x,size=M,replace=TRUE,prob=w)
  # propagacao
  var = 1/(1/V+1/W)
  mean = var*(y[t]/V+xx/W)
  x = rnorm(M,mean,sqrt(var))
  xs[,t] = x
}
qx.opf = t(apply(xs,2,quantile,c(0.05,0.5,0.95)))


# Comparing the particle filters
plot(qx[,2],pch=16,ylim=range(qx,limf,qx.apf,y),cex=0.5,xlab="Time",ylab="")
points(1:n+0.2,qx.apf[,2],col=2,pch=16,cex=0.5)
points(1:n+0.4,qx.opf[,2],col=3,pch=16,cex=0.5)
points(1:n+0.6,limf[,2],col=4,pch=16,cex=0.5)
for (t in 1:n){
  segments(t,qx[t,1],t,qx[t,3])
  segments(t+0.2,qx.apf[t,1],t+0.2,qx.apf[t,3],col=2)
  segments(t+0.4,qx.opf[t,1],t+0.4,qx.opf[t,3],col=3)
  segments(t+0.6,limf[t,1],t+0.6,limf[t,3],col=4)
}
legend("topleft",legend=c("BF","APF","OPF","Exact","Data"),col=c(1:4,6),lty=1)
title(paste("p(x_t|y^t)\n M=",M,sep=""))
points(1:n+0.3,y,col=6,pch=16,cex=0.75)
abline(v=10.8)
abline(v=11.8)
abline(v=12.8)
abline(v=13.8)
}