diff --git a/matlab/DiffuseKalmanSmoother1.m b/matlab/DiffuseKalmanSmoother1.m
index 7f54bb63cc4c57fb2a66daa2c0d105cd2ef0dfb6..da3a2c7f3959bc0802ea3a678700b86de2edd116 100644
--- a/matlab/DiffuseKalmanSmoother1.m
+++ b/matlab/DiffuseKalmanSmoother1.m
@@ -78,7 +78,7 @@ QQ      	= R*Q*transpose(R);
 QRt			= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
-r 		   	= zeros(mm,smpl);
+r	   	= zeros(mm,smpl+1);
 
 Z = zeros(pp,mm);
 for i=1:pp;
@@ -152,33 +152,26 @@ while t<smpl
     v(:,t) = Y(:,t) - a(mf,t) - trend(:,t);
     atilde(:,t) = a(:,t) + PZI*v(:,t);
     a(:,t+1) = T*a(:,t) + K_s*v(:,t);
-    aK(1,:,t+1) 	 	= a(:,t+1);
+    aK(1,:,t+1) = a(:,t+1);
     for jnk=2:nk,
-        aK(jnk,:,t+jnk) 	 	= T^(jnk-1)*a(:,t+1);
+        aK(jnk,:,t+jnk) = T^(jnk-1)*a(:,t+1);
     end
 end
 t = smpl+1;
-while t>d+1 & t>2
-	t = t-1;
-    r(:,t-1) = transpose(Z)*iF(:,:,t)*v(:,t) + transpose(L(:,:,t))*r(:,t);
-    alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t-1);
-	etahat(:,t)		= QRt*r(:,t);
+while t>d+1
+    t = t-1;
+    r(:,t) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t+1);
+    alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t);
+    etahat(:,t)		= QRt*r(:,t);
 end
 if d
-	r0 = zeros(mm,d); r0(:,d) = r(:,d);
-	r1 = zeros(mm,d);
-	for t = d:-1:2
-    	r0(:,t-1) = transpose(Linf(:,:,t))*r0(:,t);
-		r1(:,t-1) = transpose(Z)*(iFinf(:,:,t)*v(:,t)-transpose(Kstar(:,:,t))*r0(:,t)) + transpose(Linf(:,:,t))*r1(:,t);
-		alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t-1) + Pinf(:,:,t)*r1(:,t-1);
-		etahat(:,t)		= QRt*r0(:,t);
-	end
-	r0_0 = transpose(Linf(:,:,1))*r0(:,1);
-	r1_0 = transpose(Z)*(iFinf(:,:,1)*v(:,1)-transpose(Kstar(:,:,1))*r0(:,1)) + transpose(Linf(:,:,1))*r1(:,1);
-	alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0;
-	etahat(:,1)		= QRt*r0(:,1);
-else
-    r0 = transpose(Z)*iF(:,:,1)*v(:,1) + transpose(L(:,:,1))*r(:,1);
-    alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;
-    etahat(:,1)	= QRt*r(:,1);
+    r0 = zeros(mm,d+1); 
+    r0(:,d+1) = r(:,d+1);
+    r1 = zeros(mm,d+1);
+    for t = d:-1:1
+    	r0(:,t) = Linf(:,:,t)'*r0(:,t+1);
+        r1(:,t) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t+1)) + Linf(:,:,t)'*r1(:,t+1);
+        alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t) + Pinf(:,:,t)*r1(:,t);
+        etahat(:,t)	= QRt*r0(:,t);
+    end
 end
diff --git a/matlab/DiffuseKalmanSmoother1_Z.m b/matlab/DiffuseKalmanSmoother1_Z.m
index 4f843b4e8e12bfc449064df848efc1d443ee057d..2b043245a3effd60d78c4f2742e6fca6d78a8243 100644
--- a/matlab/DiffuseKalmanSmoother1_Z.m
+++ b/matlab/DiffuseKalmanSmoother1_Z.m
@@ -85,10 +85,10 @@ crit1       = 1.e-8;
 steady  	= smpl;
 rr      	= size(Q,1);
 QQ      	= R*Q*transpose(R);
-QRt			= Q*transpose(R);
+QRt		= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
-r 		   	= zeros(mm,smpl);
+r 		= zeros(mm,smpl+1);
 
 t = 0;
 while rank(Pinf(:,:,t+1),crit1) & t<smpl
@@ -169,30 +169,22 @@ while t<smpl
     end
 end
 t = smpl+1;
-while t>d+1 & t>2
+while t>d+1
   t = t-1;
-  r(:,t-1) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t);
-  alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t-1);
-  etahat(:,t)		= QRt*r(:,t);
+  r(:,t) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t+1);
+  alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t);
+  etahat(:,t)	= QRt*r(:,t);
 end
 if d
-  r0 = zeros(mm,d); 
-  r0(:,d) = r(:,d);
-  r1 = zeros(mm,d);
-  for t = d:-1:2
-    r0(:,t-1) = Linf(:,:,t)'*r0(:,t);
-    r1(:,t-1) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t)) + Linf(:,:,t)'*r1(:,t);
-    alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t-1) + Pinf(:,:,t)*r1(:,t-1);
+  r0 = zeros(mm,d+1); 
+  r0(:,d+1) = r(:,d+1);
+  r1 = zeros(mm,d+1);
+  for t = d:-1:1
+    r0(:,t) = Linf(:,:,t)'*r0(:,t+1);
+    r1(:,t) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t+1)) + Linf(:,:,t)'*r1(:,t+1);
+    alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t) + Pinf(:,:,t)*r1(:,t);
     etahat(:,t)		= QRt*r0(:,t);
   end
