/Src/filter.f

Large files files are truncated, but you can click here to view the full file

c Fortran 77 source code for the Python library PySSM
c Copyright (C) 2010  Chris Strickland

c This program is free software: you can redistribute it and/or modify
c it under the terms of the GNU General Public License as published by
c the Free Software Foundation, either version 2 or verion 3 of the
c License.

c This program is distributed in the hope that it will be useful,
c but WITHOUT ANY WARRANTY; without even the implied warranty of
c MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
c GNU General Public License for more details.

c You should have received a copy of the GNU General Public License
c along with this program.  If not, see <http://www.gnu.org/licenses/>.

c ------------------------------------------------------------------
c Filtering and smoothing code + associated functions
c ------------------------------------------------------------------
c Note: Matrix Types [ std = 0, eye = 1, diag = 2, inv = 3, chol = 4 ]
c If type = 3 then matrix is actually the inverse
c If type = 4 then matrix is the cholesky decomposition
c -----------------------------------------------------------------

c     fortran code to simulate disturbance vector for state space model
c     note fl = 1 if rvs on exit is qt & rvs, 0 otherwise
c     note if fl = 1 then gcqt should be gt, otherwise gcqt
      subroutine calc_st_er(gcqt,cqt,rvs,ser,fl,m,r,n,n_q,n_gq)
      implicit none
      integer m,r,n,t,n_q,n_gq,t_q,t_gq,timet,fl
      real*8 gcqt(m,r,n_gq),cqt(r,r,n_q),rvs(r,n),ser(m,n)
      real*8 alpha,beta
c ----------------------
cf2py intent(in) gcqt
cf2py intent(in) cqt
cf2py intent(inplace) rvs
cf2py intent(inplace) ser
cf2py intent(in) fl
cf2py intent(in) m
cf2py intent(in) r
cf2py intent(in) b
c ----------------------
      alpha = 1.0
      beta = 0.0

c$omp parallel default(shared) private(t, t_q, t_gq)
c$omp do schedule(static)
      do t = 1, n
          t_q = timet(n_q, t)
          t_gq = timet(n_gq, t)
          if (fl.eq.1) then
              call dtrmv('l','n','n',r,cqt(:,:,t_q),r,rvs(:,t),1)
          endif
          call dgemv('n',m,r,alpha,gcqt(:,:,t_gq),m,rvs(:,t),1,beta,
     + ser(:,t),1)
      enddo
c$omp end do
c$omp end parallel
      end

c ------------------------------------------------------------------

c     fortran code to simulate disturbance vector for state space model
c     note fl = 1 if rvs on exit is qt & rvs, 0 otherwise
c     note if fl = 1 then gcqt should be gt, otherwise gcqt
      subroutine nt_calc_st_er(gcqt,cqt,rvs,ser,fl,m,r,n)
      implicit none
      integer m,r,n,fl
      real*8 gcqt(m,r),cqt(r,r),rvs(r,n),ser(m,n)
      real*8 alpha,beta
c ----------------------
cf2py intent(in) cqt
cf2py intent(in) gcqt
cf2py intent(inplace) rvs
cf2py intent(inplace) ser
cf2py intent(in) fl
cf2py intent(in) m
cf2py intent(in) r
cf2py intent(in) b
c ----------------------
      alpha = 1.0
      beta = 0.0
      if (fl.eq.1) then
          call dtrmm('l','l','n','n',r,n,alpha,cqt,r,rvs,r)
      endif
      call dgemm('n','n',m,n,r,alpha,gcqt,m,rvs,r,beta,ser,m)
      end

c ------------------------------------------------------------------

c **/** NEW OPENMP ADDED - UNTESTED **/**

c     fortran code for standard filtering and smoothing algorithms
c     simulates data for standard state space model 
      subroutine simssm(y,zt,cht,tt,ser,rvm,av,n,p,m, n_z, n_t, n_h)
      implicit none
      integer n,p,m,t, t_t, t_z, n_z, n_t, t_h, n_h, timet
      real*8 y(p,n), zt(p,m,n_z), cht(p,p,n_h), tt(m,m,n_t)
      real*8 rvm(p,n), ser(m,n), av(m,n) 
      real*8 alpha, beta

c     cht is the sqrt of the ht. recall that ht is a vector
c ----------------------
cf2py intent(inout) y
cf2py intent(in) zt
cf2py intent(in) ch
cf2py intent(in) tt
cf2py intent(in) rvm
cf2py intent(in) ser
cf2py intent(inout) av
cf2py intent(in) n
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n_z
cf2py intent(in) n_t
cf2py intent(in) n_h
c ----------------------
      alpha = 1.0
      beta = 1.0
                  
      do t = 1, n-1      
          t_t = timet(n_t, t)                
          
c         Initialise: av(t+1) = ser(t)          
          call dcopy(m,ser(:,t),1,av(:,t+1),1)

c         Compute: av(t+1) = av(t+1) + tt * av(t)          
          call dgemv('n',m,m,alpha,tt(:,:,t_t),m,av(:,t),1,beta,
     + av(:,t+1),1)
      enddo
      
c$omp parallel default(shared) private(t, t_z, t_h, beta)
c$omp do schedule(static) 
      do t = 1, n
          t_z = timet(n_z, t)          
          t_h = timet(n_h, t)          
          
c         Compute: y(t) = cht(t) * rvm(t)         
          beta = 0.0
          call dgemv('n',p,p,alpha,cht(:,:,t_h),p,rvm(:,t),1,beta,
     + y(:,t),1)
     
c         Compute: y(t) = y(t) + zt(t) * av(t)     
          beta = 1.0
          call dgemv('n',p,m,alpha,zt(:,:,t_z),p,av(:,t),1, beta,
     + y(:,t),1)
      enddo
c$omp end do
c$omp end parallel

      end

c ------------------------------------------------------------------

c     non time varying version that simulates data      
      subroutine ntsimssm(y,zt,ch,tt,ser,rvm,av,n,p,m)
      integer n,p,m,t
      real*8 y(p,n), zt(p,m), ch(p,p), tt(m,m)
      real*8 rvm(p,n),av(m,n),ser(m,n)
      real*8 alpha, beta

c     cht is the sqrt of ht. recall that ht is a vector
c ----------------------
cf2py intent(inout) y
cf2py intent(in) zt
cf2py intent(in) ch
cf2py intent(in) tt
cf2py intent(in) ser
cf2py intent(in) rvm
cf2py intent(inout) av
cf2py intent(in) n
cf2py intent(in) p
cf2py intent(in) m
c ----------------------
      alpha = 1.0
      beta = 1.0
       
      do t = 1, n-1

c         Initialise: av(t+1) = ser(t)      
          call dcopy(m,ser(:,t),1,av(:,t+1),1)          
          
c         Compute: av(t+1) = av(t+1) + tt(t) * av(t)          
          call dgemv('n',m,m,alpha,tt,m,av(:,t),1,beta, av(:,t+1),1)
      enddo

      beta = 0.0
      call dgemm('n','n',p,n,p,alpha,ch,p,rvm,p,beta,y,p)
      beta = 1.0
      call dgemm('n','n',p,n,m,alpha,zt,p,av,m,beta,y,p)
      end

c ------------------------------------------------------------------

