diff --git a/mex/sources/kalman/cc/kalman.cpp b/mex/sources/kalman/cc/kalman.cpp
index 16ec7adef..8fb314d79 100644
--- a/mex/sources/kalman/cc/kalman.cpp
+++ b/mex/sources/kalman/cc/kalman.cpp
@@ -683,12 +683,12 @@ BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
   int inc =1;
   const int rcols= Rt.numCols();
   GeneralMatrix Ft (Ht.numRows(), Ht.numCols()  );
+  PLUFact Ftinv(Ht.numRows(), Ht.numCols()); 
   GeneralMatrix Lt(Tt);
   GeneralMatrix PtLttrans(m,m);
   GeneralMatrix Mt(m,n);
   GeneralMatrix Kt(m,n);
   GeneralMatrix QtRttrans(rcols,Rt.numRows());
-//  PLUFact Ftinv(Ft);
 
   bool isTunit=0;// Tt->isUnit();
   bool isQzero= Qt.isZero();
@@ -702,13 +702,14 @@ BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
   int nonsteady=1;
   for(;t<=data.numCols()&&nonsteady;++t)
     {
-    ConstVector yt(data,t-1);
+//    ConstVector yt(data,t-1);
     
     /*****************
     This calculates  $$v_t = y_t - Z_t*a_t.$$
     *****************/   
 //    Vector vt(yt);
-    vt=yt;
+//    vt=yt;
+    memcpy(vt.base(), &(data.get(0,t-1)), n*sizeof(double));
 //    Zt.multsVec(vt,at);
 	  BLAS_dgemv("N",  &n,  &m, &neg_alpha, Zt.base(), &n, at.base(), 
         &inc, &alpha, vt.base(), &inc);
@@ -727,10 +728,10 @@ BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
 		BLAS_dgemm("N", "N", &n, &n, &m, &alpha, Zt.base(), &n,
 				   Mt.base(), &m, &alpha, Ft.base(), &n); 
     
-    PLUFact Ftinv(Ft); //    Ftinv=Ft;
-    if(!Ftinv.isRegular())
-      return 0.0;
-    
+//    PLUFact Ftinv(Ft); 
+//    if(!Ftinv.isRegular())
+//      return 0.0;
+    Ftinv=Ft;
     /*****************
     This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$
     *****************/   
@@ -738,6 +739,7 @@ BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
 		BLAS_dgemm("N", "N", &m, &n, &m, &alpha, Tt.base(), &m,
 				   Mt.base(), &m, &omega, Kt.base(), &m); 
     Ftinv.multInvRight(Kt);
+//    Kt.multInvRight(Ft);
     
     /*****************
     This calculates  $$L_t = T_t-K_tZ_t.$$
@@ -824,112 +826,112 @@ BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
     }
   
   
-    /*****************
-    This runs a non-diffuse filter with the given $t$, $a_t$ and
-    $P_t$. It fills the |FilterResults|.
-    
-      First we check that the passed $P_t$ is positive definite by checking
-      that it has not negative diagonal and is symmetric diagonally
-      dominant. This is not equivalent to positive definitness but it excludes
-      ``much'' of indefinite matrices. This check is important since this
-      routine is called from a diffuse filter and it is possible due to a
-      wrong guess/calculation of a number of diffuse periods that numerical
-      instability and roundoff errors make the matrix $P_*$ broken.
-      
-        Then we cycle until the end of period and perform a classical Kalman
-        filter operations.
-  *****************/
-  void 
-    KalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
-    int first,FilterResults&fres)const
+/*****************
+This runs a non-diffuse filter with the given $t$, $a_t$ and
+$P_t$. It fills the |FilterResults|.
+
+First we check that the passed $P_t$ is positive definite by checking
+that it has not negative diagonal and is symmetric diagonally
+dominant. This is not equivalent to positive definitness but it excludes
+``much'' of indefinite matrices. This check is important since this
+routine is called from a diffuse filter and it is possible due to a
+wrong guess/calculation of a number of diffuse periods that numerical
+instability and roundoff errors make the matrix $P_*$ broken.
+
+Then we cycle until the end of period and perform a classical Kalman
+filter operations.
+*****************/
+void 
+KalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P,
+int first,FilterResults&fres)const
+  {
+  Vector at(a);
+  GeneralMatrix Pt(P);
+  if(TSUtils::hasNegativeDiagonal(Pt)||!TSUtils::isSymDiagDominant(Pt))
+    return;
+  
+  for(int t= first;t<=data.numCols();t++)
     {
-    Vector at(a);
-    GeneralMatrix Pt(P);
-    if(TSUtils::hasNegativeDiagonal(Pt)||!TSUtils::isSymDiagDominant(Pt))
+    ConstVector yt(data,t-1);
+    ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]);
+    ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]);
+    ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]);
+    ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]);
+    ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]);
+    bool isTunit= ssf.T->isUnit(t);
+    bool isQzero= ssf.Q->isZero(t);
+    bool isRzero= ssf.R->isZero(t);
+    
+    /*****************
+    This calculates  $$v_t = y_t - Z_t*a_t.$$
+    *****************/   
+    Vector vt(yt);
+    Zt.multsVec(vt,at);
+    
+    /*****************
+    This calculates $$F_t = Z_tP_tZ_t^T+H_t.$$
+    *****************/   
+    GeneralMatrix Mt(Pt,Zt,"trans");
+    GeneralMatrix Ft(Ht);
+    Ft.multAndAdd(Zt,ConstGeneralMatrix(Mt));
+    
+    
+    PLUFact Ftinv(Ft);
+    if(!Ftinv.isRegular())
       return;
     
