#########################################
#  第15章 粒子フィルタ（TSSSには含まれていない）
#########################################
# テスト的にRで作成したもの（バグ注意）

# Particle Filter
# system model  x(n) = x(n-1) + v(n)
# obs. model    y(n) = x(n) + w(n)

# Parameter setting
m <- 100
tau <- 0.1
sig <- 1.0

#########################################
#  粒子フィルタの1サイクルの例示
#########################################
#
# Initial distribution
xf <- rnorm(m,mean=0,sd=1)

# Prediction step
v  <- rnorm(m,mean=0,sd=tau)
xp <- xf + v

# Bayes factor (weights of particles)
y <- 0.3
alpha <- dnorm(y-xp,mean=0,sd=sig,log=FALSE)
alpha <- alpha/sum(alpha)

# predictive, filter distributions
plot(xp,alpha,type="h")

alpha0 <- rep(1/m,length=m)
plot(xp,alpha0,type="h")
plot(xf,alpha0,type="h")
plot(v,alpha0,type="h",xlim=c(-3,4))

# re-sampling
xf1 <- sample(xp,prob=alpha,replace=T)
plot(xf1,alpha0,type="h",xlim=c(-3,4))

alpha2 <- rep(0,length=m)


##############################################
# Particle Filter（注意：平滑化は行っていない）
##############################################
m <- 10000
tau <- 0.04
sig <- 0.25

# Generation of data
 ## trend model
  x <- rep(0,400)
  x[101:200] <- 1
  x[201:300] <- -1
  y <- x + rnorm(400, mean=0, sd=0.5)


# Initial distribution
xf <- rnorm(m,mean=0,sd=2)
n <- length(y)
trend <- rep(0,length=n)

for (ii in 1:n) {
# Prediction step
#v  <- rnorm(m,mean=0,sd=tau)
v <- rcauchy(m,location=0,scale=0.000035)
xp <- xf + v

# Bayes factor (weights of particles)
alpha <- dnorm(y[ii]-xp,mean=0,sd=sig,log=FALSE)
alpha <- alpha/sum(alpha)

# re-sampling
xf <- sample(xp,prob=alpha,replace=T)

qxf <- quantile(xf)
trend[ii] <- mean(qxf)

}

plot(y,type="l",ylim=c(-3,3))
par(new=T)
plot(trend,type="l",col="red",lwd=2,ylim=c(-3,3))