###############################################################
#Filtering and prediction

pX.filter<-pX.predict<-pX.smooth<-matrix(0,nrow=n,ncol=S)
pX.predict[1,]<-Pi
pX.filter[1,]<-dnorm(rep(Y[1],S),mu,sig)*Pi
Norm<-sum(pX.filter[1,])
pX.filter[1,]<-pX.filter[1,]/Norm

LogLike<-log(Norm)

Xpred<-Xsmooth<-rep(0,n)
Xpred[1]<-which.max(pX.filter[1,])

for(i in 2:n){
	pX.predict[i,]<- t(t(P) %*% pX.filter[i-1,])
	pX.filter[i,]<-dnorm(rep(Y[i],S),mu,sig)*pX.predict[i,]
	Norm<-sum(pX.filter[i,])
	pX.filter[i,]<-pX.filter[i,]/Norm
	LogLike<-LogLike+log(Norm)
	Xpred[i]<-which.max(pX.filter[i,])
}