#########################################################################
## 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")
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

#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")

#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")
lines(0:100,x.filtq[2,],col=4)
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)
}

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)
}

  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")