c     subroutine for filtering with the standard state SSM
      subroutine filter(y, z, h, tt, qt, nu, k, f, s, a, at, pt, ps,
     + mt, w, ls, lk, wl, ifo, ptt, ilike, pmiss, m, p, n, n_z, n_t,
     +  n_q, n_h)
      integer m, p, n, t, i, ifo, ptt, info, ilike
      integer timet, n_z, n_t, n_q, t_z, t_t, t_q, t_h, pmiss(n)
      real*8 y(p,n), z(p,m,n_z), h(p,p,n_h), tt(m,m,n_t), qt(m,m,n_q)
      real*8 nu(p,n), k(m,p,n), s(p,m,n), ls(m,m,n), a(m,n)
      real*8 ps(m,m,n), mt(m,p), at(m), pt(m,m), f(p,p), w(m,m)
      real*8 alpha, beta, lk(*), wl(p), logdet, ddot, llike
c ----------------------
cf2py intent(in) y
cf2py intent(in) z
cf2py intent(in) h
cf2py intent(in) tt
cf2py intent(in) qt
cf2py intent(inout) nu
cf2py intent(inout) k
cf2py intent(in) f
cf2py intent(inout) s
cf2py intent(inout) a
cf2py intent(in) at
cf2py intent(in) pt
cf2py intent(inout) ps
cf2py intent(in) mt
cf2py intent(in) w
cf2py intent(inplace) lk
cf2py intent(inplace) ifo
cf2py intent(in) pmiss
cf2py intent(in) ptt
cf2py intent(in) m
cf2py intent(in) p
cf2py intent(in) n
cf2py intent(in) n_z
cf2py intent(in) n_t
cf2py intent(in) n_h
cf2py intent(in) n_q
c ----------------------
c     note if lk(1).lt.-1 then calculate likelihood and return in lk(1)

      ifo = 0
      llike = 0.0

      do t = 1, n
          t_z = timet(n_z, t)
          t_t = timet(n_t, t)
          t_q = timet(n_q, t)
          t_h = timet(n_h, t)
      
c         1. Initialise: a(:,t) = at      
          call dcopy(m,at,1,a(:,t),1)
c         -------------------------------------------------------------
c         2. Initialise: ps(:,i,t) = pt(:,i)          
          do i = 1, m
              call dcopy(m, pt(:,i), 1, ps(:,i,t), 1)
          enddo
c         -------------------------------------------------------------
c         3. Compute: Nu(t) = Y(t) - Z(t) * a(t)     
c         3a. Initialise: nu(:,t) = y(:,t)
          if (pmiss(t).eq.0) then
              !data not missing
              call dcopy(p, y(:,t), 1, nu(:,t), 1)
c             3b. Compute: nu = nu - z * at          
              alpha = -1.0
              beta = 1.0          
              call dgemv('n',p,m,alpha,z(:,:,t_z),p,at,1,beta,nu(:,t),1)
c         -------------------------------------------------------------                    
c             4. Compute: m(t) = P(t)*Z(t)'   [i.e. (m,p) = (m,m) *(m,p) ]
              alpha = 1.0
              beta = 0.0         
              call dgemm('n','t',m,p,m,alpha,pt,m,z(:,:,t_z),p,beta,mt,
     + m)
c         -------------------------------------------------------------
c             5. Compute: F(t) = Z(t)*m(t) + H(t)  [i.e. (p,p) = (p,m)*(m,p) ]        
c             5a. Initialise: f(:,i) = h(:,i,t_h)
              !column of y not missing
              do i = 1, p
                  call dcopy(p, h(:,i,t_h), 1, f(:,i), 1)
              enddo
c             5b. Compute: f = f + z * mt          
              beta = 1.0
              call dgemm('n','n',p,p,m,alpha,z(:,:,t_z),p,mt,m,beta,f,p)
!             print *, "ft =", f          
c         -------------------------------------------------------------
c             6. Compute: S(t) = inv(F(t))*Z(t)     [i.e. solve F.S = Z ]
c             6a. Initialise: s = z
              do i = 1, m
                  call dcopy(p, z(:,i,t_z), 1, s(:,i,t), 1)
              enddo
c             6b. Solve: f * x = s   [Note: store x in s]  [i.e. (p,p) *(p,m) = (p,m)  ]
              call dposv('u',p,m,f,p,s(:,:,t),p,info)
              if (info.ne.0) then
                  ifo = 1
              endif                          
          !else
              !column of y missing so set S(t)=0
          !    do i=1,m
          !        do j=1,m
          !            S(i,j,t)=0.0
          !        enddo
          !    enddo
c         ------------------------------------------------------------          
              if (ilike.eq.0) then

