### Selected R commands and output used in Chapter 7
## 2-D passive sonar  tracking example
###
# function to simulate the data
###
simPassiveSonar=function(nn=200,q,r,start,s2,seed){
  set.seed(seed)
###  s2 : second sonar location at (s2,0)
  H <- c(1,0,1,0,
      0,1,0,1,
      0,0,1,0,
      0,0,0,1)
  H <- matrix(H,ncol=4,nrow=4,byrow=T)
  G <- c(1,0,0,0,
      0,1,0,0)
  G <- matrix(G,ncol=4,nrow=2,byrow=T)
  W <- c(0.5*q[1], 0,
      0, 0.5*q[2],
      q[1],0,
      0,q[2])
  W <- matrix(W,ncol=2,nrow=4,byrow=T)
  V <- diag(r)
  x <- matrix(nrow=4,ncol=nn)
  y <- matrix(nrow=2,ncol=nn)
  for(ii in 1:nn){
    if(ii == 1) x[,ii] <- start
    if(ii > 1) x[,ii] <- H%*%x[,ii-1]+W%*%rnorm(2)
    y[1,ii] <- atan(x[2,ii]/x[1,ii])
    y[2,ii] <- atan(x[2,ii]/(x[1,ii]-s2))
    y[,ii] <- y[,ii]+V%*%rnorm(2)
    y[,ii] <- (y[,ii]+0.5*pi)%%pi-0.5*pi
  }
  return(list(xx=x,yy=y,G=G,H=H,W=W,V=V))
}
####
### KF-single step ahead prediction
####
KF1pred=function(mu,SS,HH,bb,WW){
   mu <- HH%*%mu+bb
   SS <- HH%*%SS%*%t(HH)+WW%*%t(WW)
   return(list(mu=mu,SS=SS))
}
####
### KF-single step update, no likelihood
####
KF1update=function(mu,SS,yy,GG,cc,VV){
   KK <- SS%*%t(GG)%*%solve(GG%*%SS%*%t(GG)+VV%*%t(VV))
   mu <- mu+KK%*%(yy-cc-GG%*%mu)
   SS <- SS-KK%*%GG%*%SS
   return(list(mu=mu,SS=SS))
}
####
### KF-single step smoother
####
KF1smoother=function(mu,SS,mu1,SS1,muT,SST,HH,VV){
   JJ <- SS%*%HH%*%solve(SS1)
   mu <- mu+JJ%*%(muT-mu1)
   SS <- SS+JJ%*%(SST-SS1)%*%t(JJ)
   return(list(mu=mu,SS=SS))
}
####
### EKF-single step ahead prediction, with input mu_pred
####
EKF1pred=function(mu,SS,HH,bb,WW){
   SS <- HH%*%SS%*%t*t(HH)+WW%*%t(WW)
   return(list(mu=mu,SS=SS))
}
####
### EKF-single step update, with input yy_pred, no likelihood
####
EKF1update=function(mu,SS,yy,yy_pred,GG,VV){
   KK <- SS%*%t(GG)%*%solve(GG%*%SS%*%t(GG)+VV%*%t(VV))
   res <- (yy-yy_pred+0.5*pi)%%pi-0.5*pi
   mu <- mu+KK%*%res
   SS <- SS-KK%*%GG%*%SS
   return(list(mu=mu,SS=SS))
}
##### KF for passive sonar
##-- parameters
s2 <- 20  #second sonar location at (s2,0)
q <- c(0.03,0.03)
r <- c(0.02,0.02)
nn <- 200
##-- simulation
start <- c(10,10,0.01,0.01)
simu_out <- simPassiveSonar(nn,q,r,start,s2,seed=20)
yy <- simu_out$yy
##-- parameter for filtering and smoothing
HH <- c(1,0,1,0,
      0,1,0,1,
      0,0,1,0,
      0,0,0,1)
HH <- matrix(HH,ncol=4,nrow=4,byrow=T)
WW <- c(0.5*q[1], 0,
     0, 0.5*q[2],
     q[1],0,
     0,q[2])
