set.seed(1355)
sig2 = 1.0 # observational error variance
tau2 = 0.5 # evolutional error variance
sig = sqrt(sig2)
tau = sqrt(tau2)
n = 100
x = rep(0,n)
y = rep(0,n)

x0 = 0.0
x[1] = x0 + rnorm(1,0,tau)
y[1] = x[1] + rnorm(1,0,sig)

for (t in 2:n){
  x[t] = x[t-1] + rnorm(1,0,tau)
  if(t==50){
    x[t] = 15 # A multiplicative outlier at time 50
  }
  y[t] = x[t] + rnorm(1,0,sig)
}
# y[50]=15 An additive outlier at time 50

ts.plot(x,ylim=range(x,y))
points(y,pch=16)

# CLOSE FORM SOLUTION
# Online fitting 1st order DLM
# -----------------------------
  a1 = 0
  R1 = 2+tau2
  m  = rep(0,n)
  C  = rep(0,n)
  B  = rep(0,n-1)
  H  = rep(0,n-1)
  # time t=1
  a = a1
  R = R1
  f = a
  Q = R+sig2
  A = R/Q
  m[1] = a+A*(y[1]-f)
  C[1] = R-Q*A^2
  # forward filtering
  for (t in 2:n){
    a = m[t-1]
    R = C[t-1] + tau2
    f = a
    Q = R+sig2
    A = R/Q
    m[t] = a+A*(y[t]-f)
    C[t] = R-Q*A^2
}
Lx = m + qnorm(0.025)*sqrt(C)
Mx = m
Ux = m + qnorm(0.975)*sqrt(C)

par(mfrow=c(1,1))
ts.plot(cbind(Lx,Mx,Ux),col=c(2,2,2),xlab="time")

# OUR FIRST PARTICLE FILTER
N = 100000
# Posterior at time 0
x = rnorm(N,0,2)
xs = matrix(0,N,n)
ws = matrix(0,N,n)
Neff = rep(0,n)
for (t in 1:n){
  # Propagate
  x1 = x + rnorm(N,0,tau)
  
  # Compute the weights
  w = dnorm(y[t],x1,sig)
  w = w/sum(w)

  # Resampling
  k = sample(1:N,size=N,replace=TRUE,prob=w)
  x = x1[k]

  # Storing the particles and weights
  xs[,t] = x
  ws[,t] = w
  Neff[t] = 1/sum(w^2)
}

par(mfrow=c(1,1))
ts.plot(Neff,ylim=c(0,N))

ts = trunc(seq(1,n,length=9))
par(mfrow=c(3,3))
for (t in ts){
  hist(ws[,t],main=paste("time=",t,sep=""))
}

par(mfrow=c(3,3))
for (t in ts){
  hist(xs[,t],main=paste("time=",t,sep=""))
}


mx = apply(xs,2,median)
lx = apply(xs,2,quantile,0.025)
ux = apply(xs,2,quantile,0.975)


ind = 40:60
par(mfrow=c(1,1))
ts.plot(cbind(lx,mx,ux)[ind,],col=c(2,2,2),xlab="time")
lines(Lx[ind],col=4)
lines(Mx[ind],col=4)
lines(Ux[ind],col=4)