#########################################################################
## Particle filtering                                                 ###
########################################################################

# Example of Andrade-Netto
# (nonlinear AR(1), observation =squared states + noise)

f.andrade <- function(x,u,i, ...) {
  0.5*x + 25*x/(1+x^2) + 8*cos(1.2*i) + sqrt(10)*u
}

b.square <- function(z,y, ...) {dnorm(y-0.05*z^2)}

#Simulation of the model
x <- rnorm(101)
x[1] <- 5*x[1]
for (i in (1:100)) x[i+1] <- f.andrade(x[i],x[i+1],i)
y <- 0.05*x[-1]^2 + rnorm(100)
plot(x[-1],type="l",xlab="time",ylab="state/observations")
lines(y,col=2)

#Particle filter
n.rep <- 200
x0 <- 5*rnorm(n.rep) #starting values 
x.filt <- f.particle(x0,y,f.state=f.andrade,b=b.square)
x.f <- x.filt$x
lambda.f <- x.filt$lambda
ess <- 1/apply(lambda.f^2,2,sum)

#Particle filter for whole path
n.rep <- 500
x0 <- 5*rnorm(n.rep) #starting values 
x.path <- f.path.part(x0,y,f.state=f.andrade,b=b.square)
pdf.latex("bsp-path-partfilt.pdf")
par(mfrow=c(2,2))
i <- 25
plot(0:(i-1),x.path[i,1:i,1],type="l",xlim=c(0,100),
     ylim=c(-25,25),xlab="time",ylab="state")
for (j in (2:n.rep)) lines(0:(i-1),x.path[i,1:i,j])
lines(0:(i-1),x[1:i],col=2)
i <- 50
plot(0:(i-1),x.path[i,1:i,1],type="l",xlim=c(0,100),
     ylim=c(-25,25),xlab="time",ylab="state")
for (j in (2:n.rep)) lines(0:(i-1),x.path[i,1:i,j])
lines(0:(i-1),x[1:i],col=2)
i <- 75
plot(0:(i-1),x.path[i,1:i,1],type="l",xlim=c(0,100),
     ylim=c(-25,25),xlab="time",ylab="state")
for (j in (2:n.rep)) lines(0:(i-1),x.path[i,1:i,j])
lines(0:(i-1),x[1:i],col=2)
i <- 101
plot(0:(i-1),x.path[i,1:i,1],type="l",xlim=c(0,100),
     ylim=c(-25,25),xlab="time",ylab="state")
for (j in (2:n.rep)) lines(0:(i-1),x.path[i,1:i,j])
lines(0:(i-1),x[1:i],col=2)
pdf.end()



#Particle filter with adapted proposal
n.rep <- 200
x.f <- matrix(5*rnorm(n.rep),nrow=n.rep,ncol=101)
lambda.f <- matrix(1/n.rep,nrow=n.rep,ncol=101) #starting values
for (i in (1:100)) {
  erg <- f.onestep.adapt(x.f[,i],lambda.f[,i],y[i],i)
  x.f[,i+1] <- erg[,1]
  lambda.f[,i+1] <- erg[,2]
}


#Filter distributions for the first 20 time points
par(mfrow=c(4,5))
for (i in (2:21)) plot(x.f[,i],lambda.f[,i],type="h")

#Different plot of the filter samples: Area of circles proportional to weights
times <- rep(0:50,rep(200,51))
lambda.2 <- lambda.f
lambda.2[lambda.f < 0.0005] <- NA
pdf.latex("bsp-partfilt.pdf")
symbols(times, as.vector(x.f[,1:51]),circles=sqrt(as.vector(lambda.2[,1:51])),
        inches=FALSE,xlim=c(-1,51),ylim=c(-25,25),xlab="time",
        ylab="weighted particles")
lines(0:50,x[1:51],col=4)
x.filtq <- matrix(0,nrow=3,ncol=51)
for (i in (1:51)) x.filtq[,i] <- wtd.quantile(x.f[,i],n.rep*lambda.f[,i])
lines(0:50,x.filtq[2,],col=2)
lines(0:50,x.filtq[1,],col=3)
lines(0:50,x.filtq[3,],col=6)
pdf.end()




#Plotting the 10%, 50%, 90% quantiles