-  r0_0 = Linf(:,:,1)'*r0(:,1);
-  r1_0 = Z'*(iFinf(:,:,1)*v(:,1)-Kstar(:,:,1)'*r0(:,1)) + Linf(:,:,1)'*r1(:,1);
-  alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0;
-  etahat(:,1)		= QRt*r0(:,1);
-else
-  r0 = Z'*iF(:,:,1)*v(:,1) + L(:,:,1)'*r(:,1);
-  alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;
-  etahat(:,1)	= QRt*r(:,1);
 end
 
 if nargout > 7
diff --git a/matlab/DiffuseKalmanSmoother3.m b/matlab/DiffuseKalmanSmoother3.m
index be2c22235b88821703b93dac2281028f6169e4f9..2889c372e10ff1d2f99d94d9c04a844f08205bb1 100644
--- a/matlab/DiffuseKalmanSmoother3.m
+++ b/matlab/DiffuseKalmanSmoother3.m
@@ -93,7 +93,7 @@ QQ      	= R*Q*transpose(R);
 QRt			= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
-r 		= zeros(mm,smpl);
+r 		= zeros(mm,smpl+1);
 
 Z = zeros(pp,mm);
 for i=1:pp;
@@ -203,7 +203,7 @@ while notsteady & t<smpl
       P(:,:,t)=tril(P(:,:,t))+transpose(tril(P(:,:,t),-1));
     end
   end
-  a(:,t+1) = T*a(:,t);
+  a1(:,t+1) = T*a(:,t);
   for jnk=1:nk,
     aK(jnk,:,t+jnk) 	 	= T^jnk*a(:,t);
   end
@@ -235,65 +235,43 @@ while t<smpl
     aK(jnk,:,t+jnk)	= T^jnk*a(:,t);
   end
 end
-ri=r;
+ri=zeros(mm,1);
 t = smpl+1;
-while t>d+1 & t>2,
+while t>d+1
   t = t-1;
   for i=pp:-1:1
     if Fi(i,t) > crit
-      ri(:,t)=transpose(Z(i,:))/Fi(i,t)*v(i,t)+transpose(Li(:,:,i,t))*ri(:,t);
+      ri = Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri;
     end
   end
-  r(:,t-1) = ri(:,t);
-  alphahat(:,t)	= a1(:,t) + P1(:,:,t)*r(:,t-1);
+  r(:,t) = ri;
+  alphahat(:,t)	= a1(:,t) + P1(:,:,t)*r(:,t);
   etahat(:,t)		= QRt*r(:,t);
-  ri(:,t-1) = transpose(T)*ri(:,t);
+  ri = T'*ri;
 end
 if d
-  r0 = zeros(mm,d); r0(:,d) = ri(:,d);
+  r0 = zeros(mm,d);
+  r0(:,d) = ri;
   r1 = zeros(mm,d);
-  for t = d:-1:2
+  for t = d:-1:1
     for i=pp:-1:1
       if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
         %r1(:,t) = transpose(Z)*v(:,t)/Finf(i,t) + ... BUG HERE in transpose(Z)
-        r1(:,t) = transpose(Z(i,:))*v(i,t)/Finf(i,t) + ...
-          transpose(L0(:,:,i,t))*r0(:,t) + transpose(Linf(:,:,i,t))*r1(:,t);
-        r0(:,t) = transpose(Linf(:,:,i,t))*r0(:,t);
+        r1(:,t) = Z(i,:)'*v(i,t)/Finf(i,t) + ...
+          L0(:,:,i,t)'*r0(:,t) + Linf(:,:,i,t)'*r1(:,t);
+        r0(:,t) = Linf(:,:,i,t)'*r0(:,t);
       elseif Fstar(i,t) > crit % step needed whe Finf == 0
-        r0(:,t)=transpose(Z(i,:))/Fstar(i,t)*v(i,t)+Li(:,:,i,t)'*r0(:,t);
+        r0(:,t) = Z(i,:)'/Fstar(i,t)*v(i,t)+Li(:,:,i,t)'*r0(:,t);
       end
     end
     alphahat(:,t)	= a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
-    r(:,t-1)		= r0(:,t);
+    r(:,t)  = r0(:,t);
     etahat(:,t)		= QRt*r(:,t);
-    r0(:,t-1) = transpose(T)*r0(:,t);
-    r1(:,t-1) = transpose(T)*r1(:,t);
-  end
-  r0_0 = r0(:,1);
-  r1_0 = r1(:,1);
-  for i=pp:-1:1
-    if Finf(i,1) > crit,
-      %r1_0 = transpose(Z)*v(:,1)/Finf(i,1) + ... %bug with Z here
-      r1_0 = transpose(Z(i,:))*v(i,1)/Finf(i,1) + ...
-        transpose(L0(:,:,i,1))*r0_0 + transpose(Linf(:,:,i,1))*r1_0;
-      r0_0 = transpose(Linf(:,:,i,1))*r0_0;
-    elseif Fstar(i,1) > crit, % step needed when Finf=0
-      r0_0=transpose(Z(i,:))/Fstar(i,1)*v(i,1)+Li(:,:,i,1)'*r0_0;
+    if t > 1
+        r0(:,t-1) = T'*r0(:,t);
+        r1(:,t-1) = T'*r1(:,t);
     end
   end