-    for(int t= first;t<=data.numCols();t++)
+      /*****************
+      This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$
+    *****************/   
+    GeneralMatrix Kt(Tt,Mt);
+    Ftinv.multInvRight(Kt);
+    
+    /*****************
+    This calculates  $$L_t = T_t-K_tZ_t.$$
+    *****************/   
+    GeneralMatrix Lt(Tt);
+    Lt.multAndAdd(ConstGeneralMatrix(Kt),Zt,-1.0);
+    
+    
+    /*****************
+    Here we calc likelihood and store results.
+    *****************/   
+    double ll= calcStepLogLik(Ftinv,vt);
+    fres.set(t,Ftinv,vt,Lt,at,Pt,ll);
+    
+    if(t<data.numCols())
       {
-      ConstVector yt(data,t-1);
-      ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]);
-      ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]);
-      ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]);
-      ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]);
-      ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]);
-      bool isTunit= ssf.T->isUnit(t);
-      bool isQzero= ssf.Q->isZero(t);
-      bool isRzero= ssf.R->isZero(t);
-      
       /*****************
-      This calculates  $$v_t = y_t - Z_t*a_t.$$
+      This calculates $$a_{t+1} = T_ta_t + K_tv_t.$$
       *****************/   
-      Vector vt(yt);
-      Zt.multsVec(vt,at);
-      
-      /*****************
-      This calculates $$F_t = Z_tP_tZ_t^T+H_t.$$
-      *****************/   
-      GeneralMatrix Mt(Pt,Zt,"trans");
-      GeneralMatrix Ft(Ht);
-      Ft.multAndAdd(Zt,ConstGeneralMatrix(Mt));
-      
-      
-      PLUFact Ftinv(Ft);
-      if(!Ftinv.isRegular())
-        return;
-      
-        /*****************
-        This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$
-      *****************/   
-      GeneralMatrix Kt(Tt,Mt);
-      Ftinv.multInvRight(Kt);
-      
-      /*****************
-      This calculates  $$L_t = T_t-K_tZ_t.$$
-      *****************/   
-      GeneralMatrix Lt(Tt);
-      Lt.multAndAdd(ConstGeneralMatrix(Kt),Zt,-1.0);
-      
-      
-      /*****************
-      Here we calc likelihood and store results.
-      *****************/   
-      double ll= calcStepLogLik(Ftinv,vt);
-      fres.set(t,Ftinv,vt,Lt,at,Pt,ll);
-      
-      if(t<data.numCols())
+      if(!isTunit)
         {
-        /*****************
-        This calculates $$a_{t+1} = T_ta_t + K_tv_t.$$
-        *****************/   
-        if(!isTunit)
-          {
-          Vector atsave((const Vector&)at);
-          Tt.multVec(0.0,at,1.0,atsave);
-          }
-        Kt.multVec(1.0,at,1.0,ConstVector(vt));
-        
-        /*****************
-        This calculates $$P_{t+1} = T_tP_tL_t^T + R_tQ_tR_t^T.$$
-        *****************/   
-        GeneralMatrix PtLttrans(Pt,Lt,"trans");
-        if(!isTunit)
-          {
-          Pt.zeros();
-          Pt.multAndAdd(Tt,ConstGeneralMatrix(PtLttrans));
-          }
-        else
-          {
-          Pt= (const GeneralMatrix&)PtLttrans;
-          }
-        if(!isRzero&&!isQzero)
-          {
-          GeneralMatrix QtRttrans(Qt,Rt,"trans");
-          Pt.multAndAdd(Rt,ConstGeneralMatrix(QtRttrans));
-          }
+        Vector atsave((const Vector&)at);
+        Tt.multVec(0.0,at,1.0,atsave);
+        }
+      Kt.multVec(1.0,at,1.0,ConstVector(vt));
+      
+      /*****************
+      This calculates $$P_{t+1} = T_tP_tL_t^T + R_tQ_tR_t^T.$$
+      *****************/   
+      GeneralMatrix PtLttrans(Pt,Lt,"trans");
+      if(!isTunit)
+        {
+        Pt.zeros();
+        Pt.multAndAdd(Tt,ConstGeneralMatrix(PtLttrans));
+        }
+      else
+        {
+        Pt= (const GeneralMatrix&)PtLttrans;
+        }
+      if(!isRzero&&!isQzero)
+        {
+        GeneralMatrix QtRttrans(Qt,Rt,"trans");
+        Pt.multAndAdd(Rt,ConstGeneralMatrix(QtRttrans));
         }
       }
     }
