diff --git a/ChangeLog b/ChangeLog
index 641d9eb..7bdd09b 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,10 +1,15 @@
 Changes from Version 1.0.3 to 1.0.4:
- * Corrected output of LogLik method for non-Gaussian models: It now returns Inf only when the 
-   approximation algoritm failed  completely (resulting NA), and issues only warning about 
+ * Tweaked the underlying algorithms for increased numerical stability of filtering and smoothing 
+   in KFS. Note that it is still possible that exact diffuse initialization fails due to to numerical
+   issues whereas traditional 'big value' approach works and vice versa.
+ * Corrected a bug in residuals.KFS which threw an error when computing recursive residuals without 
+   diffuse initialization.
+ * Corrected output of LogLik method for non-Gaussian models: It now returns -Inf only when the 
+   approximation algoritm failedcompletely (resulting NA), and issues only warning about 
    non-convergence in other cases.
  * Added checks of degenerate model to LogLik method. If all elements in R, Q and H/u are zero, or 
    they contain any non-finite values, -Inf is returned.
- * Corrected a bug in approximation algorithm which caused the approximation to fail for seemingly 
+ * Fixed a bug in approximation algorithm which caused the approximation to fail for seemingly 
    random models.
  * Fixed a bug in SSMcycle which caused error with common components.
  * Fixed bug in SSMcycle which resulted erroneus system matrix T in all cases.
diff --git a/DESCRIPTION b/DESCRIPTION
index dc93d47..a2ba4ca 100644
--- a/DESCRIPTION
+++ b/DESCRIPTION
@@ -7,7 +7,7 @@ Maintainer: Jouni Helske <jouni.helske@jyu.fi>
 Depends:
     R (>= 3.0.0)
 Imports: stats
-Suggests: MASS, minqa, testthat
+Suggests: MASS, nloptr, testthat
 Description: Package KFAS provides functions for Kalman filtering, smoothing,
     forecasting and simulation of multivariate exponential family state space
     models with exact diffuse initialization when distributions of some or all
diff --git a/MD5 b/MD5
deleted file mode 100644
index 48d7eb7..0000000
--- a/MD5
+++ /dev/null
@@ -1,89 +0,0 @@
-08bf252fdc90b79477b9189caae2b157 *ChangeLog
-292e4d3f6ca1090b5f0b0386c67d5c20 *DESCRIPTION
-b92b3219eecf671b607ea371c770ee8d *NAMESPACE
-738da679548b96a343cb70b81334d441 *R/KFAS-package.R
-fa2b9a61f5955d93fd73e31737d963f1 *R/KFS.R
-001b7a4afa315c25d2ec7e651406ef82 *R/SSMarima.R
-bb2ef6957657f5ec6cd0596c606a911d *R/SSMcustom.R
-a024b08ed9dbc9c1476c8dfe373edf24 *R/SSMcycle.R
-838710f108f59d85adda8ef93e3cbc1f *R/SSModel.R
-908bb910406eef815b6c0384b9c75d7b *R/SSMregression.R
-f3105c0466c5cc31d3c3ee82a7ef90d1 *R/SSMseasonal.R
-ee0efaa83a64a6072f04feca6b5a16cb *R/SSMtrend.R
-8ca88a042647abbefd4f5538bbaaf367 *R/approxSSM.R
-f085733aded2e30a35df976536705efc *R/artransform.R
-7c5bdf8189be6fb45cbed6bebfb54f6b *R/checkModel.R
-4f0f1708b72d79d34ac5521d7d333a43 *R/coef.KFS.R
-1953ea8493cb6c07ab0ada06bec02eb3 *R/deviance.KFS.R
-35a0a601059f5b895a58d464c12c6cc1 *R/extract.SSModel.R
-7c65ef48666563368dba0cedc3c4f000 *R/fitSSM.R
-95ecd5fbc9d5dd704b05c1f8e6bcc2c9 *R/fitted.KFS.R
-31dfa1359a58917d14d0ee251b33ab0b *R/hatvalues.KFS.R
-bb3ba6c97df6619f0207026e263fbe5a *R/importanceSSM.R
-5e9c140939d57564d1742c3f1d62d88b *R/interval.R
-7c0ef66c107881b96b9b7fc55ddfd09e *R/ldl.R
-6ee1b367b52733acbe5646362ed7b99d *R/logLik.SSModel.R
-835132f7d7045eaae00075a7315786ec *R/predict.SSModel.R
-9b46c4d041cf98ba30a69bf312b58b9d *R/print.objects.R
-0f81e7a78d1ece402704a6d8c0a60ecb *R/residuals.KFS.R
-a0b7c3f26b1a2df8ae2de5613c2cb877 *R/rstandard.KFS.R
-8d1dfc3a8b2694f1984280fff08163c2 *R/signals.R
-feef168d7cb90ee137eba730f7f7123a *R/simulateSSM.R
-03ce71cc99370966a3c48b38c867c869 *R/subset.SSModel.R
-17f408856125c7e270ebe1380ba0ba48 *R/transformSSM.R
-85993fe9d3165948f23ee4d023dd8e78 *data/GlobalTemp.rda
-30f3f80fe83502869ff93a6662c56c5e *data/boat.rda
-cae765cd5662c7c2ed49ce79fc54114c *data/sexratio.rda
-bf7d9d6c33a41e29803f7fd0a04fc092 *man/Extract.SSModel.Rd
-003466ac2fbcb59f7016143d7eb01332 *man/GlobalTemp.Rd
-b5585f6af1d3aaef93a36c694a0fdbc5 *man/KFAS.Rd
-49a8c332388cf011a3e7ba3c6ddd668f *man/KFS.Rd
-019832b7a265e2a634464864fc8188ee *man/SSModel.Rd
-0ceb1cf2f9e61774524378c796faa9a0 *man/approxSSM.Rd
-46e7faecfb9908b838bd332f0b876196 *man/artransform.Rd
-4687ee686c817e1cf62153adee7e700b *man/boat.Rd
-4148ce0c99230395c17e0123244966ea *man/checkModel.Rd
-6dc8ba19fa2b07ff107457e555bd7858 *man/coef.KFS.Rd
-88befadfd96d84fc72b901b842b31f70 *man/deviance.KFS.Rd
-968ac00e1b493c147024615814919921 *man/fitSSM.Rd
-81efaf5fc440d6583be2df01a919c4b0 *man/fitted.KFS.Rd
-c85653a0591d43a20489038889fdc43a *man/hatvalues.KFS.Rd
-9601464df3045944a9c9d9e27e630781 *man/importanceSSM.Rd
-89ae827d4ee2c6c80899987b665897f5 *man/ldl.Rd
-dc6216850cb0fe556ad26fd0b310eae1 *man/logLik.SSModel.Rd
-9a4c2b1fd7f41f9a5ad853fd099882f5 *man/predict.SSModel.Rd
-12a30667eea81e8d7ac790958a46c57f *man/print.KFS.Rd
-3988a453fdc228e77c25bd948eb125a4 *man/print.SSModel.Rd
-57a1c67660f295e71c69f3a21290ba1b *man/residuals.KFS.Rd
-6d6814524f9817e12d40c2b162f3e862 *man/rstandard.KFS.Rd
-c8d613ecf373d20127d7813f280f6531 *man/sexratio.Rd
-c88054748cfa92534660256adb741d0d *man/signal.Rd
-edbe1424aca064f5674fa0c66bb40dc3 *man/simulateSSM.Rd
-41a4708432180dbca1c4236e8bcebd67 *man/transformSSM.Rd
-415cb7ed6d87a75c5794917a85e3af96 *src/Makevars
-dd73fc60daf1f8f74c5a364dd8baa044 *src/approx.f95
-de1d3788c2169ce2160a139d29fd222e *src/artransform.f95
-d6ac9a5fddf72c13336874e7d1522147 *src/covmeanw.f95
-4d93d8405a089000466b1334b2eb8347 *src/declarations.h
-eefeb5bc1290d902a55afd8d5512a3b2 *src/filtersimfast.f95
-b45e2b505424a0b25da90172a6923479 *src/gloglik.f95
-c1be0e18f86b63437d3572b83cf26af3 *src/glogliku.f95
-df61eb8c17a8e494cd3f295b88e3936e *src/gsmoothall.f95
-e51a8a6be2ec350f6e93b9553d058ce6 *src/init.c
-21ca4b2ab321641ed60b1011a317013a *src/isample.f95
-5a02652b8df2bad650d0a403794be5d3 *src/isamplefilter.f95
-35c4cb2405fd0d417f04c644d7fc3b01 *src/kfilter.f95
-cf50e4445a1e5ed69b0f0ac2f60dc49c *src/kfstheta.f95
-17c7ded80bcf6bf7a247e89c08b78287 *src/ldl.f95
-8f7f9b0248c734c6d43ecd6eee111dcc *src/ldlssm.f95
-ef58b1cd6e3455bdc8690f2bbdcf6024 *src/ngfilter.f95
-4dac6bb83bdca8cb507f01d61640170c *src/ngloglik.f95
-ac239290951fda3006b1f9be42cde694 *src/ngsmooth.f95
-f0968eda670f7a75427d79cfb21e62be *src/predict.f95
-8549681a6da98dfe2af49a74fd52a4f7 *src/simfilter.f95
-36c0edff2e9e311c2aa5a019bc132e2f *src/simgaussian.f95
-39fb98b7fb86f10a33977d3d10921de5 *src/simgaussiandebug.f95
-780bb86447aa596a8199ac1bc2187c2e *src/smoothsim.f95
-6d00a9c67c54a4aacfeef3fcefb98242 *src/smoothsimfast.f95
-648a5bd292765d836f32ef3a371a4c1c *tests/test-all.R
-0ba139ba905490ab75df795669826188 *tests/testthat/glmfits.R
diff --git a/R/KFAS-package.R b/R/KFAS-package.R
index c8d7d3b..6066b91 100644
--- a/R/KFAS-package.R
+++ b/R/KFAS-package.R
@@ -188,7 +188,7 @@
 #'                  data=Seatbelts,H=matrix(NA,2,2)) 
 #' 
 #' likfn<-function(pars,model,estimate=TRUE){