-  %alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0; %this line is buggy
-  alphahat(:,1)  	= a1(:,1) + Pstar1(:,:,1)*r0_0 + Pinf1(:,:,1)*r1_0;
-  etahat(:,1)		= QRt*r(:,1);
-else
-  r0 = ri(:,1);
-  for i=pp:-1:1
-    if Fi(i,1) > crit
-      r0=transpose(Z(i,:))/Fi(i,1)*v(i,1)+transpose(Li(:,:,i,1))*r0;
-    end
-  end 
-  %alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;  % this line is buggy
-  alphahat(:,1)	= a1(:,1) + P1(:,:,1)*r0;
-  etahat(:,1)	= QRt*r(:,1);
 end
 
 
diff --git a/matlab/DiffuseKalmanSmoother3_Z.m b/matlab/DiffuseKalmanSmoother3_Z.m
index e5c627e136701358c9db25812280a8989e1d304d..c2f58b3bd25249304a95baf0fb5eb0ec3c2cd47e 100644
--- a/matlab/DiffuseKalmanSmoother3_Z.m
+++ b/matlab/DiffuseKalmanSmoother3_Z.m
@@ -243,26 +243,28 @@ while t<smpl
       PK(jnk,:,:,t+jnk) = Pf;
   end
 end
-ri=r;
+ri=zeros(mm,1);
 t = smpl+1;
-while t>d+1 & t>2,
+while t > d+1
   t = t-1;
   for i=pp:-1:1
     if Fi(i,t) > crit
-      ri(:,t) = Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri(:,t);
+      ri = Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri;
     end
   end
-  r(:,t-1) = ri(:,t);
-  alphahat(:,t) = a1(:,t) + P1(:,:,t)*r(:,t-1);
+  r(:,t) = ri;
+  alphahat(:,t) = a1(:,t) + P1(:,:,t)*r(:,t);
   etahat(:,t) = QRt*r(:,t);
-  ri(:,t-1) = T'*ri(:,t);
+  ri = T'*ri;
 end
 if d
-  r0 = zeros(mm,d); r0(:,d) = ri(:,d);
+  r0 = zeros(mm,d); 
+  r0(:,d) = ri;
   r1 = zeros(mm,d);
-  for t = d:-1:2
+  for t = d:-1:1
     for i=pp:-1:1
-      if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
+%      if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
+      if Finf(i,t) > crit 
         r1(:,t) = Z(i,:)'*v(i,t)/Finf(i,t) + ...
           L0(:,:,i,t)'*r0(:,t) + Linf(:,:,i,t)'*r1(:,t);
         r0(:,t) = Linf(:,:,i,t)'*r0(:,t);
@@ -270,34 +272,14 @@ if d
         r0(:,t) = Z(i,:)'/Fstar(i,t)*v(i,t)+Li(:,:,i,t)'*r0(:,t);
       end
     end
-    alphahat(:,t)       = a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
-    r(:,t-1)            = r0(:,t);
-    etahat(:,t)         = QRt*r(:,t);
-    r0(:,t-1) = T'*r0(:,t);
-    r1(:,t-1) = T'*r1(:,t);
-  end
-  r0_0 = r0(:,1);
-  r1_0 = r1(:,1);
-  for i=pp:-1:1
-    if Finf(i,1) > crit,
-      r1_0 = Z(i,:)'*v(i,1)/Finf(i,1) + ...
-        L0(:,:,i,1)'*r0_0 + Linf(:,:,i,1)'*r1_0;
-      r0_0 = Linf(:,:,i,1)'*r0_0;
-    elseif Fstar(i,1) > crit, % step needed when Finf=0
-      r0_0=Z(i,:)'/Fstar(i,1)*v(i,1)+Li(:,:,i,1)'*r0_0;
+    alphahat(:,t) = a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
+    r(:,t)        = r0(:,t);
+    etahat(:,t)   = QRt*r(:,t);
+    if t > 1
+        r0(:,t-1) = T'*r0(:,t);
+        r1(:,t-1) = T'*r1(:,t);
     end
   end
-  alphahat(:,1)         = a1(:,1) + Pstar1(:,:,1)*r0_0 + Pinf1(:,:,1)*r1_0;
-  etahat(:,1)           = QRt*r(:,1);
-else
-  r0 = ri(:,1);
-  for i=pp:-1:1
-    if Fi(i,1) > crit
-      r0 = Z(i,:)'/Fi(i,1)*v(i,1)+Li(:,:,i,1)'*r0;
-    end
-  end 
-  alphahat(:,1) = a1(:,1) + P1(:,:,1)*r0;
-  etahat(:,1)   = QRt*r(:,1);
 end
 
 if nargout > 7
diff --git a/matlab/DiffuseKalmanSmootherH1.m b/matlab/DiffuseKalmanSmootherH1.m
index b19ff67baba1d5e159d81e2aa04312ad63a57946..199d9c883961a4643751e896f6aac2994ab5b6ee 100644
--- a/matlab/DiffuseKalmanSmootherH1.m
+++ b/matlab/DiffuseKalmanSmootherH1.m
@@ -79,7 +79,7 @@ QRt		= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
 epsilonhat      = zeros(size(Y));
-r 		= zeros(mm,smpl);
+r 		= zeros(mm,smpl+1);
 
 Z = zeros(pp,mm);
 for i=1:pp;
