From d2c324eeee5118c203218baf983a477aa8f53220 Mon Sep 17 00:00:00 2001
From: Normann Rion <normann@dynare.org>
Date: Sat, 1 Jul 2023 16:43:48 +0200
Subject: [PATCH] Amends the various filters to fit the fixed version of
 local_state_space_iteration_3

See MR !2144 for more details
---
 .../src/auxiliary_initialization.m            |  4 ++-
 .../src/auxiliary_particle_filter.m           | 21 +++++++++-------
 .../src/conditional_filter_proposal.m         |  4 ++-
 .../src/gaussian_filter_bank.m                |  4 ++-
 .../src/gaussian_mixture_filter_bank.m        |  4 ++-
 .../src/measurement_equations.m               |  4 ++-
 .../src/nonlinear_kalman_filter.m             |  4 ++-
 .../src/online_auxiliary_filter.m             | 25 +++++++++++--------
 .../sequential_importance_particle_filter.m   | 19 ++++++++------
 .../src/solve_model_for_online_filter.m       |  1 +
 10 files changed, 57 insertions(+), 33 deletions(-)

diff --git a/matlab/nonlinear-filters/src/auxiliary_initialization.m b/matlab/nonlinear-filters/src/auxiliary_initialization.m
index 3e279117b7..e05741052a 100644
--- a/matlab/nonlinear-filters/src/auxiliary_initialization.m
+++ b/matlab/nonlinear-filters/src/auxiliary_initialization.m
@@ -33,6 +33,7 @@ end
 % Get steady state and mean.
 %steadystate = ReducedForm.steadystate;
 constant = ReducedForm.constant;
+ss = ReducedForm.ys;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
 % Set persistent variables.
@@ -54,6 +55,7 @@ ghu  = ReducedForm.ghu;
 ghxx = ReducedForm.ghxx;
 ghuu = ReducedForm.ghuu;
 ghxu = ReducedForm.ghxu;
+ghs2 = ReducedForm.ghs2;
 if (order == 3)
    ghxxx = ReducedForm.ghxxx;
    ghuuu = ReducedForm.ghuuu;
@@ -100,7 +102,7 @@ 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,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,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,ThreadsOptions.local_state_space_iteration_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, ss, options_.threads.local_state_space_iteration_3, false);
 else
    error('Orders > 3 not allowed');
 end
diff --git a/matlab/nonlinear-filters/src/auxiliary_particle_filter.m b/matlab/nonlinear-filters/src/auxiliary_particle_filter.m
index b84eca837a..21caa73bd1 100644
--- a/matlab/nonlinear-filters/src/auxiliary_particle_filter.m
+++ b/matlab/nonlinear-filters/src/auxiliary_particle_filter.m
@@ -54,6 +54,7 @@ else
     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;
@@ -93,11 +94,13 @@ if pruning
         state_variables_steady_state_ = state_variables_steady_state;
         mf0_ = mf0;
     elseif order == 3
-        StateVectors_ = repmat(StateVectors,2,1);
-        state_variables_steady_state_ = repmat(state_variables_steady_state,2,1);
-        mf0_ = repmat(mf0,1,2); 
-        mask = number_of_state_variables+1:2*number_of_state_variables;
-        mf0_(mask) = mf0_(mask)+size(ghx,1);
+        StateVectors_ = repmat(StateVectors,3,1);
+        state_variables_steady_state_ = repmat(state_variables_steady_state,3,1);
+        mf0_ = repmat(mf0,1,3); 
+        mask2 = number_of_state_variables+1:2*number_of_state_variables;
+        mask3 = 2*number_of_state_variables+1:3*number_of_state_variables;
+        mf0_(mask2) = mf0_(mask2)+size(ghx,1);
+        mf0_(mask3) = mf0_(mask3)+2*size(ghx,1);
     else
         error('Pruning is not available for orders > 3');
     end
@@ -110,7 +113,7 @@ for t=1:sample_size
         if order == 2
             [tmp, 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);
         elseif order == 3
-            [tmp, tmp_] = local_state_space_iteration_3(yhat,zeros(number_of_structural_innovations,number_of_particles),ghx,ghu,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_3);
+            [tmp, 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
             error('Pruning is not available for orders > 3');
         end
@@ -121,7 +124,7 @@ for t=1:sample_size
             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,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,ThreadsOptions.local_state_space_iteration_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
                 error('Order > 3: use_k_order_solver should be set to true');
             end
@@ -142,7 +145,7 @@ for t=1:sample_size
         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);
         elseif order == 3