-#'   model$H[,,1]<-exp(0.5*pars[1:2])
+#'   diag(model$H[,,1])<-exp(0.5*pars[1:2])
 #'   model$H[1,2,1]<-model$H[2,1,1]<-tanh(pars[3])*prod(sqrt(exp(0.5*pars[1:2]))) 
 #'   model$R[28:29]<-exp(pars[4:5])
 #'   if(estimate) return(-logLik(model))
diff --git a/R/KFS.R b/R/KFS.R
index f9a085c..6c7608f 100644
--- a/R/KFS.R
+++ b/R/KFS.R
@@ -110,7 +110,7 @@ KFS <- function(model, filtering, smoothing, simplify = TRUE, transform = c("ldl
   
   if(missing(filtering)){
     if(all(model$distribution == "gaussian")){
-    filtering <- "state"
+      filtering <- "state"
     } else filtering <- "none"
   } else{
     filtering <- match.arg(arg = filtering, 
@@ -336,6 +336,7 @@ KFS <- function(model, filtering, smoothing, simplify = TRUE, transform = c("ldl
   } else KFS_transform <- "none"
   filtersignal<-("signal"%in%filtering) || ("mean"%in%filtering)
   storage.mode(ymiss) <- "integer"
+
   filterout <- .Fortran(fkfilter, NAOK = TRUE, model$y, ymiss, as.integer(tv), model$Z, model$H, model$T, 
                         model$R, model$Q, model$a1, P1 = model$P1, model$P1inf, as.integer(p), 
                         as.integer(n), as.integer(m),as.integer(k), d = integer(1), j = integer(1), 
@@ -350,7 +351,7 @@ KFS <- function(model, filtering, smoothing, simplify = TRUE, transform = c("ldl
   
   if (filterout$d == n & filterout$j == p) 
     warning("Model is degenerate, diffuse phase did not end.")
-  if (filterout$d > 0 & m > 1 & !isTRUE(all.equal(min(apply(filterout$Pinf, 3, diag)), 0))) 
+  if (filterout$d > 0 & m > 1 & min(apply(filterout$Pinf, 3, diag))<0) 
     warning("Possible error in diffuse filtering: Negative variances in Pinf, 
             try changing the tolerance parameter tol of the model.")
   if (sum(filterout$Finf > 0) != sum(diag(model$P1inf))) 
@@ -434,6 +435,7 @@ KFS <- function(model, filtering, smoothing, simplify = TRUE, transform = c("ldl
   }
   
   if (!("none"%in%smoothing)) {
+    
     smoothout <- .Fortran(fgsmoothall, NAOK = TRUE, ymiss, as.integer(tv), model$Z, model$H, 
                           model$T, model$R, model$Q, as.integer(p), as.integer(n), as.integer(m), 
                           as.integer(k), filterout$d, filterout$j, filterout$a, filterout$P, 
@@ -449,13 +451,14 @@ KFS <- function(model, filtering, smoothing, simplify = TRUE, transform = c("ldl
                           epshat = array(0, dim = c(p, n)), V_eps = array(0, dim = c(p, n)), 
                           etahat = array(0, dim = c(k, n)), V_eta = array(0, dim = c(k, k, n)), 
                           thetahat = array(0, dim = c(p, n)), V_theta = array(0, dim = c(p, p, n)), 
-                          as.integer(KFS_transform=="ldl" && ("signal" %in% smoothing || "mean" %in% smoothing)), 
-{if (KFS_transform=="ldl" && ("signal" %in% smoothing || "mean" %in% smoothing)) out$model$Z else double(1)}, 
+                          as.integer(KFS_transform=="ldl" && ("signal" %in% smoothing || "mean" %in% smoothing)), {if (KFS_transform=="ldl" && ("signal" %in% smoothing || "mean" %in% smoothing)) out$model$Z else double(1)}, 
                           as.integer(dim(out$model$Z)[3]>1), as.integer(KFS_transform != "augment"), 
                           as.integer("state" %in% smoothing), as.integer("disturbance" %in% smoothing), 
                           as.integer(("signal" %in% smoothing || "mean" %in% smoothing)))
     
-    
+    if (m > 1 & min(apply(smoothout$V, 3, diag))<0) 
+      warning("Possible error in smoothing: Negative variances in V, 
+            try changing the tolerance parameter tol of the model.")
     
     if ("state" %in% smoothing) {
       out$alphahat <- ts(t(smoothout$alphahat),start=start(model$y),frequency=frequency(model$y))
diff --git a/R/SSMseasonal.R b/R/SSMseasonal.R
index 7954eb4..f0d09ad 100644
--- a/R/SSMseasonal.R
+++ b/R/SSMseasonal.R
@@ -17,9 +17,9 @@ SSMseasonal <- function(period, sea.type = c("dummy", "trigonometric"), type, Q,
   }
   sea.type <- match.arg(arg = sea.type)
   
-  if (!(length(period) == 1 & period > 1 & abs(period - round(period)) == 0)) 
-    stop("Period of the seasonal component must be integer larger than 1.")
-  
+  if (!(length(period) == 1 & period > 1)) 
+    stop("Period of the seasonal component must be larger than 1.")
+  period <- floor(period)
   m1 <- period - 1
   
   Z_univariate <- matrix(0, 1, m1)
diff --git a/R/logLik.SSModel.R b/R/logLik.SSModel.R
index 00e5350..6f495a1 100644
--- a/R/logLik.SSModel.R
+++ b/R/logLik.SSModel.R
@@ -140,7 +140,7 @@ logLik.SSModel <- function(object, nsim = 0, antithetics = TRUE, theta, check.mo
                     convtol, as.integer(nnd), as.integer(nsim), epsplus, etaplus, aplus1, c2, object$tol, info = integer(1), as.integer(antithetics), 
                     as.integer(sim), nsim2, as.integer(nd), as.integer(length(nd)),diff=double(1))
     if (!is.finite(out$diff)){
-      warning("Non-finite difference in approximation algoritm. Returning -Inf.")
+      warning("Non-finite difference in approximation algoritm.")
       return(-.Machine$double.xmax^0.75)
     }
     if(out$maxiter==maxiter){
diff --git a/R/residuals.KFS.R b/R/residuals.KFS.R
index 5118eca..64a3ae2 100644
--- a/R/residuals.KFS.R
+++ b/R/residuals.KFS.R
@@ -47,8 +47,10 @@ residuals.KFS <- function(object, type = c("recursive","deviance", "pearson", "r
              if (is.null(object[["a",exact=TRUE]])) 
                stop("KFS object needs to contain filtered estimates of states. ")
              series <- object$v
+             if(object$d>0){
              series[1:(object$d - 1), ] <- NA
              series[object$d, 1:object$j] <- NA
+             }
              series
            },
            response = {
diff --git a/man/predict.SSModel.Rd b/man/predict.SSModel.Rd
index d052275..0a329e3 100644
--- a/man/predict.SSModel.Rd
+++ b/man/predict.SSModel.Rd
@@ -3,8 +3,8 @@
 \alias{predict.SSModel}
 \title{State Space Model Predictions}
 \usage{
-\method{predict}{SSModel}(object, newdata, interval = c("none", "confidence",
-  "prediction"), level = 0.95, type = c("response", "link"),
+\method{predict}{SSModel}(object, newdata, n.ahead, interval = c("none",
+  "confidence", "prediction"), level = 0.95, type = c("response", "link"),
   states = NULL, se.fit = FALSE, nsim = 0, prob = TRUE, maxiter = 50,
   ...)
 }
@@ -13,7 +13,15 @@
 
   \item{newdata}{A compatible \code{SSModel} object to be
   added in the end of the old object for which the
-  predictions are required.}
+  predictions are required. If omitted, predictions are
+  either for the whole data (fitted values), or if argument
+  \code{n.ahead} is given, \code{n.ahead} time steps
+  ahead.}
+
+  \item{n.ahead}{Number of steps ahead at which to predict.
+  Only used if \code{newdata} is omitted. Note that when
+  using \code{n.ahead}, object cannot contain time varying
+  system matrices.}
 
   \item{interval}{Type of interval calculation.}
 
@@ -70,4 +78,20 @@ distribution \eqn{p(y|\theta^i)}.
 If no simulations are used, the standard errors in response
 scale are computed using delta method.
 }
+\examples{
+\dontrun{
+set.seed(1)
+x<-runif(n=100,min=1,max=3)
+y<-rpois(n=100,lambda=exp(-1+x))
+model<-SSModel(y~x,distribution="poisson")
+xnew<-seq(0.5,3.5,by=0.1)
+newdata<-SSModel(rep(NA,length(xnew))~xnew,distribution="poisson")
+pred<-predict(model,newdata=newdata,interval="prediction",level=0.9,nsim=1000)
+plot(x=x,y=y,pch=19,ylim=c(0,25),xlim=c(0.5,3.5))
+matlines(x=xnew,y=pred,col=c(2,2,2),lty=c(1,2,2),type="l")
+
+model<-SSModel(Nile~SSMtrend(1,Q=1469),H=15099)
+pred<-predict(model,n.ahead=10,interval="prediction",level=0.9)
+}
+}
 
diff --git a/src/approx.f95 b/src/approx.f95
index 1d94966..b28e7b6 100644
--- a/src/approx.f95
+++ b/src/approx.f95
@@ -34,7 +34,8 @@ subroutine approx(yt, ymiss, timevar, zt, tt, rtv, ht, qt, a1, p1,p1inf, p,n,m,r
     !compute rqr
     tv = max(timevar(4),timevar(5))
     do i=1, (n-1)*tv+1
-        call dsymm('r','u',m,r,1.0d0,qt(:,:,(i-1)*timevar(5)+1),r,rtv(:,:,(i-1)*timevar(4)+1),m,0.0d0,mr,m)
+        call dgemm('n','n',m,r,r,1.0d0,rtv(:,:,(i-1)*timevar(4)+1),m,&
+        qt(:,:,(i-1)*timevar(5)+1),r,0.0d0,mr,m)
         call dgemm('n','t',m,m,r,1.0d0,mr,m,rtv(:,:,(i-1)*timevar(4)+1),m,0.0d0,rqr(:,:,i),m)
     end do
 
@@ -68,18 +69,13 @@ subroutine approx(yt, ymiss, timevar, zt, tt, rtv, ht, qt, a1, p1,p1inf, p,n,m,r
             case(4)
                 do i=1,n
                     if(ymiss(i,j).EQ.0) then
-                        !ht(j,j,i) = exp(theta(i,j))/(u(i,j)*yt(i,j))
-                        !ytilde(i,j) = theta(i,j)+1.0d0-exp(theta(i,j))/yt(i,j)
-                        ht(j,j,i) =1.0d0/u(i,j) !1.0d0
+                        ht(j,j,i) =1.0d0/u(i,j)
                         ytilde(i,j) = theta(i,j)+yt(i,j)/exp(theta(i,j))-1.0d0
                     end if
                 end do
             case(5)
                 do i=1,n
                     if(ymiss(i,j).EQ.0) then
-                        !ht(j,j,i) = (exp(theta(i,j))+u(i,j))**2/(u(i,j)*exp(theta(i,j))*(yt(i,j)+u(i,j)))
-                        !ytilde(i,j) = theta(i,j) + ht(j,j,i)*u(i,j)*(yt(i,j)-exp(theta(i,j)))/(u(i,j)+exp(theta(i,j)))
-                        !ht(j,j,i) = u(i,j)*exp(theta(i,j))/(u(i,j)+exp(theta(i,j)))
                         ht(j,j,i) = (1.0d0/u(i,j)+1.0d0/exp(theta(i,j)))
                         ytilde(i,j) = theta(i,j)+yt(i,j)/exp(theta(i,j))-1.0d0
                     end if
@@ -89,14 +85,12 @@ subroutine approx(yt, ymiss, timevar, zt, tt, rtv, ht, qt, a1, p1,p1inf, p,n,m,r
 
 
     muhat = theta
-        call mu(dist,u,n,p,muhat)
-        call deviance(yt,muhat,u,ymiss,n,p,dist,devold)
+    call mu(dist,u,n,p,muhat)
+    call deviance(yt,muhat,u,ymiss,n,p,dist,devold)
 
     do while(diff > convtol .AND. k < maxiter)
 
         k=k+1
-
-
         rankp2 = rankp
 
         call kfstheta(ytilde, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, a1, p1, p1inf, &
@@ -121,18 +115,12 @@ subroutine approx(yt, ymiss, timevar, zt, tt, rtv, ht, qt, a1, p1,p1inf, p,n,m,r
                 case(4)
                     do i=1,n
                         if(ymiss(i,j).EQ.0) then
-                               ! ht(j,j,i) = exp(theta(i,j))/(u(i,j)*yt(i,j))
-                               ! ytilde(i,j) = theta(i,j)+1.0d0-exp(theta(i,j))/yt(i,j)
-                               !ht(j,j,i) = 1.0d0
                             ytilde(i,j) = theta(i,j)+yt(i,j)/exp(theta(i,j))-1.0d0
                         end if
                     end do
                 case(5)
                     do i=1,n
                         if(ymiss(i,j).EQ.0) then
-                                !ht(j,j,i) = (exp(theta(i,j))+u(i,j))**2/(u(i,j)*exp(theta(i,j))*(yt(i,j)+u(i,j)))
-                                !ytilde(i,j) = theta(i,j) + ht(j,j,i)*u(i,j)*(yt(i,j)-exp(theta(i,j)))/(u(i,j)+exp(theta(i,j)))
-                            !ht(j,j,i) = u(i,j)*exp(theta(i,j))/(u(i,j)+exp(theta(i,j)))
                             ht(j,j,i) = (1.0d0/u(i,j)+1.0d0/exp(theta(i,j)))
                             ytilde(i,j) = theta(i,j)+yt(i,j)/exp(theta(i,j))-1.0d0
                         end if
diff --git a/src/gsmoothall.f95 b/src/gsmoothall.f95
index 1974189..2954c2a 100644
--- a/src/gsmoothall.f95
+++ b/src/gsmoothall.f95
@@ -38,11 +38,13 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
 
     double precision, intent(in), dimension(ldlsignal*p,ldlsignal*m,ldlsignal*((n-1)*zorigtv+1)) :: zorig
 
-    double precision, dimension(m,m) :: linf,lt,l0
+    double precision, dimension(m,m) :: linf,l0
     double precision, dimension(m,m) :: nrec,nrec1,nrec2,im,mm,mm2
     double precision, dimension(m) :: rrec,rrec1,rhelp, help
     double precision, dimension(m,r) :: mr, mr2
     double precision, dimension(p,m) :: pm
+    double precision, dimension(p,n) ::  ftinv
+    double precision, dimension(p,d) ::  finfinv
     double precision, external :: ddot
 
     if(aug.EQ.1 .AND. dist.EQ.1) then
@@ -53,6 +55,10 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
         end do
     end if
 
+
+    ftinv = 1.0d0/ft
+    finfinv = 1.0d0/finf
+
     im = 0.0d0
     do i = 1, m
         im(i,i) = 1.0d0
@@ -72,30 +78,37 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
                 if(ft(i,t) .GT. 0.0d0) then
                     if(aug.EQ.1 .AND. dist.EQ.1) then
                         epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)/ft(i,t)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))
-                        call dsymv('u',m,1.0d0/ft(i,t)**2, nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
+                        call dgemv('n',m,m,ftinv(i,t)**2, nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
                         epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
-                        (1.0d0/ft(i,t)+ddot(m,kt(:,i,t),1,rhelp,1))
+                        (ftinv(i,t)+ddot(m,kt(:,i,t),1,rhelp,1))
                     end if
-                    lt = im
-                    call dger(m,m,-1.0d0/ft(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,lt,m) !l = I -kz !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-                    call dgemv('t',m,m,1.0d0,lt,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)/ft(i,t)*zt(i,:,(t-1)*timevar(1)+1)
-                    call dsymm('l','u',m,m,1.0d0,nrec,m,lt,m,0.0d0,mm,m) !n*l
-                    call dgemm('t','n',m,m,m,1.0d0,lt,m,mm,m,0.0d0,nrec,m) !n = l'nl
-                    call dger(m,m,(1.0d0/ft(i,t)),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec,m) ! n = n+z'z/f
+
+                    rhelp = -zt(i,:,(t-1)*timevar(1)+1)*ftinv(i,t)
+                    l0 = im
+                    call dgemm('n','n',m,m,1,1.0d0,kt(:,i,t),&
+                    m,rhelp,1,1.0d0,l0,m)
+
+
+                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                    rrec = rhelp + vt(i,t)*ftinv(i,t)*zt(i,:,(t-1)*timevar(1)+1)
+                    call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !n = l'nl
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,nrec,m)
+                    call dgemm('t','n',m,m,1,1.0d0,zt(i,:,(t-1)*timevar(1)+1),&
+                    1,zt(i,:,(t-1)*timevar(1)+1),1,0.0d0,mm,m)
+                    nrec = nrec+mm*ftinv(i,t)
 
                 end if
             end if
         end do
 
-        call dcopy(m,rrec,1,rt(:,t),1) !r_t-1 = r_t,0
+        rt(:,t) =rrec
         nt(:,:,t) = nrec !n_t-1 = n_t,0
 
         if(t.GT.1) then
             call dgemv('t',m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1) !r_t,p=t_t-1'*r_t+1
             rrec = rhelp
-            call dsymm('l','u',m,m,1.0d0,nrec,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,mm,m) !n*t
-            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,mm,m,0.0d0,nrec,m) !n_t,p = t'nt
+            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec,m,0.0d0,mm,m) !n*t
+            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec,m) !n_t,p = t'nt
         end if
 
     end do
@@ -106,24 +119,29 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
         rt0(:,d+1)=rt(:,d+1)
         nt0(:,:,d+1) =  nt(:,:,d+1)
 
-
-
         do i = p, (j+1) , -1
             if(ymiss(t,i).EQ.0) then
                 if(ft(i,t) .GT. 0.0d0) then
                     if(aug .EQ. 1 .AND. dist.EQ.1) then
                         epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)/ft(i,t)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))
-                        call dsymv('u',m,1.0d0/ft(i,t)**2,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
+                        call dgemv('n',m,m,ftinv(i,t)**2,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
                         epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
-                        (1.0d0/ft(i,t)+ddot(m,kt(:,i,t),1,rhelp,1))
+                        (ftinv(i,t)+ddot(m,kt(:,i,t),1,rhelp,1))
                     end if
-                    lt = im
-                    call dger(m,m,-1.0d0/ft(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,lt,m)
-                    call dgemv('t',m,m,1.0d0,lt,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)/ft(i,t)*zt(i,:,(t-1)*timevar(1)+1)
-                    call dgemm('n','n',m,m,m,1.0d0,nrec,m,lt,m,0.0d0,mm,m) !n*l
-                    call dgemm('t','n',m,m,m,1.0d0,lt,m,mm,m,0.0d0,nrec,m) !n = l'nl
-                    call dger(m,m,(1.0d0)/ft(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec,m) ! n = n+z'z/f
+
+                    rhelp = -zt(i,:,(t-1)*timevar(1)+1)*ftinv(i,t)
+                    l0 = im
+                    call dgemm('n','n',m,m,1,1.0d0,kt(:,i,t),&
+                    m,rhelp,1,1.0d0,l0,m)
+
+
+                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                    rrec = rhelp + vt(i,t)*ftinv(i,t)*zt(i,:,(t-1)*timevar(1)+1)
+                    call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !n = l'nl
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,nrec,m)
+                    call dgemm('t','n',m,m,1,1.0d0,zt(i,:,(t-1)*timevar(1)+1),&
+                    1,zt(i,:,(t-1)*timevar(1)+1),1,0.0d0,mm,m)
+                    nrec = mm*ftinv(i,t)+nrec
                 end if
             end if
         end do
@@ -137,66 +155,72 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
                 if(finf(i,t).GT.tol) then
                     if(aug .EQ. 1 .AND. dist.EQ.1) then
                         epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)/finf(i,t)
-                        call dsymv('u',m,1.0d0,nrec,m,kinf(:,i,t),1,0.0d0,rhelp,1)
+                        call dgemv('n',m,m,1.0d0,nrec,m,kinf(:,i,t),1,0.0d0,rhelp,1)
                         epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
                         ddot(m,kinf(:,i,t),1,rhelp,1)/finf(i,t)**2
                     end if
+
+
                     linf = im
-                    call dger(m,m,-1.0d0/finf(i,t),kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m)
-                    rhelp = -kt(:,i,t)
-                    call daxpy(m,ft(i,t)/finf(i,t),kinf(:,i,t),1,rhelp,1) !rhelp = -kt + ft/finf*kinf
+                    rhelp = -zt(i,:,(t-1)*timevar(1)+1)*finfinv(i,t)
+                    call dger(m,m,1.0d0,kinf(:,i,t),1,rhelp,1,linf,m)
+
+                    rhelp = (kinf(:,i,t)*ft(i,t)*finfinv(i,t)-kt(:,i,t))*finfinv(i,t)
                     l0=0.0d0
-                    call dger(m,m,(1.0d0/finf(i,t)),rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0=  (-kt + ft/finf*kinf)*z/finf
+                    call dger(m,m,1.0d0,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0=  (-kt + ft/finf*kinf)*z/finf
+
                     call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
-                    call dcopy(m,rhelp,1,rrec1,1)
+                    rrec1 = rhelp + zt(i,:,(t-1)*timevar(1)+1)*vt(i,t)*finfinv(i,t)
                     call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                    call daxpy(m,(vt(i,t)/finf(i,t)),zt(i,:,(t-1)*timevar(1)+1),1,rrec1,1)
                     call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
                     rrec = rhelp
 
                     call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec2,m,0.0d0,mm,m) !mm =linf'*nt2
                     call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec2,m) !nt2 = linf'*nt2*linf
-                    call dger(m,m,-1.0d0*ft(i,t)/(finf(i,t)**2.0d0),zt(i,:,(t-1)*timevar(1)+1)&
+                    call dger(m,m,-1.0d0*ft(i,t)*finfinv(i,t)**2.0d0,zt(i,:,(t-1)*timevar(1)+1)&
                     ,1,zt(i,:,(t-1)*timevar(1)+1),1,nrec2,m) !nt2 = linf'nt2'linf + z'z*ft/finf^2
-                    call dsymm('l','u',m,m,1.0d0,nrec,m,l0,m,0.0d0,mm,m) !mm= nt0*l0
-                    call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec2,m) !nt2 = linf'nt2'linf + z'z*ft/finf^2 + l0'*nt0*l0
+                    call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm= nt0*l0
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,1.0d0,nrec2,m) !nt2 = linf'nt2'linf + z'z*ft/finf^2 + l0'*nt0*l0
+
+                    call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec1,m,0.0d0,mm,m) !mm = linf'*nt1
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,mm2,m) !nt2 = nt2 + linf'*nt1*l0
+                    nrec2 = nrec2 +mm2 + transpose(mm2)
+
                     call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec1,m,0.0d0,mm,m) !mm = linf'*nt1
-                    call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,1.0d0,nrec2,m) !nt2 = nt2 + linf'*nt1*l0
-                    call dgemm('t','n',m,m,m,1.0d0,nrec1,m,linf,m,0.0d0,mm,m) !mm = nt1'*linf
-                    call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec2,m) !nt2 = nt2 + l0'*nt1'*linf hUOm ntrans
-      
-                    call dgemm('n','n',m,m,m,1.0d0,nrec1,m,linf,m,0.0d0,mm,m) !mm = nt1*linf !!!!!!!!!!
-                    call dgemm('t','n',m,m,m,1.0d0,linf,m,mm,m,0.0d0,nrec1,m) !nt1 = linf'*mm
-                    call dger(m,m,(1.0d0)/finf(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec1,m)
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec1,m) !nt1 = mm*linf
+
+                    call dger(m,m,finfinv(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec1,m)
                     !nt1 = linf'nt1'linf + z'z/finf
-                    call dsymm('l','u',m,m,1.0d0,nrec,m,linf,m,0.0d0,mm,m) !mm= nt0*linf
-                    call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec1,m) !nt1 = l0'*nt0*linf+ linf'nt1*linf + z'z/finf
-                    call dgemm('t','n',m,m,m,1.0d0,linf,m,mm,m,0.0d0,nrec,m) !nt0 = linf'*mm
-      
+                    call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm= nt0*linf
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,1.0d0,nrec1,m) !nt1 = l0'*nt0*linf+ linf'nt1*linf + z'z/finf
+
+                    call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec,m,0.0d0,mm,m) !mm= nt0*linf
+                    call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec,m)
+
                 else
                     if(ft(i,t).GT.0.0d0) then
                         if(aug .EQ. 1 .AND. dist.EQ.1) then
                             epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)/ft(i,t)-&
                             ddot(m,kt(:,i,t),1,rrec,1)/ft(i,t))
-                            call dsymv('u',m,1.0d0,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
+                            call dgemv('n',m,m,1.0d0,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
                             epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
-                            (1.0d0/ft(i,t)+ddot(m,kt(:,i,t),1,rhelp,1)/ft(i,t)**2)
+                            (ftinv(i,t)+ddot(m,kt(:,i,t),1,rhelp,1)/ft(i,t)**2)
                         end if
-                        lt= im
-                        call dger(m,m,-1.0d0/ft(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,lt,m) !lt = I -Kt*Z/Ft
-                        call dgemv('t',m,m,1.0d0,lt,m,rrec,1,0.0d0,rhelp,1)
-                        rrec = rhelp
-                        call daxpy(m,vt(i,t)/ft(i,t),zt(i,:,(t-1)*timevar(1)+1),1,rrec,1) !r0 = Z'vt/Ft - Lt'r0
-                        call dgemv('t',m,m,1.0d0,lt,m,rrec1,1,0.0d0,rhelp,1)
+                        l0= im
+                        call dger(m,m,-ftinv(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0 = I -Kt*Z/Ft
+                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                        rrec = rhelp+zt(i,:,(t-1)*timevar(1)+1)*vt(i,t)*ftinv(i,t)
+                        call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
                         rrec1=rhelp
 
-                        call dgemm('t','n',m,m,m,1.0d0,lt,m,nrec,m,0.0d0,mm,m) !mm =lt'*nt0
-                        call dgemm('n','n',m,m,m,1.0d0,mm,m,lt,m,0.0d0,nrec,m) !nt0 = lt'*nt0*lt
-                        call dger(m,m,(1.0d0)/ft(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec,m)  !nt0 = z'z/ft+lt'*nt0*lt
-                        call dgemm('n','n',m,m,m,1.0d0,nrec1,m,lt,m,0.0d0,mm,m) !mm = nt1*lt
+                        call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm =l0'*nt0
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,nrec,m) !nt0 = l0'*nt0*l0
+                        call dger(m,m,ftinv(i,t),zt(i,:,(t-1)*timevar(1)+1),1,&
+                        zt(i,:,(t-1)*timevar(1)+1),1,nrec,m)  !nt0 = z'z/ft+l0'*nt0*l0
+                        call dgemm('n','n',m,m,m,1.0d0,nrec1,m,l0,m,0.0d0,mm,m) !mm = nt1*l0
                         nrec1 = mm
-                        call dgemm('n','n',m,m,m,1.0d0,nrec2,m,lt,m,0.0d0,mm,m) !mm = nt1*lt
-                        nrec2 = mm
+                        call dgemm('n','n',m,m,m,1.0d0,nrec2,m,l0,m,0.0d0,mm,m) !mm = nt1*l0
+                        nrec2 = mm !onko oikein?
                     end if
                 end if
             end if
@@ -214,95 +238,93 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
             rrec = rhelp
             call dgemv('t',m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,rrec1,1,0.0d0,rhelp,1,1)
             rrec1 = rhelp
-            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec2,m,0.0d0,mm,m) !mm =t'*nt2
-            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec2,m) !nt2 = t'*nt2*t
-            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec1,m,0.0d0,mm,m) !mm =t'*nt2
-            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec1,m) !nt2 = t'*nt2*t
-            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec,m,0.0d0,mm,m) !mm =t'*nt2
-            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec,m) !nt2 = t'*nt2*t
+            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec,m,0.0d0,mm,m) !n*t
+            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec,m) !n_t,p = t'nt
+            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec1,m,0.0d0,mm,m) !n*t
+            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec1,m) !n_t,p = t'nt
+            call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec2,m,0.0d0,mm,m) !n*t
+            call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec2,m) !n_t,p = t'nt
+
         end if
 
         do t=(d-1), 1, -1
 