x.filtq <- matrix(0,nrow=3,ncol=101)
for (i in (1:101)) x.filtq[,i] <- wtd.quantile(x.f[,i],n.rep*lambda.f[,i])

par(mfrow=c(1,1))
plot(0:100,x,type="l",xlab="time",ylab="state")
points(1:100,sqrt(20*pmax(0,y)),col=4) # arg max_{x_t} p(y_t | x_t)
points(1:100,-sqrt(20*pmax(0,y)),col=4)
lines(0:100,x.filtq[2,],col=2)
lines(0:100,x.filtq[1,],col=3)
lines(0:100,x.filtq[3,],col=6)
sum(x<x.filtq[1,])
sum(x<x.filtq[2,])
sum(x<x.filtq[3,])

#Effective sample size
plot(1:101,1/apply(lambda.f^2,2,sum),ylim=c(1,n.rep))


## Stochastic volatility model

f.ar <- function(x,u,i,phi, seps, ...) {phi*x + seps*u}

b.vol <- function(z,y,mu, ...) {exp(-0.5*(z+mu + y^2*exp(-z-mu)))}

#Simulation of the model
phi <- 0.9
seps <- 0.6
mu <- 0.5
x <- rnorm(501)
x[1] <- seps*x[1]/sqrt(1-phi^2)
for (i in (1:500)) x[i+1] <- f.ar(x[i],x[i+1],i,phi,seps)
y <- exp(0.5*(x[-1]+mu))*rnorm(500)
par(mfrow=c(2,1))
plot(exp(0.5*x[-1]+mu),type="l",ylab="volatility",xlab="time")
plot(y,type="l",ylab="returns",xlab="time")


#Particle filter
x0 <- seps*rnorm(200)/sqrt(1-phi^2)  #starting values
x.filt <- f.particle(x0,y,f.state=f.ar,b=b.vol, phi=phi,seps=seps,mu=mu)
x.f <- x.filt$x
lambda.f <- x.filt$lambda

#Filter distributions for the first 20 time points
par(mfrow=c(4,5))
for (i in (2:21)) plot(x.f[,i],lambda.f[,i],type="h")



#Plotting the 10%, 50%, 90% quantiles
x.filtq <- matrix(0,nrow=3,ncol=501)
for (i in (1:501)) x.filtq[,i] <- wtd.quantile(x.f[,i],n.rep*lambda.f[,i])
par(mfrow=c(1,1))
plot(0:500,x,type="l",xlab="time",ylab="state")
lines(0:500,x.filtq[2,],col=4)
lines(0:500,x.filtq[1,],col=2)
lines(0:500,x.filtq[3,],col=3)
sum(x<x.filtq[1,])
sum(x<x.filtq[2,])
sum(x<x.filtq[3,])



f.particle <- function(x0,y, f.state, b, ...)
{
  ## Purpose: Computing the basic particle filter
  ## ----------------------------------------------------------------------
  ## Arguments: x0=sample from the starting density
  ##            y=observed values
  ##            f.state=function describing the evolution of states
  ##                    x_t = f(x_{t-1},u_t,t) with u_t standard normal
  ##            b=observation density (time homogeneous)
  ## ----------------------------------------------------------------------
  ## Author: H. R. Kuensch, Date: 13 Mar 2005, 22:11
  n.rep <- length(x0)
  n <- length(y)
  x <- matrix(x0,nrow=n.rep,ncol=n+1)
  lambda <- matrix(1/n.rep,nrow=n.rep,ncol=n+1)
  for (i in (1:n)){
    index <- bal.sample(n.rep,lambda[,i]) #resampling
    z <- x[index,i]
    x[,i+1] <- f.state(z,rnorm(n.rep),i, ...) #propagation
    w <- b(x[,i+1],y[i], ...)  #reweighting
    lambda[,i+1] <- w/sum(w)
  }
  list(x=x,lambda=lambda)
}