-            [tmp, tmp_] = local_state_space_iteration_3(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_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
             error('Pruning is not available for orders > 3');
         end
@@ -154,7 +157,7 @@ for t=1:sample_size
             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, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_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
                 error('Order > 3: use_k_order_solver should be set to true');
             end
diff --git a/matlab/nonlinear-filters/src/conditional_filter_proposal.m b/matlab/nonlinear-filters/src/conditional_filter_proposal.m
index 45d7886ae2..4d60a44358 100644
--- a/matlab/nonlinear-filters/src/conditional_filter_proposal.m
+++ b/matlab/nonlinear-filters/src/conditional_filter_proposal.m
@@ -54,6 +54,7 @@ else
     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;
@@ -66,6 +67,7 @@ else
 end
 
 constant = ReducedForm.constant;
+steadystate = ReducedForm.steadystate;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
 mf0 = ReducedForm.mf0;
@@ -96,7 +98,7 @@ 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, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_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);
     else
         error('Order > 3: use_k_order_solver should be set to true');
     end
diff --git a/matlab/nonlinear-filters/src/gaussian_filter_bank.m b/matlab/nonlinear-filters/src/gaussian_filter_bank.m
index d7a649fa41..17ae110cc6 100644
--- a/matlab/nonlinear-filters/src/gaussian_filter_bank.m
+++ b/matlab/nonlinear-filters/src/gaussian_filter_bank.m
@@ -52,6 +52,7 @@ else
     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;
@@ -64,6 +65,7 @@ else
 end
 
 constant = ReducedForm.constant;
+steadystate = ReducedForm.steadystate;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
 mf0 = ReducedForm.mf0;
@@ -98,7 +100,7 @@ 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, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_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);
     else
         error('Order > 3: use_k_order_solver should be set to true');
     end
diff --git a/matlab/nonlinear-filters/src/gaussian_mixture_filter_bank.m b/matlab/nonlinear-filters/src/gaussian_mixture_filter_bank.m
index 69e3eb6137..acbe77f2b0 100644
--- a/matlab/nonlinear-filters/src/gaussian_mixture_filter_bank.m
+++ b/matlab/nonlinear-filters/src/gaussian_mixture_filter_bank.m
@@ -54,6 +54,7 @@ else
     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;
@@ -66,6 +67,7 @@ else
 end
 
 constant = ReducedForm.constant;
+steadystate = ReducedForm.steadystate;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
 mf0 = ReducedForm.mf0;
@@ -94,7 +96,7 @@ 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, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_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);
     else
         error('Order > 3: use_k_order_solver should be set to true');
     end
diff --git a/matlab/nonlinear-filters/src/measurement_equations.m b/matlab/nonlinear-filters/src/measurement_equations.m
index 477cc1782c..8ed06349d9 100644
--- a/matlab/nonlinear-filters/src/measurement_equations.m
+++ b/matlab/nonlinear-filters/src/measurement_equations.m
@@ -28,6 +28,7 @@ else
     ghxx = ReducedForm.ghxx(mf1,:);
     ghuu = ReducedForm.ghuu(mf1,:);
     ghxu = ReducedForm.ghxu(mf1,:);
+    ghs2 = ReducedForm.ghs2(mf1,:);
     if order == 3
         ghxxx = ReducedForm.ghxxx(mf1,:);
         ghuuu = ReducedForm.ghuuu(mf1,:);
@@ -37,6 +38,7 @@ else
         ghuss = ReducedForm.ghuss(mf1,:);
     end
 end
+steadystate = ReducedForm.steadystate(mf1,:);
 constant = ReducedForm.constant(mf1,:);
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 number_of_structural_innovations = length(ReducedForm.Q);
@@ -48,7 +50,7 @@ else
     if order == 2
         measure = local_state_space_iteration_2(yhat, zeros(number_of_structural_innovations, size(yhat,2)), ghx, ghu, constant, ghxx, ghuu, ghxu, ThreadsOptions.local_state_space_iteration_2);
     elseif order == 3
-        measure = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations, size(yhat,2)), ghx, ghu, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_3);
+        measure = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations, size(yhat,2)), ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, ThreadsOptions.local_state_space_iteration_3, false);
     else
         error('Order > 3: use_k_order_solver should be set to true');
     end
diff --git a/matlab/nonlinear-filters/src/nonlinear_kalman_filter.m b/matlab/nonlinear-filters/src/nonlinear_kalman_filter.m
index d46c0cd7ea..ccae0731db 100644
--- a/matlab/nonlinear-filters/src/nonlinear_kalman_filter.m
+++ b/matlab/nonlinear-filters/src/nonlinear_kalman_filter.m
@@ -67,6 +67,7 @@ else
     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;
@@ -79,6 +80,7 @@ else
 end
 
 constant = ReducedForm.constant;
+steadystate = ReducedForm.steadystate;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
 mf0 = ReducedForm.mf0;