-
-
             do i = p, 1, -1
                 if(ymiss(t,i).EQ.0) then
                     if(finf(i,t).GT. tol) then
                         if(aug .EQ. 1 .AND. dist.EQ.1) then
                             epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)/finf(i,t)
-                            call dsymv('u',m,1.0d0,nrec,m,kinf(:,i,t),1,0.0d0,rhelp,1)
+                            call dgemv('n',m,m,1.0d0,nrec,m,kinf(:,i,t),1,0.0d0,rhelp,1)
                             epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
                             ddot(m,kinf(:,i,t),1,rhelp,1)/finf(i,t)**2
                         end if
-                        linf = im !linf = I
-                        call dger(m,m,-1.0d0/finf(i,t),kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m) !linf
-                        rhelp = -kt(:,i,t)
-                        call daxpy(m,ft(i,t)/finf(i,t),kinf(:,i,t),1,rhelp,1)
+
+                        linf = im
+                        rhelp = -zt(i,:,(t-1)*timevar(1)+1)*finfinv(i,t)
+                        call dger(m,m,1.0d0,kinf(:,i,t),1,rhelp,1,linf,m)
+
+                        rhelp = (kinf(:,i,t)*ft(i,t)*finfinv(i,t)-kt(:,i,t))*finfinv(i,t)
                         l0=0.0d0
