diff --git a/R/KFS.R b/R/KFS.R
index f6f2863..2157742 100644
--- a/R/KFS.R
+++ b/R/KFS.R
@@ -396,6 +396,7 @@ KFS <-
     
     if (filterout$d == n & filterout$j == p) 
       warning("Model is degenerate, diffuse phase did not end.")
+    filterout$Pinf <- filterout$Pinf[1:m, 1:m, 1:filterout$d, drop = FALSE]
     if (filterout$d > 0 & m > 1 & min(apply(filterout$Pinf, 3, diag)) < 0) 
       warning(paste0("Possible error in diffuse filtering: Negative variances in Pinf, ",
                      "check the model or try changing the tolerance parameter tol or P1/P1inf of the model."))
@@ -404,7 +405,7 @@ KFS <-
                      "Finf is not equal to the number of diffuse states. \n",
                      "Either model is degenerate or numerical errors occured. ",
                      "Check the model or try changing the tolerance parameter tol or P1/P1inf of the model."))
-    filterout$Pinf <- filterout$Pinf[1:m, 1:m, 1:(filterout$d + 1), drop = FALSE]
+    
     if (filterout$d > 0) {
       filterout$Finf <- filterout$Finf[, 1:filterout$d, drop = FALSE]
       filterout$Kinf <- filterout$Kinf[, , 1:filterout$d, drop = FALSE]
diff --git a/src/filteronestep.f95 b/src/filteronestep.f95
new file mode 100644
index 0000000..8dde156
--- /dev/null
+++ b/src/filteronestep.f95
@@ -0,0 +1,130 @@
+!non-diffuse filtering for single time point
+subroutine filteronestep(ymiss, yt, zt, ht, tt, rqr, at, pt, vt, ft,kt,lik,tol,meps,c,p,m,j)
+
+    implicit none
+
+    integer, intent(in) ::  p, m,j
+    integer ::  i
+    integer, intent(in), dimension(p) :: ymiss
+    double precision, intent(in), dimension(p) :: yt
+    double precision, intent(in), dimension(m,p) :: zt
+    double precision, intent(in), dimension(p,p) :: ht
+    double precision, intent(in), dimension(m,m) :: tt
+    double precision, dimension(m,m) :: rqr
+    double precision, intent(in) :: tol,c,meps
+    double precision, intent(inout) :: lik
+    double precision, intent(inout), dimension(m) :: at
+    double precision, intent(inout), dimension(p) :: vt,ft
+    double precision, intent(inout), dimension(m,p) :: kt
+    double precision, intent(inout), dimension(m,m) :: pt
+    double precision, dimension(m,m) :: mm
+    double precision, dimension(m) :: ahelp
+    double precision :: finv
+
+    double precision, external :: ddot
+
+    external dgemm, dsymm, dgemv, dsymv, dsyr
+
+    do i = j+1, p
+        call dsymv('u',m,1.0d0,pt,m,zt(:,i),1,0.0d0,kt(:,i),1)
+        ft(i) = ddot(m,zt(:,i),1,kt(:,i),1) + ht(i,i)
+        if(ymiss(i).EQ.0) then
+            vt(i) = yt(i) - ddot(m,zt(:,i),1,at,1)
+            if (ft(i) .GT. tol*maxval(zt(:,i)**2)) then
+                finv = 1.0d0/ft(i)
+                at = at + vt(i)*finv*kt(:,i)
+                call dsyr('u',m,-finv,kt(:,i),1,pt,m)
+                lik = lik - c - 0.5d0*(log(ft(i)) + vt(i)**2*finv)
+            else
+                ft(i)=0.0d0
+            end if
+        end if
+    end do
+
+    call dgemv('n',m,m,1.0d0,tt,m,at,1,0.0d0,ahelp,1)
+    at = ahelp
+
+    call dsymm('r','u',m,m,1.0d0,pt,m,tt,m,0.0d0,mm,m)
+    call dgemm('n','t',m,m,m,1.0d0,mm,m,tt,m,0.0d0,pt,m)
+    pt = pt + rqr
+
+end subroutine filteronestep
+
+
+!diffuse filtering for single time point
+subroutine diffusefilteronestep(ymiss, yt, zt, ht, tt, rqr, at, pt, vt, ft,kt,&
+pinf,finf,kinf,rankp,lik,tol,meps,c,p,m,i)
+
+    implicit none
+
+    integer, intent(in) ::  p, m
+    integer, intent(inout) ::  i,rankp
+    integer, intent(in), dimension(p) :: ymiss
+    double precision, intent(in), dimension(p) :: yt
+    double precision, intent(in), dimension(m,p) :: zt
+    double precision, intent(in), dimension(p,p) :: ht
+    double precision, intent(in), dimension(m,m) :: tt
+    double precision, dimension(m,m) :: rqr
+    double precision, intent(in) :: tol,c,meps
+    double precision, intent(inout) :: lik
+    double precision, intent(inout), dimension(m) :: at
+    double precision, intent(inout), dimension(p) :: vt,ft,finf
+    double precision, intent(inout), dimension(m,p) :: kt,kinf
+    double precision, intent(inout), dimension(m,m) :: pt,pinf
+    double precision, dimension(m,m) :: mm
+    double precision, dimension(m) :: ahelp
+    double precision :: finv
+
+    double precision, external :: ddot
+
+    external dgemm, dsymm, dgemv, dsymv, dsyr, dsyr2
+
+    do i=1, p
+        call dsymv('u',m,1.0d0,pt,m,zt(:,i),1,0.0d0,kt(:,i),1)
+        ft(i) = ddot(m,zt(:,i),1,kt(:,i),1) + ht(i,i)
+        if(ymiss(i) .EQ. 0) then
+            call dsymv('u',m,1.0d0,pinf,m,zt(:,i),1,0.0d0,kinf(:,i),1)
+            finf(i) = ddot(m,zt(:,i),1,kinf(:,i),1)
+            vt(i) = yt(i) - ddot(m,zt(:,i),1,at,1)
+            if (finf(i) .GT. tol*maxval(zt(:,i)**2)) then
+                finv = 1.0d0/finf(i)
+                at = at + vt(i)*finv*kinf(:,i)
+                call dsyr('u',m,ft(i)*finv**2,kinf(:,i),1,pt,m)
+                call dsyr2('u',m,-finv,kt(:,i),1,kinf(:,i),1,pt,m)
+                call dsyr('u',m,-finv,kinf(:,i),1,pinf,m)
+                lik = lik - 0.5d0*log(finf(i))
+                rankp = rankp -1
+            else
+                finf(i) = 0.0d0
+                if(ft(i) .GT. tol*maxval(zt(:,i)**2)) then
+                    finv = 1.0d0/ft(i)
+                    at = at + vt(i)*finv*kt(:,i)
+                    call dsyr('u',m,-finv,kt(:,i),1,pt,m)
+                    lik = lik - c - 0.5d0*(log(ft(i)) + vt(i)**2*finv)
+                end if
+            end if
+            if (ft(i) .LE. tol*maxval(zt(:,i)**2)) then
+                ft(i) = 0.0d0
+            end if
+            if(rankp .EQ. 0 .AND. i .LT. p) then
+                return
+            end if
+        end if
+    end do
+
+    call dgemv('n',m,m,1.0d0,tt,m,at,1,0.0d0,ahelp,1)
+    at = ahelp
+    call dsymm('r','u',m,m,1.0d0,pt,m,tt,m,0.0d0,mm,m)
+    call dgemm('n','t',m,m,m,1.0d0,mm,m,tt,m,0.0d0,pt,m)
+    pt = pt + rqr
+
+    call dsymm('r','u',m,m,1.0d0,pinf,m,tt,m,0.0d0,mm,m)
+    call dgemm('n','t',m,m,m,1.0d0,mm,m,tt,m,0.0d0,pinf,m)
+    do i = 1, m
+        if(pinf(i,i) .LT. meps) then
+            pinf(i,:) = 0.0d0
+            pinf(:,i) = 0.0d0
+        end if
+    end do
+
+end subroutine diffusefilteronestep
diff --git a/src/gloglik.f95 b/src/gloglik.f95
index 8f2ca57..f0f91d4 100644
--- a/src/gloglik.f95
+++ b/src/gloglik.f95
@@ -49,79 +49,26 @@ subroutine gloglik(yt, ymiss, timevar, zt, ht, tt, rt, qt, a1, p1, p1inf,&
     j=0
     d=0
     pt = p1
-    arec = a1
+    at = a1
+    pinf=p1inf
     ! Diffuse initialization
     if(rankp .GT. 0) then
-        pinf=p1inf
         diffuse: do while(d .LT. n .AND. rankp .GT. 0)
             d = d+1
-            do j=1, p
-                if(ymiss(d,j).EQ.0) then
-                    vt(j) = yt(d,j) - ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,arec,1)
-                    call dsymv('u',m,1.0d0,pt,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kt(:,j),1)
-                    ft(j) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kt(:,j),1)+ ht(j,j,(d-1)*timevar(2)+1)
-                    call dsymv('u',m,1.0d0,pinf,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kinf(:,j),1)
-                    finf(j) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kinf(:,j),1)
-                    if (finf(j)  .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/finf(j)
-                        arec = arec +vt(j)*finv*kinf(:,j)
-                        call dsyr('u',m,ft(j)*finv**2,kinf(:,j),1,pt,m)
-                        call dsyr2('u',m,-finv,kt(:,j),1,kinf(:,j),1,pt,m)
-                        call dsyr('u',m,-finv,kinf(:,j),1,pinf,m)
-
-                        lik = lik - 0.5d0*log(finf(j))
-                        rankp = rankp -1
-                    else
-                        if (ft(j) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                            finv = 1.0d0/ft(j)
-                            arec = arec + vt(j)*finv*kt(:,j)
-                            call dsyr('u',m,-finv,kt(:,j),1,pt,m)
-                            lik = lik - c - 0.5d0*(log(ft(j)) + vt(j)**2*finv)
-                        end if
-                    end if
-                    if(rankp .EQ. 0) then
-                        exit diffuse
-                    end if
-                end if
-            end do
            
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            call dsymm('r','u',m,m,1.0d0,pt,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,m)
-            pt = pt + rqr(:,:,(d-1)*tv+1)
-            call dsymm('r','u',m,m,1.0d0,pinf,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,m)
-            do i = 1, m ! try to deal with possible rounding errors, non-diffuse states should have zeros in pinf
-                if(pinf(i,i) .LT. meps) then
-                    pinf(i,:) = 0.0d0
-                    pinf(:,i) = 0.0d0
-                end if
-            end do
+            call diffusefilteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1), at,pt,vt,ft,kt,pinf,finf,kinf,rankp,lik,tol,meps,c,p,m,j)
+
         end do diffuse
 
