Allow k order approximation in nonlinear Kalman Filter (nlkf).

Ref. dynare#1673

src/nonlinear_kalman_filter.m

-function [LIK,lik] = nonlinear_kalman_filter(ReducedForm, Y, start, ParticleOptions, ThreadsOptions)
+function [LIK,lik] = nonlinear_kalman_filter(ReducedForm, Y, start, ParticleOptions, ThreadsOptions, DynareOptions, Model)
+
 % Evaluates the likelihood of a non-linear model approximating the
 % predictive (prior) and filtered (posterior) densities for state variables
 % by a Kalman filter.
@@ -30,7 +31,8 @@ function [LIK,lik] = nonlinear_kalman_filter(ReducedForm, Y, start, ParticleOpti
 %
 % NOTES
 %   The vector "lik" is used to evaluate the jacobian of the likelihood.
-% Copyright (C) 2009-2017 Dynare Team
+
+% Copyright (C) 2009-2019 Dynare Team
 %
 % This file is part of Dynare.
 %
@@ -47,14 +49,14 @@ function [LIK,lik] = nonlinear_kalman_filter(ReducedForm, Y, start, ParticleOpti
 % You should have received a copy of the GNU General Public License
 % along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
 
-persistent init_flag mf0 mf1 nodes weights weights_c
-persistent sample_size number_of_state_variables number_of_observed_variables number_of_structural_innovations
-
 % Set default
 if isempty(start)
     start = 1;
 end
 
+if ReducedForm.use_k_order_solver
+    dr = ReducedForm.dr;
+else
     % Set local state space model (first-order approximation).
     ghx  = ReducedForm.ghx;
     ghu  = ReducedForm.ghu;
@@ -62,33 +64,19 @@ ghu  = ReducedForm.ghu;
     ghxx = ReducedForm.ghxx;
     ghuu = ReducedForm.ghuu;
     ghxu = ReducedForm.ghxu;
-
-if any(any(isnan(ghx))) || any(any(isnan(ghu))) || any(any(isnan(ghxx))) || any(any(isnan(ghuu))) || any(any(isnan(ghxu))) || ...
-        any(any(isinf(ghx))) || any(any(isinf(ghu))) || any(any(isinf(ghxx))) || any(any(isinf(ghuu))) || any(any(isinf(ghxu))) ...
-        any(any(abs(ghx)>1e4)) || any(any(abs(ghu)>1e4)) || any(any(abs(ghxx)>1e4)) || any(any(abs(ghuu)>1e4)) || any(any(abs(ghxu)>1e4))
-    ghx
-    ghu
-    ghxx
-    ghuu
-    ghxu
 end
 
 constant = ReducedForm.constant;
 state_variables_steady_state = ReducedForm.state_variables_steady_state;
 
-% Set persistent variables.
-if isempty(init_flag)
 mf0 = ReducedForm.mf0;
 mf1 = ReducedForm.mf1;
 sample_size = size(Y,2);
 number_of_state_variables = length(mf0);
 number_of_observed_variables = length(mf1);
 number_of_structural_innovations = length(ReducedForm.Q);
-    init_flag = 1;
-end
 
 % compute gaussian quadrature nodes and weights on states and shocks
-
 if ParticleOptions.proposal_approximation.montecarlo
     nodes = randn(ParticleOptions.number_of_particles,number_of_state_variables+number_of_structural_innovations);
     weights = 1/ParticleOptions.number_of_particles;
@@ -120,27 +108,28 @@ lik  = NaN(sample_size,1);
 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 ] ];
+    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];
     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,:);
     yhat = bsxfun(@minus,StateVectors,state_variables_steady_state);
+    if ReducedForm.use_k_order_solver
+        tmp = local_state_space_iteration_k(yhat, epsilon, dr, Model, DynareOptions);
+    else
         tmp = local_state_space_iteration_2(yhat, epsilon, ghx, ghu, constant, ghxx, ghuu, ghxu, ThreadsOptions.local_state_space_iteration_2);
+    end
     PredictedStateMean = tmp(mf0,:)*weights ;
     PredictedObservedMean = tmp(mf1,:)*weights;
-
     if ParticleOptions.proposal_approximation.cubature || ParticleOptions.proposal_approximation.montecarlo
         PredictedStateMean = sum(PredictedStateMean,2);
         PredictedObservedMean = sum(PredictedObservedMean,2);
         dState = bsxfun(@minus,tmp(mf0,:),PredictedStateMean)'.*sqrt(weights);
         dObserved = bsxfun(@minus,tmp(mf1,:),PredictedObservedMean)'.*sqrt(weights);
         big_mat = [dObserved  dState ; [H_lower_triangular_cholesky zeros(number_of_observed_variables,number_of_state_variables)] ];
-        [mat1,mat] = qr2(big_mat,0);
+        [~, mat] = qr2(big_mat,0);
         mat = mat';
-        clear('mat1');
         PredictedObservedVarianceSquareRoot = mat(1:number_of_observed_variables,1:number_of_observed_variables);
         CovarianceObservedStateSquareRoot = mat(number_of_observed_variables+(1:number_of_state_variables),1:number_of_observed_variables);
         StateVectorVarianceSquareRoot = mat(number_of_observed_variables+(1:number_of_state_variables),number_of_observed_variables+(1:number_of_state_variables));
@@ -162,16 +151,14 @@ for t=1:sample_size
             lik(t)=-Inf;
             return
         end
-        [PredictedObservedVarianceSquareRoot, p]= chol(PredictedObservedVariance,'lower');
+        [~, p]= chol(PredictedObservedVariance,'lower');
         if p
             LIK=-Inf;
             lik(t)=-Inf;
             return
         end
     end
-%    lik(t) = log( probability2(0,PredictedObservedVarianceSquareRoot,PredictionError) ) ;
     lik(t) = log( sum(probability2(Y(:,t),H_lower_triangular_cholesky,tmp(mf1,:)).*weights,1) ) ;
-%    lik(t) = log(sum(probability2(Y(:,t),PredictedObservedVarianceSquareRoot,tmp(mf1,:)).*weights,1) ) ;
 end
 
 LIK = -sum(lik(start:end));
\ No newline at end of file