diff --git a/matlab/nonlinear-filters/auxiliary_particle_filter.m b/matlab/nonlinear-filters/auxiliary_particle_filter.m
index f0c67c34ab8f9878938628b802c1b40a2823fa99..74354dc69ea3f234ea1e182c4dcad2b661fa8093 100644
--- a/matlab/nonlinear-filters/auxiliary_particle_filter.m
+++ b/matlab/nonlinear-filters/auxiliary_particle_filter.m
@@ -50,19 +50,21 @@ else
     % Set local state space model (first order approximation).
     ghx  = ReducedForm.ghx;
     ghu  = ReducedForm.ghu;
-    % Set local state space model (second order approximation).
-    ghxx = ReducedForm.ghxx;
-    ghuu = ReducedForm.ghuu;
-    ghxu = ReducedForm.ghxu;
-    ghs2 = ReducedForm.ghs2;
-    if (order == 3)
-        % Set local state space model (third order approximation).
-        ghxxx = ReducedForm.ghxxx;
-        ghuuu = ReducedForm.ghuuu;
-        ghxxu = ReducedForm.ghxxu;
-        ghxuu = ReducedForm.ghxuu;
-        ghxss = ReducedForm.ghxss;
-        ghuss = ReducedForm.ghuss;
+    if order>1
+        % Set local state space model (second order approximation).
+        ghxx = ReducedForm.ghxx;
+        ghuu = ReducedForm.ghuu;
+        ghxu = ReducedForm.ghxu;
+        ghs2 = ReducedForm.ghs2;
+        if (order == 3)
+            % Set local state space model (third order approximation).
+            ghxxx = ReducedForm.ghxxx;
+            ghuuu = ReducedForm.ghuuu;
+            ghxxu = ReducedForm.ghxxu;
+            ghxuu = ReducedForm.ghxuu;
+            ghxss = ReducedForm.ghxss;
+            ghuss = ReducedForm.ghuss;
+        end
     end
 end
 
@@ -88,7 +90,7 @@ LIK  = NaN;
 weights = ones(1,number_of_particles)/number_of_particles ;
 StateVectors = bsxfun(@plus,StateVectorVarianceSquareRoot*randn(state_variance_rank,number_of_particles),StateVectorMean);
 %StateVectors = bsxfun(@plus,zeros(state_variance_rank,number_of_particles),StateVectorMean);
-if pruning
+if order>1 && pruning
     if order == 2
         StateVectors_ = StateVectors;
         state_variables_steady_state_ = state_variables_steady_state;
@@ -106,33 +108,41 @@ if pruning
     end
 end
 
+epsilon_ = zeros(number_of_structural_innovations,number_of_particles);
+
 for t=1:sample_size
     yhat = bsxfun(@minus,StateVectors,state_variables_steady_state);
     if pruning
         yhat_ = bsxfun(@minus,StateVectors_,state_variables_steady_state_);
         if order == 2
-            tmp = local_state_space_iteration_2(yhat,zeros(number_of_structural_innovations,number_of_particles),ghx,ghu,constant,ghxx,ghuu,ghxu,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_2);
+            tmp = local_state_space_iteration_2(yhat, epsilon_, ghx, ghu, constant, ghxx, ghuu, ghxu, yhat_, steadystate, ThreadsOptions.local_state_space_iteration_2);
         elseif order == 3