WW <- matrix(WW,ncol=2,nrow=4,byrow=T)
GG <- matrix(rep(0,8),ncol=4,nrow=2)
VV <- diag(r)
##-- setup storage
muall <- matrix(nrow=4,ncol=nn)
mu1all <- matrix(nrow=4,ncol=nn)
muTall <- matrix(nrow=4,ncol=nn)
SSall <- array(dim=c(4,4,nn))
SS1all <- array(dim=c(4,4,nn))
SSTall <- array(dim=c(4,4,nn))
##initial value
mu0 <- c(10,10,0.1,0.1)
SS0 <- diag(c(1,1,1,1))*10000
bb <- 0*mu0
yy_pred <- yy[,1]*0
##start
for(ii in 1:nn){
  if(ii==1) out_pred <- KF1pred(mu0,SS0,HH,bb,WW)
  if(ii > 1) out_pred <- KF1pred(muall[,ii-1],SSall[,,ii-1],HH,bb,WW)
  mu1all[,ii] <- out_pred$mu
  SS1all[,,ii] <- out_pred$SS
  temp1 <- mu1all[2,ii]/mu1all[1,ii]
  temp2 <- mu1all[2,ii]/(mu1all[1,ii]-s2)
  yy_pred[1] <- atan(temp1)
  yy_pred[2] <- atan(temp2)
  GG[1,1] <- -mu1all[2,ii]/(1+temp1**2)/mu1all[1,ii]**2
  GG[1,2] <- 1/(1+temp1**2)/mu1all[1,ii]
  GG[2,1] <- -mu1all[2,ii]/(1+temp2**2)/(mu1all[1,ii]-s2)**2
  GG[2,2] <- 1/(1+temp2**2)/(mu1all[1,ii]-s2)
  out_update <- EKF1update(mu1all[,ii],SS1all[,,ii],yy[,ii],yy_pred,GG,VV)
  muall[,ii] <- out_update$mu
  SSall[,,ii] <- out_update$SS
}
### smoothing
muTall[,nn] <- muall[,nn]
SSTall[,,nn] <- SSall[,,nn]
for(ii in (nn-1):1){
  out_smooth <- KF1smoother(muall[,ii],SSall[,,ii],
                            mu1all[,ii+1],SS1all[,,ii+1],
                            muTall[,ii+1],SSTall[,,ii+1],HH,VV)
  muTall[,ii] <- out_smooth$mu
  SSTall[,,ii] <- out_smooth$SS
}
### Plotting
plot(simu_out$xx[1,],simu_out$xx[2,],type='l',lwd=2,
     xlim=c(0,40),ylim=c(-20,20),xlab="x",ylab="y")
points(c(0,s2),c(0,0),pch=15,cex=2)
title("target trajectory")
#Figure 7.1

plot(simu_out$yy[1,],simu_out$y[2,],type='l',lwd=2,
     xlab="phi 1",ylab="phi 2")
title("observed bearings")
#Figure 7.2

tt <- 100:200
plot(simu_out$xx[1,tt],simu_out$xx[2,tt],xlab="x",ylab="y",
     ylim=c(-18,13),xlim=c(26,34))
title("target trajectory, filtering and smoothing")
lines(muall[1,tt],muall[2,tt])
lines(muTall[1,tt],muTall[2,tt],lty=2)
legend(32,10,legend=c("true","filtered","smoothed"),
      pch=c(1,NA,NA),lty=c(0,1,2))
#Figure 7.3

plot(simu_out$xx[1,]-muall[1,],ylab='error',xlab='time')
lines(simu_out$xx[1,]-muTall[1,])
abline(h=0,lty=2)
title("x-director tracking error")
legend(2,2.3,legend=c("filtering error","smoothing error"),
       pch=c(1,NA),lty=c(0,1))
#Figure 7.4

plot(simu_out$xx[2,]-muall[2,],ylab='error',xlab='time')
lines(simu_out$xx[2,]-muTall[2,])
abline(h=0,lty=2)
title("y-director tracking error")
legend(2,-0.7,legend=c("filtering error","smoothing error"),
      pch=c(1,NA),lty=c(0,1))
#Figure 7.5

plot(simu_out$xx[3,]-muall[3,],ylab='error',xlab='time')
lines(simu_out$xx[3,]-muTall[3,])
abline(h=0,lty=2)
title("x-director speed error")
legend(2,0.27,legend=c("filtering error","smoothing error"),
      pch=c(1,NA),lty=c(0,1))
#Figure 7.6

plot(simu_out$xx[4,]-muall[4,],ylab='error',xlab='time')
lines(simu_out$xx[4,]-muTall[4,])
abline(h=0,lty=2)
title("y-director speed error")
legend(150,0.5,legend=c("filtering error","smoothing error"),
      pch=c(1,NA),lty=c(0,1))
#Figure 7.7

