diff --git a/matlab/discretionary_policy.m b/matlab/discretionary_policy.m
index faff08ba9f8bf8fe138481f2f4297577b779cc5b..778d1789db328591ee1aaa01ac997397a7d97cb4 100644
--- a/matlab/discretionary_policy.m
+++ b/matlab/discretionary_policy.m
@@ -1,4 +1,15 @@
 function [info, oo_, options_] = discretionary_policy(M_, options_, oo_, var_list)
+% function [info, oo_, options_] = discretionary_policy(M_, options_, oo_, var_list)
+% INPUTS
+% - M_            [structure]     Matlab's structure describing the model (M_).
+% - options_      [structure]     Matlab's structure describing the current options (options_).
+% - oo_           [structure]     Matlab's structure containing the results (oo_).
+% - var_list      [cell]          list of variables
+%
+% OUTPUTS
+% - info          [integer]       scalar or vector, error code.
+% - oo_           [structure]     Matlab's structure containing the results (oo_).
+% - options_      [structure]     Matlab's structure describing the current options (options_).
 
 % Copyright (C) 2007-2019 Dynare Team
 %
@@ -17,10 +28,7 @@ function [info, oo_, options_] = discretionary_policy(M_, options_, oo_, var_lis
 % You should have received a copy of the GNU General Public License
 % along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
 
-if options_.loglinear
-    % Ensure it's ok to ignore options_ returned from stoch_simul. #1197
-    error('discretionary_policy is not compatible with `loglinear` option set to 1')
-end
+M_=discretionary_policy_initialization(M_,options_);
 
 origorder = options_.order;
 options_.discretionary_policy = 1;
diff --git a/matlab/discretionary_policy_1.m b/matlab/discretionary_policy_1.m
index df38ce4c7ee9dbdd719df57db90cc65554143fd5..1efa7ff0d0c27417a5ecfd072380e69e8bfb2fb0 100644
--- a/matlab/discretionary_policy_1.m
+++ b/matlab/discretionary_policy_1.m
@@ -34,134 +34,75 @@ persistent Hold
 
 info = 0;
 
-if isempty(options_.qz_criterium)
-    options_.qz_criterium = 1+1e-6;
-end
-
-% safeguard against issues like running ramsey policy first and then running discretion
-if isfield(M_,'orig_model')
-    orig_model = M_.orig_model;
-    M_.endo_nbr = orig_model.endo_nbr;
-    M_.endo_names = orig_model.endo_names;
-    M_.lead_lag_incidence = orig_model.lead_lag_incidence;
-    M_.maximum_lead = orig_model.maximum_lead;
-    M_.maximum_endo_lead = orig_model.maximum_endo_lead;
-    M_.maximum_lag = orig_model.maximum_lag;
-    M_.maximum_endo_lag = orig_model.maximum_endo_lag;
-else
-    M_.orig_model = M_;
-end
-
 beta = get_optimal_policy_discount_factor(M_.params, M_.param_names);
 
-exo_nbr = M_.exo_nbr;
-if isfield(M_,'orig_model')
-    orig_model = M_.orig_model;
-    endo_nbr = orig_model.endo_nbr;
-    endo_names = orig_model.endo_names;
-    lead_lag_incidence = orig_model.lead_lag_incidence;
-    MaxLead = orig_model.maximum_lead;
-    MaxLag = orig_model.maximum_lag;
-else
-    endo_names = M_.endo_names;
-    endo_nbr = M_.endo_nbr;
-    MaxLag=M_.maximum_lag;
-    MaxLead=M_.maximum_lead;
-    lead_lag_incidence = M_.lead_lag_incidence;
-end
-
 %call steady_state_file if present to update parameters
 if options_.steadystate_flag
     % explicit steady state file
     [~,M_.params,info] = evaluate_steady_state_file(oo_.steady_state,[oo_.exo_steady_state; oo_.exo_det_steady_state],M_, ...
                                                     options_,false);
 end
-[U,Uy,W] = feval([M_.fname,'.objective.static'],zeros(endo_nbr,1),[], M_.params);
+[U,Uy,W] = feval([M_.fname,'.objective.static'],zeros(M_.endo_nbr,1),[], M_.params);
 if any(any(isnan(Uy)))
-    error(['discretionary_policy: the derivatives of the objective function contain NaN'])
+    info = 64 ; %the derivatives of the objective function contain NaN
+    return;
 end
 if any(any(Uy~=0))
-    non_zero_derivs=find(any(Uy~=0));
-    for ii=1:length(non_zero_derivs)
-        non_zero_deriv_names{ii,1} = M_.endo_names{non_zero_derivs(ii)};
-    end
-    disp_string=[non_zero_deriv_names{1,:}];
-    for ii=2:size(non_zero_deriv_names,1)
-        disp_string=[disp_string,', ',non_zero_deriv_names{ii,:}];
+    if options_.debug
+        non_zero_derivs=find(any(Uy~=0));
+        for ii=1:length(non_zero_derivs)
+            non_zero_deriv_names{ii,1} = M_.endo_names{non_zero_derivs(ii)};
+        end
+        disp_string=[non_zero_deriv_names{1,:}];
+        for ii=2:size(non_zero_deriv_names,1)
+            disp_string=[disp_string,', ',non_zero_deriv_names{ii,:}];
+        end
+        fprintf('\nThe derivative of the objective function w.r.t. to variable(s) %s is not 0\n',disp_string);
     end
-    fprintf('\nThe derivative of the objective function w.r.t. to variable(s) %s is not 0\n',disp_string)
-    error(['discretionary_policy: the objective function must have zero ' ...
-           'first order derivatives'])
+    info = 66;
+    return;
 end
 
-W=reshape(W,endo_nbr,endo_nbr);
+W=reshape(W,M_.endo_nbr,M_.endo_nbr);
 
-klen = MaxLag + MaxLead + 1;
-iyv=lead_lag_incidence';
+klen = M_.maximum_lag + M_.maximum_lead + 1;
+iyv=M_.lead_lag_incidence';
 % Find the jacobian
-z = repmat(zeros(endo_nbr,1),1,klen);
+z = repmat(zeros(M_.endo_nbr,1),1,klen);
 z = z(nonzeros(iyv)) ;
-it_ = MaxLag + 1 ;
+it_ = M_.maximum_lag + 1 ;
 
-if exo_nbr == 0
+if M_.exo_nbr == 0
     oo_.exo_steady_state = [] ;
 end
 
 [junk,jacobia_] = feval([M_.fname '.dynamic'],z, [zeros(size(oo_.exo_simul)) ...
-                    oo_.exo_det_simul], M_.params, zeros(endo_nbr,1), it_);
+                    oo_.exo_det_simul], M_.params, zeros(M_.endo_nbr,1), it_);
 if any(junk~=0)