c                 Compute: logdet = 2.0 * sum(i: log(f(i,i))          
                  logdet = 0.0
                  do i = 1, p
                      logdet = logdet + log(f(i,i))
                  enddo
                  logdet = 2.0 * logdet
                  
c                 Initialise: wl = nu(:,t)              
                  call dcopy(p, nu(:,t), 1, wl, 1)

c                 Solve:  f.x = wl  (i.e. Compute: x = inv[f]*wl ) (Note: store x as wl)
                  call dtrsv('u','t','n',p,f,p,wl,1)
                  
c                 Subtract: -(0.5 * nu^2) - (0.5 * logdet)                           
                  llike = llike - 0.5 * (logdet + ddot(p,wl,1,wl,1))

              endif
c             -------------------------------------------------------------          
c             7. Compute: W(t) = T(t)*P(t)
              beta = 0.0
              if (ptt.eq.0) then
c                 7a. Compute: w = tt * pt
                  call dgemm('n','n', m, m, m, alpha, tt(:,:,t_t),
     + m, pt, m, beta, w, m)
              else
c                 7b. Initialise: w = pt          
                  do i = 1, m
                      call dcopy(m,pt(:,i),1,w(:,i),1)
                  enddo
              endif
c             -------------------------------------------------------------          
c             8. Compute: K(t) = W(t) * S(t)'
              call dgemm('n','t',m,p,m,alpha,w,m,s(:,:,t),p,beta,
     + k(:,:,t),m)
!              print *, "kt =", k(:,:,t)
c             -------------------------------------------------------------
c             9. Compute: L(t) = T(t) - K(t)*Z(t)   [i.e.  (m,m) = (m,p) * (p,m) ] 
c             9a. Initialise: ls(:,i,t) = tt(:,t,t_t)
              do i = 1, m
                  call dcopy(m,tt(:,i,t_t),1,ls(:,i,t),1)
              enddo
c             9b. Compute: ls = ls - k(:,:,t) *  z(:,:,t_z)   [i.e. (m,m) = (m,p) * (p,m) ]
              alpha = -1.0
              beta = 1.0
              call dgemm('n','n',m,m,p,alpha,k(:,:,t),m,z(:,:,t_z),p,
     + beta,
     + ls(:,:,t),m)        
!              print *, "lt =", ls(:,:,t)
c             -------------------------------------------------------------          
c             10. Compute: a(t+1) = T(t)*a(t) + K(t)*nu(t)
              alpha = 1.0
              beta = 0.0
              if (ptt.eq.0) then
c                 10a. Compute: at = tt(:,:,t_t) * a(:,t)          
                  call dgemv('n',m,m,alpha,tt(:,:,t_t),m,a(:,t),1,beta,
     + at,1)
              else
c                 10a. Initialise: at = a(:,t)          
                  call dcopy(m, a(:,t), 1, at, 1)
              endif
c             10b. Compute: at = at + k(:,:,t) * nu(:,t)   [i.e. (m) = (m,p) *(p)   ]
              beta = 1.0
              call dgemv('n',m,p,alpha,k(:,:,t),m,nu(:,t),1,beta,at,1)
              beta = 0.0
!              print *, "at =", at
          else
              !Compute L(t)=T(t)
              do i = 1, m
                  call dcopy(m,tt(:,i,t_t),1,ls(:,i,t),1)
              enddo
c             10b. Compute a(t+1) = T(t)*a(t)
              alpha = 1.0
              beta = 0.0
              if (ptt.eq.0) then
c                 10a. Compute: at = tt(:,:,t_t) * a(:,t)          
                  call dgemv('n',m,m,alpha,tt(:,:,t_t),m,a(:,t),1,beta,
     + at,1)
              else
c                 10a. Initialise: at = a(:,t)          
                  call dcopy(m, a(:,t), 1, at, 1)
              endif
c             10b. Compute: W(t) = T(t)*P(t)
              beta = 0.0
              if (ptt.eq.0) then
c                 Compute: w = tt * pt
                  call dgemm('n','n', m, m, m, alpha, tt(:,:,t_t),
     + m, pt, m, beta, w, m)
              else
c                 Initialise: w = pt          
                  do i = 1, m
                      call dcopy(m,pt(:,i),1,w(:,i),1)
                  enddo
              endif
          endif

c         -------------------------------------------------------------          
c         11. Compute: P(t+1) = W(t)*L(t)' + Q(t)
c         call dgemm('n','n',m,m,m,alpha,tt(:,:,t),m,pt,m,beta,w,m)
c         11a. Initialise: pt = qt(:,:,t_q) 
          do i = 1, m
              call dcopy(m, qt(:,i,t_q), 1, pt(:,i), 1)
          enddo
c         11b. Compute: pt = pt + w * ls' where w = tt * pt  
c         [i.e. (m,m) = (m,m) * (m,m) * (m,m) ]
          beta = 1.0
          call dgemm('n','t',m,m,m,alpha,w,m,ls(:,:,t),m,beta,pt,m)
      enddo
c     -------------------------------------------------------------
      lk(1)=llike
      end

c ------------------------------------------------------------------

c     subroutine for filtering with the standard non time varying state SSM
      subroutine ntfilter(y,z,h,tt,qt,nu,k,f,s,a,at,pt,ps,mt,w,ls,lt,
     + lk,wl,ifo,ilike,m,p,n)
      integer m,p,n,t,i,ifo,info,chk,indr,lt,ilike
      real*8 y(p,n),z(p,m),h(p,p),tt(m,m),qt(m,m),lk,wl(p)
      real*8 nu(p,n),k(m,p,n),s(p,m,n),ls(m,m,n),a(m,n)
      real*8 ps(m,m,n),mt(m,p),at(m),pt(m,m),f(p,p),w(m,m)
      real*8 alpha, beta,fnorm, tol,logdet,llike,ddot, temp
c ----------------------
cf2py intent(in) y
cf2py intent(in) z
cf2py intent(in) h
cf2py intent(in) tt
cf2py intent(in) qt
cf2py intent(inplace) nu
cf2py intent(inplace) k
cf2py intent(in) f
cf2py intent(inplace) s
cf2py intent(inplace) a
cf2py intent(in) at
cf2py intent(in) pt
cf2py intent(inplace) ps
cf2py intent(in) mt
cf2py intent(in) w
cf2py intent(inout) lk
cf2py intent(in) wl
cf2py intent(inout) ifo
cf2py intent(inout) lt
cf2py intent(in) m
cf2py intent(in) p
cf2py intent(in) n
c ----------------------
      ifo = 0
      tol = 1E-10
      chk = 10
      indr = 0  ! Indicator for steady state
      lt = n+1
      llike = 0.0
       
      do t = 1, n
c         Not yet reached steady state:      
          if (indr.eq.0) then
          
c             1. Initialise: a(:,t) = at           
              call dcopy(m,at,1,a(:,t),1)
c             ------------------------------
c             2. Initialise: ps(:,:,t)  = pt
              do i = 1, m
                  call dcopy(m,pt(:,i),1,ps(:,i,t),1)
              enddo
c             ------------------------------              
c             3. Compute: Nu(t) = Y(t) - Z(t)*a(t)     
c             3a. Initialise nu(:,t) = y(:, t)
              call dcopy(p, y(:,t), 1, nu(:,t), 1)
              
c             3b. Compute: nu(:,t) = nu(:,t) - z * at      
              alpha = -1.0
              beta = 1.0                                 
              call dgemv('n',p,m,alpha,z,p,at,1,beta,nu(:,t),1)
                            
c             ------------------------------
c             4. Compute: m(t) = P(t)*Z(t)' , i.e. mt = pt * z
              alpha = 1.0
              beta = 0.0
              call dgemm('n','t',m,p,m,alpha,pt,m,z,p,beta,mt,m)
c             ------------------------------
c             5. Compute: F(t) = Z(t)*m(t) + H(t)    
c             5a. Initialise f(:,i) = h(:,i)      
              do i = 1, p
                  call dcopy(p,h(:,i),1,f(:,i),1)
              enddo                            
c             5b. Compute: f = f + z*mt
              beta = 1.0
              call dgemm('n','n',p,p,m,alpha,z,p,mt,m,beta,f,p)
c             ------------------------------              
c             6. Compute: F(t)*S(t) = Z(t); solve for S(t)=inv(F(t))*Z(t)          
c             6a.Initialise s(:,:,t) = z
              do i = 1, m
                  call dcopy(p,z(:,i),1,s(:,i,t),1)
              enddo
c             6b. Solve f . x =  s and set s = x             
              call dposv('u',p,m,f,p,s(:,:,t),p,info)
              if (info.ne.0) then
                  ifo = 1
              endif
              
!              if(t.eq.1) then
!                  print *, "S = ", s(:,:,t)
!              endif
c             ------------------------------              
              beta = 0.0
              
              if (ilike.eq.0) then
                  logdet = 0.0
                  do i = 1, p
                      logdet = logdet + log(f(i,i))
                  enddo
                  logdet = 2.0 * logdet

c                 Initialise wl = nu(:,t)                  
                  call dcopy(p,nu(:,t),1,wl,1)
                  
                  call dtrsv('u','t','n',p,f,p,wl,1)
                  
                  llike = llike-0.5*(logdet+ddot(p,wl,1,wl,1))
                  
c                  call dcopy(p,nu(:,t),1,wl,1)
c                  call dtrsm('l','u','t','n',p,1,alpha,f,p,wl,p)
c                  call dtrsm('l','u','n','n',p,1,alpha,f,p,wl,p)
c                  llike = llike-0.5*(logdet+ddot(p,wl,1,nu(:,t),1))
              endif
c             ------------------------------          
c             7. Compute: W(t) = T(t)*P(t),   i.e. w = tt * pt
              call dgemm('n','n',m,m,m,alpha,tt,m,pt,m,beta,w,m)
c             ------------------------------              
c             8. Compute: K(t) = W(t)*S(t)',  i.e. k = w * s
              call dgemm('n','t',m,p,m,alpha,w,m,s(:,:,t),p,beta,
     + k(:,:,t),m)
!              if(t.eq.1) then
!                  print *, "K = ", k(:,:,t)
!              endif
c             ------------------------------     
c             9.  Compute: L(t)= T(t) - K(t)*Z(t)
c             9a. Initialise ls(:,:,t) = tt 
              do i = 1, m
                  call dcopy(m,tt(:,i),1,ls(:,i,t),1)
              enddo             
c             9b. Compute: ls = ls - k * z 
              alpha = -1.0
              beta = 1.0             
              call dgemm('n','n',m,m,p,alpha,k(:,:,t),m,z,p,beta,
     + ls(:,:,t),m)
!              if(t.eq.1) then
!                  print *, "L = ", ls(:,:,t)
!              endif
c             ------------------------------
c             Monitor convergence to steady state
              if(t.gt.chk) then
                  if(mod(t,chk).eq.0) then 
                      temp = fnorm(ps(:,:,t),ps(:,:,t-chk),m,m)
                      if(temp.lt.tol) then
                          indr = 1
                          lt = t
                      endif                
                  endif
              endif
c             ------------------------------
c             10. Compute: a(t+1)= T(t)*a(t) + K(t)*nu(t)
c             10a. Compute: at = tt * a
              alpha = 1.0
              beta = 0.0
              call dgemv('n',m,m,alpha,tt,m,a(:,t),1,beta,at,1)
c             10b. Compute: at = at + k * nu              
              beta = 1.0
              call dgemv('n',m,p,alpha,k(:,:,t),m,nu(:,t),1,beta,at,1)
c             ------------------------------
c             11. Compute: P(t+1)= W(t)*L(t)' + Q(t)
c             11a. Initialise: pt = qt
              do i = 1, m
                  call dcopy(m,qt(:,i),1,pt(:,i),1)
              enddo              
c             11b. Compute: pt = pt + w*ls 
              beta = 1.0             
              call dgemm('n','t',m,m,m,alpha,w,m,ls(:,:,t),m,beta,pt,m)

c         Reached steady state:          
          else              
              call dcopy(m,at,1,a(:,t),1)
c              do i=1,m
c                  call dcopy(m,pt(:,i),1,ps(:,i,t),1)
c              enddo
c             ------------------------------
c             12. Compute: Nu(t)= Y(t) - Z(t)*a(t)    
c             12a. Initialise: nu = y 
              call dcopy(p,y(:,t),1,nu(:,t),1)              
c             12b. Compute: nu = nu + z * at 
              alpha = -1.0
              beta = 1.0             
              call dgemv('n',p,m,alpha,z,p,at,1,beta,nu(:,t),1)
              
!               if(t.eq.1) then
!                  print *, "nu = ", nu(:,t)
!              endif
c             ----------------------------------------            
              if (ilike.eq.0) then
                  call dcopy(p,nu(:,t),1,wl,1)
                  call dtrsv('u','t','n',p,f,p,wl,1)
                  llike = llike - 0.5*(logdet+ddot(p,wl,1,wl,1))
c              call dtrsm('l','u','t','n',p,1,alpha,f,p,wl,p)
c              call dtrsm('l','u','n','n',p,1,alpha,f,p,wl,p)
c              llike=llike-0.5*(logdet+ddot(p,wl,1,nu(:,t),1))
              endif      
c             ------------------------------
c             13. Compute: a(t+1)= T(t)*a(t)+K(t)*nu(t)
c             13a. Compute: at = tt * a
              alpha = 1.0
              beta = 0.0
              call dgemv('n',m,m,alpha,tt,m,a(:,t),1,beta,at,1)
c             ------------------------------                            
c             14. Compute: at = at + k * nu              
c             Note: lt replaces t in k, i.e. k(:,:,lt)              
              beta = 1.0
              call dgemv('n',m,p,alpha,k(:,:,lt),m,nu(:,t),1,beta,at,1)
              beta = 0.0
c             ------------------------------              
c             P(t+1) = W(t)*L(t)' + Q(t)
              do i=1,m
                  call dcopy(m,qt(:,i),1,pt(:,i),1)
              enddo
              beta = 1.0
              call dgemm('n','t',m,m,m,alpha,w,m,ls(:,:,lt),m,beta,pt,m)

          endif
      enddo
      
      lk = llike
      end

c ------------------------------------------------------------------
c     benchmark subroutine for filtering with the standard state SSM
      subroutine bfilter(y,z,h,tt,qt,nu,k,f,fi,wz,a,at,pt,ps,mt,w,ls,
     + ifo, m, p, n, n_h, n_t, n_q, n_z)
      integer m,p,n,t,i,ifo, info
      integer n_h, n_t, n_q, n_z, t_t, t_z, t_h, t_q, timet
      real*8 y(p,n),z(p,m,n_z),h(p,p,n_h),tt(m,m,n_t),qt(m,m,n_q)
      real*8 nu(p,n),k(m,p,n),fi(p,p,n),ls(m,m,n),a(m,n),wz(m,p)
      real*8 ps(m,m,n),mt(m,p),at(m),pt(m,m),f(p,p),w(m,m)
      real*8 alpha, beta
c ----------------------
cf2py intent(in) y
cf2py intent(in) z
cf2py intent(in) h
cf2py intent(in) tt
cf2py intent(in) qt
cf2py intent(inout) nu
cf2py intent(inout) k
cf2py intent(in) f
cf2py intent(inout) fi
cf2py intent(in) wz
cf2py intent(inout) s
cf2py intent(inout) a
cf2py intent(in) at
cf2py intent(in) pt
cf2py intent(inout) ps
cf2py intent(in) mt
cf2py intent(in) w
cf2py intent(inout) ifo
cf2py intent(in) m
cf2py intent(in) p
cf2py intent(in) n
cf2py intent(in) n_z
cf2py intent(in) n_t
cf2py intent(in) n_h
cf2py intent(in) n_q
c ----------------------
      ifo = 0

      do t = 1, n
      
          t_q = timet(n_q, t)
          t_z = timet(n_z, t)
          t_h = timet(n_h, t)
          t_t = timet(n_t, t)
      
          call dcopy(m,at,1,a(:,t),1)
          do i = 1, m
              call dcopy(m,pt(:,i),1,ps(:,i,t),1)
          enddo
          
c         Nu(t)= Y(t) - Z(t)*a(t)     
          call dcopy(p,y(:,t),1,nu(:,t),1)
          alpha=-1.0
          beta=1.0
          call dgemv('n',p,m,alpha,z(:,:,t_z),p,at,1,beta,nu(:,t),1)
          alpha=1.0
          beta=0.0
          
c         m(t) = P(t)*Z(t)'          
          call dgemm('n','t',m,p,m,alpha,pt,m,z(:,:,t_z),p,beta,mt,m)

c         F(t) = Z(t)*m(t)+H(t)          
          do i=1,p
              call dcopy(p,h(:,i,t_h),1,f(:,i),1)
          enddo
          beta=1.0
          call dgemm('n','n',p,p,m,alpha,z(:,:,t_z),p,mt,m,beta,f,p)

c         calculate fi = inverse(f)          
          call eye(fi(:,:,t),p)
          call dposv('u',p,p,f,p,fi(:,:,t),p,info)
          if (info.ne.0) then
              ifo=1
          endif
          beta=0.0
          
c         W(t) = T(t)*P(t)
          call dgemm('n','n',m,m,m,alpha,tt(:,:,t_t),m,pt,m,beta,w,m)

c         K(t) = W(t)*Z(t)'*inv(F(t))
          call dgemm('n','t',m,p,m,alpha,w,m,z(:,:,t_z),p,beta,wz,m)
          call dgemm('n','n',m,p,p,alpha,wz,m,fi(:,:,t),p,beta,
     + k(:,:,t),m)

c         L(t)=T(t)-K(t)*Z(t)
          do i=1,m
              call dcopy(m,tt(:,i,t_t),1,ls(:,i,t),1)
          enddo
          alpha=-1.0
          beta=1.0
          call dgemm('n','n',m,m,p,alpha,k(:,:,t),m,z(:,:,t_z),p,beta,
     + ls(:,:,t),m)

          alpha=1.0
          beta=0.0
          
c         a(t+1)=T(t)*a(t)+K(t)*nu(t)
          call dgemv('n',m,m,alpha,tt(:,:,t_t),m,a(:,t),1,beta,at,1)
          beta=1.0
          call dgemv('n',m,p,alpha,k(:,:,t),m,nu(:,t),1,beta,at,1)
          beta=0.0
          
c         P(t+1)=W(t)*L(t)'+Q(t)
c          call dgemm('n','n',m,m,m,alpha,tt(:,:,t),m,pt,m,beta,w,m)
          do i=1,m
              call dcopy(m,qt(:,i,t_q),1,pt(:,i),1)
          enddo
          beta=1.0
          call dgemm('n','t',m,m,m,alpha,w,m,ls(:,:,t),m,beta,pt,m)
      enddo
      end

c ------------------------------------------------------------------

c     contemporaneous version of the kalman filter
c     subroutine for filtering with the standard state SSM
      subroutine cfilter(y,z,h,tt,qt,nu,f,s,a,at,pt,ps,mt,w,lk,wl,ifo,
     + ilike, m, p, n, n_z, n_t, n_h, n_q)
      integer m,p,n,t,i,ifo, info,ilike
      integer n_z, n_t, n_h, n_q, t_z, t_t, t_h, t_q, timet
      real*8 y(p,n),z(p,m,n_z),h(p,p,n_h),tt(m,m,n_t),qt(m,m,n_q)
      real*8 nu(p,n),s(p,m,n),a(m,n+1)
      real*8 ps(m,m,n+1),mt(p,m),at(m,n),pt(m,m,n),f(p,p),w(m,m,n)
      real*8 alpha, beta,lk(*),logdet,ddot,llike,wl(p)
c ----------------------
cf2py intent(in) y
cf2py intent(in) z
cf2py intent(in) h
cf2py intent(in) tt
cf2py intent(in) qt
cf2py intent(inout) nu
cf2py intent(inout) k
cf2py intent(in) f
cf2py intent(inout) s
cf2py intent(inout) a
cf2py intent(in) at
cf2py intent(in) pt
cf2py intent(inout) ps
cf2py intent(in) mt
cf2py intent(in) w
cf2py intent(inout) lk
cf2py intent(in) wl
cf2py intent(inout) ifo
cf2py intent(in) m
cf2py intent(in) p
cf2py intent(in) n
cf2py intent(in) n_t
cf2py intent(in) n_q
cf2py intent(in) n_z
cf2py intent(in) n_h
c ----------------------
c     note if lk(1).lt.-1 then calculate likelihood and return in lk(1)

      ifo=0
      llike=0.0

      do t=1,n
      
          t_t = timet(n_t, t)
          t_z = timet(n_z, t)
          t_q = timet(n_q, t)
          t_h = timet(n_h, t)
      
c         Nu(t)= Y(t) - Z(t)*a(t)     
          call dcopy(p,y(:,t),1,nu(:,t),1)
          alpha=-1.0
          beta=1.0
          call dgemv('n',p,m,alpha,z(:,:,t_z),p,a(:,t),1,beta,nu(:,t),1)
          alpha=1.0
          beta=0.0
          
c         m(t) = Z(t)*P(t)          
          call dgemm('n','n',p,m,m,alpha,z(:,:,t_z),p,ps(:,:,t),m,beta,
     + mt,p)
     
c         F(t) = m(t)*Z(t)'+H(t)          
          do i=1,p
              call dcopy(p,h(:,i,t_h),1,f(:,i),1)
          enddo
          beta=1.0
          call dgemm('n','t',p,p,m,alpha,mt,p,z(:,:,t_h),p,beta,f,p)
          
c         F(t)*S(t) = m(t); solve for S(t)=inv(F(t))*m(t)          
          do i=1,m
              call dcopy(p,mt(:,i),1,s(:,i,t),1)
          enddo
          call dposv('u',p,m,f,p,s(:,:,t),p,info)
          if (info.ne.0) then
              ifo=1
          endif
          beta=0.0

          if (ilike.eq.0) then
              logdet=0.0
              do i=1,p
                  logdet=logdet+log(f(i,i))
              enddo
              logdet=2.0*logdet
              call dcopy(p,nu(:,t),1,wl,1)
              call dtrsv('u','t','n',p,f,p,wl,1)
              llike=llike-0.5*(logdet+ddot(p,wl,1,wl,1))
c              call dtrsm('l','u','t','n',p,1,alpha,f,p,wl,p)
c              call dtrsm('l','u','n','n',p,1,alpha,f,p,wl,p)
c              llike=llike-0.5*(logdet+ddot(p,wl,1,nu(:,t),1))
          endif
          
c         a(t|t)=a(t)+S(t)'nu(t)
          alpha=1.0
          beta=1.0
          call dcopy(m,a(:,t),1,at(:,t),1)
          call dgemv('t',p,m,alpha,s(:,:,t),p,nu(:,t),1,beta,at(:,t),1)

c         P(t|t)=P(t)-m(t)'S(t)
          do i=1,m
              call dcopy(m,ps(:,i,t),1,pt(:,i,t),1)
          enddo
          alpha=-1.0
          beta=1.0
          call dgemm('t','n',m,m,p,alpha,mt,p,s(:,:,t),p,beta,
     + pt(:,:,t),m)

c         W(t) = P(t)*T(t)'
          alpha=1.0
          beta=0.0
          call dgemm('n','t',m,m,m,alpha,pt(:,:,t),m,tt(:,:,t_t),m,beta,
     + w(:,:,t), m)

c         a(t+1)=T(t)*a(t|t)
          call dgemv('n',m,m,alpha,tt(:,:,t_t),m,at(:,t),1,beta,
     + a(:,t+1), 1)

c         P(t+1)=T(t)*W(t)+Q(t)
          beta=1.0
          do i=1,m
              call dcopy(m,qt(:,i,t),1,ps(:,i,t+1),1)
          enddo
          call dgemm('n','n',m,m,m,alpha,tt(:,:,t),m,w(:,:,t),m,beta,
     + ps(:,:,t+1),m)

      enddo
      lk(1)=llike
      end

c ------------------------------------------------------------------
c     contemporaneous version of the kalman filter
c     subroutine for filtering with the standard state SSM
c     non-time varying system matricies

      subroutine ntcfilter(t_last,y,z,h,tt,qt,nu,f,s,a,at,pt,ps,mt,
     + w, lk, wl,ifo, ilike, m,p,n)
     
      integer m,p,n,t,it,t_last,i,ifo,info,ilike,chk,flag_steady
      real*8 y(p,n),z(p,m),h(p,p),tt(m,m),qt(m,m), tol
      real*8 nu(p,n),s(p,m,n),a(m,n+1),fnorm
      real*8 ps(m,m,n+1),mt(p,m),at(m,n),pt(m,m,n),f(p,p),w(m,m,n)
      real*8 alpha, beta,lk(*),logdet,ddot,llike,wl(p), temp
c ----------------------
cf2py intent(inout) t_last
cf2py intent(in) y
cf2py intent(in) z
cf2py intent(in) h
cf2py intent(in) tt
cf2py intent(in) qt
cf2py intent(inout) nu
cf2py intent(inout) k
cf2py intent(in) f
cf2py intent(inout) s
cf2py intent(inout) a
cf2py intent(inout) at
cf2py intent(in) pt
cf2py intent(inout) ps
cf2py intent(in) mt
cf2py intent(in) w
cf2py intent(inout) lk
cf2py intent(in) wl
cf2py intent(inout) ifo
cf2py intent(in) m
cf2py intent(in) p
cf2py intent(in) n
c ----------------------
c     Note: ps == P(t)
c     note if lk(1).lt.-1 then calculate likelihood and return in lk(1)    
  
      chk = 10
      ifo = 0
      t_last = n + 1      
      tol = 1E-10
      logdet = 0.0
      llike = 0.0
      flag_steady = 0  ! Indicator that steady state reached
      
      do t = 1, n
      
c         1. Compute: Nu(t)= Y(t) - Z(t)*a(t)     
c         1a. Initialise: nu(:,t) = y(:,t)
          call dcopy(p,y(:,t),1,nu(:,t),1)          
c         1b. Compute: nu(:,t) = nu(:,t) + z * a(:,t)          
          alpha = -1.0
          beta = 1.0
          call dgemv('n',p,m,alpha,z,p,a(:,t),1,beta,nu(:,t),1)
c         ---------------------------------------
c         NOTE: Step 2 - 7 do not need to be computed once steady state reached
          if(flag_steady.eq.0) then
          
c             2. Compute: m(t) = Z(t)*P(t), i.e.  mt = z * ps(:,:,t)        
              alpha = 1.0
              beta = 0.0
              call dgemm('n','n',p,m,m,alpha,z,p,ps(:,:,t),m,beta,
     + mt,p)
c             ---------------------------------------           
c             3. Compute: F(t) = m(t)*Z(t)'+ H(t)          
c             3a. Initialise: f(:,:) = h(:,:)
              do i = 1, p
                  call dcopy(p, h(:,i), 1, f(:,i), 1)
              enddo          
c             3b. Compute: f = f + mt * z'
              beta = 1.0
              call dgemm('n','t',p,p,m,alpha,mt,p,z,p,beta,f,p)
c             ---------------------------------------                     
c             Calculate log-likelihood component
              if (ilike.eq.0) then  
                  logdet = 0.0
                  do i = 1, p
                      logdet = logdet + 2.0 * log(f(i,i))
                  enddo                
              endif          
c             ---------------------------------------                     
c             4. Compute: S(t) = inv(F(t)) * m(t)  
c             by solving system  F(t)*S(t) = m(t)
c             4a. Initialise s(:,:,t) = mt
              do i = 1, m
                  call dcopy(p,mt(:,i),1,s(:,i,t),1)
              enddo
c             4b. Solve f x = s  and set s = x       
              call dposv('u',p,m,f,p,s(:,:,t),p,info)
              if (info.ne.0) then
                  ifo = 1
                  print *, 'Error: Failed to solve [ntcfilter:4b]'
              endif
c             ---------------------------------------                                
c             5. Compute: P(t|t) = P(t) - S(t)'m(t)
c             5a. Initialise: pt(:,:,t) = ps(:,:,t)
              do i = 1, m
                  call dcopy(m,ps(:,i,t),1,pt(:,i,t),1)
              enddo
c             5b. Compute: pt = pt - s' * mt
              alpha = -1.0
              beta = 1.0
              call dgemm('t','n',m,m,p,alpha,s(:,:,t),p,mt,p,beta,
     + pt(:,:,t),m)   
c             ---------------------------------------                     
c             6. Compute: W(t) = T(t) * P(t|t), i.e. w = tt * pt
              alpha = 1.0
              beta = 0.0
              call dgemm('n','t',m,m,m,alpha,tt,m,pt(:,:,t),m,beta,
     + w(:,:,t), m)
c             ---------------------------------------                     
c             7. Compute: P(t+1) = W(t)*T(t)' + Q(t)
c             7a. Initialise: ps(:,:, t+1) = qt
              beta = 1.0
              do i = 1, m
                  call dcopy(m,qt(:,i),1,ps(:,i,t+1),1)
              enddo
c             7b. Compute: ps = ps + w * tt'
              call dgemm('n','t',m,m,m,alpha,w(:,:,t),m,tt,m,beta,
     + ps(:,:,t+1),m)
c             ---------------------------------------           
          endif ! FINISH CALCS FOR NO STEADY STATE SITUATION
c         ----------------------------------------------------          
c         NOTE: This must always be computed at time t          
          if (ilike.eq.0) then              
c            Initialise: wl = nu(:,t)
             call dcopy(p,nu(:,t),1,wl,1)             
c            Solve: f x = wl  and set wl = x            
             call dtrsv('u','t','n',p,f,p,wl,1)
             
             llike = llike -0.5*(logdet + ddot(p,wl,1,wl,1))
          endif       
          
          if (flag_steady.eq.0) then 
              it = t
          else
              it = t_last
          endif     
c         ---------------------------------------                     
c         NOTE: Step 8 and 9 must always be computed at time t           
c         8. Compute: a(t|t) = a(t) + S(t)'nu(t)
          alpha = 1.0
          beta = 1.0
c         8a. Initialise: at(:,t) = a(:,t)          
          call dcopy(m,a(:,t),1,at(:,t),1)
c         8b. Compute: at = at  + s' * nu     
c         NOTE: s(:,:,t or lt)  depending on whether steady state reached  
          call dgemv('t',p,m,alpha,s(:,:,it),p,nu(:,t),1,beta,at(:,t),1)
c         ---------------------------------------           
c         9. Compute: a(t+1) = T(t) * a(t|t), i.e. a = tt * at
          call dgemv('n',m,m,alpha,tt,m,at(:,t),1,beta,a(:,t+1),
     + 1)
c         ---------------------------------------                
c         Monitor convergence to steady state if not already occurred
          if (flag_steady.eq.0) then   ! i.e  Not yet reached 
              if(t.gt.chk) then  
                  if (mod(t,chk).eq.0) then   ! i.e. check regularly
                      temp = fnorm(ps(:,:,t),ps(:,:,t-chk),m,m)
                      if(temp.lt.tol) then
                          flag_steady = 1    ! Update flag                  
                          t_last = t   ! Record when steady state occurs
                      endif
                  endif
              endif
          endif
                    
      enddo  
      
      lk(1) = llike
      
      end

c ------------------------------------------------------------------

c     subroutine for smoothing. Standard state space model. Time varying
c     system matricies.
      subroutine smoother(ah,s,nu,ls,r,ltr,as,ps,pmiss,p,m,n) 
      integer p,m,n,t,i,pmiss(n)
      real*8 ah(m,n),s(p,m,n),nu(p,n),ls(m,m,n),r(m),ltr(m)
      real*8 as(m,n), ps(m,m,n)
      real*8 alpha, beta
c ----------------------
cf2py intent(inout) ah
cf2py intent(in) s
cf2py intent(in) nu
cf2py intent(in) ls
cf2py intent(in) r
cf2py intent(in) ltr
cf2py intent(in) as
cf2py intent(in) ps
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n
c ----------------------
c     Initialise vector
      do i = 1, m
          r(i) = 0.0
      enddo

      alpha = 1.0
      
      do t = n, 1, -1
      
c         Step 1: Compute r(t-1) = S(t)' * nu(t)  +  L(t)' * r(t)          
          beta = 0.0
          
c         1a. Compute: ltr = ls(:,:,t) * r          
          call dgemv('t',m,m,alpha,ls(:,:,t),m,r,1,beta,ltr,1)
          
c         1b. Initialise r: i.e. r = ltr
          call dcopy(m, ltr, 1, r, 1)
          
          if (pmiss(t).eq.0) then
              !data not missing
              beta = 1.0

c             1c. Compute: r = (s * nu) + r
              call dgemv('t',p,m,alpha,s(:,:,t),p,nu(:,t),1,beta,r,1)
          endif

c         Step 2: Compute ahat(t) = a(t) + P(t) * r(t-1)

c         2a. Initialise as: i.e. ah(:,t) = as(:, t)
          call dcopy(m, as(:,t), 1, ah(:,t), 1)
          
c         2b. Compute: ah = ah + ps*r
          beta = 1.0
          call dgemv('n',m,m,alpha, ps(:,:,t), m, r,1, beta, ah(:,t),1)
          
      enddo
      end

c ------------------------------------------------------------------

c     subroutine for smoothing. Standard state space model. Non time varying system matricies.
      subroutine ntsmoother(ah,s,nu,ls,r,ltr,as,ps,lt,p,m,n) 
      integer p,m,n,t,i,lt,it
      real*8 ah(m,n),s(p,m,n),nu(p,n),ls(m,m,n),r(m),ltr(m)
      real*8 as(m,n), ps(m,m,n)
      real*8 alpha, beta
c ----------------------
cf2py intent(inout) ah
cf2py intent(in) s
cf2py intent(in) nu
cf2py intent(in) ls
cf2py intent(in) r
cf2py intent(in) ltr
cf2py intent(in) as
cf2py intent(in) ps
cf2py intent(in) lt
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n
c ----------------------
c     Initialise vector
      do i = 1, m
          r(i) = 0.0
      enddo

      alpha = 1.0
      do t = n, 1, -1
      
          if (t.gt.lt) then
              it = lt
          else
              it = t
          endif

c         Step 1: Compute r(t-1) = S(t)'*nu(t) + L(t)'r(t)
          beta = 0.0
c         1a. Compute: ltr = ls * r     
          call dgemv('t',m,m,alpha,ls(:,:,it),m,r,1,beta,ltr,1)
c         1b. Initialise: r = ltr
          call dcopy(m,ltr,1,r,1)
c         1c. Compute: r = r + s * nu
          beta = 1.0
          call dgemv('t',p,m,alpha,s(:,:,it),p,nu(:,t),1,beta,r,1)
c         -----------------------------------------------
c         Step 2: Compute ahat(t) = a(t) + P(t)*r(t-1)
c         2. Initialise ah = as
          call dcopy(m,as(:,t),1,ah(:,t),1)
c         2b. Compute: ah = ah + ps * r          
          call dgemv('n',m,m,alpha,ps(:,:,it),m,r,1,beta,ah(:,t),1)
      enddo
      end

c ------------------------------------------------------------------

c     subroutine for state disturbance smoother
      subroutine dsmoother(ehat,nu,st,jt,rt,ls,ltr,pmiss,p,m,n,r)
      implicit none
      integer p, m, n, r, t, i, pmiss(n)
      real*8 nu(p,n), st(p,m,n), jt(r,m,n), rt(m), ls(m,m,n)
      real*8 alpha, beta, ehat(r,n), ltr(m)
c ----------------------
cf2py intent(in) nu
cf2py intent(in) st
cf2py intent(in) jt
cf2py intent(in) rt
cf2py intent(in) ls
cf2py intent(inout) ehat
cf2py intent(in) ltr
cf2py intent(in) pmiss
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n
cf2py intent(in) r
c ----------------------
c     Initialise rt array
      do i = 1, m
          rt(i) = 0.0
      enddo
      
      alpha = 1.0
      do t = n, 1, -1
      
c         1. Compute: ehat(t) = J(t) * rt
          beta = 0.0
          call dgemv('n',r,m,alpha,jt(:,:,t),r,rt,1,beta,ehat(:,t),1)
c         ---------------------------------------------
c         2.  Compute: r(t-1) = S(t)'.nu(t) + L(t)'.r(t)
          beta = 0.0
c         2a. Compute: ltr = ls(t)' * rt
          call dgemv('t',m,m,alpha,ls(:,:,t),m,rt,1,beta,ltr,1)
c         2b. Initialise: rt = ltr
          call dcopy(m,ltr,1,rt,1)
          beta = 1.0
          if (pmiss(t).eq.0) then
c             2c. Compute: rt = rt + st'.nu          
              call dgemv('t',p,m,alpha,st(:,:,t),p,nu(:,t),1,beta,rt,1)
          endif
      enddo
      end
      
c ------------------------------------------------------------------

c     subroutine for state disturbance smoother
      subroutine ntdsmoother(ehat,nu,st,jt,rt,ls,ltr,lt,p,m,n,r)
      implicit none
      integer p,m,n,r,lt,it,t,i
      real*8 nu(p,n),st(p,m,n),jt(r,m),rt(m),ls(m,m,n)
      real*8 alpha,beta,ehat(r,n),ltr(m)

c     NOTE: r(t) is input, used in calcs, then over written with r(t-1)
c ----------------------
cf2py intent(in) nu
cf2py intent(in) st
cf2py intent(in) jt
cf2py intent(in) rt
cf2py intent(in) ls
cf2py intent(inout) ehat
cf2py intent(in) ltr
cf2py intent(in) lt
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n
cf2py intent(in) r
c ----------------------
c     Initialise rt array
      do i = 1, m
          rt(i) = 0.0
      enddo

      alpha = 1.0
      do t = n, 1, -1
      
          if (t.gt.lt) then
              it = lt
          else
              it = t
          endif
c         --------------------------------
c         1. Compute: etahat = J(t)*r(t)
          beta = 0.0
          call dgemv('n',r,m,alpha,jt,r,rt,1,beta,ehat(:,t),1)
c         -----------------------------------------------
c         2. Compute: r(t-1) = S(t)'.nu(t) + L(t)'.r(t)
c         2a. Compute: ltr = ls'.rt
          beta = 0.0
          call dgemv('t',m,m,alpha,ls(:,:,it),m,rt,1,beta,ltr,1)
c         2b. Initialise: rt = ltr          
          call dcopy(m,ltr,1,rt,1)
          beta = 1.0
c         2c. Compute: rt = rt + st'.nu          
          call dgemv('t',p,m,alpha,st(:,:,it),p,nu(:,t),1,beta,rt,1)
      enddo
      end
      
c ------------------------------------------------------------------

c     subroutine calculates the Frobenius norm for the difference of two matrices
      real*8 function fnorm(a,b,m,n)
      implicit none
      integer m,n,i,j
      real*8 a(m,n), b(m,n), sumd

      sumd = 0.0
      do j = 1, n
          do i = 1, m
              sumd = sumd + (a(i,j)-b(i,j))**2
          enddo
      enddo
      fnorm = sqrt(sumd)
      return
      end
      
c ------------------------------------------------------------------

c     benchmark simulation smoothing routine
      subroutine bsmoother(ah,z,fi,fn,nu,ls,r,ltr,as,ps,p,m,n, n_z) 
      integer p,m,n,t,i, n_z, t_z, timet
      real*8 ah(m,n),z(p,m,n_z),fi(p,p,n),fn(p),nu(p,n),ls(m,m,n),r(m)
      real*8 as(m,n), ps(m,m,n),ltr(m)
      real*8 alpha, beta
c ----------------------
cf2py intent(inout) ah
cf2py intent(in) fi
cf2py intent(in) fn
cf2py intent(in) nu
cf2py intent(in) ls
cf2py intent(in) r
cf2py intent(in) ltr
cf2py intent(in) as
cf2py intent(in) ps
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) n
cf2py intent(in) n_z
c ----------------------
      do i = 1,m
          r(i) = 0.0
      enddo

      alpha = 1.0
      do t = n, 1, -1
      
          t_z = timet(n_z, t)
      