-        if(rankp .EQ. 0) then
+        if(rankp .EQ. 0 .AND. j .LT. p) then
             !non-diffuse filtering begins
-            do i = j+1, p
-                if(ymiss(d,i).EQ.0) then
-                    vt(i) = yt(d,i) - ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,arec,1)
-                    call dsymv('u',m,1.0d0,pt,m,zt(i,:,(d-1)*timevar(1)+1),1,0.0d0,kt(:,i),1)
-                    ft(i) = ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,kt(:,i),1) + ht(i,i,(d-1)*timevar(2)+1)
-                    if (ft(i).GT. tol*maxval(zt(i,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/ft(i)
-                        arec = arec + vt(i)*finv*kt(:,i)
-                        call dsyr('u',m,-finv,kt(:,i),1,pt,m)
-                        lik = lik - 0.5d0*(log(ft(i)) + vt(i)**2*finv)-c
-                    end if
-                end if
-            end do
-   
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            call dsymm('r','u',m,m,1.0d0,pt,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,m)
- 
-            pt = pt + rqr(:,:,(d-1)*tv+1)
+            call filteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),at,pt,vt,ft,kt,lik,tol,meps,c,p,m,j)
+
+        else
+            j = p
+
         end if
     end if
 
@@ -130,25 +77,8 @@ subroutine gloglik(yt, ymiss, timevar, zt, ht, tt, rt, qt, a1, p1, p1inf,&
     !Non-diffuse filtering continues from t=d+1, i=1
 
     do t = d+1, n
-        do i = 1, p
-            if(ymiss(t,i).EQ.0) then
-                vt(i) = yt(t,i) - ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,arec,1)
-                call dsymv('u',m,1.0d0,pt,m,zt(i,:,(t-1)*timevar(1)+1),1,0.0d0,kt(:,i),1)
-                ft(i) = ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,kt(:,i),1) + ht(i,i,(t-1)*timevar(2)+1)
-                if (ft(i).GT. tol*maxval(zt(i,:,(t-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/ft(i)
-                    arec = arec + vt(i)*finv*kt(:,i)
-                    call dsyr('u',m,-finv,kt(:,i),1,pt,m)
-                    lik = lik - 0.5d0*(log(ft(i)) + vt(i)**2*finv)-c
-                end if
-            end if
-        end do
-        call dgemv('n',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-        arec = at
-        call dsymm('r','u',m,m,1.0d0,pt,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,m)
-        pt = pt + rqr(:,:,(t-1)*tv+1)
-
+        call filteronestep(ymiss(t,:),yt(t,:),transpose(zt(:,:,(t-1)*timevar(1)+1)),ht(:,:,(t-1)*timevar(2)+1),&
+        tt(:,:,(t-1)*timevar(3)+1),rqr(:,:,(t-1)*tv+1),at,pt,vt,ft,kt,lik,tol,meps,c,p,m,0)
 
     end do
 
diff --git a/src/kfilter.f95 b/src/kfilter.f95
index 387fe20..7147f89 100644
--- a/src/kfilter.f95
+++ b/src/kfilter.f95
@@ -57,124 +57,36 @@ subroutine kfilter(yt, ymiss, timevar, zt, ht,tt, rt, qt, a1, p1, p1inf, p,n,m,r
     if(rankp .GT. 0) then
         diffuse: do while(d .LT. n .AND. rankp .GT. 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)
-                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)
-                    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*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/finf(j,d)
-                        arec = arec + vt(j,d)*finv*kinf(:,j,d)
-                        call dsyr('u',m,ft(j,d)*finv**2,kinf(:,j,d),1,prec,m)
-                        call dsyr2('u',m,-finv,kt(:,j,d),1,kinf(:,j,d),1,prec,m)
-                        call dsyr('u',m,-finv,kinf(:,j,d),1,pirec,m)
-
-                        lik = lik - 0.5d0*log(finf(j,d))
-                        rankp = rankp -1
-                    else
-                        finf(j,d) = 0.0d0
-                        if(ft(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                            finv = 1.0d0/ft(j,d)
-                            arec = arec + vt(j,d)*finv*kt(:,j,d)
-                            call dsyr('u',m,-finv,kt(:,j,d),1,prec,m)
-                            lik = lik - c - 0.5d0*(log(ft(j,d)) + vt(j,d)**2*finv)
-                        end if
-                    end if
-                    if (ft(j,d) .LE. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        ft(j,d) = 0.0d0
-                    end if
-                    if(rankp .EQ. 0) then
-                        exit diffuse
-                    end if
-                end if
-            end do
-
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at(:,d+1),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)
-
-            pt(:,:,d+1) = pt(:,:,d+1) + rqr(:,:,(d-1)*tv+1)
-            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)
-
-            do i = 1, m
-                if(pinf(i,i,d+1) .LT. meps) then
-                    pinf(i,:,d+1) = 0.0d0
-                    pinf(:,i,d+1) = 0.0d0
-                end if
-            end do
-            pirec = pinf(:,:,d+1)
-
+            at(:,d+1) = at(:,d)
+            pt(:,:,d+1) = pt(:,:,d)
+            pinf(:,:,d+1) = pinf(:,:,d)
+            call diffusefilteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at(:,d+1),pt(:,:,d+1),vt(:,d),ft(:,d),kt(:,:,d),pinf(:,:,d+1),finf(:,d),kinf(:,:,d),rankp,lik,tol,meps,c,p,m,j)
         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)
-                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. tol*maxval(zt(i,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/ft(i,d)
-                        arec = arec + vt(i,d)*finv*kt(:,i,d)
-                        call dsyr('u',m,-finv,kt(:,i,d),1,prec,m)
-                        lik = lik - c - 0.5d0*(log(ft(i,d)) + vt(i,d)**2*finv)
-                    else
-                        ft(i,d)=0.0d0
-                    end if
-                end if
-            end do
+        if(rankp .EQ. 0 .AND. j .LT. p) then
+                !non-diffuse filtering begins
 
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at(:,d+1),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)
+            call filteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at(:,d+1),pt(:,:,d+1),vt(:,d),ft(:,d),kt(:,:,d),lik,tol,meps,c,p,m,j)
 
-            pt(:,:,d+1) = pt(:,:,d+1) + rqr(:,:,(d-1)*tv+1)
-            prec = pt(:,:,d+1)
-            arec = at(:,d+1)
+        else
+            j = p
         end if
     end if
 
     !Non-diffuse filtering continues from t=d+1, i=1
 
 
-    if(d .EQ. n .AND. j .EQ. p+1) then
-        j = p
-    end if
-
     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)
-            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. tol*maxval(zt(i,:,(t-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/ft(i,t)
-                    arec = arec + vt(i,t)*finv*kt(:,i,t)
-                    call dsyr('u',m,-finv,kt(:,i,t),1,prec,m)
-                    lik = lik - c - 0.5d0*(log(ft(i,t)) + vt(i,t)**2*finv)
-                else
-                    ft(i,t)=0.0d0
-                end if
-            end if
-        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)
-
-        pt(:,:,t+1) = pt(:,:,t+1) + rqr(:,:,(t-1)*tv+1)
-        prec = pt(:,:,t+1)
-        arec = at(:,t+1)
-
+        at(:,t+1) = at(:,t)
+        pt(:,:,t+1) = pt(:,:,t)
+        call filteronestep(ymiss(t,:),yt(t,:),transpose(zt(:,:,(t-1)*timevar(1)+1)),ht(:,:,(t-1)*timevar(2)+1),&
+        tt(:,:,(t-1)*timevar(3)+1),rqr(:,:,(t-1)*tv+1),&
+        at(:,t+1),pt(:,:,t+1),vt(:,t),ft(:,t),kt(:,:,t),lik,tol,meps,c,p,m,0)
     end do
 
     if(filtersignal.EQ.1) then
diff --git a/src/kfstheta.f95 b/src/kfstheta.f95
index 96075ef..a4f6b25 100644
--- a/src/kfstheta.f95
+++ b/src/kfstheta.f95
@@ -22,7 +22,7 @@ subroutine kfstheta(yt, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, tv, a1, p1, p1inf
     double precision, dimension(m,p,n) :: kt,kinf
     double precision, dimension(p,n) :: vt
     double precision, dimension(m) :: at, arec
-    double precision, dimension(m,m) :: mm,pirec,prec
+    double precision, dimension(m,m) :: mm,pinf,pt
     double precision, intent(in) :: tol
     double precision, dimension(m) :: rrec,rrec1,rhelp,help
     double precision, dimension(m,m) :: im,linf,l0
@@ -43,124 +43,34 @@ subroutine kfstheta(yt, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, tv, a1, p1, p1inf
     end do
     j=0
     d=0
-    prec = p1
-    pirec = p1inf
-    arec = a1
-    if(maxval(pirec) .GT.  0.0d0) then
-        diffuse: do while(d .LT. n)
+    pt = p1
+    pinf = p1inf
+    at = a1
+    if(rankp .GT. 0) then
+        diffuse: do while(d .LT. n .AND. rankp .GT. 0)
             d = d+1
-            do j=1, p
-                if(ymiss(d,j).EQ.0) then
-                    vt(j,d) = yt(d,j) - ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,arec,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)
-
-                    call dsymv('u',m,1.0d0,pirec,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kinf(:,j,d),1)
-                    finf(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kinf(:,j,d),1)
-
-
-                    if (finf(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/finf(j,d)
-                        arec = arec + vt(j,d)*finv*kinf(:,j,d)
-                        call dsyr('u',m,ft(j,d)*finv**2,kinf(:,j,d),1,prec,m)
-                        call dsyr2('u',m,-finv,kt(:,j,d),1,kinf(:,j,d),1,prec,m)
-                        call dsyr('u',m,-finv,kinf(:,j,d),1,pirec,m)
-
-                        lik = lik - 0.5d0*log(finf(j,d))
-                        rankp = rankp -1
-
-                    else
-                        finf(j,d) = 0.0d0
-                        if(ft(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                            finv = 1.0d0/ft(j,d)
-                            arec = arec + vt(j,d)*finv*kt(:,j,d)
-                            call dsyr('u',m,-finv,kt(:,j,d),1,prec,m)
-
-                            lik = lik - c - 0.5d0*(log(ft(j,d)) + vt(j,d)**2*finv)
-                        end if
-                    end if
-                    if (ft(j,d) .LE. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        ft(j,d) = 0.0d0
-                    end if
-
-                    if(rankp .EQ. 0) then
-                        exit diffuse
-                    end if
-                end if
-            end do
-
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            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,prec,m)
-
-            prec = prec + rqr(:,:,(d-1)*tv+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,pirec,m)
-            do i = 1, m
-                if(pirec(i,i) .LT. meps) then
-                    pirec(i,:) = 0.0d0
-                    pirec(:,i) = 0.0d0
-                end if
-            end do
-
+            call diffusefilteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt(:,d),ft(:,d),kt(:,:,d),pinf,finf(:,d),kinf(:,:,d),rankp,lik,tol,meps,c,p,m,j)
         end do diffuse
 
-        !non-diffuse filtering begins
-        if(rankp .EQ. 0) then
-            do i = j+1, p
-                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)
-                    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)
-                    if (ft(i,d) .GT. tol*maxval(zt(i,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/ft(i,d)
-                        arec = arec + vt(i,d)*finv*kt(:,i,d)
-                        call dsyr('u',m,-finv,kt(:,i,d),1,prec,m)
-
-                        lik = lik - c - 0.5d0*(log(ft(i,d)) + vt(i,d)**2*finv)
-                    else
-                        ft(i,d)=0.0d0
-                    end if
-                end if
-            end do
+        if(rankp .EQ. 0 .AND. j .LT. p) then
+            call filteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt(:,d),ft(:,d),kt(:,:,d),lik,tol,meps,c,p,m,j)
 
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,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,prec,m)
-            prec = prec + rqr(:,:,(d-1)*tv+1)
-            arec = at
+        else
+            j = p
         end if
-    end if
 
-    !Non-diffuse filtering continues from t=d+1, i=1
-    if(d .EQ. n .AND. j .EQ. p+1) then
-        j = p
     end if
 
+    !Non-diffuse filtering continues from t=d+1, i=1
     do t = d+1, n
-        do i = 1, p
-            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)
-                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)
-                if (ft(i,t) .GT. tol*maxval(zt(i,:,(t-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/ft(i,t)
-                    arec = arec + vt(i,t)*finv*kt(:,i,t)
-                    call dsyr('u',m,-finv,kt(:,i,t),1,prec,m)
-                    lik = lik - c - 0.5d0*(log(ft(i,t)) + vt(i,t)**2*finv)
-                else
-                    ft(i,t)=0.0d0
-                end if
-            end if
-        end do
+        call filteronestep(ymiss(t,:),yt(t,:),transpose(zt(:,:,(t-1)*timevar(1)+1)),ht(:,:,(t-1)*timevar(2)+1),&
+        tt(:,:,(t-1)*timevar(3)+1),rqr(:,:,(t-1)*tv+1),&
+        at,pt,vt(:,t),ft(:,t),kt(:,:,t),lik,tol,meps,c,p,m,0)
 
-        call dgemv('n',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,arec,1,0.0d0,at,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,prec,m)
-        prec = prec + rqr(:,:,(t-1)*tv+1)
-        arec = at
     end do
 
     !smoothing begins
@@ -215,23 +125,23 @@ subroutine kfstheta(yt, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, tv, a1, p1, p1inf
                     l0=0.0d0
                     call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
 
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
+                    call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1)
                     rrec1 = rhelp
                     call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
                     rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
 
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
+                    call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1)
                     rrec = rhelp
                 else
                     if(ft(i,t).GT. 0.0d0) then
                         finv = 1.0d0/ft(i,t)
                         l0 = im
-                        call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !lt = I -Kt*Z/Ft
+                        call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
 
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1) !r0
-                        rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)   !!r0 = Z'vt/Ft - Lt'r0
+                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                        rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
 
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1) !r1
+                        call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
                         rrec1 = rhelp
                     end if
                 end if
@@ -258,23 +168,23 @@ subroutine kfstheta(yt, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, tv, a1, p1, p1inf
                         l0=0.0d0
                         call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
 
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
+                        call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1)
                         rrec1 = rhelp
                         call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
                         rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
 
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
+                        call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1)
                         rrec = rhelp
                     else
                         if(ft(i,t).GT.  0.0d0) then
                             finv = 1.0d0/ft(i,t)
                             l0 = im
-                            call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m) !lt = I -Kt*Z/Ft
+                            call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
 
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1) !r0
-                            rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)   !!r0 = Z'vt/Ft - Lt'r0
+                            call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
+                            rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
 
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1) !r1
+                            call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
                             rrec1 = rhelp
 
                         end if
diff --git a/src/ptheta.f95 b/src/ptheta.f95
index 0e129f0..38560f9 100644
--- a/src/ptheta.f95
+++ b/src/ptheta.f95
@@ -28,128 +28,52 @@ subroutine pthetafirst(yt, timevar, zt, tt, rqr, a1, p1, p1inf,&
     double precision, external :: ddot
     double precision, intent(inout), dimension(m,m,(n-1)*max(timevar(4),timevar(5))+1) :: rqr
     double precision :: meps, finv
- 
+    integer, dimension(p) :: ymiss
+    double precision, dimension(p,p) :: ht
 
     external dgemm, dsymm, dgemv, dsymv, dsyr, dsyr2
 
     meps = epsilon(meps)
     tv= max(timevar(4),timevar(5))
-
+    ymiss = 0
+    ht = 0.0d0
 
     rankp = rankp2
     j=0
     d=0
-    arec = a1
+    at = a1
     pt = p1
     pinf=p1inf
 
     ! Diffuse initialization
-    if(maxval(pinf) .GT.  0.0d0) then
-        diffuse: do while(d .LT. n)
+    if(rankp .GT. 0) then
+        diffuse: do while(d .LT. n .AND. rankp .GT. 0)
             d = d+1
-            do j=1, p
-                vt(j) = yt(d,j) - ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,arec,1)
-                call dsymv('u',m,1.0d0,pt,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)
-                call dsymv('u',m,1.0d0,pinf,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kinf(:,j,d),1)
-                finf(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kinf(:,j,d),1)
-                if (finf(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/finf(j,d)
-                    arec = arec +vt(j)*finv*kinf(:,j,d)
-                    call dsyr('u',m,ft(j,d)*finv**2,kinf(:,j,d),1,pt,m)
-                    call dsyr2('u',m,-finv,kt(:,j,d),1,kinf(:,j,d),1,pt,m)
-                    call dsyr('u',m,-finv,kinf(:,j,d),1,pinf,m)
+            call diffusefilteronestep(ymiss,yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht,&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt,ft(:,d),kt(:,:,d),pinf,finf(:,d),kinf(:,:,d),rankp,lik,tol,meps,0.0d0,p,m,j)
 
-                    lik = lik - 0.5d0*log(finf(j,d))
-                    rankp = rankp -1
 
-                else
-                    finf(j,d) = 0.0d0
-                    if (ft(j,d).GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/ft(j,d)
-                        arec = arec + vt(j)*finv*kt(:,j,d)
-                        call dsyr('u',m,-finv,kt(:,j,d),1,pt,m)
-                        lik = lik - 0.5d0*(log(ft(j,d)) + vt(j)**2*finv)
-                    end if
-                end if
-                if(ft(j,d) .LE. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                    ft(j,d) = 0.0d0
-                end if
-                if(rankp .EQ. 0) then
-                    exit diffuse
-                end if
-
-            end do
-           
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            call dsymm('r','u',m,m,1.0d0,pt,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,m)
-            pt = pt + rqr(:,:,(d-1)*tv+1)
-            call dsymm('r','u',m,m,1.0d0,pinf,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,m)
-            do i = 1, m
-                if(pinf(i,i) .LT. meps) then
-                    pinf(i,:) = 0.0d0
-                    pinf(:,i) = 0.0d0
-                end if
-            end do
         end do diffuse
-        if(rankp .EQ. 0) then
-            !non-diffuse filtering begins
-            do i = j+1, p
-
-                vt(i) = yt(d,i) - ddot(m,zt(i,:,(d-1)*timevar(1)+1),1,arec,1)
-                call dsymv('u',m,1.0d0,pt,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)
-                if (ft(i,d).GT. tol*maxval(zt(i,:,(d-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/ft(i,d)
-                    arec = arec + vt(i)*finv*kt(:,i,d)
-                    call dsyr('u',m,-finv,kt(:,i,d),1,pt,m)
-                    lik = lik - 0.5d0*(log(ft(i,d)) + vt(i)**2*finv)
+        if(rankp .EQ. 0 .AND. j .LT. p) then
+            call filteronestep(ymiss,yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht,&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt,ft(:,d),kt(:,:,d),lik,tol,meps,0.0d0,p,m,j)
 
-
-                else
-                    ft(i,d) = 0.0d0
-                end if
-
-            end do
-   
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            call dsymm('r','u',m,m,1.0d0,pt,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,m)
- 
-            pt = pt + rqr(:,:,(d-1)*tv+1)
+        else
+            j = p
         end if
+
     end if
 
 
 
     !Non-diffuse filtering continues from t=d+1, i=1
-    if(d .EQ. n .AND. j .EQ. p+1) then
-        j = p
-    end if
-    do t = d+1, n
-        do i = 1, p
-            vt(i) = yt(t,i) - ddot(m,zt(i,:,(t-1)*timevar(1)+1),1,arec,1)
-            call dsymv('u',m,1.0d0,pt,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)
-            if (ft(i,t) .GT. tol*maxval(zt(i,:,(t-1)*timevar(1)+1)**2)) then
-                finv = 1.0d0/ft(i,t)
-                arec = arec + vt(i)*finv*kt(:,i,t)
-                call dsyr('u',m,-finv,kt(:,i,t),1,pt,m)
-                lik = lik - 0.5d0*(log(ft(i,t)) + vt(i)**2*finv)
 
-            else
-                ft(i,t) = 0.0d0
-            end if
-        end do
-        call dgemv('n',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-        arec = at
-        call dsymm('r','u',m,m,1.0d0,pt,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,m)
-        pt = pt + rqr(:,:,(t-1)*tv+1)
+    do t = d+1, n
+        call filteronestep(ymiss,yt(t,:),transpose(zt(:,:,(t-1)*timevar(1)+1)),ht,&
+        tt(:,:,(t-1)*timevar(3)+1),rqr(:,:,(t-1)*tv+1),&
+        at,pt,vt,ft(:,t),kt(:,:,t),lik,tol,meps,0.0d0,p,m,0)
     end do
 
 
diff --git a/src/smoothonestep.f95 b/src/smoothonestep.f95
new file mode 100644
index 0000000..1bc8541
--- /dev/null
+++ b/src/smoothonestep.f95
@@ -0,0 +1,131 @@
+!non-diffuse smoothing for single time point
+subroutine smoothonestep(ymiss, zt, ht, tt, rtv, qt, vt, ft,kt,&
+im,p,m,r,j,rt,etahat,epshat,needeps)
+
+    implicit none
+
+    logical, intent(in) :: needeps
+    integer, intent(in) ::  p, m,j,r
+    integer ::  i
+    integer, intent(in), dimension(p) :: ymiss
+    double precision, intent(in), dimension(m,p) :: zt
+    double precision, intent(in), dimension(p,p) :: ht
+    double precision, intent(in), dimension(m,m) :: tt
+    double precision, intent(in), dimension(m,r) :: rtv
+    double precision, intent(in), dimension(r,r) :: qt
+    double precision, intent(inout), dimension(m) :: rt
+    double precision, intent(inout), dimension(r) :: etahat
+    double precision, intent(inout), dimension(p) :: epshat
+    double precision, intent(in), dimension(p) :: vt,ft
+    double precision, intent(in), dimension(m,p) :: kt
+    double precision, intent(in), dimension(m,m) :: im
+    double precision, dimension(m,m) :: l0
+    double precision, dimension(m) :: mhelp
+    double precision, dimension(r) :: rhelp
+    double precision :: finv
+
+    double precision, external :: ddot
+
+    external dgemv, dsymv, dger
+
+    call dgemv('t',m,r,1.0d0,rtv,m,rt,1,0.0d0,rhelp,1)
+    call dsymv('l',r,1.0d0,qt,r,rhelp,1,0.0d0,etahat,1)
+    call dgemv('t',m,m,1.0d0,tt,m,rt,1,0.0d0,mhelp,1)
+    rt = mhelp
+    do i = p, j , -1
+        if(ymiss(i) .EQ. 0) then
+            if(ft(i) .GT. 0.0d0) then
+                finv = 1.0d0/ft(i)
+                if(needeps) then
+                    epshat(i) = ht(i,i)*(vt(i)-ddot(m,kt(:,i),1,rt,1))*finv
+                end if
+                l0 = im
+                call dger(m,m,-finv,kt(:,i),1,zt(:,i),1,l0,m)
+                call dgemv('t',m,m,1.0d0,l0,m,rt,1,0.0d0,mhelp,1)
+                rt = mhelp + vt(i)*finv*zt(:,i)
+            end if
+        end if
+    end do
+
+end subroutine smoothonestep
+
+subroutine diffusesmoothonestep(ymiss, zt, ht, tt, rtv, qt, vt, ft,kt,&
+im,p,m,r,j,rt,rt1,finf,kinf,etahat,epshat,needeps)
+
+    implicit none
+
+    logical, intent(in) :: needeps
+    integer, intent(in) ::  p, m,j,r
+    integer ::  i
+    integer, intent(in), dimension(p) :: ymiss
+    double precision, intent(in), dimension(m,p) :: zt
+    double precision, intent(in), dimension(p,p) :: ht
+    double precision, intent(in), dimension(m,m) :: tt
+    double precision, intent(in), dimension(m,r) :: rtv
+    double precision, intent(in), dimension(r,r) :: qt
+    double precision, intent(inout), dimension(m) :: rt,rt1
+    double precision, intent(inout), dimension(r) :: etahat
+    double precision, intent(inout), dimension(p) :: epshat
+    double precision, intent(in), dimension(p) :: vt,ft,finf
+    double precision, intent(in), dimension(m,p) :: kt,kinf
+    double precision, intent(in), dimension(m,m) :: im
+    double precision, dimension(m,m) :: l0,linf
+    double precision, dimension(m) :: mhelp
+    double precision, dimension(r) :: rhelp
+    double precision :: finv
+
+    double precision, external :: ddot
+
+    external dgemv, dsymv, dger
+
+    if(j .EQ. p) then
+        call dgemv('t',m,r,1.0d0,rtv,m,rt,1,0.0d0,rhelp,1)
+        call dsymv('l',r,1.0d0,qt,r,rhelp,1,0.0d0,etahat,1)
+        call dgemv('t',m,m,1.0d0,tt,m,rt,1,0.0d0,mhelp,1)
+        rt = mhelp
+        call dgemv('t',m,m,1.0d0,tt,m,rt1,1,0.0d0,mhelp,1)
+        rt1 = mhelp
+    end if
+
+    do i = j, 1, -1
+        if(ymiss(i) .EQ. 0) then
+            if(finf(i) .GT. 0.0d0) then
+                finv = 1.0d0/finf(i)
+                if(needeps) then
+                    epshat(i) = -ht(i,i)*ddot(m,kinf(:,i),1,rt,1)*finv
+                end if
+                linf = im
+                call dger(m,m,-finv,kinf(:,i),1,zt(:,i),1,linf,m)
+
+                mhelp = kinf(:,i)*ft(i)*finv - kt(:,i)
+                l0 = 0.0d0
+                call dger(m,m,finv,mhelp,1,zt(:,i),1,l0,m)
+
+                call dgemv('t',m,m,1.0d0,linf,m,rt1,1,0.0d0,mhelp,1)
+                rt1 = mhelp
+                call dgemv('t',m,m,1.0d0,l0,m,rt,1,1.0d0,rt1,1)
+                rt1 = rt1 + vt(i)*finv*zt(:,i)
+
+                call dgemv('t',m,m,1.0d0,linf,m,rt,1,0.0d0,mhelp,1)
+                rt = mhelp
+            else
+                if(ft(i) .GT. 0.0d0) then
+                    finv = 1.0d0/ft(i)
+                    if(needeps) then
+                        epshat(i) = ht(i,i)*(vt(i)-ddot(m,kt(:,i),1,rt,1))*finv
+                    end if
+
+                    l0 = im
+                    call dger(m,m,-finv,kt(:,i),1,zt(:,i),1,l0,m)
+
+                    call dgemv('t',m,m,1.0d0,l0,m,rt,1,0.0d0,mhelp,1)
+                    rt = mhelp + vt(i)*finv*zt(:,i)
+
+                    call dgemv('t',m,m,1.0d0,l0,m,rt1,1,0.0d0,mhelp,1)
+                    rt1 = mhelp
+                end if
+            end if
+        end if
+    end do
+
+end subroutine diffusesmoothonestep
diff --git a/src/smoothsim.f95 b/src/smoothsim.f95
index 1365ca6..ddbce63 100644
--- a/src/smoothsim.f95
+++ b/src/smoothsim.f95
@@ -23,295 +23,91 @@ subroutine smoothsim(yt, ymiss, timevar, zt, ht,tt, rtv,qt,rqr, a1, p1, p1inf, &
     double precision, intent(inout), dimension(m,p,n) :: kt,kinf
     double precision, dimension(p,n) :: vt
     double precision, dimension(m) :: at, arec
-    double precision, dimension(m,m) :: prec,pirec,mm
+    double precision, dimension(m,m) :: pt,pinf,mm
     double precision, intent(in) :: tol
     double precision, dimension(m) :: rrec,rrec1,rhelp,help
     double precision, dimension(m,m) :: im,linf,l0
     double precision, intent(inout), dimension(r,n) :: etahat
     double precision, intent(inout), dimension(p,n) :: epshat
     double precision, intent(inout), dimension(m) :: rt0,rt1
-    double precision :: meps, finv
+    double precision :: meps, finv,lik
     double precision, external :: ddot
 
     external dgemm, dsymm, dgemv, dsymv, dsyr, dsyr2, dger
  
     meps = epsilon(meps)
     tv = max(timevar(4),timevar(5))
-
+    lik = 0.0d0
     im = 0.0d0
     do i = 1, m
         im(i,i) = 1.0d0
     end do
     j=0
     d=0
-    pirec = p1inf
-    prec = p1
-    arec = a1
-    if(maxval(pirec) .GT.  0.0d0) then
-
-
-        diffuse: do while(d .LT. n)
+    pinf = p1inf
+    pt = p1
+    at = a1
+    if(rankp .GT. 0) then
+        diffuse: do while(d .LT. n .AND. rankp .GT. 0)
             d = d+1
-            do j=1, p
-                if(ymiss(d,j).EQ.0) then
-
-                    vt(j,d) = yt(d,j) - ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,arec,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)
-
-                    call dsymv('u',m,1.0d0,pirec,m,zt(j,:,(d-1)*timevar(1)+1),1,0.0d0,kinf(:,j,d),1)
-                    finf(j,d) = ddot(m,zt(j,:,(d-1)*timevar(1)+1),1,kinf(:,j,d),1)
-
-
-                    if (finf(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/finf(j,d)
-                        arec = arec + vt(j,d)*finv*kinf(:,j,d)
-                        call dsyr('u',m,ft(j,d)*finv**2,kinf(:,j,d),1,prec,m)
-                        call dsyr2('u',m,-finv,kt(:,j,d),1,kinf(:,j,d),1,prec,m)
-                        call dsyr('u',m,-finv,kinf(:,j,d),1,pirec,m)
-
-                        rankp = rankp -1
-                    else
-                        finf(j,d) = 0.0d0
-                        if(ft(j,d) .GT. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                            finv = 1.0d0/ft(j,d)
-                            arec = arec + vt(j,d)*finv*kt(:,j,d)
-                            call dsyr('u',m,-finv,kt(:,j,d),1,prec,m)
-                        end if
-                    end if
-                    if (ft(j,d) .LE. tol*maxval(zt(j,:,(d-1)*timevar(1)+1)**2)) then
-                        ft(j,d) = 0.0d0
-                    end if
-                    if(rankp .EQ. 0) then
-                        exit diffuse
-                    end if
-                end if
-            end do
-
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,1)
-            arec = at
-            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,prec,m)
-
-            prec = prec + rqr(:,:,(d-1)*tv+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,pirec,m)
-
-            do i = 1, m
-                if(pirec(i,i) .LT. meps) then
-                    pirec(i,:) = 0.0d0
-                    pirec(:,i) = 0.0d0
-                end if
-            end do
-
+            call diffusefilteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt(:,d),ft(:,d),kt(:,:,d),pinf,finf(:,d),kinf(:,:,d),rankp,lik,tol,meps,0.0d0,p,m,j)
         end do diffuse
+        if(rankp .EQ. 0 .AND. j .LT. p) then
+            call filteronestep(ymiss(d,:),yt(d,:),transpose(zt(:,:,(d-1)*timevar(1)+1)),ht(:,:,(d-1)*timevar(2)+1),&
+            tt(:,:,(d-1)*timevar(3)+1),rqr(:,:,(d-1)*tv+1),&
+            at,pt,vt(:,d),ft(:,d),kt(:,:,d),lik,tol,meps,0.0d0,p,m,j)
 
-        !non-diffuse filtering begins
-        if(rankp .EQ. 0) then
-            do i = j+1, p
-                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)
-                    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)
-                    if (ft(i,d) .GT. tol*maxval(zt(i,:,(d-1)*timevar(1)+1)**2)) then
-                        finv = 1.0d0/ft(i,d)
-                        arec = arec + vt(i,d)*finv*kt(:,i,d)
-                        call dsyr('u',m,-finv,kt(:,i,d),1,prec,m)
-                    else
-                        ft(i,d)=0.0d0
-                    end if
-                end if
-            end do
-
-            call dgemv('n',m,m,1.0d0,tt(:,:,(d-1)*timevar(3)+1),m,arec,1,0.0d0,at,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,prec,m)
-
-            prec = prec + rqr(:,:,(d-1)*tv+1)
-            arec = at
+        else
+            j = p
         end if
+
     end if
 
     !Non-diffuse filtering continues from t=d+1, i=1
 
-
-
-    if(d .EQ. n .AND. j .EQ. p+1) then
-        j = p
-    end if
-
     do t = d+1, n
-        do i = 1, p
-            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)
-                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)
-                if (ft(i,t) .GT. tol*maxval(zt(i,:,(t-1)*timevar(1)+1)**2)) then
-                    finv = 1.0d0/ft(i,t)
-                    arec = arec + vt(i,t)*finv*kt(:,i,t)
-                    call dsyr('u',m,-finv,kt(:,i,t),1,prec,m)
-                else
-                    ft(i,t)=0.0d0
-                end if
-
-            end if
-        end do
-
-        call dgemv('n',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,arec,1,0.0d0,at,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,prec,m)
-
-        prec = prec + rqr(:,:,(t-1)*tv+1)
-
-        arec = at
+        call filteronestep(ymiss(t,:),yt(t,:),transpose(zt(:,:,(t-1)*timevar(1)+1)),ht(:,:,(t-1)*timevar(2)+1),&
+        tt(:,:,(t-1)*timevar(3)+1),rqr(:,:,(t-1)*tv+1),&
+        at,pt,vt(:,t),ft(:,t),kt(:,:,t),lik,tol,meps,0.0d0,p,m,0)
     end do
 
     !smoothing begins
 
-    rrec = 0.0d0
+    rt0 = 0.0d0
 
     do t = n, d+1, -1
-        call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-        call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-        call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-        rrec = rhelp
-        do i = p, 1 , -1
-            if(ymiss(t,i).EQ.0) then
-                if(ft(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/ft(i,t)
-                    if(needeps) then
-                        epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                    end if
-                    l0 = im
-                    call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-                end if
-            end if
-        end do
-    end do
+        call smoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+        tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+        ft(:,t),kt(:,:,t), im,p,m,r,1,rt0,etahat(:,t),epshat(:,t),needeps)
 
-    if(d.GT.0) then
-        t=d
-        call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-        call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-        call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-        rrec = rhelp
-        do i = p, (j+1) , -1
-            if(ymiss(t,i).EQ.0) then
-                if(ft(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/ft(i,t)
-                    if(needeps) then
-                        epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*finv*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))
-                    end if
-                    l0 = im
-                    call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-                end if
-            end if
-        end do
-        rrec1 = 0.0d0
-        do i = j, 1, -1
-            if(ymiss(t,i).EQ.0) then
-                if(finf(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/finf(i,t)
-                    if(needeps) then
-                        epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)*finv
-                    end if
-                    linf = im
-                    call dger(m,m,-finv,kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m)
-
-                    rhelp = kinf(:,i,t)*ft(i,t)*finv - kt(:,i,t)
-                    l0=0.0d0
-                    call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
+    end do
 
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1)
-                    rrec1 = rhelp
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                    rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
+    if(d .GT. 0) then
+        t = d
+        if(j .LT. p) then
+            call smoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+            tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+            ft(:,t),kt(:,:,t), im,p,m,r,j+1,rt0,etahat(:,t),epshat(:,t),needeps)
+        end if
 
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp
-                else
-                    if(ft(i,t) .GT. 0.0d0) then
-                        finv = 1.0d0/ft(i,t)
-                        if(needeps) then
-                            epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                        end if
 
-                        l0 = im
-                        call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
+        rt1 = 0.0d0
+        call diffusesmoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+        tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+        ft(:,t),kt(:,:,t), im,p,m,r,j,rt0,rt1,finf(:,t),kinf(:,:,t),etahat(:,t),epshat(:,t),needeps)
 
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                        rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
-                        rrec1 = rhelp
-                    end if
-                end if
-            end if
-        end do
 
         do t=(d-1), 1, -1
-            call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-            call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-            call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-            rrec = rhelp
-            call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec1,1,0.0d0,rhelp,1)
-            rrec1 = rhelp
-
-            do i = p, 1, -1
-                if(ymiss(t,i).EQ.0) then
-                    if(finf(i,t) .GT. 0.0d0) then
-                        finv = 1.0d0/finf(i,t)
-                        if(needeps) then
-                            epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)*finv
-                        end if
-
-
-                        linf = im
-                        call dger(m,m,-finv,kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m)
-
-                        rhelp = kinf(:,i,t)*ft(i,t)*finv - kt(:,i,t)
-                        l0=0.0d0
-                        call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
 
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
-                        rrec1 = rhelp
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                        rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
+        call diffusesmoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+        tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+        ft(:,t),kt(:,:,t), im,p,m,r,p,rt0,rt1,finf(:,t),kinf(:,:,t),etahat(:,t),epshat(:,t),needeps)
 
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
-                        rrec = rhelp
-                    else
-                        if(ft(i,t) .GT. 0.0d0) then
-                            finv = 1.0d0/ft(i,t)
-                            if(needeps) then
-                                epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                            end if
 
-                            l0 = im
-                            call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                            rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
-                            rrec1 = rhelp
-
-                        end if
-
-                    end if
-                end if
-            end do
 
         end do
     end if
 
-    rt0=rrec
-    rt1=rrec1
-
 end subroutine smoothsim
diff --git a/src/smoothsimfast.f95 b/src/smoothsimfast.f95
index a23d712..4a54e42 100644
--- a/src/smoothsimfast.f95
+++ b/src/smoothsimfast.f95
@@ -29,7 +29,6 @@ subroutine smoothsimfast(yt, ymiss, timevar, zt, ht,tt, rtv,qt,a1, ft,kt,&
 
     external dgemv, dger, dsymv
 
-
     j = 0
     d = 0
     if(dt.GT.0) then
@@ -114,151 +113,30 @@ subroutine smoothsimfast(yt, ymiss, timevar, zt, ht,tt, rtv,qt,a1, ft,kt,&
         im(i,i) = 1.0d0
     end do
 
-    rrec = 0.0d0
+    rt0 = 0.0d0
 
-    do t = n, dt+1, -1
-        call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-        call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-        call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-        rrec = rhelp
-        do i = p, 1 , -1
-            if(ymiss(t,i).EQ.0) then
-                if(ft(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/ft(i,t)
-                    if(needeps) then
-                        epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                    end if
-                    l0 = im
-                    call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-                end if
-            end if
-        end do
+    do t = n, d+1, -1
+        call smoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+        tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+        ft(:,t),kt(:,:,t), im,p,m,r,1,rt0,etahat(:,t),epshat(:,t),needeps)
     end do
 
     if(dt.GT.0) then
         t=dt
-        call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-        call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-   
-        call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-        rrec = rhelp       
-  
-        do i = p, (jt+1) , -1
-
-            if(ymiss(t,i).EQ.0) then
-                if(ft(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/ft(i,t)
-                    if(needeps) then
-                        epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                    end if
-                    l0 = im
-                    call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                    rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-                end if
-            end if
-        end do
-
-        rrec1 = 0.0d0
-        do i = jt, 1, -1
-            if(ymiss(t,i).EQ.0) then
-                if(finf(i,t) .GT. 0.0d0) then
-                    finv = 1.0d0/finf(i,t)
-                    if(needeps) then
-                        epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)*finv
-                    end if
-                    linf = im
-                    call dger(m,m,-finv,kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m)
-
-                    rhelp = kinf(:,i,t)*ft(i,t)*finv - kt(:,i,t)
-                    l0=0.0d0
-                    call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1) !rt1
-                    rrec1 = rhelp
-                    call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                    rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                    call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1) !rt0
-                    rrec = rhelp
-                else
-                    if(ft(i,t) .GT. 0.0d0) then
-                        finv = 1.0d0/ft(i,t)
-                        if(needeps) then
-                            epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                        end if
-
-                        l0 = im
-                        call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                        rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
-                        rrec1 = rhelp
-                    end if
-                end if
-            end if
-        end do
-
+        if(jt .LT. p) then
+            call smoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+            tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+            ft(:,t),kt(:,:,t), im,p,m,r,jt+1,rt0,etahat(:,t),epshat(:,t),needeps)
+        end if
+        rt1 = 0.0d0
+        call diffusesmoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+        tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+        ft(:,t),kt(:,:,t), im,p,m,r,jt,rt0,rt1,finf(:,t),kinf(:,:,t),etahat(:,t),epshat(:,t),needeps)
         do t=(dt-1), 1, -1
-            call dgemv('t',m,r,1.0d0,rtv(:,:,(t-1)*timevar(4)+1),m,rrec,1,0.0d0,help,1)
-            call dsymv('l',r,1.0d0,qt(:,:,(t-1)*timevar(5)+1),r,help,1,0.0d0,etahat(:,t),1)
-            call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec,1,0.0d0,rhelp,1)
-            rrec = rhelp
-            call dgemv('t',m,m,1.0d0,tt(:,:,(t-1)*timevar(3)+1),m,rrec1,1,0.0d0,rhelp,1)
-            rrec1 = rhelp
-
-            do i = p, 1, -1
-                if(ymiss(t,i).EQ.0) then
-                    if(finf(i,t) .GT. 0.0d0) then
-                        finv = 1.0d0/finf(i,t)
-                        if(needeps) then
-                            epshat(i,t) = -ht(i,i,(t-1)*timevar(2)+1)*ddot(m,kinf(:,i,t),1,rrec,1)*finv
-                        end if
-
-
-                        linf = im
-                        call dger(m,m,-finv,kinf(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,linf,m)
-
-                        rhelp = kinf(:,i,t)*ft(i,t)*finv - kt(:,i,t)
-                        l0=0.0d0
-                        call dger(m,m,finv,rhelp,1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec1,1,0.0d0,rhelp,1)
-                        rrec1 = rhelp
-                        call dgemv('t',m,m,1.0d0,l0,m,rrec,1,1.0d0,rrec1,1)
-                        rrec1 = rrec1 + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                        call dgemv('t',m,m,1.0d0,linf,m,rrec,1,0.0d0,rhelp,1)
-                        rrec = rhelp
-                    else
-                        if(ft(i,t) .GT. 0.0d0) then
-                            finv = 1.0d0/ft(i,t)
-                            if(needeps) then
-                                epshat(i,t) = ht(i,i,(t-1)*timevar(2)+1)*(vt(i,t)-ddot(m,kt(:,i,t),1,rrec,1))*finv
-                            end if
-
-                            l0 = im
-                            call dger(m,m,-finv,kt(:,i,t),1,zt(i,:,(t-1)*timevar(1)+1),1,l0,m)
-
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec,1,0.0d0,rhelp,1)
-                            rrec = rhelp + vt(i,t)*finv*zt(i,:,(t-1)*timevar(1)+1)
-
-                            call dgemv('t',m,m,1.0d0,l0,m,rrec1,1,0.0d0,rhelp,1)
-                            rrec1 = rhelp
-                        end if
-
-                    end if
-                end if
-            end do
-
-
+            call diffusesmoothonestep(ymiss(t,:), transpose(zt(:,:,(t-1)*timevar(1)+1)), ht(:,:,(t-1)*timevar(2)+1), &
+            tt(:,:,(t-1)*timevar(3)+1), rtv(:,:,(t-1)*timevar(4)+1), qt(:,:,(t-1)*timevar(5)+1), vt(:,t), &
+            ft(:,t),kt(:,:,t), im,p,m,r,p,rt0,rt1,finf(:,t),kinf(:,:,t),etahat(:,t),epshat(:,t),needeps)
         end do
     end if
-    rt0=rrec
-    rt1=rrec1
 
 end subroutine smoothsimfast