@@ -156,28 +156,21 @@ while t<smpl
     end
 end
 t = smpl+1;
-while t>d+1 & t>2
-	t = t-1;
-    r(:,t-1) = transpose(Z)*iF(:,:,t)*v(:,t) + transpose(L(:,:,t))*r(:,t);
-    alphahat(:,t) = a(:,t) + P(:,:,t)*r(:,t-1);
+while t>d+1
+    t = t-1;
+    r(:,t) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t+1);
+    alphahat(:,t) = a(:,t) + P(:,:,t)*r(:,t);
     etahat(:,t) = QRt*r(:,t);
 end
 if d
-    r0 = zeros(mm,d); r0(:,d) = r(:,d);
-    r1 = zeros(mm,d);
-    for t = d:-1:2
-    	r0(:,t-1) = transpose(Linf(:,:,t))*r0(:,t);
-        r1(:,t-1) = transpose(Z)*(iFinf(:,:,t)*v(:,t)-transpose(Kstar(:,:,t))*r0(:,t)) + transpose(Linf(:,:,t))*r1(:,t);
-        alphahat(:,t) = a(:,t) + Pstar(:,:,t)*r0(:,t-1) + Pinf(:,:,t)*r1(:,t-1);
+    r0 = zeros(mm,d+1); 
+    r0(:,d+1) = r(:,d+1);
+    r1 = zeros(mm,d+1);
+    for t = d:-1:1
+    	r0(:,t) = Linf(:,:,t)'*r0(:,t+1);
+        r1(:,t) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t+1)) + Linf(:,:,t)'*r1(:,t+1);
+        alphahat(:,t) = a(:,t) + Pstar(:,:,t)*r0(:,t) + Pinf(:,:,t)*r1(:,t);
         etahat(:,t) = QRt*r0(:,t);
     end
-    r0_0 = transpose(Linf(:,:,1))*r0(:,1);
-    r1_0 = transpose(Z)*(iFinf(:,:,1)*v(:,1)-transpose(Kstar(:,:,1))*r0(:,1)) + transpose(Linf(:,:,1))*r1(:,1);
-    alphahat(:,1) = a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0;
-    etahat(:,1)	= QRt*r0(:,1);
-else
-    r0 = transpose(Z)*iF(:,:,1)*v(:,1) + transpose(L(:,:,1))*r(:,1);
-    alphahat(:,1) = a(:,1) + P(:,:,1)*r0;
-    etahat(:,1)	= QRt*r(:,1);
 end
 epsilonhat = Y-alphahat(mf,:)-trend;
diff --git a/matlab/DiffuseKalmanSmootherH1_Z.m b/matlab/DiffuseKalmanSmootherH1_Z.m
index 02ff7ce765c557f04f7b6e16d16c69bfb40a7f66..73d5569df377be8521ff42526a14a637b44b457b 100644
--- a/matlab/DiffuseKalmanSmootherH1_Z.m
+++ b/matlab/DiffuseKalmanSmootherH1_Z.m
@@ -91,7 +91,7 @@ QRt			= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
 epsilonhat      = zeros(size(Y));
-r 		   	= zeros(mm,smpl);
+r 		   	= zeros(mm,smpl+1);
 
 t = 0;
 while rank(Pinf(:,:,t+1),crit1) & t<smpl
@@ -172,30 +172,22 @@ while t<smpl
     end
 end
 t = smpl+1;
-while t>d+1 & t>2
+while t>d+1 
   t = t-1;
-  r(:,t-1) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t);
-  alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t-1);
+  r(:,t) = Z'*iF(:,:,t)*v(:,t) + L(:,:,t)'*r(:,t+1);
+  alphahat(:,t)	= a(:,t) + P(:,:,t)*r(:,t);
   etahat(:,t)		= QRt*r(:,t);
 end
 if d
-  r0 = zeros(mm,d); 
-  r0(:,d) = r(:,d);
-  r1 = zeros(mm,d);
-  for t = d:-1:2
-    r0(:,t-1) = Linf(:,:,t)'*r0(:,t);
-    r1(:,t-1) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t)) + Linf(:,:,t)'*r1(:,t);
-    alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t-1) + Pinf(:,:,t)*r1(:,t-1);
+  r0 = zeros(mm,d+1); 
+  r0(:,d+1) = r(:,d+1);
+  r1 = zeros(mm,d+1);
+  for t = d:-1:1
+    r0(:,t) = Linf(:,:,t)'*r0(:,t+1);
+    r1(:,t) = Z'*(iFinf(:,:,t)*v(:,t)-Kstar(:,:,t)'*r0(:,t+1)) + Linf(:,:,t)'*r1(:,t+1);
+    alphahat(:,t)	= a(:,t) + Pstar(:,:,t)*r0(:,t) + Pinf(:,:,t)*r1(:,t);
     etahat(:,t)		= QRt*r0(:,t);
   end
-  r0_0 = Linf(:,:,1)'*r0(:,1);
-  r1_0 = Z'*(iFinf(:,:,1)*v(:,1)-Kstar(:,:,1)'*r0(:,1)) + Linf(:,:,1)'*r1(:,1);
-  alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0;
-  etahat(:,1)		= QRt*r0(:,1);
-else
-  r0 = Z'*iF(:,:,1)*v(:,1) + L(:,:,1)'*r(:,1);
-  alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;
-  etahat(:,1)	= QRt*r(:,1);
 end
 
 if nargout > 7