c         r(t-1)=Z(t)'inv(F(t))*nu(t)+L(t)'r(t)
          beta = 0.0
          call dgemv('t',m,m,alpha,ls(:,:,t),m,r,1,beta,ltr,1)
          call dcopy(m,ltr,1,r,1)
          call dgemv('n',p,p,alpha,fi(:,:,t),p,nu(:,t),1,beta,fn,1)
          beta = 1.0
          call dgemv('t',p,m,alpha,z(:,:,t_z),p,fn,1,beta,ltr,1)
          call dcopy(m,ltr,1,r,1)
          
c         ahat(t)=a(t)+P(t)*r(t-1)
          call dcopy(m,as(:,t),1,ah(:,t),1)
          call dgemv('n',m,m,alpha,ps(:,:,t),m,r,1,beta,ah(:,t),1)
      enddo
      end

c ------------------------------------------------------------------

c     Procedure to initialise an identity matrix of specified dimension
      subroutine eye(a,m)
      integer m, i, j
      real*8 a(m,m)

      do i = 1, m
          do j = 1, m
              a(i,j) = 0.0
          enddo
          a(i,i) = 1.0
      enddo
      
      end
      
c ------------------------------------------------------------------

c     de Jong and Shephard simulation smoother
      subroutine dssimsm(eta,s,nu,z,w,u,ls,rt,nt,v,nl,jt,
     + ct,nj,lr,qt,ifo,pmiss,p,m,r,n, n_z, n_q)
      implicit none
      integer ifo,info,p,m,r,n,t,i,j, t_z, n_z, t_q, n_q, timet
      integer pmiss(n)
      real*8 eta(r,n),s(p,m,n),nu(p,n), z(p,m,n_z), w(r,m), u(r)
      real*8 ls(m,m,n),rt(m),nt(m,m),v(r,m),nl(m,m),jt(r,m,n)
      real*8 ct(r,r),nj(m,r),lr(m), qt(r,r,n_q)
      real*8 alpha,beta