f.path.part<- function(x0,y, f.state, b, ...)
{
  ## Purpose: Computing the particle filter for paths from zero to t
  ## ----------------------------------------------------------------------
  ## Arguments: x0=sample from the starting density
  ##            y=observed values
  ##            f.state=function describing the evolution of states
  ##                    x_t = f(x_{t-1},u_t,t) with u_t standard normal
  ##            b=observation density (time homogeneous)
  ## ----------------------------------------------------------------------
  ## Author: H. R. Kuensch, Date: 13 Mar 2005, 22:11
  n.rep <- length(x0)
  n <- length(y)
  x <- array(NA,dim=c(n+1,n+1,n.rep))
  x[1,1,] <- x0
  for (i in (1:n)){
      x.p <- f.state(x[i,i,],rnorm(n.rep),i, ...) #propagation
      w <- b(x.p,y[i], ...)
      w <- w/sum(w) #reweighting
      index <- bal.sample(n.rep,w) 
      x[i+1,1:i,] <- x[i,1:i,index]
      x[i+1,i+1,] <- x.p[index] #resampling
  }
  x
}

bal.sample <- function(size=length(prob),prob)
{
  ## Purpose: generating a balanced sample from (1,2,..,length(prob))
  ## ----------------------------------------------------------------------
  ## Arguments: size=sample size
  ##            prob=vector of probabilites
  ## ----------------------------------------------------------------------
  ## Author: Hans-Ruedi Kuensch, Date:  9 Jun 2005, 14:22
  w <- floor(size*cumsum(prob) + runif(1))
  w <- w-c(0,w[-length(prob)])
  rep(1:length(prob),w)
}

bal.sample.2 <- function(size=length(prob),prob)
{
  ## Purpose: generating a balanced sample from (1,2,..,length(prob))
  ##          with additional output
  ## ----------------------------------------------------------------------
  ## Arguments: size=sample size
  ##            prob=vector of probabilites
  ## Results: mult=multiplicities of each value in the sample
  ##          index=index of values in the sample
  ## ----------------------------------------------------------------------
  ## Author: Hans-Ruedi Kuensch, Date:  9 Jun 2005, 14:22
  w <- floor(size*cumsum(prob) + runif(1))
  w <- w-c(0,w[-length(prob)])
  list(mult=w,index=rep(1:length(prob),w))
}

  wtd.quantile <- function(x,wts)
{
  ## Purpose: Computing the 10%, 50% and 90% quantile
  ## ----------------------------------------------------------------------
  ## Arguments:
  ## ----------------------------------------------------------------------
  ## Author: Hans-Rudolf Kuensch, Date:  7 Apr 2009, 18:45
  n <- length(x)
  index <- sort.list(x)
  x <- x[index]
  wts <- cumsum(wts[index])
  i1 <- min((1:n)[wts>0.1*n])
  i2 <- min((1:n)[wts>0.5*n])
  i3 <- min((1:n)[wts>0.9*n])
  c(x[i1],x[i2],x[i3])
}



f.onestep.adapt<- function(x,lambda,y,i)
{
  ## Purpose: Computing one step with proposal adapted for the
  ##          Andrade-Netto model
  ## ----------------------------------------------------------------------
  ## Arguments: x=sample from the filter density at time i-1
  ##            lambda=weights for filter density at time i-1
  ##            y=observed value at time i
  ## ----------------------------------------------------------------------
  ## Author: H. R. Kuensch, Date: 7 April 2009
  n.rep <- length(x)
  f <- 0.5*x + 25*x/(1+x^2) + 8*cos(1.2*i)
  if (y < 0.25) {
    s2 <- 1/(1.5-y) #proposal for index
    tau <- exp(0.05*f^2*(s2-1))*lambda
    tau <- tau/sum(tau)
    index <- bal.sample(n.rep,tau) #resampling
    mu <- f[index] # propagating the resampled particles
    x.new <- mu*s2 + sqrt(10*s2)*rnorm(n.rep) # new particles
    l.new <- exp(-0.05*(x.new-mu)^2-0.5*(y-0.05*x.new^2)^2
             +0.05*(x.new-mu*s2)^2/s2)*lambda[index]/tau[index]
  } else {
    s2 <- 1/(1+0.5*y) #proposal for index
    tau1 <- exp(-0.025*y*s2*(f-sqrt(20*y))^2)
    tau2 <- exp(-0.025*y*s2*(f+sqrt(20*y))^2)
    tau <- tau1+tau2
    tau1 <- tau1/tau
    tau <- tau*lambda
    tau <- tau/sum(tau)
    index <- bal.sample(n.rep,tau) #resampling
    mu <- f[index]
    nu1 <- mu*s2 + (1-s2)*sqrt(20*y)
    nu2 <- mu*s2 - (1-s2)*sqrt(20*y)
    u <- runif(n.rep)<tau1[index]
    x.new <- u*nu1+(1-u)*nu2 + sqrt(10*s2)*rnorm(n.rep) # new particles
    l.new <- exp(-0.5*(y-0.05*x.new^2)^2)/(exp(-0.025*y*(x.new-sqrt(20*y))^2) +
             exp(-0.025*y*(x.new+sqrt(20*y))^2)) #reweighting
  }
  cbind(x.new,l.new/sum(l.new))
}