-                        call dger(m,m,(1.0d0/finf(i,t)),rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0
+                        call dger(m,m,1.0d0,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0=  (-kt + ft/finf*kinf)*z/finf
 
                         call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
-                        call dcopy(m,rhelp,1,rrec1,1)
+                        rrec1 = rhelp + zt(i,:,(t-1)*timevar(1)+1)*vt(i,t)*finfinv(i,t)
                         call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                        call daxpy(m,vt(i,t)/finf(i,t),zt(i,:,(t-1)*timevar(1)+1),1,rrec1,1)!rt1
                         call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
                         rrec = rhelp
-            
+
                         call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec2,m,0.0d0,mm,m) !mm =linf'*nt2
                         call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec2,m) !nt2 = linf'*nt2*linf
-            
-                        call dger(m,m,-1.0d0*ft(i,t)/(finf(i,t)**2.0d0),&
-                        zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec2,m) !nt2 = linf'nt2'linf - z'z*ft/finf^2
-  
-                        call dsymm('l','u',m,m,1.0d0,nrec,m,l0,m,0.0d0,mm,m) !mm= nt0*l0
-           
-                        call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec2,m) !nt2 = linf'nt2'linf - z'z*ft/finf^2 + l0'*nt0*l0
+                        call dger(m,m,-1.0d0*ft(i,t)*finfinv(i,t)**2.0d0,zt(i,:,(t-1)*timevar(1)+1)&
+                        ,1,zt(i,:,(t-1)*timevar(1)+1),1,nrec2,m) !nt2 = linf'nt2'linf + z'z*ft/finf^2
+                        call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm= nt0*l0
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,1.0d0,nrec2,m) !nt2 = linf'nt2'linf + z'z*ft/finf^2 + l0'*nt0*l0
+
                         call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec1,m,0.0d0,mm,m) !mm = linf'*nt1