c     on entry eta is a matrix of random normals on exit it is a
c     sample of eta
c ----------------------
cf2py intent(inout) eta
cf2py intent(in) s
cf2py intent(in) nu 
cf2py intent(in) z
cf2py intent(in) w
cf2py intent(in) u
cf2py intent(in) ls
cf2py intent(inout) rt
cf2py intent(in) nt
cf2py intent(in) v
cf2py intent(in) nl
cf2py intent(in) jt
cf2py intent(in) ct
cf2py intent(in) nj
cf2py intent(in) lr
cf2py intent(in) qt
cf2py intent(in) pmiss
cf2py intent(inout) ifo
cf2py intent(in) p
cf2py intent(in) m
cf2py intent(in) r
cf2py intent(in) n
cf2py intent(in) n_z
cf2py intent(in) n_q
c ----------------------
      do j = 1, m
          rt(j) = 0.0
          do i = 1, m
              nt(i,j) = 0.0
          enddo
      enddo
          
      do t = n, 1, -1
      
          t_z = timet(n_z, t)
          t_q = timet(n_q, t)
          
          alpha = 1.0
          beta = 0.0
c          NL(t) = N(t) * L(t)
          call dgemm('n','n',m,m,m,alpha,nt,m,ls(:,:,t),m,beta,nl,m)

