# Simulating the data
set.seed(13579)
n      = 50
sig2   = 1.0
tau2   = 0.1
alpha  = 0.05
beta   = 0.95
x0     = 0.0
x     = rep(0,n)
x[1]  = rnorm(1,alpha+beta*x0,sqrt(tau2))
for (t in 2:n)
  x[t] = rnorm(1,alpha+beta*x[t-1],sqrt(tau2))
y = rnorm(n,x,sqrt(sig2))  

y[25]=10

# Plotting the simulated data
par(mfrow=c(1,1))
plot(y,type="b")
lines(x,col=2)

#Prior hyperparameters
m0 = 0
C0 = 10

# Running the kalman filter
m   = m0
C   = C0
mm  = rep(0,n)
CC  = rep(0,n)
for (t in 1:n){
    a = alpha+beta*m
    R = beta^2*C+tau2
    Q = R+sig2
    A = R/Q
    m = (1-A)*a+A*y[t]
    C = A*sig2
    mm[t] = m
    CC[t] = C
}
L = mm-2*sqrt(CC)
U = mm+2*sqrt(CC)

# Running the bootstrap filter
set.seed(1234)
M  = 1000
x  = rnorm(M,m0,sqrt(C0))
xs = matrix(0,n,M)
for (t in 1:n){
  x      = rnorm(M,alpha+beta*x,sqrt(tau2))
  w      = dnorm(y[t],x,sqrt(sig2))
  k      = sample(1:M,size=M,prob=w,replace=TRUE)
  x      = x[k]
  xs[t,] = x 
}
qbf = t(apply(xs,1,quantile,c(0.025,0.5,0.975)))

# Running the auxiliary particle filter
set.seed(1234)
M   = 1000
x   = rnorm(M,m0,sqrt(C0))
xs1 = matrix(0,n,M)
for (t in 1:n){
  	  a      = alpha+beta*x
  	  w      = dnorm(y[t],a,sqrt(sig2))
    k       = sample(1:M,size=M,prob=w,replace=TRUE)
  	  x      = rnorm(M,a[k],sqrt(tau2))
  	  w1     = dnorm(y[t],x,sqrt(sig2))/w[k]
  	  k      = sample(1:M,size=M,prob=w1,replace=TRUE)
    x       = x[k]
    xs1[t,] = x 
}
qapf = t(apply(xs1,1,quantile,c(0.025,0.5,0.975)))

# Ploting exact quantiles versus BF and APF approximate quantiles
plot(mm,ylim=range(L,U),type="l",xlab="Time",ylab="State")
lines(L)
lines(U)
lines(qbf[,1],col=2)
lines(qbf[,2],col=2)
lines(qbf[,3],col=2)
lines(qapf[,1],col=4)
lines(qapf[,2],col=4)
lines(qapf[,3],col=4)
legend(10,3,legend=c("EXACT","BF","APF"),col=c(1,2,4),lty=c(1,1,1),bty="n",cex=2,lwd=3)

# Closer look for a few time periods
par(mfrow=c(2,6))
for (t in 24:29){
  L = min(mm[t]-4*sqrt(CC[t]),xs[t,],xs1[t,])
  U = max(mm[t]+4*sqrt(CC[t]),xs[t,],xs1[t,])
  xx = seq(L,U,length=1000)
  breaks = seq(L,U,length=20)
  hist(xs[t,],xlab="",prob=TRUE,main="",xlim=range(xs[t,],xs1[t,],L,U),breaks=breaks)
  lines(xx,dnorm(xx,mm[t],sqrt(CC[t])),col=2)
  title(paste("BF\n Posterior at t=",t,sep=""))
}
for (t in 24:29){
  L = min(mm[t]-4*sqrt(CC[t]),xs[t,],xs1[t,])
  U = max(mm[t]+4*sqrt(CC[t]),xs[t,],xs1[t,])
  xx = seq(L,U,length=1000)
  breaks = seq(L,U,length=20)
  hist(xs1[t,],xlab="",prob=TRUE,main="",xlim=range(xs[t,],xs1[t,],L,U),breaks=breaks)
  lines(xx,dnorm(xx,mm[t],sqrt(CC[t])),col=2)
  title(paste("APF\n Posterior at t=",t,sep=""))
}