-    error(['discretionary_policy: the model must be written in deviation ' ...
-           'form and not have constant terms'])
+    info = 65; %the model must be written in deviation form and not have constant terms
+    return;
 end
 
-eq_nbr= size(jacobia_,1);
-instr_nbr=endo_nbr-eq_nbr;
-
-if instr_nbr==0
-    error('discretionary_policy:: There are no available instruments, because the model has as many equations as variables.')
-end
-if size(Instruments,1)< instr_nbr
-    error('discretionary_policy:: There are fewer declared instruments than omitted equations.')
-elseif size(Instruments,1)> instr_nbr
-    error('discretionary_policy:: There are more declared instruments than omitted equations.')
-end
-
-instr_id=nan(instr_nbr,1);
-for j=1:instr_nbr
-    vj=deblank(Instruments{j});
-    vj_id=strmatch(vj, endo_names, 'exact');
-    if ~isempty(vj_id)
-        instr_id(j)=vj_id;
-    else
-        error([mfilename,':: instrument ',vj,' not found'])
-    end
-end
-
-Indices={'lag','0','lead'};
+Indices={'lag','contemp','lead'};
 iter=1;
 for j=1:numel(Indices)
-    eval(['A',Indices{j},'=zeros(eq_nbr,endo_nbr);'])
-    if strcmp(Indices{j},'0')||(strcmp(Indices{j},'lag') && MaxLag)||(strcmp(Indices{j},'lead') && MaxLead)
-        [~,row,col]=find(lead_lag_incidence(iter,:));
-        eval(['A',Indices{j},'(:,row)=jacobia_(:,col);'])
+    A.(Indices{j})=zeros(M_.orig_eq_nbr,M_.endo_nbr);
+    if strcmp(Indices{j},'contemp')||(strcmp(Indices{j},'lag') && M_.maximum_lag)||(strcmp(Indices{j},'lead') && M_.maximum_lead)
+        [~,row,col]=find(M_.lead_lag_incidence(iter,:));
+        A.(Indices{j})(:,row)=jacobia_(:,col);
         iter=iter+1;
     end
 end
 B=jacobia_(:,nnz(iyv)+1:end);
 
 %%% MAIN ENGINE %%%