####
> ## SP500 HMM Mixture Gaussian Example
> data <- read.csv(file="SP500.csv",header=T)
> sp <- data[,3]
> tt <- as.Date(data[,1],format="%d-%b-%Y")
> plot(tt,sp,type='l',xlab='Time',ylab='SP500')
> abline(h=0)
> hist(sp,nclass=40,main="")
> ## HMM modeling
> require("HiddenMarkov")
> #-- three-regime --
> ## ------ setting initial values
> Pi <- diag(rep(0.8,3))+matrix(rep(1,9)/3*0.2,ncol=3,nrow=3)
> delta <- rep(1,3)/3
> dist <- 'norm'      # mixture normal
> pm <- list(mean=c(-0.3,0.05,0.2),sd=c(0.1,0.1,0.5))
> temp <- dthmm(x,Pi,delta,dist,pm)   #setting model
> out3 <- BaumWelch(temp,control=bwcontrol(prt=FALSE)) #Baum Welch estimation
> summary(out3)
$delta
[1]  1.222375e-79 6.275890e-243  1.000000e+00
$Pi
             [,1]         [,2]       [,3]
[1,] 6.379404e-01 2.597030e-22 0.36205961
[2,] 7.020327e-08 9.717455e-01 0.02825443
[3,] 6.626644e-02 2.645275e-02 0.90728081
$nonstat
[1] TRUE
$distn
[1] "norm"
$pm
$pm$mean
[1] -0.021637295  0.005144475  0.006810866
$pm$sd
[1] 0.030305953 0.009929002 0.018494987

> # smoothed State  probability
> par(mfrow=c(3,1))
> plot(tt,out3$u[,1],type='l',xlab='time',ylim=c(0,1),ylab='prob',main='State 1 Prob')
> plot(tt,out3$u[,2],type='l',xlab='time',ylim=c(0,1),ylab='prob',main='State 2 Prob')
> plot(tt,out3$u[,3],type='l',xlab='time',ylim=c(0,1),ylab='prob',main='State 3 Prob')
> par(mfrow=c(1,1))

> logLik(out3)    # log likelihood value
[1] 961.0367
> v3 <- Viterbi(out3)   # most likely path: Viterbi
> plot(tt,v3,cex=0.5,xlab='time',ylab='state'); lines(tt,v3) #plot most likely path
> pp <- 1:360; for(i in 1:360)pp[i] <- out3$u[i,v3[i]]
> # obtain smoothed probability of the Viterbi states
> plot(tt,pp,type='l',ylim=c(0,1),xlab='time',ylab='prob')
> abline(h=0.5,lty=2)
> res3 <- residuals(out3)  # get residuals
> pacf(res3**2,main='PACF residual squared')    # pacf residual squared
> pacf(sp**2, main='PACF SP return squared')    # pacf sp return squared
> qqnorm(res3, main='residual')    # qq plot residual
> qqnorm(sp,main='SP return')      # qq plot sp return
> #-- two-regime --
> Pi <- diag(c(0.9,0.9))+matrix(rep(1,4)/20,ncol=2,nrow=2)
> delta <- rep(1,2)/2
> dist <- 'norm'    # mixture normal
> pm <- list(mean=c(-0.1,0.1),sd=c(0.1,0.1))
> temp <- dthmm(sp,Pi,delta,dist,pm,nonstat=FALSE)  #setting model
> out2 <- BaumWelch(temp,control=bwcontrol(prt=FALSE))#Baum Welch estimation
> summary(out2)
$delta
[1] 0.289552 0.710448
$Pi
           [,1]      [,2]
[1,] 0.86500964 0.1349904
[2,] 0.05501701 0.9449830
$nonstat
[1] FALSE
$distn
[1] "norm"
$pm
$pm$mean
[1] -0.003436932  0.006498526
$pm$sd
[1] 0.02793554 0.01243074

> logLik(out2)
[1] 949.0639

> #-- four-regime --
> Pi <- diag(rep(0.8,4))+matrix(rep(1,16)/4*0.2,ncol=4,nrow=4)
> delta <- rep(1,4)/4
> dist <- 'norm'
> pm <- list(mean=c(-0.1,0,0.1,0.3),sd=c(0.1,0.1,0.1,0.1))
> temp <- dthmm(x,Pi,delta,dist,pm,nonstat=FALSE)
> out4 <- BaumWelch(temp,control=bwcontrol(prt=FALSE))
> summary(out4)
$delta
[1] 0.17424141 0.75917705 0.03457448 0.03200705
$Pi
             [,1]         [,2]         [,3]         [,4]