-                        call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,1.0d0,nrec2,m) !nt2 = nt2 + linf'*nt1*l0
-                        call dgemm('t','n',m,m,m,1.0d0,nrec1,m,linf,m,0.0d0,mm,m) !mm = nt1'*linf
-                        call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec2,m) !nt2 = nt2 + l0'*nt1'*linf hUOm ntrans
-          
-                        call dgemm('n','n',m,m,m,1.0d0,nrec1,m,linf,m,0.0d0,mm,m) !mm = nt1*linf !!!!!!!!!!
-                        call dgemm('t','n',m,m,m,1.0d0,linf,m,mm,m,0.0d0,nrec1,m) !nt1 = linf'*mm
-                        call dger(m,m,(1.0d0)/finf(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec1,m)
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,mm2,m) !nt2 = nt2 + linf'*nt1*l0
+                        nrec2 = nrec2 +mm2 + transpose(mm2)
+
+                        call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec1,m,0.0d0,mm,m) !mm = linf'*nt1
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec1,m) !nt1 = mm*linf
+
+                        call dger(m,m,finfinv(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec1,m)
                         !nt1 = linf'nt1'linf + z'z/finf
-                        call dsymm('l','u',m,m,1.0d0,nrec,m,linf,m,0.0d0,mm,m) !mm= nt0*linf
-                        call dgemm('t','n',m,m,m,1.0d0,l0,m,mm,m,1.0d0,nrec1,m) !nt1 = l0'*nt0*linf+ linf'nt1*linf + z'z/finf
-            
-                        call dgemm('t','n',m,m,m,1.0d0,linf,m,mm,m,0.0d0,nrec,m) !nt0 = linf'*mm
-            
-           
+                        call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm= nt0*linf
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,1.0d0,nrec1,m) !nt1 = l0'*nt0*linf+ linf'nt1*linf + z'z/finf
+
+                        call dgemm('t','n',m,m,m,1.0d0,linf,m,nrec,m,0.0d0,mm,m) !mm= nt0*linf
+                        call dgemm('n','n',m,m,m,1.0d0,mm,m,linf,m,0.0d0,nrec,m)
+
                     else
-                        if(ft(i,t).GT.0.0d0) then !lis�tty 12.1.2012
+                        if(ft(i,t).GT.0.0d0) then !lis?tty 12.1.2012
                             if(aug .EQ. 1 .AND. dist.EQ.1) then
                                 epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)/ft(i,t)-&
                                 ddot(m,kt(:,i,t),1,rrec,1)/ft(i,t))