+  }
   
 /*****************
 This runs a diffuse filter. Similarly as for
diff --git a/mex/sources/kalman/cc/utils.cpp b/mex/sources/kalman/cc/utils.cpp
index ff22657bb..be851aee8 100644
--- a/mex/sources/kalman/cc/utils.cpp
+++ b/mex/sources/kalman/cc/utils.cpp
@@ -83,23 +83,17 @@ NormCholesky::NormCholesky(const GeneralMatrix&a)
   TS_RAISE_IF(info<0,
     "Internal error in NormCholesky constructor");
   
-  ;
-  
   for(int i= 0;i<L.numRows();i++)
     for(int j= i+1;j<L.numCols();j++)
       L.get(i,j)= 0.0;
     
-    ;
-    
-    for(int j= 0;j<L.numCols();j++){
-      double d= L.get(j,j);
-      Vector Lj(L,j);
-      Lj.mult(1.0/d);
-      D[j]= d*d;
-      }
-    
-    
-    ;
+  for(int j= 0;j<L.numCols();j++){
+    double d= L.get(j,j);
+    Vector Lj(L,j);
+    Lj.mult(1.0/d);
+    D[j]= d*d;
+    }
+  
   }
 
 ;
@@ -132,7 +126,6 @@ rows(m.numRows())
   calcDetSign();
   }
 
-;
 
 PLUFact::PLUFact(const PLUFact&fact)
 :inv(fact.inv),ipiv(new int[fact.rows]),
@@ -141,6 +134,41 @@ rows(fact.rows),rcond(fact.rcond),detsign(fact.detsign),info(fact.info)
   memcpy(ipiv,fact.ipiv,rows*sizeof(int));
   }
 
+PLUFact::PLUFact(const int nc,const int nr )
+    :inv(nr*nc),ipiv(new int[nr]),rows(nr)
+  {
+  TS_RAISE_IF(nr!=nc,
+    "Matrix not square in PLUFact constructor");
+  }
+
+const PLUFact&
+PLUFact::operator = (const GeneralMatrix&m)
+  {
+  TS_RAISE_IF(!m.isFinite(),
+    "Matrix is not finite in PLUFact assignement");
+  TS_RAISE_IF(m.numRows()!=m.numCols(),
+    "Matrix not square in PLUFact assignement");
+  TS_RAISE_IF(m.numRows()!=rows,
+    "Matrix not matching PLUFact size for assignement");
+  inv= m.getData();
+  LAPACK_dgetrf(&rows,&rows,inv.base(),&rows,ipiv,&info);
+  TS_RAISE_IF(info<0,
+    "Internal error in PLUFact assignement");
+  
+  double mnorm= m.getNormInf();
+  double*work= new double[4*rows];
+  int*iwork= new int[rows];
+  int infotmp;
+  LAPACK_dgecon("I",&rows,inv.base(),&rows,&mnorm,&rcond,work,
+    iwork,&infotmp);
+  delete[]iwork;
+  delete[]work;
+  TS_RAISE_IF(infotmp<0,
+    "Internal error in PLUFact assignement");
+  calcDetSign();
+  return *this;
+  }
+
 ;
 
 void PLUFact::PL_dgetrs(const char*trans,double*b,int ldb,int bcols)const
@@ -230,13 +258,10 @@ void PLUFact::calcDetSign()
     if(ipiv[i]!=i+1)
       detsign*= -1;
     
-    ;
-    
-    for(int i= 0;i<rows;i++)
-      if(inv[i*(rows+1)]<0)
-        detsign*= -1;
+  for(int i= 0;i<rows;i++)
+    if(inv[i*(rows+1)]<0)
+      detsign*= -1;
       
-      ;
   }
 
 ;
diff --git a/mex/sources/kalman/cc/utils.h b/mex/sources/kalman/cc/utils.h
index 2d9daccda..c6e6c0e8b 100644
--- a/mex/sources/kalman/cc/utils.h
+++ b/mex/sources/kalman/cc/utils.h
@@ -61,8 +61,10 @@ class PLUFact{
   public:
     PLUFact(const GeneralMatrix&m);
     PLUFact(const PLUFact&plu);
+    PLUFact(const int nc,const int nr );
     virtual~PLUFact()
       {delete[]ipiv;}
+    const PLUFact& operator = (const GeneralMatrix&m);
     void multInvLeft(GeneralMatrix&a)const;
     void multInvRight(GeneralMatrix&a)const;
     void multInvLeft(Vector&a)const;
diff --git a/mex/sources/kalman/sylv/cc/GeneralMatrix.cpp b/mex/sources/kalman/sylv/cc/GeneralMatrix.cpp
index f16885de7..a4c37a373 100644
--- a/mex/sources/kalman/sylv/cc/GeneralMatrix.cpp
+++ b/mex/sources/kalman/sylv/cc/GeneralMatrix.cpp
@@ -85,8 +85,6 @@ GeneralMatrix::GeneralMatrix(const GeneralMatrix& a, const char* dum1,
 
 GeneralMatrix::~GeneralMatrix()
   {
-     if(tmpGMp)
-       delete tmpGMp;
   }
 
 
@@ -180,13 +178,15 @@ void
 GeneralMatrix::multInvRight( GeneralMatrix&A)
   {
   // check or allocate tmp space for Transpose *this
+  /**
   if (tmpGMp)
     {
     if (tmpGMp->numCols()!=rows || tmpGMp->numRows()!=cols)
       delete (tmpGMp);
     }
   if (!tmpGMp) 
-    tmpGMp= new GeneralMatrix(cols,rows); // allocate space only once if and when needed!
+  ********/
+  tmpGMp= new GeneralMatrix(cols,rows); // allocate space only once if and when needed!
 
   // tmpGMp=(*this)' i.e. Transpose (*this)
   for (int i = 0; i < rows; i++)
@@ -216,7 +216,7 @@ GeneralMatrix::multInvRight( GeneralMatrix&A)
       for (int j = 0; j < cols; j++)
         get(i,j) = tmpGMp->get(j,i);
     }
-
+  delete tmpGMp;  
   }