c         W(t) = J(t) * NL(t)
          call dgemm('n','n',r,m,m,alpha,jt(:,:,t),r,nl,m,beta,w,r)

c         NJ(t) = N(t) * J(t)'     
          call dgemm('n','t',m,r,m,alpha,nt,m,jt(:,:,t),r,beta,nj,m)

c         C(t) = Q(t)-J(t) * NJ(t)
          do i=1,r
              call dcopy(r,qt(:,i,t_q),1,ct(:,i),1)
          enddo
          alpha = -1.0
          beta = 1.0
          call dgemm('n','n',r,r,m,alpha,jt(:,:,t),r,nj,m,beta,ct,r)

c         C(t) * V(t) = W(t); solve for V(t) 
          do i = 1, m
              call dcopy(r,w(:,i),1,v(:,i),1)
          enddo
          
          call dposv('u',r,m,ct,r,v,r,info)
          if (info.ne.0) then
              ifo = 1
          endif
          
c          d(t)~N(0,C(t)); on exit eta(:,t) = d(t)
           call dtrmv('u','t','n',r,ct,r,eta(:,t),1)

c          C(t) * U(t) = d(t); solve for U(t)
           call dcopy(r,eta(:,t),1,u,1)
           call dtrsv('u','t','n',r,ct,r,u,1)
           call dtrsv('u','n','n',r,ct,r,u,1)