-                                call dsymv('u',m,1.0d0,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
+                                call dgemv('n',m,m,1.0d0,nrec,m,kt(:,i,t),1,0.0d0,rhelp,1)
                                 epshatvar(i,t) = ht(i,i,(t-1)*timevar(2)+1)-(ht(i,i,(t-1)*timevar(2)+1)**2)*&
-                                (1.0d0/ft(i,t)+ddot(m,kt(:,i,t),1,rhelp,1)/ft(i,t)**2 )
+                                (ftinv(i,t)+ddot(m,kt(:,i,t),1,rhelp,1)/ft(i,t)**2 )
                             end if
-                            lt= im
-                            call dger(m,m,(-1.0d0)/ft(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,lt,m) !lt = I -Kt*Z/Ft
-                            call dgemv('t',m,m,1.0d0,lt,m,rrec,1,0.0d0,rhelp,1)
-                            rrec = rhelp
-                            call daxpy(m,vt(i,t)/ft(i,t),zt(i,:,(t-1)*timevar(1)+1),1,rrec,1) !r0 = Z'vt/Ft - Lt'r0
-                            call dgemv('t',m,m,1.0d0,lt,m,rrec1,1,0.0d0,rhelp,1)
+                            l0= im
+                            call dger(m,m,-ftinv(i,t),kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !l0 = I -Kt*Z/Ft
+                            call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                            rrec = rhelp+zt(i,:,(t-1)*timevar(1)+1)*vt(i,t)*ftinv(i,t)
+                            call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
                             rrec1=rhelp
-               
-                            call dgemm('t','n',m,m,m,1.0d0,lt,m,nrec,m,0.0d0,mm,m) !mm =lt'*nt0
-                            call dgemm('n','n',m,m,m,1.0d0,mm,m,lt,m,0.0d0,nrec,m) !nt0 = lt'*nt0*lt
-                            call dger(m,m,(1.0d0)/ft(i,t),zt(i,:,(t-1)*timevar(1)+1),1,zt(i,:,(t-1)*timevar(1)+1),1,nrec,m)  !nt0 = z'z/ft+lt'*nt0*lt
-                            call dgemm('n','n',m,m,m,1.0d0,nrec1,m,lt,m,0.0d0,mm,m) !mm = nt1*lt
+
+                            call dgemm('t','n',m,m,m,1.0d0,l0,m,nrec,m,0.0d0,mm,m) !mm =l0'*nt0
+                            call dgemm('n','n',m,m,m,1.0d0,mm,m,l0,m,0.0d0,nrec,m) !nt0 = l0'*nt0*l0
+                            call dger(m,m,ftinv(i,t),zt(i,:,(t-1)*timevar(1)+1),1,&
+                            zt(i,:,(t-1)*timevar(1)+1),1,nrec,m)  !nt0 = z'z/ft+l0'*nt0*l0
+                            call dgemm('n','n',m,m,m,1.0d0,nrec1,m,l0,m,0.0d0,mm,m) !mm = nt1*l0
                             nrec1 = mm
-                            call dgemm('n','n',m,m,m,1.0d0,nrec2,m,lt,m,0.0d0,mm,m) !mm = nt2*lt
-                            nrec2 = mm
+                            call dgemm('n','n',m,m,m,1.0d0,nrec2,m,l0,m,0.0d0,mm,m) !mm = nt1*l0
+                            nrec2 = mm !onko oikein?
                         end if
                     end if
                 end if
             end do
-          
-      
+
+
             rt0(:,t) = rrec
             rt1(:,t) = rrec1
             nt0(:,:,t) = nrec
@@ -315,42 +337,82 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
                 rrec = rhelp
                 call dgemv('t',m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,rrec1,1,0.0d0,rhelp,1,1)
                 rrec1 = rhelp
-                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec2,m,0.0d0,mm,m) !mm =t'*nt2
-                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec2,m) !nt2 = t'*nt2*t
-                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec1,m,0.0d0,mm,m) !mm =t'*nt2
-                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec1,m) !nt2 = t'*nt2*t
-                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec,m,0.0d0,mm,m) !mm =t'*nt2
-                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec,m) !nt2 = t'*nt2*t
+                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec,m,0.0d0,mm,m) !n*t
+                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec,m) !n_t,p = t'nt
+                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec1,m,0.0d0,mm,m) !n*t
+                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec1,m) !n_t,p = t'nt
+                call dgemm('t','n',m,m,m,1.0d0,tt(:,:,(t-2)*timevar(3)+1),m,nrec2,m,0.0d0,mm,m) !n*t
+                call dgemm('n','n',m,m,m,1.0d0,mm,m,tt(:,:,(t-2)*timevar(3)+1),m,0.0d0,nrec2,m) !n_t,p = t'nt
             end if
 
 
         end do
     end if
 
-
     if(state.EQ.1) then
         do t = 1, d
-            call dcopy(m,at(:,t),1,ahat(:,t),1) !ahat = at
-            call dgemv('n',m,m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,ahat(:,t),1) !ahat = at + pt * rt0_t
-            call dgemv('n',m,m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,ahat(:,t),1) !ahat = at + pt * rt0_t + pinf*rt1_t
+            ahat(:,t) = at(:,t)
+            call dsymv('u',m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,ahat(:,t),1)
+            call dsymv('u',m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,ahat(:,t),1)
+
             vvt(:,:,t) = pt(:,:,t)
-            call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
-            call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,1.0d0,vvt(:,:,t),m) !vvt = pt - pt*nt0*pt
-            call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
-            call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
+            call dsymm('l','u',m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
+            call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,1.0d0,vvt(:,:,t),m) !vvt = pt - pt*nt0*pt
+            call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
+            call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
             vvt(:,:,t) = vvt(:,:,t) + mm2 + transpose(mm2) !vvt = pt - pt*nt0*pt  -pinf*nt1*pt - t(pinf*nt1*pt)
-            call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
-            call dgemm('n','n',m,m,m,-1.0d0,mm,m,pinf(:,:,t),m,1.0d0,vvt(:,:,t),m) !vvt = vvt - pinf*nt2*pinf
+            call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
+            call dsymm('r','u',m,m,-1.0d0,pinf(:,:,t),m,mm,m,1.0d0,vvt(:,:,t),m) !vvt = vvt - pinf*nt2*pinf
         end do
         do t = d+1, n
-            call dcopy(m,at(:,t),1,ahat(:,t),1) !ahat = at
+            ahat(:,t) = at(:,t)
             call dsymv('u',m,1.0d0,pt(:,:,t),m,rt(:,t),1,1.0d0,ahat(:,t),1) !ahat = ahat+pt*r_t-1
-            vvt(:,:,t) = pt(:,:,t)
+            !call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt(:,:,t),m,0.0d0,mm,m) !pt*n_t-1
+            !call dgemm('n','t',m,m,m,1.0d0,mm,m,pt(:,:,t),m,0.0d0,vvt(:,:,t),m) !pt*n_t-1*pt
+            !vvt(:,:,t) = pt(:,:,t)-vvt(:,:,t)
+            !vvt(:,:,t) = pt(:,:,t)
             call dsymm('l','u',m,m,1.0d0,pt(:,:,t),m,nt(:,:,t),m,0.0d0,mm,m) !pt*n_t-1
-            call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,1.0d0,vvt(:,:,t),m) !pt*n_t-1*pt
-
+            mm = im - mm
+            call dsymm('r','u',m,m,1.0d0,pt(:,:,t),m,mm,m,0.0d0,vvt(:,:,t),m) !pt*n_t-1*pt
         end do
+        if(m > 1) then
+            do t=1, n
+                do i=1,m-1
+                    vvt((i+1):m,i,t) =vvt(i,(i+1):m,t)
+                end do
+            end do
+        end if
     end if
+    !    if(state.EQ.1) then
+    !        do t = 1, d
+    !            call dcopy(m,at(:,t),1,ahat(:,t),1) !ahat = at
+    !            call dgemv('n',m,m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,ahat(:,t),1) !ahat = at + pt * rt0_t
+    !            call dgemv('n',m,m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,ahat(:,t),1) !ahat = at + pt * rt0_t + pinf*rt1_t
+    !            vvt(:,:,t) = pt(:,:,t)
+    !            call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
+    !            call dgemm('n','t',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,1.0d0,vvt(:,:,t),m) !vvt = pt - pt*nt0*pt
+    !            call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
+    !            call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
+    !            vvt(:,:,t) = vvt(:,:,t) + mm2 + transpose(mm2) !vvt = pt - pt*nt0*pt  -pinf*nt1*pt - t(pinf*nt1*pt)
+    !            call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
+    !            call dgemm('n','t',m,m,m,-1.0d0,mm,m,pinf(:,:,t),m,1.0d0,vvt(:,:,t),m) !vvt = vvt - pinf*nt2*pinf
+    !        end do
+    !        do t = d+1, n
+    !            call dcopy(m,at(:,t),1,ahat(:,t),1) !ahat = at
+    !            call dgemv('n',m,m,1.0d0,pt(:,:,t),m,rt(:,t),1,1.0d0,ahat(:,t),1) !ahat = ahat+pt*r_t-1
+    !            !vvt(:,:,t) = pt(:,:,t)
+    !            !call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt(:,:,t),m,0.0d0,mm,m) !pt*n_t-1
+    !            !call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,1.0d0,vvt(:,:,t),m) !pt*n_t-1*pt
+    !
+    !            call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt(:,:,t),m,0.0d0,mm,m)
+    !            mm = im - mm
+    !            call dgemm('n','n',m,m,m,1.0d0,mm,m,pt(:,:,t),m,0.0d0,vvt(:,:,t),m) !pt*n_t-1*pt
+    !
+    !        end do
+    !        do t= 1,n
+    !            vvt(:,:,t) = (vvt(:,:,t)+transpose(vvt(:,:,t)))/2.0d0
+    !        end do
+    !    end if
 
     if(dist.EQ.1) then
         do t = 1, d
@@ -382,18 +444,18 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
             else
                 do t = 1, d
                     call dcopy(m,at(:,t),1,rrec,1) !ahat = at