@@ -133,7 +135,7 @@ for t=1:sample_size
         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, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, ThreadsOptions.local_state_space_iteration_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);
         end
     end
     PredictedStateMean = tmp(mf0,:)*weights ;
diff --git a/matlab/nonlinear-filters/src/online_auxiliary_filter.m b/matlab/nonlinear-filters/src/online_auxiliary_filter.m
index 16bf903e99..bbfb21e3e5 100644
--- a/matlab/nonlinear-filters/src/online_auxiliary_filter.m
+++ b/matlab/nonlinear-filters/src/online_auxiliary_filter.m
@@ -69,7 +69,7 @@ if pruning
     if order == 2
         StateVectors_ = StateVectors;
     elseif order == 3
-        StateVectors_ = repmat(StateVectors,2,1);
+        StateVectors_ = repmat(StateVectors,3,1);
     else
         error('Pruning is not available for orders > 3');
     end
@@ -134,6 +134,7 @@ for t=1:sample_size
                 dr = ReducedForm.dr;
                 udr = ReducedForm.udr;
             else
+                steadystate = ReducedForm.steadystate;
                 constant = ReducedForm.constant;
                 % Set local state space model (first-order approximation).
                 ghx  = ReducedForm.ghx;
@@ -142,6 +143,7 @@ for t=1:sample_size
                 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;
@@ -155,7 +157,7 @@ for t=1:sample_size
                     if order == 2
                         state_variables_steady_state_ = state_variables_steady_state;
                     elseif order == 3
-                        state_variables_steady_state_ = repmat(state_variables_steady_state,2,1);
+                        state_variables_steady_state_ = repmat(state_variables_steady_state,3,1);
                     else
                         error('Pruning is not available for orders > 3');
                     end
@@ -172,7 +174,7 @@ for t=1:sample_size
                     if order == 2
                         [tmp, ~] = local_state_space_iteration_2(yhat, zeros(number_of_structural_innovations, 1), ghx, ghu, constant, ghxx, ghuu, ghxu, yhat_, steadystate, DynareOptions.threads.local_state_space_iteration_2);
                     elseif order == 3
-                        [tmp, ~] = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations, 1), ghx, ghu, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, yhat_, steadystate, DynareOptions.threads.local_state_space_iteration_3);
+                        [tmp, tmp_] = local_state_space_iteration_3(yhat_, zeros(number_of_structural_innovations, 1), ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, DynareOptions.threads.local_state_space_iteration_3, pruning);
                     else
                         error('Pruning is not available for orders > 3');
                     end
@@ -180,7 +182,7 @@ for t=1:sample_size
                     if order == 2
                         tmp = local_state_space_iteration_2(yhat, zeros(number_of_structural_innovations, 1), ghx, ghu, constant, ghxx, ghuu, ghxu, DynareOptions.threads.local_state_space_iteration_2);
                     elseif order == 3
-                        tmp = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations, 1), ghx, ghu, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, DynareOptions.threads.local_state_space_iteration_3);
+                        tmp = local_state_space_iteration_3(yhat, zeros(number_of_structural_innovations, 1), ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, DynareOptions.threads.local_state_space_iteration_3, pruning);
                     else
                         error('Order > 3: use_k_order_solver should be set to true');
                     end
@@ -230,6 +232,7 @@ for t=1:sample_size
                         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;
@@ -244,10 +247,12 @@ for t=1:sample_size
                                 state_variables_steady_state_ = state_variables_steady_state;
                                 mf0_ = mf0;
                             elseif order == 3
-                                state_variables_steady_state_ = repmat(state_variables_steady_state,2,1);
-                                mf0_ = repmat(mf0,1,2); 
-                                mask = number_of_state_variables+1:2*number_of_state_variables;
-                                mf0_(mask) = mf0_(mask)+size(ghx,1);
+                                state_variables_steady_state_ = repmat(state_variables_steady_state,3,1);
+                                mf0_ = repmat(mf0,1,3); 
+                                mask2 = number_of_state_variables+1:2*number_of_state_variables;
+                                 mask3 = 2*number_of_state_variables+1:number_of_state_variables;
+                                mf0_(mask2) = mf0_(mask2)+size(ghx,1);
+                                mf0_(mask3) = mf0_(mask3)+2*size(ghx,1);
                             else
                                 error('Pruning is not available for orders > 3');
                             end
@@ -265,7 +270,7 @@ for t=1:sample_size
                             if order == 2
                                 [tmp, tmp_] = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, yhat_, steadystate, DynareOptions.threads.local_state_space_iteration_2);
                             elseif order == 3