diff --git a/matlab/DiffuseKalmanSmootherH3.m b/matlab/DiffuseKalmanSmootherH3.m
index 926b8f2ce659f01a91b5409ee6b4a2c59815e87e..fd2764d87c2733248c98f5621fc3f7634a80478f 100644
--- a/matlab/DiffuseKalmanSmootherH3.m
+++ b/matlab/DiffuseKalmanSmootherH3.m
@@ -87,15 +87,15 @@ Pinf    	= zeros(spinf(1),spinf(2),smpl_diff+1); Pinf(:,:,1) = Pinf1;
 Pstar1 		= Pstar;
 Pinf1  		= Pinf;
 crit   	 	= options_.kalman_tol;
-crit1       = 1.e-6;
+crit1           = 1.e-6;
 steady  	= smpl;
 rr      	= size(Q,1);
 QQ      	= R*Q*transpose(R);
-QRt			= Q*transpose(R);
+QRt		= Q*transpose(R);
 alphahat   	= zeros(mm,smpl);
 etahat	   	= zeros(rr,smpl);
 epsilonhat      = zeros(size(Y));
-r 		   	= zeros(mm,smpl);
+r 		= zeros(mm,smpl+1);
 
 Z = zeros(pp,mm);
 for i=1:pp;
@@ -237,65 +237,42 @@ while t<smpl
     aK(jnk,:,t+jnk)	= T^jnk*a(:,t);
   end
 end
-ri=r;
+ri=zeros(mm,1);
 t = smpl+1;
-while t>d+1 & t>2,
+while t>d+1
   t = t-1;
   for i=pp:-1:1
     if Fi(i,t) > crit
-      ri(:,t)=transpose(Z(i,:))/Fi(i,t)*v(i,t)+transpose(Li(:,:,i,t))*ri(:,t);
+      ri=Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri;
     end
   end
-  r(:,t-1) = ri(:,t);
-  alphahat(:,t)	= a1(:,t) + P1(:,:,t)*r(:,t-1);
+  r(:,t) = ri;
+  alphahat(:,t)	= a1(:,t) + P1(:,:,t)*r(:,t);
   etahat(:,t)		= QRt*r(:,t);
-  ri(:,t-1) = transpose(T)*ri(:,t);
+  ri = T'*ri;
 end
 if d
-  r0 = zeros(mm,d); r0(:,d) = ri(:,d);
+  r0 = zeros(mm,d); 
+  r0(:,d) = ri;
   r1 = zeros(mm,d);
-  for t = d:-1:2
+  for t = d:-1:1
     for i=pp:-1:1
       if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
-					     %r1(:,t) = transpose(Z)*v(:,t)/Finf(i,t) + ... BUG HERE in transpose(Z)
-					     r1(:,t) = transpose(Z(i,:))*v(i,t)/Finf(i,t) + ...
-						       transpose(L0(:,:,i,t))*r0(:,t) + transpose(Linf(:,:,i,t))*r1(:,t);
-					     r0(:,t) = transpose(Linf(:,:,i,t))*r0(:,t);
+          r1(:,t) = Z(i,:)'*v(i,t)/Finf(i,t) + ...
+                    L0(:,:,i,t)'*r0(:,t) + Linf(:,:,i,t)'*r1(:,t);
+          r0(:,t) = Linf(:,:,i,t)'*r0(:,t);
       elseif Fstar(i,t) > crit % step needed whe Finf == 0
-	r0(:,t)=transpose(Z(i,:))/Fstar(i,t)*v(i,t)+Li(:,:,i,t)'*r0(:,t);
+	r0(:,t)=Z(i,:)'/Fstar(i,t)*v(i,t)+Li(:,:,i,t)'*r0(:,t);
       end
     end
     alphahat(:,t)	= a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
-    r(:,t-1)		= r0(:,t);
+    r(:,t)		= r0(:,t);
     etahat(:,t)		= QRt*r(:,t);
-    r0(:,t-1) = transpose(T)*r0(:,t);
-    r1(:,t-1) = transpose(T)*r1(:,t);
-  end
-  r0_0 = r0(:,1);
-  r1_0 = r1(:,1);
-  for i=pp:-1:1
-    if Finf(i,1) > crit,
-      %r1_0 = transpose(Z)*v(:,1)/Finf(i,1) + ... %bug with Z here
-      r1_0 = transpose(Z(i,:))*v(i,1)/Finf(i,1) + ...
-	     transpose(L0(:,:,i,1))*r0_0 + transpose(Linf(:,:,i,1))*r1_0;
-      r0_0 = transpose(Linf(:,:,i,1))*r0_0;
-    elseif Fstar(i,1) > crit, % step needed when Finf=0
-      r0_0=transpose(Z(i,:))/Fstar(i,1)*v(i,1)+Li(:,:,i,1)'*r0_0;
+    if t > 1
+        r0(:,t-1) = transpose(T)*r0(:,t);
+        r1(:,t-1) = transpose(T)*r1(:,t);
     end
   end
-  %alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0; %this line is buggy
-  alphahat(:,1)  	= a1(:,1) + Pstar1(:,:,1)*r0_0 + Pinf1(:,:,1)*r1_0;
-  etahat(:,1)		= QRt*r(:,1);
-else
-  r0 = ri(:,1);
-  for i=pp:-1:1
-    if Fi(i,1) > crit
-      r0=transpose(Z(i,:))/Fi(i,1)*v(i,1)+transpose(Li(:,:,i,1))*r0;
-    end
-  end 
-  %alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;  % this line is buggy
-  alphahat(:,1)	= a1(:,1) + P1(:,:,1)*r0;
-  etahat(:,1)	= QRt*r(:,1);
 end
 epsilonhat = Y-alphahat(mf,:)-trend;
 