-qz_criterium = options_.qz_criterium;
-solve_maxit = options_.dp.maxit;
-discretion_tol = options_.discretionary_tol;
 
 if ~isempty(Hold)
-    [H,G,info]=discretionary_policy_engine(Alag,A0,Alead,B,W,instr_id,beta,solve_maxit,discretion_tol,qz_criterium,Hold);
+    [H,G,info]=discretionary_policy_engine(A.lag,A.contemp,A.lead,B,W,M_.instr_id,beta,options_.dp.maxit,options_.discretionary_tol,options_.qz_criterium,Hold);
 else
-    [H,G,info]=discretionary_policy_engine(Alag,A0,Alead,B,W,instr_id,beta,solve_maxit,discretion_tol,qz_criterium);
+    [H,G,info]=discretionary_policy_engine(A.lag,A.contemp,A.lead,B,W,M_.instr_id,beta,options_.dp.maxit,options_.discretionary_tol,options_.qz_criterium);
 end
 
-% set the state
-dr=oo_.dr;
 
 if info
     return
@@ -169,13 +110,13 @@ else
     Hold=H; %save previous solution
             % Hold=[]; use this line if persistent command is not used.
 end
-dr.ys =zeros(endo_nbr,1);
-dr=set_state_space(dr,M_,options_);
-order_var=dr.order_var;
 
-T=H(order_var,order_var);
-dr.ghu=G(order_var,:);
-Selection=lead_lag_incidence(1,order_var)>0;%select state variables
+%write back solution to dr
+dr=oo_.dr;
+dr.ys =zeros(M_.endo_nbr,1);
+dr=set_state_space(dr,M_,options_);
+T=H(dr.order_var,dr.order_var);
+dr.ghu=G(dr.order_var,:);
+Selection=M_.lead_lag_incidence(1,dr.order_var)>0;%select state variables
 dr.ghx=T(:,Selection);
-
 oo_.dr = dr;
