switch from Tpp to ppT

R/LinKF.R

@@ -23,11 +23,11 @@
 #'
 #' \item{h1}{T by q the estimated conditional expectations of the latent states given t-1 (a priori)}
 #' \item{h2}{T by q the estimated conditional expectations of the latent states given t (a posteriori)}
-#' \item{P1}{T by q by q array of covariances of the a priori states}
-#' \item{P2}{T by q by q array of covariances of the a posteriori states}
+#' \item{P1}{q by q by T array of covariances of the a priori states}
+#' \item{P2}{q by q by T array of covariances of the a posteriori states}
 #' \item{v}{T by p residuals in the measurement eq.}
-#' \item{S}{T by p by p array of covariances of z}
-#' \item{K}{T by q by p array of Kalman gains}
+#' \item{S}{p by p by T array of covariances of z}
+#' \item{K}{q by p by T array of Kalman gains}
 #'
 #' @author Yukai Yang, \email{yukai.yang@@statistik.uu.se}
 #'
@@ -54,14 +54,14 @@ LinKF<-function(mZ,mX,Gam,Bet,mG,h0,P0,mX0)
   if(dim(mX0)[1]!=iu) return(0)
   if(dim(mX0)[2]!=1) return(0)
 
-  h1=array(0,dim=c(iT,iq))# h_t|t-1
-  P1=array(0,dim=c(iT,iq,iq))# P_t|t-1
+  h1=matrix(0,iT,iq)# h_t|t-1
+  P1=array(0,dim=c(iq,iq,iT))# P_t|t-1
 
-  v=array(0,dim=c(iT,ip))# residual in z eq.
-  S=array(0,dim=c(iT,ip,ip))# cov of residual
-  K=array(0,dim=c(iT,iq,ip))# Kalman gain
-  h2=array(0,dim=c(iT,iq))# h_t|t
-  P2=array(0,dim=c(iT,iq,iq))# P_t|t
+  v=matrix(0,iT,ip)# residual in z eq.
+  S=array(0,dim=c(ip,ip,iT))# cov of residual
+  K=array(0,dim=c(iq,ip,iT))# Kalman gain
+  h2=matrix(0,iT,iq)# h_t|t
+  P2=array(0,dim=c(iq,iq,iT))# P_t|t
 
   F=Gam[(ip+1):(ip+iq),]
   B=Bet[(ip+1):(ip+iq),]
@@ -78,24 +78,24 @@ LinKF<-function(mZ,mX,Gam,Bet,mG,h0,P0,mX0)
 
   #Predict
   h1[1,]=F%*%h0+B%*%mX0
-  P1[1,,]=F%*%P0%*%t(F)+QQ
+  P1[,,1]=F%*%P0%*%t(F)+QQ
   #Update
   v[1,]=mZ[1,]-H%*%h1[1,]-A%*%mX[1,]
-  S[1,,]=H%*%P1[1,,]%*%t(H)+RR
-  K[1,,]=P1[1,,]%*%t(H)%*%solve(S[1,,])
-  h2[1,]=h1[1,]+c(K[1,,]%*%v[1,])
-  P2[1,,]=(diag(1,iq)-K[1,,]%*%H)%*%P1[1,,]
+  S[,,1]=H%*%P1[,,1]%*%t(H)+RR
+  K[,,1]=P1[,,1]%*%t(H)%*%chol2inv(chol(S[,,1]))
+  h2[1,]=h1[1,]+c(K[,,1]%*%v[1,])
+  P2[,,1]=(diag(1,iq)-K[,,1]%*%H)%*%P1[,,1]
 
   for(i in 2:iT){
     #Predict
     h1[i,]=F%*%h2[(i-1),]+B%*%mX[(i-1),]
-    P1[i,,]=F%*%P2[(i-1),,]%*%t(F)+QQ
+    P1[,,i]=F%*%P2[,,(i-1)]%*%t(F)+QQ
     #Update
     v[i,]=mZ[i,]-H%*%h1[i,]-A%*%mX[i,]
-    S[i,,]=H%*%P1[i,,]%*%t(H)+RR
-    K[i,,]=P1[i,,]%*%t(H)%*%solve(S[i,,])
-    h2[i,]=h1[i,]+c(K[i,,]%*%v[i,])
-    P2[i,,]=(diag(1,iq)-K[i,,]%*%H)%*%P1[i,,]
+    S[,,i]=H%*%P1[,,i]%*%t(H)+RR
+    K[,,i]=P1[,,i]%*%t(H)%*%chol2inv(chol(S[,,i]))
+    h2[i,]=h1[i,]+c(K[,,i]%*%v[i,])
+    P2[,,i]=(diag(1,iq)-K[,,i]%*%H)%*%P1[,,i]
   }
 
   return(list(h1=h1,h2=h2,P1=P1,P2=P2,v=v,S=S,K=K))