[1,] 7.295873e-01 6.368364e-12 9.077792e-02 1.796348e-01
[2,] 5.497348e-02 9.450265e-01 8.427622e-12 1.581627e-16
[3,] 1.556781e-01 3.115264e-01 5.327956e-01 5.263778e-19
[4,] 4.188804e-09 9.674036e-01 1.050020e-02 2.209623e-02
$nonstat
[1] FALSE
$distn
[1] "norm"
$pm
$pm$mean
[1] -0.017055821  0.005591701  0.027254630  0.037185901
$pm$sd
[1] 0.025168944 0.012357184 0.005992786 0.008122566
> logLik(out4)
[1] 963.0156

####
## FF HMM Mixture Regression Example
xxx <- read.csv(file="FF_example.csv",header=T)
year0 <- 1946:2010
y <- xxx[,2]
xx <- xxx[,3:6]
xx <- as.matrix(xx)
ntotal <- length(y)

## HMM modeling
require("HiddenMarkov")
#-- two-regime --
Pi <- diag(rep(0.95,2))+matrix(rep(1,4)/2*0.05,ncol=2,nrow=2)
delta <- rep(1,2)/2
sd <- c(1,1)*10
beta1 <- c(-1,-1,-1,-1,-1)
beta2 <- c(1,1,1,1,1)
beta <- cbind(beta1,beta2)

glmformula <- formula(~xx)
glmfamily <- gaussian(link="identity")
ones <- rep(1,ntotal)
data <- data.frame(xx=cbind(ones,xx))
Xdesign <- model.matrix(glmformula,data)
MyModel <- mmglm1(y,Pi,delta,glmfamily,beta,Xdesign,sigma=sd)
out <- BaumWelch(MyModel, bwcontrol(posdiff=FALSE, maxiter=200))
summary(out)

tt <- (1:780)/12+1946
par(mfrow=c(1,1),mai=0.9*c(1,1,1,1))
plot(tt,out$u[,1],type='l',xlab='time',ylim=c(0,1),ylab='prob')
title('State 1 Prob')  # smoothed State 1 probability
## Figure 7.14

par(mfrow=c(1,1),mai=c(1,1,1,1))
logLik(out)    # log likelihood value
v2 <- Viterbi(out)   # most likely path: Viterbi
plot(tt,v2,cex=0.5,xlab='time',ylab='state'); lines(tt,v2)
title("Most Likely Path")
#plot most likely path  #Figure 7.15

### compare with time-varying_beta example
ntotal <- length(y)
nyear <- ntotal/12
beta.v <- matrix(ncol=5,nrow=(nyear-2))
for(i in 3:nyear){
  t0 <- (i-3)*12+(1:36)
  y0 <- y[t0]   ##excess return of portfolio 22
  xx0 <- as.matrix(xx[t0,])
  out.v <- lm(y0~xx0)
  beta.v[i-2,] <- out.v$coef
}
year <- 1948:2010
par(mfrow=c(3,2),mai=0.7*c(1,1,1,1))
for(jj in 1:5){
plot(year,beta.v[,1],type='p',xlab='time',ylab='beta',main=name[jj])
pp <- 1:780; for(i in 1:780)pp[i] <- out$beta[1,v2[i]]
lines(tt,pp,lwd=2);
lines(tt,ss2$s[-1,jj])
}
## ss2 from time-varying beta example
## fig:HMM_FF_compare

### --- three regimes
## ------ setting initial values
Pi <- diag(rep(0.8,3))+matrix(rep(1,9)/3*0.2,ncol=3,nrow=3)
delta <- rep(1,3)/3
sd <- c(1,1,1)*10
beta1 <- c(-1,-1,-1,-1,-1)
beta2 <- c(1,-1,-1,1,1)
beta3 <- c(1,1,1,1,1)
beta <- cbind(beta1,beta2,beta3)

glmformula <- formula(~xx)
glmfamily <- gaussian(link="identity")
Xdesign <- model.matrix(glmformula,data)
MyModel <- mmglm1(y,Pi,delta,glmfamily,beta,Xdesign,sigma=sd)
out3 <- BaumWelch(MyModel, bwcontrol(posdiff=FALSE, maxiter=200))
summary(out)

par(mfrow=c(3,1),mai=0.5*c(1,1,1,1))
plot(tt,out3$u[,1],type='l',xlab='time',ylim=c(0,1),ylab='prob',
      main='State 1 Prob')
   # smoothed State 1 probability
plot(tt,out3$u[,2],type='l',xlab='time',ylim=c(0,1),ylab='prob',
       main='State 2 Prob')
   # smoothed State 2 probability
plot(tt,out3$u[,3],type='l',xlab='time',ylim=c(0,1),ylab='prob',
       main='State 3 Prob')
    # smoothed State 3 probability
## Figure 7.17

####