diff --git a/matlab/nonlinear-filters/nonlinear_kalman_filter.m b/matlab/nonlinear-filters/nonlinear_kalman_filter.m
index 1065a79b63a2b96f7a64e15eefb6e45e9d2dca0a..3caf3934af5111cd54170c17450d2cc19000d1e3 100644
--- a/matlab/nonlinear-filters/nonlinear_kalman_filter.m
+++ b/matlab/nonlinear-filters/nonlinear_kalman_filter.m
@@ -64,18 +64,20 @@ else
     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
+        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
 
@@ -124,7 +126,7 @@ LIK  = NaN;
 for t=1:sample_size
     xbar = [StateVectorMean ; zeros(number_of_structural_innovations,1) ] ;
     sqr_Px = [StateVectorVarianceSquareRoot zeros(number_of_state_variables,number_of_structural_innovations);
-               zeros(number_of_structural_innovations,number_of_state_variables) Q_lower_triangular_cholesky];
+              zeros(number_of_structural_innovations,number_of_state_variables) Q_lower_triangular_cholesky];
     sigma_points = bsxfun(@plus,xbar,sqr_Px*(nodes'));
     StateVectors = sigma_points(1:number_of_state_variables,:);
     epsilon = sigma_points(number_of_state_variables+1:number_of_state_variables+number_of_structural_innovations,:);
@@ -132,10 +134,18 @@ 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
-            tmp = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, ThreadsOptions.local_state_space_iteration_2);
-        elseif order == 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, false);
+        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
     PredictedStateMean = tmp(mf0,:)*weights ;
diff --git a/tests/particle/noisy_ar1_nlf_nlkf.mod b/tests/particle/noisy_ar1_nlf_nlkf.mod
new file mode 100644
index 0000000000000000000000000000000000000000..77a9f790d2109443e35604487fd2d9f8c156eab8
--- /dev/null
+++ b/tests/particle/noisy_ar1_nlf_nlkf.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,
+           proposal_approximation = unscented,
+           filter_algorithm=nlkf);
+
+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