c          eta = d(t) + J(t) * r(t)
           alpha = 1.0
           beta = 1.0
           call dgemv('n',r,m,alpha,jt(:,:,t),r,rt,1,beta,eta(:,t),1)

c          r(t) = S(t)' * nu(t) - W(t)' * U(t) + l(t)'r(t)
           beta = 0.0
           call dgemv('t',m,m,alpha,ls(:,:,t),m,rt,1,beta,lr,1)
           call dcopy(m,lr,1,rt,1)
           alpha = -1.0
           beta = 1.0
           call dgemv('t',r,m,alpha,w,r,u,1,beta,rt,1)
           alpha = 1.0
           if (pmiss(t).eq.0) then
               call dgemv('t',p,m,alpha,s(:,:,t),p,nu(:,t),1,beta,rt,1)
           endif

c          N(t) = S(t)'Z(t) + W(t)'V(t) +L(t)'NL(t) 
           beta = 0.0
           call dgemm('t','n',m,m,m,alpha,ls(:,:,t),m,nl,m,beta,nt,m)
           beta = 1.0
           call dgemm('t','n',m,m,r,alpha,w,r,v,r,beta,nt,m)
           if (pmiss(t).eq.0) then
               call dgemm('t','n',m,m,p,alpha,s(:,:,t),p,z(:,:,t_z),p,
     + beta,nt,m)
           endif
       enddo
       end
       