#ratio of true and approximate observation density
par(mfrow=c(1,2))
y <- 10 # approximation for y > 0.25 with 2 Gaussian components
xx <- seq(-30,30,by=0.05)
b <- exp(-0.5*(y-0.05*xx^2)^2)
q <- exp(-0.025*y*(xx-sqrt(20*y))^2) + exp(-0.025*y*(xx+sqrt(20*y))^2)
plot(xx,b,type="l")
lines(xx,0.5*q,col=2)
plot(xx,2*b/q,type="l")

y <- 0.25 # approximation for y < 0.25 with one Gaussian component
xx <- seq(-30,30,by=0.05)
b <- exp(-0.5*(y-0.05*xx^2)^2)
q <- exp(-0.025*(1-2*y)*xx^2)
plot(xx,b,type="l",ylim=c(0,1))
lines(xx,q,col=2)
plot(xx,b/q,type="l")


# Animation for the farewell lecture
## Not used: we animate in latex-beamer: library("animation")
x <- rnorm(21)
x[1] <- 5*x[1]
for (i in (1:20)) x[i+1] <- f.andrade(x[i],x[i+1],i)
y <- 0.05*x[-1]^2 + rnorm(20)
n.rep <- 200
x.f <- array(NA,dim=c(21,21,n.rep)) # filter paths
x.p <- array(NA,dim=c(21,21,n.rep)) # prediction paths
x.f[1,1,] <- 5*rnorm(n.rep)
w <- matrix(1/n.rep,nrow=20,ncol=n.rep)
for (i in (1:20)) {
    x.p[i+1,,] <- x.f[i,,]
    x.p[i+1,i+1,] <- f.andrade(x.p[i+1,i,],rnorm(n.rep),i) #propagation
    w[i,] <- b.square(x.p[i+1,i+1,],y[i]) #reweighting
    w[i,] <- w[i,]/sum(w[i,])
    index <- bal.sample(n.rep, w[i,]) # resampling
    x.f[i+1,,] <- x.p[i+1,,index]
}



f.one.plot <- function(i, x.f, x.p, x, y.low, y.up) {
    stopifnot(is.numeric(n.rep <- dim(x.f)[3]))
    if (i==1) {
        plot(rep(0,n.rep),x.f[1,1,],xlim=c(0,20),ylim=c(-25,25),
             xlab="Zeit",ylab="Zustand")
    } else {
      plot(0:(i-1),x[1:i],type="l",col=2,xlim=c(0,20),ylim=c(-25,25),
           xlab="Zeit",ylab="Zustand")
      for (j in (1:n.rep)) lines(0:(i-1), x.p[i,1:i, j], col=8)
      for (j in (1:n.rep)) lines(0:(i-1), x.f[i,1:i, j], col=1)
      lines(0:(i-1),x[1:i],col=2,lwd=2)
      segments(i-0.90,-y.up[i-1],i-0.90,-y.low[i-1],col=4,lwd=2)
      segments(i-0.90,y.low[i-1],i-0.90,y.up[i-1],col=4,lwd=2)
  }
}

## create a pdf with evolution:
animate.plot <- function(plotfun, ind=1:10, filename="anim.pdf", ...) {
  #open pdf device:
  pdf(file=filename, width=10, height=5)
  for (ti in ind){
    cat("time: ",ti,"\n")
    plotfun(ti, ...)
  }
  dev.off()
}

animate.plot(f.one.plot, ind=1:21, filename="anim.pdf",
             x.f=x.f, x.p=x.p, x=x, 
             y.low = sqrt(pmax(rep(0,20),20*(y[1:20]-1.96))),
             y.up  = sqrt(20*(y[1:20]+1.96)))