# OUR FIRS PARTICLE FILTER
# THE BOOTSTRAP FILTER
# SALMOND, GORDON AND SMITH (1993)
#
# Local level model

pdf(file="bootstrapfilter.pdf",width=12,height=8)
# Simulating some data
set.seed(4321)
n        = 100
V        = 1
W        = 0.5
sV       = sqrt(V)
sW       = sqrt(W)
theta0   = 0
theta    = rep(0,n)
theta[1] = theta0 + rnorm(1,0,sW)
for (t in 2:n)
  theta[t] = theta[t-1] + rnorm(1,0,sW)
y = theta + rnorm(n,0,sV)

#y[61:70]= seq(4,y[70],length=10)
plot(y,type="b",xlab="Time",ylab="Observed time series data")


# Exact filtering densities
m0 = 0
C0 = 1
m  = rep(0,n)
C  = rep(0,n)
# time t=1
R    = C0 + W 
Q    = R + V
A    = R/Q
m[1] = (1-A)*m0 + A*y[1]
C[1] = R-Q*A^2
# forward filtering
for (t in 2:n){
  R    = C[t-1] + W
  Q    = R+V
  A    = R/Q
  m[t] = (1-A)*m[t-1] + A*y[t]
  C[t] = R-Q*A^2  
}
qexact = cbind(m+qnorm(0.05)*sqrt(C),m,m+qnorm(0.95)*sqrt(C))

ts.plot(qexact[,2],col=2,xlab="Time",ylab="Latent states",ylim=range(qexact))
lines(qexact[,1],col=3)
lines(qexact[,3],col=3)


# Sequential Monte Carlo via the Bootstrap filter 
set.seed(31415)
M = 10000


# Let us first run the step-by-step from time t=0 to time t=1
theta0      = rnorm(M,m0,sqrt(C0))                           # Posterior at time t=0
theta.tilde = theta0 + rnorm(M,0,sW)                         # Prior at time t=1
w           = dnorm(y[1],theta.tilde,sV)                     # Evaluate likelihood at time t=1
theta       = sample(theta.tilde,size=M,replace=TRUE,prob=w) # Resampling

par(mfrow=c(1,1))
plot(density(theta),xlim=range(theta0,theta),xlab=expression(theta),main="",lwd=2)
lines(density(theta.tilde),col=2,lwd=2)
lines(density(theta0),col=3,lwd=2)
thetas = seq(min(theta0,theta,theta.tilde),max(theta0,theta,theta.tilde),length=200)
lines(thetas,dnorm(y[1],thetas,sV),col=4,lwd=2)
points(y[1],0,cex=2,col=4,pch=16)
legend("topleft",legend=c("Posterior at t=0","Prior at t=1","Likelihood at t=1",
       "Posterior at t=1"),col=c(3,2,4,1),lwd=2,bty="n")
title("Bootstrap filter\nPropagate, reweight & resample")       
legend("topright",legend=c("SIR:reweight & resample"),bty="n")

# Implementing the Boostrap Filter for t=2,...,n
thetas     = matrix(0,M,n)
thetas[,1] = theta
for (t in 2:n){
 theta.tilde = theta + rnorm(M,0,sW)
 w           = dnorm(y[t],theta.tilde,sV)
 theta       = sample(theta.tilde,size=M,replace=TRUE,prob=w)
 thetas[,t]  = theta
}

# Printing the results
par(mfrow=c(2,3))
for (t in trunc(seq(1,n,length=6))){
 hist(thetas[,t],main=paste("Posterior at time t=",t,sep=""),xlab="")
 points(y[t],0,col=2,pch=16,cex=2)
}

par(mfrow=c(1,1))
boxplot(thetas,outline=FALSE,xlab="Time",ylab="Latent states")
points(y,col=2,pch=16)
qtheta = t(apply(thetas,2,quantile,c(0.05,0.5,0.95)))
legend("topleft",legend=c("Posterior quantiles","Data points"),col=1:2,lwd=2,bty="n")


par(mfrow=c(1,1))
plot(qtheta[,2],ylim=range(qtheta),pch=16)
for (t in 1:n)
 segments(t,qtheta[t,1],t,qtheta[t,3])
points(y,pch=16,col=2)
legend("topleft",legend=c("Posterior quantiles","Data points"),col=1:2,lwd=2,bty="n")

ts.plot(qtheta,xlab="Time",ylab="Latent states",ylim=range(qtheta),col=grey(0.75),lwd=4)
lines(qexact[,1],lwd=1.25)
lines(qexact[,2],lwd=1.25)
lines(qexact[,3],lwd=1.25)
legend("topleft",legend=c("Exact quantiles","PF quantiles"),col=c(1,grey(0.75)),lwd=2,bty="n")

dev.off()