-                    call dgemv('n',m,m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t
-                    call dgemv('n',m,m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t + pinf*rt1_t
+                    call dsymv('u',m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t
+                    call dsymv('u',m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t + pinf*rt1_t
                     call dgemv('n',p,m,1.0d0,zorig(:,:,(t-1)*zorigtv+1),p,rrec,1,0.0d0,thetahat(:,t),1)
 
                     nrec = pt(:,:,t)
-                    call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,1.0d0,nrec,m) !vvt = pt - pt*nt0*pt
-                    call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
+                    call dsymm('l','u',m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
+                    call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,1.0d0,nrec,m) !vvt = pt - pt*nt0*pt
+                    call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
+                    call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
                     nrec = nrec + mm2 + transpose(mm2) !vvt = pt - pt*nt0*pt  -pinf*nt1*pt - t(pinf*nt1*pt)
-                    call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pinf(:,:,t),m,1.0d0,nrec,m) !vvt = vvt - pinf*nt2*pinf
+                    call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
+                    call dsymm('r','u',m,m,-1.0d0,pinf(:,:,t),m,mm,m,1.0d0,nrec,m) !vvt = vvt - pinf*nt2*pinf
                     call dsymm('r','u',p,m,1.0d0,nrec,m,zorig(:,:,(t-1)*zorigtv+1),p,0.0d0,pm,p)
                     call dgemm('n','t',p,p,m,1.0d0,pm,p,zorig(:,:,(t-1)*zorigtv+1),p,0.0d0,thetahatvar(:,:,t),p)
                 end do
@@ -419,18 +481,18 @@ subroutine gsmoothall(ymiss, timevar, zt, ht,tt, rtv, qt, p, n, m, r, d,j, at, p
             else
                 do t = 1, d
                     call dcopy(m,at(:,t),1,rrec,1) !ahat = at
-                    call dgemv('n',m,m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t
-                    call dgemv('n',m,m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t + pinf*rt1_t
+                    call dsymv('u',m,1.0d0,pt(:,:,t),m,rt0(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t
+                    call dsymv('u',m,1.0d0,pinf(:,:,t),m,rt1(:,t),1,1.0d0,rrec,1) !ahat = at + pt * rt0_t + pinf*rt1_t
                     call dgemv('n',p,m,1.0d0,zt(:,:,(t-1)*timevar(1)+1),p,rrec,1,0.0d0,thetahat(:,t),1)
 
                     nrec = pt(:,:,t)
-                    call dgemm('n','n',m,m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,1.0d0,nrec,m) !vvt = pt - pt*nt0*pt
-                    call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pt(:,:,t),m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
+                    call dsymm('l','u',m,m,1.0d0,pt(:,:,t),m,nt0(:,:,t),m,0.0d0,mm,m) !mm = pt*nt0
+                    call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,1.0d0,nrec,m) !vvt = pt - pt*nt0*pt
+                    call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt1(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt1
+                    call dsymm('r','u',m,m,-1.0d0,pt(:,:,t),m,mm,m,0.0d0,mm2,m) !mm2 = -pinf*nt1*pt
                     nrec = nrec + mm2 + transpose(mm2) !vvt = pt - pt*nt0*pt  -pinf*nt1*pt - t(pinf*nt1*pt)
-                    call dgemm('n','n',m,m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
-                    call dgemm('n','n',m,m,m,-1.0d0,mm,m,pinf(:,:,t),m,1.0d0,nrec,m) !vvt = vvt - pinf*nt2*pinf
+                    call dsymm('l','u',m,m,1.0d0,pinf(:,:,t),m,nt2(:,:,t),m,0.0d0,mm,m) !mm = pinf*nt2
+                    call dsymm('r','u',m,m,-1.0d0,pinf(:,:,t),m,mm,m,1.0d0,nrec,m) !vvt = vvt - pinf*nt2*pinf
                     call dsymm('r','u',p,m,1.0d0,nrec,m,zt(:,:,(t-1)*timevar(1)+1),p,0.0d0,pm,p)
                     call dgemm('n','t',p,p,m,1.0d0,pm,p,zt(:,:,(t-1)*timevar(1)+1),p,0.0d0,thetahatvar(:,:,t),p)
                 end do
diff --git a/src/kfilter.f95 b/src/kfilter.f95
index af8c7db..342bd33 100644
--- a/src/kfilter.f95
+++ b/src/kfilter.f95
@@ -5,9 +5,9 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
 
     implicit none
 
-    integer, intent(in) ::  p, m, r, n,filtersignal
+    integer, intent(in) :: p, m, r, n,filtersignal
     integer, intent(inout) :: d, j, rankp
-    integer ::  t, i
+    integer :: t, i
     integer, intent(in), dimension(n,p) :: ymiss
     integer, intent(in), dimension(5) :: timevar
     double precision, intent(in), dimension(n,p) :: yt
@@ -17,7 +17,7 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
     double precision, intent(in), dimension(m,r,(n-1)*timevar(4)+1) :: rt
     double precision, intent(in), dimension(r,r,(n-1)*timevar(5)+1) :: qt
     double precision, intent(in), dimension(m) :: a1
-    double precision, intent(in), dimension(m,m) ::  p1,p1inf
+    double precision, intent(in), dimension(m,m) :: p1,p1inf
     double precision, intent(in) :: tol
     double precision, intent(inout), dimension(m,n+1) :: at
     double precision, intent(inout), dimension(m,m,n+1) :: pt,pinf
@@ -27,12 +27,12 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
     double precision, intent(inout), dimension(p,p,n) :: thetavar
     double precision, intent(inout), dimension(n,p) :: theta
     double precision, dimension(m) :: arec
-    double precision, dimension(m,m) :: prec, pirec,im,mm
+    double precision, dimension(m,m) ::pirec,im,mm,prec
     double precision, dimension(m,r) :: mr
     double precision, dimension(p,m) :: pm
     double precision :: c
     double precision, external :: ddot
-    double precision :: meps
+    double precision :: meps,finv
 
     meps = tiny(meps) !was epsilon!
 
@@ -50,46 +50,50 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
 
     ! diffuse initialization
 
-    if(maxval(pinf(:,:,1)) .GT.  0.0d0) then
+    if(rankp > 0) then
 
         pt(:,:,1) = p1
         prec = pt(:,:,1)
         pirec = pinf(:,:,1)
         at(:,1) = a1
         arec = a1
-        diffuse: do while(d .LT. n)
+        diffuse: do while(d < n .AND. rankp > 0)
             d = d+1
             do j=1, p
-                call dsymv('u',m,1.0d0,prec,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kt(:,j,d),1) ! kt_t,i = pt_t,i*t(z_t,i)
-                ft(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kt(:,j,d),1)  + ht(j,j,(d-1)*timevar(2)+1)
+                call dsymv('u',m,1.0d0,prec,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kt(:,j,d),1)
+                ft(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kt(:,j,d),1) + ht(j,j,(d-1)*timevar(2)+1)
                 if(ymiss(d,j) .EQ. 0) then
                     call dsymv('u',m,1.0d0,pirec,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kinf(:,j,d),1) ! kinf_t,i = pinf_t,i*t(z_t,i)
                     finf(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kinf(:,j,d),1)
 
                     vt(j,d) = yt(d,j) - ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,arec,1)
                     if (finf(j,d) .GT. tol) then
-                        call daxpy(m,vt(j,d)/finf(j,d),kinf(:,j,d),1,arec,1) !a_rec = a_rec + kinf(:,i,t)*vt(:,t)/finf(j,d)
-                        call dsyr('u',m,ft(j,d)/(finf(j,d)**2),kinf(:,j,d),1,prec,m) !prec = prec +  kinf*kinf'*ft/finf^2
-                        call dsyr2('u',m,-1.0d0/finf(j,d),kt(:,j,d),1,kinf(:,j,d),1,prec,m) !prec = prec -(kt*kinf'+kinf*kt')/finf
-                        call dsyr('u',m,-1.0d0/finf(j,d),kinf(:,j,d),1,pirec,m) !pirec = pirec -kinf*kinf'/finf
+                        finv = 1.0d0/finf(j,d)
+                        arec = arec + kinf(:,j,d)*finv*vt(j,d)
+                        call dsyr('u',m,ft(j,d)*finv**2,kinf(:,j,d),1,prec,m) !prec = prec + kinf*kinf'*ft/finf^2
+                        call dsyr2('u',m,-finv,kt(:,j,d),1,kinf(:,j,d),1,prec,m) !prec = prec -(kt*kinf'+kinf*kt')/finf
+                        call dsyr('u',m,-finv,kinf(:,j,d),1,pirec,m) !pirec = pirec -kinf*kinf'/finf
+
                         lik = lik - 0.5d0*log(finf(j,d))
                         rankp = rankp -1
+
                         do i = 1, m
-                            if(pirec(i,i) .LT. tol) then
+                            if(pirec(i,i) < tol) then
                                 pirec(i,:) = 0.0d0
                                 pirec(:,i) = 0.0d0
                             end if
                         end do
                     else
                         finf(j,d) = 0.0d0
-                        if(ft(j,d) .GT. meps) then
-                            call daxpy(m,vt(j,d)/ft(j,d),kt(:,j,d),1,arec,1) !a_rec = a_rec + kt(:,i,t)*vt(:,t)/ft(i,t)
-                            call dsyr('u',m,(-1.0d0)/ft(j,d),kt(:,j,d),1,prec,m) !prec = prec -kt*kt'/ft
-                            lik = lik - 0.5d0*(log(ft(j,d)) + vt(j,d)**2/ft(j,d))
+                        if(ft(j,d) > meps) then
+                            finv = 1.0d0/ft(j,d)
+                            call daxpy(m,vt(j,d)*finv,kt(:,j,d),1,arec,1) !a_rec = a_rec + kt(:,i,t)*vt(:,t)/ft(i,t)
+                            call dsyr('u',m,-finv,kt(:,j,d),1,prec,m) !prec = prec -kt*kt'/ft
+                            lik = lik - 0.5d0*(log(ft(j,d)) + vt(j,d)**2*finv)
 
                         end if
                     end if
-                    if (ft(j,d) .GT. meps) then
+                    if (ft(j,d) > meps) then
                         lik = lik -c
                     else
                         ft(j,d)=0.0d0
@@ -101,7 +105,7 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
             end do
 
             call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at(:,d+1),1)
-            call dcopy(m,at(:,d+1),1,arec,1)
+            arec = at(:,d+1)
             call dsymm('r','u',m,m,1.0d0,prec,m,tt(:,:,(d-1)*timevar(3)+1),m,0.0d0,mm,m)
             call dgemm('n','t',m,m,m,1.0d0,mm,m,tt(:,:,(d-1)*timevar(3)+1),m,0.0d0,pt(:,:,d+1),m)
 
@@ -109,30 +113,25 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
                 call dsymm('r','u',m,r,1.0d0,qt(:,:,(d-1)*timevar(5)+1),r,rt(:,:,(d-1)*timevar(4)+1),m,0.0d0,mr,m)
                 call dgemm('n','t',m,m,r,1.0d0,mr,m,rt(:,:,(d-1)*timevar(4)+1),m,1.0d0,pt(:,:,d+1),m)
             else
-                call dger(m,m,qt(1,1,(d-1)*timevar(5)+1),rt(:,1,(d-1)*timevar(4)+1),1,rt(:,1,(d-1)*timevar(4)+1),1,&
-                pt(:,:,d+1),m)
+                call dsyr('u',m,qt(1,1,(d-1)*timevar(5)+1),rt(:,1,(d-1)*timevar(4)+1),1,pt(:,:,d+1),m)
             end if
             prec = pt(:,:,d+1)
             call dsymm('r','u',m,m,1.0d0,pirec,m,tt(:,:,(d-1)*timevar(3)+1),m,0.0d0,mm,m)
             call dgemm('n','t',m,m,m,1.0d0,mm,m,tt(:,:,(d-1)*timevar(3)+1),m,0.0d0,pinf(:,:,d+1),m)
             pirec = pinf(:,:,d+1)
-            !do i = 1, m
-            !    if(pirec(i,i) .LT. tol) then
-            !        pirec(i,:) = 0.0d0
-            !        pirec(:,i) = 0.0d0
-            !    end if
-            !end do
+
         end do diffuse
 
+
         if(rankp .EQ. 0) then
                 !non-diffuse filtering begins
             do i = j+1, p
                 call dsymv('u',m,1.0d0,prec,m,zt(i,:,(d-1)*timevar(1)+1),1,0.0d0,kt(:,i,d),1)
-                ft(i,d) = ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,kt(:,i,d),1)  +  ht(i,i,(d-1)*timevar(2)+1)
+                ft(i,d) = ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,kt(:,i,d),1) + ht(i,i,(d-1)*timevar(2)+1)
                 if(ymiss(d,i).EQ.0) then
                     vt(i,d) = yt(d,i) - ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,arec,1) !vt
                     if (ft(i,d) .GT. meps) then
-                        call daxpy(m,vt(i,d)/ft(i,d),kt(:,i,d),1,arec,1) !a_rec = a_rec + kt(:,i,t)*vt(:,t)
+                        arec = arec + kt(:,i,d)*vt(i,d)/ft(i,d)
                         call dsyr('u',m,-1.0d0/ft(i,d),kt(:,i,d),1,prec,m) !p_rec = p_rec - kt*kt'*ft(i,t)
                         lik = lik - 0.5d0*(log(ft(i,d)) + vt(i,d)**2/ft(i,d))-c
                     else
@@ -149,11 +148,13 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
                 call dsymm('r','u',m,r,1.0d0,qt(:,:,(d-1)*timevar(5)+1),r,rt(:,:,(d-1)*timevar(4)+1),m,0.0d0,mr,m)
                 call dgemm('n','t',m,m,r,1.0d0,mr,m,rt(:,:,(d-1)*timevar(4)+1),m,1.0d0,pt(:,:,d+1),m)
             else
-                call dger(m,m,qt(1,1,(d-1)*timevar(5)+1),rt(:,1,(d-1)*timevar(4)+1),1,rt(:,1,(d-1)*timevar(4)+1),1,&
-                pt(:,:,d+1),m)
+                call dsyr('u',m,qt(1,1,(d-1)*timevar(5)+1),rt(:,1,(d-1)*timevar(4)+1),1,pt(:,:,d+1),m)
+
+               ! call dger(m,m,qt(1,1,(d-1)*timevar(5)+1),rt(:,1,(d-1)*timevar(4)+1),1,rt(:,1,(d-1)*timevar(4)+1),1,&
+               ! pt(:,:,d+1),m)
             end if
 
-            call dcopy(m,at(:,d+1),1,arec,1)
+            arec = at(:,d+1)
             prec = pt(:,:,d+1)
         end if
     end if
@@ -174,11 +175,12 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
     do t = d+1, n
         do i = 1, p
             call dsymv('u',m,1.0d0,prec,m,zt(i,:,(t-1)*timevar(1)+1),1,0.0d0,kt(:,i,t),1)
-            ft(i,t) = ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,kt(:,i,t),1) +  ht(i,i,(t-1)*timevar(2)+1)
+
+            ft(i,t) = ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,kt(:,i,t),1) + ht(i,i,(t-1)*timevar(2)+1)
             if(ymiss(t,i).EQ.0) then
                 vt(i,t) = yt(t,i) - ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,arec,1)
                 if (ft(i,t) .GT. meps) then