-                                [tmp, tmp_] = local_state_space_iteration_3(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, yhat_, steadystate, DynareOptions.threads.local_state_space_iteration_3);
+                                [tmp, tmp_] = local_state_space_iteration_3(yhat_, epsilon, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, DynareOptions.threads.local_state_space_iteration_3, pruning);
                             else
                                 error('Pruning is not available for orders > 3');
                             end
@@ -274,7 +279,7 @@ for t=1:sample_size
                             if order == 2
                                 tmp = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, DynareOptions.threads.local_state_space_iteration_2);
                             elseif order == 3
-                                tmp = local_state_space_iteration_3(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, DynareOptions.threads.local_state_space_iteration_3);
+                                tmp = local_state_space_iteration_3(yhat, epsilon, ghx, ghu, ghxx, ghuu, ghxu, ghs2, ghxxx, ghuuu, ghxxu, ghxuu, ghxss, ghuss, steadystate, DynareOptions.threads.local_state_space_iteration_3, pruning);
                             else
                                 error('Order > 3: use_k_order_solver should be set to true');
                             end
diff --git a/matlab/nonlinear-filters/src/sequential_importance_particle_filter.m b/matlab/nonlinear-filters/src/sequential_importance_particle_filter.m
index 4c9c8313e6..c69ded02f2 100644
--- a/matlab/nonlinear-filters/src/sequential_importance_particle_filter.m
+++ b/matlab/nonlinear-filters/src/sequential_importance_particle_filter.m
@@ -63,6 +63,7 @@ else
     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;
@@ -111,11 +112,13 @@ if pruning
         state_variables_steady_state_ = state_variables_steady_state;
         mf0_ = mf0;
     elseif order == 3
-        StateVectors_ = repmat(StateVectors,2,1);
-        state_variables_steady_state_ = repmat(state_variables_steady_state,2,1);
-        mf0_ = repmat(mf0,1,2); 
-        mask = number_of_state_variables+1:2*number_of_state_variables;
-        mf0_(mask) = mf0_(mask)+size(ghx,1);
+        StateVectors_ = repmat(StateVectors,3,1);
+        state_variables_steady_state_ = repmat(state_variables_steady_state,3,1);
+        mf0_ = repmat(mf0,1,3); 
+        mask2 = number_of_state_variables+1:2*number_of_state_variables;
+        mask3 = 2*number_of_state_variables+1:3*number_of_state_variables;
+        mf0_(mask2) = mf0_(mask2)+size(ghx,1);
+        mf0_(mask3) = mf0_(mask3)+2*size(ghx,1);
     else
         error('Pruning is not available for orders > 3');
     end
@@ -130,7 +133,7 @@ for t=1:sample_size
         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);
         elseif order == 3
-            [tmp, tmp_] = local_state_space_iteration_3(yhat,epsilon,ghx,ghu,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,yhat_,steadystate,ThreadsOptions.local_state_space_iteration_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
             error('Pruning is not available for orders > 3');
         end
@@ -141,7 +144,7 @@ for t=1:sample_size
             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,constant,ghxx,ghuu,ghxu,ghxxx,ghuuu,ghxxu,ghxuu,ghxss,ghuss,ThreadsOptions.local_state_space_iteration_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
                 error('Order > 3: use_k_order_solver should be set to true');
             end
@@ -166,7 +169,7 @@ for t=1:sample_size
         if pruning
             temp = resample([tmp(mf0,:)' tmp_(mf0_,:)'],weights',ParticleOptions);
             StateVectors = temp(:,1:number_of_state_variables)';
-            StateVectors_ = temp(:,number_of_state_variables+1:2*number_of_state_variables)';
+            StateVectors_ = temp(:,number_of_state_variables+1:end)';
         else
             StateVectors = resample(tmp(mf0,:)',weights',ParticleOptions)';
         end
diff --git a/matlab/nonlinear-filters/src/solve_model_for_online_filter.m b/matlab/nonlinear-filters/src/solve_model_for_online_filter.m
index 7f65649bc2..92cbf4c740 100644
--- a/matlab/nonlinear-filters/src/solve_model_for_online_filter.m
+++ b/matlab/nonlinear-filters/src/solve_model_for_online_filter.m
@@ -161,6 +161,7 @@ if nargout>4
         ReducedForm.ghuu = dr.ghuu(restrict_variables_idx,:);
         ReducedForm.ghxu = dr.ghxu(restrict_variables_idx,:);
         ReducedForm.constant = ReducedForm.steadystate + .5*dr.ghs2(restrict_variables_idx);
+        ReducedForm.ghs2 = dr.ghs2(restrict_variables_idx,:);
     elseif DynareOptions.order>=3
         ReducedForm.use_k_order_solver = true;
         ReducedForm.dr = dr;
-- 
GitLab