diff --git a/matlab/DiffuseKalmanSmootherH3_Z.m b/matlab/DiffuseKalmanSmootherH3_Z.m
index ec21b2348ac8c21fe4ddbadb9defba5c275c50d0..1fbc193902463dd6e3e7b5e9a5976eca65b9effe 100644
--- a/matlab/DiffuseKalmanSmootherH3_Z.m
+++ b/matlab/DiffuseKalmanSmootherH3_Z.m
@@ -245,26 +245,28 @@ while t<smpl
       PK(jnk,:,:,t+jnk) = Pf;
   end
 end
-ri=r;
+ri=zeros(mm,1);
 t = smpl+1;
-while t>d+1 & t>2,
+while t>d+1
   t = t-1;
   for i=pp:-1:1
     if Fi(i,t) > crit
-      ri(:,t) = Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri(:,t);
+      ri = Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri;
     end
   end
-  r(:,t-1) = ri(:,t);
-  alphahat(:,t) = a1(:,t) + P1(:,:,t)*r(:,t-1);
+  r(:,t) = ri(:,t);
+  alphahat(:,t) = a1(:,t) + P1(:,:,t)*r(:,t);
   etahat(:,t) = QRt*r(:,t);
-  ri(:,t-1) = T'*ri(:,t);
+  ri = T'*ri;
 end
 if d
-  r0 = zeros(mm,d); r0(:,d) = ri(:,d);
+  r0 = zeros(mm,d); 
+  r0(:,d) = ri;
   r1 = zeros(mm,d);
   for t = d:-1:2
     for i=pp:-1:1
-      if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
+%      if Finf(i,t) > crit & ~(t==d & i>options_.diffuse_d),  % use of options_.diffuse_d to be sure of DKF termination
+      if Finf(i,t) > crit 
         r1(:,t) = Z(i,:)'*v(i,t)/Finf(i,t) + ...
           L0(:,:,i,t)'*r0(:,t) + Linf(:,:,i,t)'*r1(:,t);
         r0(:,t) = Linf(:,:,i,t)'*r0(:,t);
@@ -273,33 +275,13 @@ if d
       end
     end
     alphahat(:,t)       = a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
-    r(:,t-1)            = r0(:,t);
+    r(:,t)            = r0(:,t);
     etahat(:,t)         = QRt*r(:,t);
-    r0(:,t-1) = T'*r0(:,t);
-    r1(:,t-1) = T'*r1(:,t);
-  end
-  r0_0 = r0(:,1);
-  r1_0 = r1(:,1);
-  for i=pp:-1:1
-    if Finf(i,1) > crit,
-      r1_0 = Z(i,:)'*v(i,1)/Finf(i,1) + ...
-        L0(:,:,i,1)'*r0_0 + Linf(:,:,i,1)'*r1_0;
-      r0_0 = Linf(:,:,i,1)'*r0_0;
-    elseif Fstar(i,1) > crit, % step needed when Finf=0
-      r0_0=Z(i,:)'/Fstar(i,1)*v(i,1)+Li(:,:,i,1)'*r0_0;
+    if t > 1
+        r0(:,t-1) = T'*r0(:,t);
+        r1(:,t-1) = T'*r1(:,t);
     end
   end
-  alphahat(:,1)         = a1(:,1) + Pstar1(:,:,1)*r0_0 + Pinf1(:,:,1)*r1_0;
-  etahat(:,1)           = QRt*r(:,1);
-else
-  r0 = ri(:,1);
-  for i=pp:-1:1
-    if Fi(i,1) > crit
-      r0 = Z(i,:)'/Fi(i,1)*v(i,1)+Li(:,:,i,1)'*r0;
-    end
-  end 
-  alphahat(:,1) = a1(:,1) + P1(:,:,1)*r0;
-  etahat(:,1)   = QRt*r(:,1);
 end
 
 if nargout > 7
diff --git a/matlab/DiffuseKalmanSmootherH3corr.m b/matlab/DiffuseKalmanSmootherH3corr.m
index 438a0da9b362531601dc72ddf5644df07d32b3d2..5fe780745a0c9a197482c35a06b00ec16387b729 100644
--- a/matlab/DiffuseKalmanSmootherH3corr.m
+++ b/matlab/DiffuseKalmanSmootherH3corr.m
@@ -85,7 +85,7 @@ QRt		 = Q*transpose(R);
 alphahat 	= zeros(mm+pp,smpl);
 etahat	 	= zeros(rr,smpl);
 epsilonhat 	= zeros(pp,smpl);
-r 		 	= zeros(mm+pp,smpl);
+r 		 	= zeros(mm+pp,smpl+1);
 Z = zeros(pp,mm+pp);
 for i=1:pp;
 	Z(i,mf(i)) = 1;
@@ -192,64 +192,43 @@ while t<smpl
 end
 
 %% [2] Kalman smoother...
-ri=r;
+ri=zeros(mm,1);
 t = smpl+1;