-            tmp = local_state_space_iteration_3(yhat_, zeros(number_of_structural_innovations,number_of_particles), ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
+            tmp = local_state_space_iteration_3(yhat_, epsilon_, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
         else
             error('Pruning is not available for orders > 3');
         end
     else
         if ReducedForm.use_k_order_solver
-            tmp = local_state_space_iteration_k(yhat, zeros(number_of_structural_innovations,number_of_particles), dr, M_, options_, udr);
+            tmp = local_state_space_iteration_k(yhat, epsilon, dr, M_, options_, udr);
         else
-            if order == 2
-                tmp = local_state_space_iteration_2(yhat,zeros(number_of_structural_innovations,number_of_particles),ghx,ghu,constant,ghxx,ghuu,ghxu,ThreadsOptions.local_state_space_iteration_2);
-            elseif order == 3
-                tmp = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations,number_of_particles), ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
-            else
+            switch order
+              case 1
+                tmp = NaN(rows(ghx), columns(yhat));
+                parfor i=1:columns(yhat)
+                    tmp(:,i) = constant + ghx*yhat(:,i);% + ghu*epsilon(:,i)
+                end
+              case 2
+                tmp = local_state_space_iteration_2(yhat, epsilon_, ghx, ghu, constant, ghxx, ghuu, ghxu, ThreadsOptions.local_state_space_iteration_2);
+              case 3
+                tmp = local_state_space_iteration_3(yhat, epsilon_, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
+              otherwise
                 error('Order > 3: use_k_order_solver should be set to true');
             end
         end
     end
     PredictionError = bsxfun(@minus,Y(:,t),tmp(mf1,:));
     z = sum(PredictionError.*(H\PredictionError),1) ;
-%    tau_tilde = weights.*(tpdf(z,3*ones(size(z)))+1e-99) ;
+    %    tau_tilde = weights.*(tpdf(z,3*ones(size(z)))+1e-99) ;
     ddl = 3 ;
     tau_tilde = weights.*(exp(gammaln((ddl + 1) / 2) - gammaln(ddl/2))./(sqrt(ddl*pi).*(1 + (z.^2)./ddl).^((ddl + 1)/2))+1e-99) ;
     tau_tilde = tau_tilde/sum(tau_tilde) ;
@@ -145,7 +155,7 @@ for t=1:sample_size
     epsilon = Q_lower_triangular_cholesky*randn(number_of_structural_innovations,number_of_particles);
     if pruning
         if order == 2
-            [tmp, tmp_] = local_state_space_iteration_2(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_2);
+            [tmp, tmp_] = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, yhat_, steadystate, ThreadsOptions.local_state_space_iteration_2);
         elseif order == 3
             [tmp, tmp_] = local_state_space_iteration_3(yhat_, epsilon, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
         else
@@ -156,11 +166,17 @@ for t=1:sample_size
         if ReducedForm.use_k_order_solver
             tmp = local_state_space_iteration_k(yhat, epsilon, dr, M_, options_, udr);
         else
-            if order == 2
+            switch order
+              case 1
+                tmp = NaN(rows(ghx), columns(yhat));
+                parfor i=1:columns(yhat)
+                    tmp(:,i) = constant + ghx*yhat(:,i) + ghu*epsilon(:,i);
+                end
+              case 2
                 tmp = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, ThreadsOptions.local_state_space_iteration_2);
-            elseif order == 3
+              case 3
                 tmp = local_state_space_iteration_3(yhat, epsilon, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, pruning);
-            else
+              otherwise
                 error('Order > 3: use_k_order_solver should be set to true');
             end
         end
diff --git a/matlab/nonlinear-filters/nonlinear_kalman_filter.m b/matlab/nonlinear-filters/nonlinear_kalman_filter.m
index 3caf3934af5111cd54170c17450d2cc19000d1e3..354100c2f04be2f90135e25f95e2546a22a828fb 100644
--- a/matlab/nonlinear-filters/nonlinear_kalman_filter.m
+++ b/matlab/nonlinear-filters/nonlinear_kalman_filter.m
@@ -138,7 +138,7 @@ for t=1:sample_size
           case 1
             tmp = NaN(rows(ghx), columns(yhat));
             parfor i=1:columns(yhat)
-                tmp(:,i) = constant + ghx*yhat(:,i) + ghu*epsilon(:,i)
+                tmp(:,i) = constant + ghx*yhat(:,i) + ghu*epsilon(:,i);
             end
           case 2
             tmp = local_state_space_iteration_2(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,ThreadsOptions.local_state_space_iteration_2);
diff --git a/tests/particle/noisy_ar1_nlf_apf.mod b/tests/particle/noisy_ar1_nlf_apf.mod
new file mode 100644
index 0000000000000000000000000000000000000000..a54a454b37fe9b9a187882d086f9ded6f06c1e2e
--- /dev/null
+++ b/tests/particle/noisy_ar1_nlf_apf.mod
@@ -0,0 +1,49 @@
+var x y;
+varexo v;
+
+parameters r;
+
+r = .975;
+
+model;
+  y = x;
+  x = r*x(-1) + v;
+end;
+
+shocks;
+  var v; stderr sqrt(0.02);
+end;
+
+steady;
+
+check;
+
+estimated_params;
+  r,  .975, uniform_pdf,,,0,1;
+  stderr y, sqrt(.2), uniform_pdf,,,0,1;
+  stderr v, sqrt(.02), uniform_pdf,,,0,1;
+end;
+
+varobs y;
+
+
+options_.particle.status = 1; // Required because we intend to use a nonlinear filter with a linear model
+
+    estimation(nobs=100,
+           mode_compute=0,
+           mh_replic=0,
+           cova_compute=0,
+           mode_file=noisy,
+           datafile=noisy_ar1_data,
+           order=1,
+           number_of_particles = 20000000,
+           filter_algorithm=apf);
+
+
+load optimal_value_with_linear_kalman_filter
+
+diff = 100*abs((oo_.likelihood_at_initial_parameters-(-ovalue))/(-ovalue));
+
+if diff>0.05
+    error('Difference between NLKF and linear Kalman filter is bigger than 0.05%% (%.2f%%)', diff)
+end