-                    call daxpy(m,vt(i,t)/ft(i,t),kt(:,i,t),1,arec,1) !a_rec = a_rec + kt(:,i,t)*vt(:,t)
+                    arec = arec + kt(:,i,t)/ft(i,t)*vt(i,t)
                     call dsyr('u',m,-1.0d0/ft(i,t),kt(:,i,t),1,prec,m) !p_rec = p_rec - kt*kt'*ft(i,i,t)
                     lik = lik - 0.5d0*(log(ft(i,t)) + vt(i,t)**2/ft(i,t))-c
                 else
@@ -188,18 +190,22 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
         end do
 
         call dgemv('n',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,arec,1,0.0d0,at(:,t+1),1)
+
         call dsymm('r','u',m,m,1.0d0,prec,m,tt(:,:,(t-1)*timevar(3)+1),m,0.0d0,mm,m)
         call dgemm('n','t',m,m,m,1.0d0,mm,m,tt(:,:,(t-1)*timevar(3)+1),m,0.0d0,pt(:,:,t+1),m)
+        !call dgemm('n','n',m,m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,prec,m,0.0d0,mm,m)
+        !call dgemm('n','t',m,m,m,1.0d0,mm,m,tt(:,:,(t-1)*timevar(3)+1),m,0.0d0,pt(:,:,t+1),m)
 
         if(r.GT.1) then
             call dsymm('r','u',m,r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,rt(:,:,(t-1)*timevar(4)+1),m,0.0d0,mr,m)
             call dgemm('n','t',m,m,r,1.0d0,mr,m,rt(:,:,(t-1)*timevar(4)+1),m,1.0d0,pt(:,:,t+1),m)
         else
-            call dger(m,m,qt(1,1,(t-1)*timevar(5)+1),rt(:,1,(t-1)*timevar(4)+1),1,rt(:,1,(t-1)*timevar(4)+1),&
-            1,pt(:,:,t+1),m)
+            !   call dger(m,m,qt(1,1,(t-1)*timevar(5)+1),rt(:,1,(t-1)*timevar(4)+1),1,rt(:,1,(t-1)*timevar(4)+1),&
+            !   1,pt(:,:,t+1),m)
+            call dsyr('u',m,qt(1,1,(t-1)*timevar(5)+1),rt(:,1,(t-1)*timevar(4)+1),1,pt(:,:,t+1),m)
         end if
 
-        call dcopy(m,at(:,t+1),1,arec,1)
+        arec = at(:,t+1)
         prec = pt(:,:,t+1)
     end do
 
@@ -210,4 +216,21 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
             call dgemm('n','t',p,p,m,1.0d0,pm,p,zt(:,:,(t-1)*timevar(1)+1),p,0.0d0,thetavar(:,:,t),p)
         end do
     end if
+
+    if(m > 1) then
+        do t=1, n
+            do i=1,m-1
+                pt((i+1):m,i,t) =pt(i,(i+1):m,t)
+            end do
+            ! pt(:,:,t) =(pt(:,:,t)+transpose(pt(:,:,t)))/2.0d0
+        end do
+        if(d>0) then
+            do t=1, d
+                do i=1,m-1
+                    pinf((i+1):m,i,t) =pinf(i,(i+1):m,t)
+                end do
+            !pinf(:,:,t) =(pinf(:,:,t)+transpose(pinf(:,:,t)))/2.0d0
+            end do
+        end if
+    end if
 end subroutine kfilter
diff --git a/tests/testthat/testGLM.R b/tests/testthat/testGLM.R
index c8bbdac..ad52974 100644
--- a/tests/testthat/testGLM.R
+++ b/tests/testthat/testGLM.R
@@ -157,6 +157,8 @@ test_that("negative binomial GLM fitting works properly",{
   # prediction variances on response scale
   expect_equivalent(c(kfas.NB$V_mu),predict(glm.NB,type='response',se.fit=TRUE,)$se.fit^2)
   
+  expect_equivalent(model.NB$u[1],glm.NB$theta)
+  
   likfn<-function(pars,model,estimate=TRUE){ 
     model$u[]<-exp(pars[1])
     model$P1inf[]<-0