c ------------------------------------------------------------------

c     de Jong and Shephard simulation smoother non time varying system
c     matricies
      subroutine ntdssimsm(eta,s,nu,z,w,u,ls,rt,nt,v,nl,jt,
     + ct,nj,lr,qt,lt,ifo,p,m,r,n)
      implicit none
      integer ifo,info,lt,it,p,m,r,n,t,i,j
      real*8 eta(r,n),s(p,m,n),nu(p,n),z(p,m),w(r,m),u(r)
      real*8 ls(m,m,n),rt(m),nt(m,m),v(r,m),nl(m,m),jt(r,m)
      real*8 ct(r,r),nj(m,r),lr(m),qt(r,r)
      real*8 alpha,beta
c     on entry eta is a matrix of random normals on exit it is a
c     sample of eta
c ----------------------
cf2py intent(inout) eta
cf2py intent(in) s
cf2py intent(in) nu 
cf2py intent(in) z
cf2py intent(in) w
cf2py intent(in) u
cf2py intent(in) ls
cf2py intent(inout) rt
cf2py intent(in) nt
cf2py intent(in) lt
cf2py intent(in) v
cf2py intent(in) nl
cf2py intent(in) jt
cf2py intent(in) ct
cf2py intent(in) nj
cf2py intent(in) lr
cf2py intent…