diff --git a/matlab/discretionary_policy_initialization.m b/matlab/discretionary_policy_initialization.m
new file mode 100644
index 0000000000000000000000000000000000000000..2c417f44087f7cccff080b984ae7f321853ab4b0
--- /dev/null
+++ b/matlab/discretionary_policy_initialization.m
@@ -0,0 +1,65 @@
+function M_=discretionary_policy_initialization(M_,options_)
+% function M_=discretionary_policy_initialization(M_,options_)
+% INPUTS
+% - M_            [structure]     Matlab's structure describing the model (M_).
+% - options_      [structure]     Matlab's structure describing the current options (options_).
+%
+% OUTPUTS
+% - M_            [structure]     Matlab's structure describing the model (M_).
+
+% Copyright (C) 2020 Dynare Team
+%
+% This file is part of Dynare.
+%
+% Dynare is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% Dynare is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU General Public License for more details.
+%
+% You should have received a copy of the GNU General Public License
+% along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
+
+
+if options_.loglinear
+    % Ensure it's ok to ignore options_ returned from stoch_simul. #1197
+    error('discretionary_policy is not compatible with `loglinear` option set to 1')
+end
+
+% safeguard against issues like running ramsey policy first and then running discretion
+if isfield(M_,'orig_model')
+    M_.endo_nbr = M_.orig_model.endo_nbr;
+    M_.endo_names = M_.orig_model.endo_names;
+    M_.lead_lag_incidence = M_.orig_model.lead_lag_incidence;
+    M_.maximum_lead = M_.orig_model.maximum_lead;
+    M_.maximum_endo_lead = M_.orig_model.maximum_endo_lead;
+    M_.maximum_lag = M_.orig_model.maximum_lag;
+    M_.maximum_endo_lag = M_.orig_model.maximum_endo_lag;
+end
+
+instr_nbr=M_.orig_endo_nbr-M_.orig_eq_nbr;
+
+if instr_nbr==0
+    error('discretionary_policy:: There are no available instruments, because the model has as many equations as variables.')
+end
+if size(options_.instruments,1)< instr_nbr
+    error('discretionary_policy:: There are fewer declared instruments than omitted equations.')
+elseif size(options_.instruments,1)> instr_nbr
+    error('discretionary_policy:: There are more declared instruments than omitted equations.')
+end
+
+instr_id=NaN(size(options_.instruments,1),1);
+for j=1:size(options_.instruments,1)
+    vj=deblank(options_.instruments{j});
+    vj_id=strmatch(vj, M_.endo_names, 'exact');
+    if ~isempty(vj_id)
+        instr_id(j)=vj_id;
+    else
+        error([mfilename,':: instrument ',vj,' not found'])
+    end
+end
+M_.instr_id=instr_id;
diff --git a/matlab/dynare_estimation_init.m b/matlab/dynare_estimation_init.m
index d914964b34c1c01e5051dea3b23e44434bdcfc86..3495a7d04a04825ca72b49aabca0880bc5f1d056 100644
--- a/matlab/dynare_estimation_init.m
+++ b/matlab/dynare_estimation_init.m
@@ -32,7 +32,7 @@ function [dataset_, dataset_info, xparam1, hh, M_, options_, oo_, estim_params_,
 % SPECIAL REQUIREMENTS
 %   none
 
-% Copyright (C) 2003-2018 Dynare Team
+% Copyright (C) 2003-2020 Dynare Team
 %
 % This file is part of Dynare.
 %
@@ -100,6 +100,14 @@ if length(unique(options_.varobs))<length(options_.varobs)
     end
 end
 
+if options_.discretionary_policy
+    if options_.order>1
+        error('discretionary_policy does not support order>1');
+    else
+        M_=discretionary_policy_initialization(M_,options_);
+    end
+end
+
 % Check the perturbation order (k order perturbation based nonlinear filters are not yet implemented for k>1).
 if options_.order>2 && options_.particle.pruning
     error('Higher order nonlinear filters are not compatible with pruning option.')
diff --git a/matlab/get_error_message.m b/matlab/get_error_message.m
index 12facad132038c93eee6e19d68791b553bcd0b3a..4380f3a9088a656fd6ac441db03fe663f75eaa40 100644
--- a/matlab/get_error_message.m
+++ b/matlab/get_error_message.m
@@ -121,6 +121,12 @@ if ~noprint
             message = 'Discretionary policy: some eigenvalues greater than options_.qz_criterium. Model potentially unstable.';
         case 63
             message = 'Discretionary policy: NaN elements are present in the solution. Procedure failed.';
+        case 64
+            message = 'discretionary_policy: the derivatives of the objective function contain NaN.';   
+        case 65
+            message = 'discretionary_policy: the model must be written in deviation form and not have constant terms.';   
+        case 66
+            message = 'discretionary_policy: the objective function must have zero first order derivatives.';
         case 71
             message = 'Calibrated covariance of the structural errors implies correlation larger than +-1.';
         case 72