-while t>d+1 & t>2,
-	t = t-1;
-	for i=pp:-1:1
-		if Fi(i,t) > crit
-            ri(:,t)=transpose(Z(i,:))/Fi(i,t)*v(i,t)+transpose(Li(:,:,i,t))*ri(:,t);
+while t>d+1 
+    t = t-1;
+    for i=pp:-1:1
+        if Fi(i,t) > crit
+            ri=Z(i,:)'/Fi(i,t)*v(i,t)+Li(:,:,i,t)'*ri;
         end
     end
-    r(:,t-1) = ri(:,t);
-    alphahat(:,t)	= a1(:,t) + P1(:,:,t)*r(:,t-1);
-	tmp				= QRt*r(:,t);
-	etahat(:,t)		= tmp(1:rr);
-	epsilonhat(:,t)	= tmp(rr+1:rr+pp);
-    ri(:,t-1) = transpose(T)*ri(:,t);
+    r(:,t) = ri(:,t);
+    alphahat(:,t) = a1(:,t) + P1(:,:,t)*r(:,t);
+    tmp	  = QRt*r(:,t);
+    etahat(:,t)		= tmp(1:rr);
+    epsilonhat(:,t)	= tmp(rr+(1:pp));
+    ri = T'*ri;
 end
 if d
-	r0 = zeros(mm+pp,d); r0(:,d) = ri(:,d);
-	r1 = zeros(mm+pp,d);
-	for t = d:-1:2
+    r0 = zeros(mm+pp,d); 
+    r0(:,d) = ri;
+    r1 = zeros(mm+pp,d);
+    for t = d:-1:1
     	for i=pp:-1:1
-		    if Finf(i,t) > crit
-         		r1(:,t) = transpose(Z)*v(:,t)/Finf(i,t) + ...
-                    transpose(L0(:,:,i,t))*r0(:,t) + transpose(Linf(:,:,i,t))*r1(:,t);
+            if Finf(i,t) > crit
+                r1(:,t) = transpose(Z)*v(:,t)/Finf(i,t) + ...
+                          L0(:,:,i,t)'*r0(:,t) + Linf(:,:,i,t)'*r1(:,t);
                 r0(:,t) = transpose(Linf(:,:,i,t))*r0(:,t);
             end
         end
-		alphahat(:,t)	= a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
-		r(:,t-1)		= r0(:,t);
-		tmp				= QRt*r(:,t);
-		etahat(:,t)		= tmp(1:rr);
-		epsilonhat(:,t)	= tmp(rr+1:rr+pp);
-        r0(:,t-1) = transpose(T)*r0(:,t);
-        r1(:,t-1) = transpose(T)*r1(:,t);
-	end
-	r0_0 = r0(:,1);
-	r1_0 = r1(:,1);
-    for i=pp:-1:1
-        if Finf(i,1) > crit,
-            r1_0 = transpose(Z)*v(:,1)/Finf(i,1) + ...
-                transpose(L0(:,:,i,1))*r0_0 + transpose(Linf(:,:,i,1))*r1_0;
-            r0_0 = transpose(Linf(:,:,i,1))*r0_0;
+        alphahat(:,t) = a1(:,t) + Pstar1(:,:,t)*r0(:,t) + Pinf1(:,:,t)*r1(:,t);
+        r(:,t-1)      = r0(:,t);
+        tmp	      = QRt*r(:,t);
+        etahat(:,t)   = tmp(1:rr);
+        epsilonhat(:,t)	= tmp(rr+(1:pp));
+        if t > 1
+            r0(:,t-1) = T'*r0(:,t);
+            r1(:,t-1) = T'*r1(:,t);
         end
     end
-	alphahat(:,1)  	= a(:,1) + Pstar(:,:,1)*r0_0 + Pinf(:,:,1)*r1_0;
-	tmp				= QRt*r(:,1);
-	etahat(:,1)		= tmp(1:rr);
-	epsilonhat(:,1)	= tmp(rr+1:rr+pp);
-else
-    r0 = ri(:,1);
-    for i=pp:-1:1
-		if Fi(i,1) > crit
-            r0=transpose(Z(i,:))/Fi(i,1)*v(i,1)+transpose(Li(:,:,i,1))*r0;
-        end
-    end 
-    alphahat(:,1)	= a(:,1) + P(:,:,1)*r0;
-	tmp 			= QRt*r(:,1);	    
-    etahat(:,1)		= tmp(1:rr);
-    epsilonhat(:,1)	= tmp(rr+1:rr+pp);
 end
 alphahat = alphahat(1:mm,:);
