# Bootstrap filter in action!

set.seed(4321)
n = 100
eta = 10
W = 1
nu = 3
V = 2
#eta = 100
#nu = 100

sy = sqrt((nu-2)/nu*V)
sx = sqrt((eta-2)/eta*W)

gx = function(x){sqrt(abs(x))}
hy = function(x){abs(x)}

gx = function(x){x}
hy = function(x){x}

x0 = 0
x = rep(0,n)
x[1] =  gx(x0)+ sx*rt(1,eta)
for (t in 2:n)
  x[t] = gx(x[t-1]) + sx*rt(1,eta)
y = hy(x) + sy*rt(n,nu)

ts.plot(y,ylim=range(y,x))
lines(x,col=2)
x.true = x

# Bayesian sequential learning of states
# via particle filter (bootstrap filter!)
m0 = 0
C0 = 9

# Filtro de particulas
set.seed(1234)
M = 100000
x = rnorm(M,m0,sqrt(C0))
xs = matrix(0,M,n)
for (t in 1:n){
  # propagacao
  x1 = gx(x) + sx*rt(M,eta)
  # resampling
  w = dt((y[t]-hy(x1))/sy,df=nu)/sy
  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)))

par(mfrow=c(1,1))
ts.plot(qx,col=c(3,2,3))
lines(x.true,col=4)

par(mfrow=c(5,5))
for (t in trunc(seq(1,n,length=25))){
hist(xs[,t],main=t)
abline(v=x.true[t],col=2)
}

dlm = function(y,m0,c0,phi,V,W,moments){
  n = length(y)	
  mf = rep(0,n)
  Cf = rep(0,n)
  af = rep(0,n)
  Rf = rep(0,n)
  m = m0
  C = C0
  loglike = 0.0
  for (t in 1:n){
    a = phi*m
    R = phi^2*C+W
    loglike = loglike + dnorm(y[t],a,sqrt(R+V),log=TRUE)
    C = 1/(1/R+1/V)
    m = C*(a/R+y[t]/V)
    af[t] = a
    Rf[t] = R
    Cf[t] = C
    mf[t] = m
  }
  if (moments==1){
    mb = rep(0,n)
    Cb = rep(0,n)
    mb[n] = mf[n]
    Cb[n] = Cf[n]
    for (t in (n-1):1){
      B = phi*Cf[t]/Rf[t+1]
      Cb[t] = Cf[t] - B^2*(Rf[t+1]-Cb[t+1])
      mb[t] = mf[t] + B*(mb[t+1]-af[t+1])
    }
    return(list(loglike=loglike,mf=mf,Cf=Cf,mb=mb,Cb=Cb))
  }else{
    return(loglike)
  }
}

run.exact = dlm(y,m0,c0,1,V,W,1)
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(1,1))
ts.plot(qx,col=c(3,3,3))
lines(limf[,1],col=6)
lines(limf[,2],col=6)
lines(limf[,3],col=6)