\ No newline at end of file
diff --git a/matlab/DsgeSmoother.m b/matlab/DsgeSmoother.m
index cc8b050ff7188ce81cbd1762243393c0ace2f6f4..86ffbd175d52d345f7f1891ca1a1b604ddacbc24 100644
--- a/matlab/DsgeSmoother.m
+++ b/matlab/DsgeSmoother.m
@@ -108,15 +108,21 @@ function [alphahat,etahat,epsilonhat,ahat,SteadyState,trend_coeff,aK,T,R,P,PK,d,
   
   kalman_algo = options_.kalman_algo;
   if options_.lik_init == 1		% Kalman filter
-      kalman_algo = 1;
+      if kalman_algo ~= 2
+          kalman_algo = 1;
+      end
       Pstar = lyapunov_symm(T,R*Q*transpose(R),options_.qz_criterium);
       Pinf	= [];
   elseif options_.lik_init == 2 % Old Diffuse Kalman filter
-      kalman_algo = 1;
+      if kalman_algo ~= 2
+          kalman_algo = 1;
+      end
       Pstar = 10*eye(np);
       Pinf	= [];
   elseif options_.lik_init == 3 % Diffuse Kalman filter
-      kalman_algo = 3;
+      if kalman_algo ~= 4
+          kalman_algo = 3;
+      end
       [QT,ST] = schur(T);
       if exist('OCTAVE_VERSION') || matlab_ver_less_than('7.0.1')
           e1 = abs(my_ordeig(ST)) > 2-options_.qz_criterium;
@@ -212,31 +218,47 @@ function [alphahat,etahat,epsilonhat,ahat,SteadyState,trend_coeff,aK,T,R,P,PK,d,
                   DiffuseKalmanSmootherH3corr(T,R,Q,H,Pinf,Pstar,Y,trend, ...
                                               nobs,np,smpl,mf);
           end
-      elseif kalman_algo == 3
+      elseif kalman_algo == 3 | kalman_algo == 4
           data1 = Y - trend;
-          [alphahat,epsilonhat,etahat,ahat,aK] = ...
-              DiffuseKalmanSmootherH1_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1,nobs,np,smpl);
-          if all(alphahat(:)==0)
-              kalman_algo = 4;
+          if kalman_algo == 3
+              [alphahat,epsilonhat,etahat,ahat,P,aK,PK,d,decomp] = ...
+                  DiffuseKalmanSmootherH1_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1,nobs,np,smpl);
+              if all(alphahat(:)==0)
+                  kalman_algo = 4;
+                  if ~estim_params_.ncn
+                      [alphahat,epsilonhat,etahat,ahat,P,aK,PK,d,decomp] = ...
+                          DiffuseKalmanSmootherH3_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1,nobs,np,smpl);
+                  else
+                      [alphahat,epsilonhat,etahat,ahat,P,aK,PK,d,decomp] = ...
+                          DiffuseKalmanSmootherH3corr_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1, ...
+                                                        nobs,np,smpl);
+                  end
+              end
+          else
               if ~estim_params_.ncn
-                  [alphahat,epsilonhat,etahat,ahat,aK] = ...
-                      DiffuseKalmanSmootherH3_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1,nobs,np,smpl);
+                  [alphahat,epsilonhat,etahat,ahat,P,aK,PK,d,decomp] = ...
+                      DiffuseKalmanSmootherH3_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1, ...
+                                                nobs,np,smpl);
               else
-                  [alphahat,epsilonhat,etahat,ahat,aK] = ...
+                  [alphahat,epsilonhat,etahat,ahat,P,aK,PK,d,decomp] = ...
                       DiffuseKalmanSmootherH3corr_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1, ...
                                                     nobs,np,smpl);
               end
           end
-      elseif kalman_algo == 4
-          data1 = Y - trend;
-          if ~estim_params_.ncn
-              [alphahat,epsilonhat,etahat,ahat,aK] = ...
-                  DiffuseKalmanSmootherH3_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1, ...
-                                            nobs,np,smpl);
-          else
-              [alphahat,epsilonhat,etahat,ahat,aK] = ...
-                  DiffuseKalmanSmootherH3corr_Z(ST,Z,R1,Q,H,Pinf,Pstar,data1, ...
-                                                nobs,np,smpl);
+          alphahat = QT*alphahat;
+          ahat = QT*ahat;
+          nk = options_.nk;
+          for jnk=1:nk
+              aK(jnk,:,:) = QT*squeeze(aK(jnk,:,:));
+              for i=1:size(PK,4)
+                  PK(jnk,:,:,i) = QT*squeeze(PK(jnk,:,:,i))*QT';
+              end
+              for i=1:size(decomp,4)
+                  decomp(jnk,:,:,i) = QT*squeeze(decomp(jnk,:,:,i));
+              end
+          end
+          for i=1:size(P,4)
+              P(:,:,i) = QT*squeeze(P(:,:,i))*QT';
           end
       end
   else
diff --git a/matlab/dynare_estimation.m b/matlab/dynare_estimation.m
index c56b9308e48d61ec9163efb01c680137784c84e8..1ba8f412bcc47836e46dff864b5cd360b90e32b3 100644
--- a/matlab/dynare_estimation.m
+++ b/matlab/dynare_estimation.m
@@ -316,7 +316,7 @@ if options_.mode_compute == 0 & length(options_.mode_file) == 0
 	end
 	for i=1:M_.endo_nbr
 	    eval(['oo_.SmoothedVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = atT(i,:)'';']);
-	    eval(['oo_.FilteredVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = squeeze(aK(1,i,:))'';']);
+	    eval(['oo_.FilteredVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = squeeze(aK(1,i,:));']);
 	    eval(['oo_.UpdatedVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = updated_variables(i,:)'';']);
 	end
 	for i=1:M_.exo_nbr
@@ -978,7 +978,7 @@ if (~((any(bayestopt_.pshape > 0) & options_.mh_replic) | (any(bayestopt_.pshape
   end
   for i=1:M_.endo_nbr
     eval(['oo_.SmoothedVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = atT(i,:)'';']);
-    eval(['oo_.FilteredVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = squeeze(aK(1,i,:))'';']);
+    eval(['oo_.FilteredVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ' = squeeze(aK(1,i,:));']);
     eval(['oo_.UpdatedVariables.' deblank(M_.endo_names(dr.order_var(i),:)) ...
           ' = updated_variables(i,:)'';']);
   end