diff --git a/matlab/dyn_risky_steadystate_solver.m b/matlab/dyn_risky_steadystate_solver.m
index 6067aa19ebdbab2ac72cac45cdce962a70caf050..d739a8b19927290617c0d2a9afc6baa99a5170a9 100644
--- a/matlab/dyn_risky_steadystate_solver.m
+++ b/matlab/dyn_risky_steadystate_solver.m
@@ -1,92 +1,93 @@
 function [dr,info] = dyn_risky_steadystate_solver(ys0,M, ...
                                                   dr,options,oo)
 
-%@info:
-%! @deftypefn {Function File} {[@var{dr},@var{info}] =} dyn_risky_steadystate_solver (@var{ys0},@var{M},@var{dr},@var{options},@var{oo})
-%! @anchor{dyn_risky_steadystate_solver}
-%! @sp 1
-%! Computes the second order risky steady state and first and second order reduced form of the DSGE model.
-%! @sp 2
-%! @strong{Inputs}
-%! @sp 1
-%! @table @ @var
-%! @item ys0
-%! Vector containing a guess value for the risky steady state
-%! @item M
-%! Matlab's structure describing the model (initialized by @code{dynare}).
-%! @item dr
-%! Matlab's structure describing the reduced form solution of the model.
-%! @item options
-%! Matlab's structure describing the options (initialized by @code{dynare}).
-%! @item oo
-%! Matlab's structure gathering the results (initialized by @code{dynare}).
-%! @end table
-%! @sp 2
-%! @strong{Outputs}
-%! @sp 1
-%! @table @ @var
-%! @item dr
-%! Matlab's structure describing the reduced form solution of the model.
-%! @item info
-%! Integer scalar, error code.
-%! @sp 1
-%! @table @ @code
-%! @item info==0
-%! No error.
-%! @item info==1
-%! The model doesn't determine the current variables uniquely.
-%! @item info==2
-%! MJDGGES returned an error code.
-%! @item info==3
-%! Blanchard & Kahn conditions are not satisfied: no stable equilibrium.
-%! @item info==4
-%! Blanchard & Kahn conditions are not satisfied: indeterminacy.
-%! @item info==5
-%! Blanchard & Kahn conditions are not satisfied: indeterminacy due to rank failure.
-%! @item info==6
-%! The jacobian evaluated at the deterministic steady state is complex.
-%! @item info==19
-%! The steadystate routine thrown an exception (inconsistent deep parameters).
-%! @item info==20
-%! Cannot find the steady state, info(2) contains the sum of square residuals (of the static equations).
-%! @item info==21
-%! The steady state is complex, info(2) contains the sum of square of imaginary parts of the steady state.
-%! @item info==22
-%! The steady has NaNs.
-%! @item info==23
-%! M_.params has been updated in the steadystate routine and has complex valued scalars.
-%! @item info==24
-%! M_.params has been updated in the steadystate routine and has some NaNs.
-%! @end table
-%! @end table
-%! @end deftypefn
-%@eod:
-
-% Copyright (C) 2001-2012 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/>.
+    %@info:
+    %! @deftypefn {Function File} {[@var{dr},@var{info}] =} dyn_risky_steadystate_solver (@var{ys0},@var{M},@var{dr},@var{options},@var{oo})
+    %! @anchor{dyn_risky_steadystate_solver}
+    %! @sp 1
+    %! Computes the second order risky steady state and first and second order reduced form of the DSGE model.
+    %! @sp 2
+    %! @strong{Inputs}
+    %! @sp 1
+    %! @table @ @var
+    %! @item ys0
+    %! Vector containing a guess value for the risky steady state
+    %! @item M
+    %! Matlab's structure describing the model (initialized by @code{dynare}).
+    %! @item dr
+    %! Matlab's structure describing the reduced form solution of the model.
+    %! @item options
+    %! Matlab's structure describing the options (initialized by @code{dynare}).
+    %! @item oo
+    %! Matlab's structure gathering the results (initialized by @code{dynare}).
+    %! @end table
+    %! @sp 2
+    %! @strong{Outputs}
+    %! @sp 1
+    %! @table @ @var
+    %! @item dr
+    %! Matlab's structure describing the reduced form solution of the model.
+    %! @item info
+    %! Integer scalar, error code.
+    %! @sp 1
+    %! @table @ @code
+    %! @item info==0
+    %! No error.
+    %! @item info==1
+    %! The model doesn't determine the current variables uniquely.
+    %! @item info==2
+    %! MJDGGES returned an error code.
+    %! @item info==3
+    %! Blanchard & Kahn conditions are not satisfied: no stable equilibrium.
+    %! @item info==4
+    %! Blanchard & Kahn conditions are not satisfied: indeterminacy.
+    %! @item info==5
+    %! Blanchard & Kahn conditions are not satisfied: indeterminacy due to rank failure.
+    %! @item info==6
+    %! The jacobian evaluated at the deterministic steady state is complex.
+    %! @item info==19
+    %! The steadystate routine thrown an exception (inconsistent deep parameters).
+    %! @item info==20
+    %! Cannot find the steady state, info(2) contains the sum of square residuals (of the static equations).
+    %! @item info==21
+    %! The steady state is complex, info(2) contains the sum of square of imaginary parts of the steady state.
+    %! @item info==22
+    %! The steady has NaNs.
+    %! @item info==23
+    %! M_.params has been updated in the steadystate routine and has complex valued scalars.
+    %! @item info==24
+    %! M_.params has been updated in the steadystate routine and has some NaNs.
+    %! @end table
+    %! @end table
+    %! @end deftypefn
+    %@eod:
+
+    % Copyright (C) 2001-2012 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/>.
 
     
     info = 0;
     lead_lag_incidence = M.lead_lag_incidence;
     order_var = dr.order_var;
+    endo_nbr = M.endo_nbr;
     exo_nbr = M.exo_nbr;
     
     M.var_order_endo_names = M.endo_names(dr.order_var,:);
- 
+    
     [junk,dr.i_fwrd_g,i_fwrd_f] = find(lead_lag_incidence(3,order_var));
     dr.i_fwrd_f = i_fwrd_f;
     nd = nnz(lead_lag_incidence) + M.exo_nbr;
@@ -104,54 +105,41 @@ function [dr,info] = dyn_risky_steadystate_solver(ys0,M, ...
     end
     
     if isfield(options,'portfolio') && options.portfolio == 1
-        eq_tags = M.equations_tags;
-        n_tags = size(eq_tags,1);
-        l_var = zeros(n_tags,1);
-        for i=1:n_tags
-            l_var(i) = find(strncmp(eq_tags(i,3),M.endo_names, ...
-                                    length(cell2mat(eq_tags(i,3)))));
+        pm = portfolio_model_structure(M,options);
+        
+        x0 = ys0(pm.v_p);
+        n = length(x0);
+        [x, info] = solve1(@risky_residuals_ds,x0,1:n,1:n,0,1, options.gstep, ...
+                           options.solve_tolf,options.solve_tolx, ...
+                           options.solve_maxit,options.debug,pm,M,dr, ...
+                           options,oo);
+        if info
+            error('DS approach can''t be computed')
         end
-        dr.ys = ys0;
-        x0 = ys0(l_var);
-        %        dr = first_step_ds(x0,M,dr,options,oo);
-        n = size(ys0);
-        %x0 = ys0;
-        [x, info] = solve1(@risky_residuals_ds,x0,1:n_tags,1:n_tags,0,1,M,dr,options,oo);
-        %[x, info] = solve1(@risky_residuals,x0,1:n,1:n,0,1,M,dr,options,oo);
+        %[x, info] = csolve(@risky_residuals_ds,x0,[],1e-10,100,M,dr,options,oo);
         %        ys0(l_var) = x;
-        ys0(l_var) = x;
-        dr.ys = ys0;
-        oo.dr = dr;
-        oo.steady_state = ys0;
-        disp_steady_state(M,oo);
+        [resids,dr1] = risky_residuals_ds(x,pm,M,dr,options,oo); 
+        ys1 = dr1.ys;
     end
-        
-    [ys, info] = csolve(func,ys0,[],1e-10,100,M,dr,options,oo);
     
-    if options.k_order_solver
-        [resid,dr] = risky_residuals_k_order(ys,M,dr,options,oo);
-    else
-        [resid,dr] = risky_residuals(ys,M,dr,options,oo);
+    [ys, info] = solve1(func,ys0,1:endo_nbr,1:endo_nbr,0,1, options.gstep, ...
+                        options.solve_tolf,options.solve_tolx, ...
+                        options.solve_maxit,options.debug,pm,M,dr,options,oo);
+    %    [ys, info] = csolve(func,ys0,[],1e-10,100,M,dr,options,oo);
+    if info
+        error('RSS approach can''t be computed')
     end
-    
     dr.ys = ys;
+
+    [resid,dr] = func(ys,pm,M,dr,options,oo);
+
     for i=1:M.endo_nbr
-        disp(sprintf('%16s %12.6f %12.6f',M.endo_names(i,:),ys0(i), ys(i)))
+        disp(sprintf('%16s %12.6f %12.6f',M.endo_names(i,:),ys1(i), ys(i)))
     end
-    
-    dr.ghs2 = zeros(size(dr.ghs2));
-
-    k_var = setdiff(1:M.endo_nbr,l_var);
-    dr.ghx(k_var,:) = dr.ghx;
-    dr.ghu(k_var,:) = dr.ghu;
-    dr.ghxx(k_var,:) = dr.ghxx;
-    dr.ghxu(k_var,:) = dr.ghxu;
-    dr.ghuu(k_var,:) = dr.ghuu;
-    dr.ghs2(k_var,:) = dr.ghs2;
+
 end
 
-function [resid,dr] = risky_residuals(ys,M,dr,options,oo)
-    persistent old_ys old_resid
+function [resid,dr] = risky_residuals(ys,pm,M,dr,options,oo)
     
     lead_lag_incidence = M.lead_lag_incidence;
     iyv = lead_lag_incidence';
@@ -165,7 +153,7 @@ function [resid,dr] = risky_residuals(ys,M,dr,options,oo)
     z = repmat(ys,1,3);
     z = z(iyr0) ;
     [resid1,d1,d2] = feval([M.fname '_dynamic'],z,...
-                                     [oo.exo_simul ...
+                           [oo.exo_simul ...
                         oo.exo_det_simul], M.params, dr.ys, 2);
     if ~isreal(d1) || ~isreal(d2)
         pause
@@ -174,150 +162,53 @@ function [resid,dr] = risky_residuals(ys,M,dr,options,oo)
     if options.use_dll
         % In USE_DLL mode, the hessian is in the 3-column sparse representation
         d2 = sparse(d2(:,1), d2(:,2), d2(:,3), ...
-                         size(d1, 1), size(d1, 2)*size(d1, 2));
+                    size(d1, 1), size(d1, 2)*size(d1, 2));
     end
 
     if isfield(options,'portfolio') && options.portfolio == 1
-        eq_tags = M.equations_tags;
-        n_tags = size(eq_tags,1);
-        portfolios_eq = cell2mat(eq_tags(strcmp(eq_tags(:,2), ...
-                                                'portfolio'),1));
-        eq = setdiff(1:M.endo_nbr,portfolios_eq);
-        l_var = zeros(n_tags,1);
-        for i=1:n_tags
-            l_var(i) = find(strncmp(eq_tags(i,3),M.endo_names, ...
-                                    length(cell2mat(eq_tags(i,3)))));
-        end
-        k_var = setdiff(1:M.endo_nbr,l_var);
-        lli1 = lead_lag_incidence(:,k_var);
-        lead_incidence = lli1(3,:)';
-        k = find(lli1');
-        lli2 = lli1';
-        lli2(k) = 1:nnz(lli1);
-        lead_lag_incidence = lli2';
-        x = ys(l_var);
-        dr = first_step_ds(x,M,dr,options,oo);
-
-        
-        M.lead_lag_incidence = lead_lag_incidence;
-        lli1a = [nonzeros(lli1'); size(d1,2)+(-M.exo_nbr+1:0)'];
-        d1a = d1(eq,lli1a);
-        ih = 1:size(d2,2);
-        ih = reshape(ih,size(d1,2),size(d1,2));
-        ih1 = ih(lli1a,lli1a);
-        d2a = d2(eq,ih1);
-        
-        M.endo_nbr = M.endo_nbr-n_tags;
-        dr = set_state_space(dr,M,options);
-    
-        [junk,dr.i_fwrd_g] = find(lead_lag_incidence(3,dr.order_var));
-        i_fwrd_f = nonzeros(lead_incidence(dr.order_var));
-        i_fwrd2_f = ih(i_fwrd_f,i_fwrd_f);
-        dr.i_fwrd_f = i_fwrd_f;
-        dr.i_fwrd2_f = i_fwrd2_f;
-        nd = nnz(lead_lag_incidence) + M.exo_nbr;
-        dr.nd = nd;
-        kk = reshape(1:nd^2,nd,nd);
-        kkk = reshape(1:nd^3,nd^2,nd);
-        dr.i_fwrd2a_f = kk(i_fwrd_f,:);
-        %        dr.i_fwrd3_f = kkk(i_fwrd2_f,:);
-        dr.i_uu = kk(end-M.exo_nbr+1:end,end-M.exo_nbr+1:end);
+        pm = portfolio_model_structure(M,options);
+        x = ys(pm.v_p);
+        dr = first_step_ds(x,pm,M,dr,options,oo);
+        dr.ys = ys;
     else
         d1a = d1;
         d2a = d2;
     end
     
-% $$$     [junk,cols_b,cols_j] = find(lead_lag_incidence(2,dr.order_var));
-% $$$     b = zeros(M.endo_nbr,M.endo_nbr);
-% $$$     b(:,cols_b) = d1a(:,cols_j);
-% $$$ 
-% $$$     [dr,info] = dyn_first_order_solver(d1a,b,M,dr,options,0);
-% $$$     if info
-% $$$         [m1,m2]=max(abs(ys-old_ys));
-% $$$         disp([m1 m2])
-% $$$         %        print_info(info,options.noprint);
-% $$$         resid = old_resid+info(2)/40;
-% $$$         return
-% $$$     end
-% $$$     
-% $$$     dr = dyn_second_order_solver(d1a,d2a,dr,M);
-    
-    gu1 = dr.ghu(dr.i_fwrd_g,:);
-
-    resid = resid1+0.5*(d1(:,dr.i_fwrd_f)*dr.ghuu(dr.i_fwrd_g,:)+ ...
-                        d2(:,dr.i_fwrd2_f)*kron(gu1,gu1))*vec(M.Sigma_e);
-    disp(d1(:,dr.i_fwrd_f)*dr.ghuu(dr.i_fwrd_g,:)*vec(M.Sigma_e));
-    old_ys = ys;
-    disp(max(abs(resid)))
-    old_resid = resid;
+    gu1 = dr.ghu(pm.i_fwrd_g,:);
+
+    resid = resid1+0.5*(d1(:,pm.i_fwrd_f1)*dr.ghuu(pm.i_fwrd_g,:)+ ...
+                        d2(:,pm.i_fwrd_f2)*kron(gu1,gu1))*vec(M.Sigma_e);
 end
 
-function [resid,dr] = risky_residuals_ds(x,M,dr,options,oo)
-    persistent old_ys old_resid old_resid1 old_d1 old_d2
+function [resid,dr] = risky_residuals_ds(x,pm,M,dr,options,oo)
+    
+    v_p = pm.v_p;
+    v_np = pm.v_np;
+    
+    % computing steady state of non-portfolio variables  consistent with
+    % assumed portfolio 
+    dr.ys(v_p) = x;
+    ys0 = dr.ys(v_np);
+    f_h =str2func([M.fname '_static']);
+    [dr.ys(v_np),info] = csolve(@ds_static_model,ys0,[],1e-10,100,f_h,x,pm.eq_np,v_np,v_p, ...
+                                M.endo_nbr,M.exo_nbr,M.params);
+    if info
+        error('can''t compute non-portfolio steady state')
+    end
     
-    dr = first_step_ds(x,M,dr,options,oo);
+    dr_np = first_step_ds(x,pm,M,dr,options,oo);
 
     lead_lag_incidence = M.lead_lag_incidence;
     iyv = lead_lag_incidence';
     iyv = iyv(:);
     iyr0 = find(iyv) ;
     
-    if M.exo_nbr == 0
-        oo.exo_steady_state = [] ;
-    end
-    
-    eq_tags = M.equations_tags;
-    n_tags = size(eq_tags,1);
-    portfolios_eq = cell2mat(eq_tags(strcmp(eq_tags(:,2), ...
-                                            'portfolio'),1));
-    eq = setdiff(1:M.endo_nbr,portfolios_eq);
-    l_var = zeros(n_tags,1);
-    for i=1:n_tags
-        l_var(i) = find(strncmp(eq_tags(i,3),M.endo_names, ...
-                                length(cell2mat(eq_tags(i,3)))));
-    end
-    k_var = setdiff(1:M.endo_nbr,l_var);
-    lli1 = lead_lag_incidence(:,k_var);
-    k = find(lli1');
-    lli2 = lli1';
-    lli2(k) = 1:nnz(lli1);
-    lead_lag_incidence = lli2';
-
-    ys = dr.ys;
-    ys(l_var) = x;
-    
-    z = repmat(ys,1,3);
+    z = repmat(dr.ys,1,3);
     z = z(iyr0) ;
     [resid1,d1,d2] = feval([M.fname '_dynamic'],z,...
-                                     [oo.exo_simul ...
+                           [oo.exo_simul ...
                         oo.exo_det_simul], M.params, dr.ys, 2);
-% $$$     if isempty(old_resid)
-% $$$         old_resid1 = resid1;
-% $$$         old_d1 = d1;
-% $$$         old_d2 = d2;
-% $$$         old_ys = ys;
-% $$$     else
-% $$$         if ~isequal(resid1,old_resid)
-% $$$             disp('ys')
-% $$$             disp((ys-old_ys)');
-% $$$             disp('resids1')
-% $$$             disp((resid1-old_resid1)')
-% $$$             old_resid1 = resid1;
-% $$$             pause
-% $$$         end
-% $$$         if ~isequal(d1,old_d1)
-% $$$             disp('d1')
-% $$$             disp(d1-old_d1);
-% $$$             old_d1 = d1;
-% $$$             pause
-% $$$         end
-% $$$         if ~isequal(d2,old_d2)
-% $$$             disp('d2')
-% $$$             disp(d2-old_d2);
-% $$$             old_d2 = d2;
-% $$$             pause
-% $$$         end
-% $$$     end
     if ~isreal(d1) || ~isreal(d2)
         pause
     end
@@ -325,95 +216,31 @@ function [resid,dr] = risky_residuals_ds(x,M,dr,options,oo)
     if options.use_dll
         % In USE_DLL mode, the hessian is in the 3-column sparse representation
         d2 = sparse(d2(:,1), d2(:,2), d2(:,3), ...
-                         size(d1, 1), size(d1, 2)*size(d1, 2));
+                    size(d1, 1), size(d1, 2)*size(d1, 2));
     end
 
-% $$$     if isfield(options,'portfolio') && options.portfolio == 1
-% $$$         lli1a = [nonzeros(lli1'); size(d1,2)+(-M.exo_nbr+1:0)'];
-% $$$         d1a = d1(eq,lli1a);
-% $$$         ih = 1:size(d2,2);
-% $$$         ih = reshape(ih,size(d1,2),size(d1,2));
-% $$$         ih1 = ih(lli1a,lli1a);
-% $$$         d2a = d2(eq,ih1);
-% $$$         
-% $$$         M.endo_nbr = M.endo_nbr-n_tags;
-% $$$         dr = set_state_space(dr,M);
-% $$$     
-% $$$         dr.i_fwrd_g = find(lead_lag_incidence(3,dr.order_var)');
-% $$$     else
-% $$$         d1a = d1;
-% $$$         d2a = d2;
-% $$$     end
-% $$$     
-% $$$     [junk,cols_b,cols_j] = find(lead_lag_incidence(2,dr.order_var));
-% $$$     b = zeros(M.endo_nbr,M.endo_nbr);
-% $$$     b(:,cols_b) = d1a(:,cols_j);
-% $$$ 
-% $$$     [dr,info] = dyn_first_order_solver(d1a,b,M,dr,options,0);
-% $$$     if info
-% $$$         [m1,m2]=max(abs(ys-old_ys));
-% $$$         disp([m1 m2])
-% $$$         %        print_info(info,options.noprint);
-% $$$         resid = old_resid+info(2)/40;
-% $$$         return
-% $$$     end
-% $$$     
-% $$$     dr = dyn_second_order_solver(d1a,d2a,dr,M);
     
-    gu1 = dr.ghu(dr.i_fwrd_g,:);
-
-    %    resid = resid1+0.5*(d1(:,dr.i_fwrd_f)*dr.ghuu(dr.i_fwrd_g,:)+ ...
-    %                    d2(:,dr.i_fwrd2_f)*kron(gu1,gu1))*vec(M.Sigma_e);
-    resid = resid1+0.5*(d2(:,dr.i_fwrd2_f)*kron(gu1,gu1))*vec(M.Sigma_e);
-
-% $$$     if isempty(old_resid)
-% $$$         old_resid = resid;
-% $$$     else
-% $$$         disp('resid')
-% $$$         dr = (resid-old_resid)';
-% $$$         %        disp(dr)
-% $$$         %        disp(dr(portfolios_eq))
-% $$$         old_resid = resid;
-% $$$     end
-    resid = resid(portfolios_eq)
+    gu1 = dr_np.ghu(pm.i_fwrd_g,:);
+
+    resid = resid1+0.5*(d2(:,pm.i_fwrd_f2)*kron(gu1,gu1))*vec(M.Sigma_e);
+
+    resid = resid(pm.eq_p)
 end
 
-function [dr] = first_step_ds(x,M,dr,options,oo)
-    
+function dr_np = first_step_ds(x,pm,M,dr,options,oo)
+
     lead_lag_incidence = M.lead_lag_incidence;
     iyv = lead_lag_incidence';
     iyv = iyv(:);
     iyr0 = find(iyv) ;
-    
-    if M.exo_nbr == 0
-        oo.exo_steady_state = [] ;
-    end
-    
-    eq_tags = M.equations_tags;
-    n_tags = size(eq_tags,1);
-    portfolios_eq = cell2mat(eq_tags(strcmp(eq_tags(:,2), ...
-                                            'portfolio'),1));
-    eq = setdiff(1:M.endo_nbr,portfolios_eq);
-    l_var = zeros(n_tags,1);
-    for i=1:n_tags
-        l_var(i) = find(strncmp(eq_tags(i,3),M.endo_names, ...
-                                length(cell2mat(eq_tags(i,3)))));
-    end
-    k_var = setdiff(1:M.endo_nbr,l_var);
-    lli1 = lead_lag_incidence(:,k_var);
-    k = find(lli1');
-    lli2 = lli1';
-    lli2(k) = 1:nnz(lli1);
-    lead_lag_incidence = lli2';
-    M.lead_lag_incidence = lead_lag_incidence;
 
     ys = dr.ys;
-    ys(l_var) = x;
+    ys(pm.v_p) = x;
     
     z = repmat(ys,1,3);
     z = z(iyr0) ;
     [resid1,d1,d2] = feval([M.fname '_dynamic'],z,...
-                                     [oo.exo_simul ...
+                           [oo.exo_simul ...
                         oo.exo_det_simul], M.params, dr.ys, 2);
     if ~isreal(d1) || ~isreal(d2)
         pause
@@ -422,102 +249,139 @@ function [dr] = first_step_ds(x,M,dr,options,oo)
     if options.use_dll
         % In USE_DLL mode, the hessian is in the 3-column sparse representation
         d2 = sparse(d2(:,1), d2(:,2), d2(:,3), ...
-                         size(d1, 1), size(d1, 2)*size(d1, 2));
+                    size(d1, 1), size(d1, 2)*size(d1, 2));
     end
 
-    if isfield(options,'portfolio') && options.portfolio == 1
-        lli1a = [nonzeros(lli1'); size(d1,2)+(-M.exo_nbr+1:0)'];
-        d1a = d1(eq,lli1a);
-        ih = 1:size(d2,2);
-        ih = reshape(ih,size(d1,2),size(d1,2));
-        ih1 = ih(lli1a,lli1a);
-        d2a = d2(eq,ih1);
-        
-        M.endo_nbr = M.endo_nbr-n_tags;
-        dr = set_state_space(dr,M,options);
+    d1_np = d1(pm.eq_np,pm.i_d1_np);
+    d2_np = d2(pm.eq_np,pm.i_d2_np);
     
-        dr.i_fwrd_g = find(lead_lag_incidence(3,dr.order_var)');
-    else
-        d1a = d1;
-        d2a = d2;
-    end
-    
-    [junk,cols_b,cols_j] = find(lead_lag_incidence(2,dr.order_var));
-    b = zeros(M.endo_nbr,M.endo_nbr);
-    b(:,cols_b) = d1a(:,cols_j);
-
-    [dr,info] = dyn_first_order_solver(d1a,M,dr,options,0);
+    [dr_np,info] = dyn_first_order_solver(d1_np,pm.M_np,pm.dr_np,options,0);
     if info
-        [m1,m2]=max(abs(ys-old_ys));
-        disp([m1 m2])
-        %        print_info(info,options.noprint);
-        resid = old_resid+info(2)/40;
+        print_info(info,0);
         return
     end
     
-    dr = dyn_second_order_solver(d1a,d2a,dr,M,...
-                                 options.threads.kronecker.A_times_B_kronecker_C,...
-                                 options.threads.kronecker.sparse_hessian_times_B_kronecker_C);
+    dr_np = dyn_second_order_solver(d1_np,d2_np,dr_np,pm.M_np,...
+                                    options.threads.kronecker.A_times_B_kronecker_C,...
+                                    options.threads.kronecker.sparse_hessian_times_B_kronecker_C);
 end
 
-function [resid,dr] = risky_residuals_k_order(ys,M,dr,options,oo)
-    
-    lead_lag_incidence = M.lead_lag_incidence;
-    npred = dr.npred;
+function [resid,dr] = risky_residuals_k_order(ys,pm,M,dr,options,oo)
     exo_nbr = M.exo_nbr;
-    vSigma_e = vec(M.Sigma_e);
+    endo_nbr = M.endo_nbr;
     
-    iyv = lead_lag_incidence';
+    iyv = M.lead_lag_incidence';
     iyv = iyv(:);
     iyr0 = find(iyv) ;
     
-    if M.exo_nbr == 0
+    if exo_nbr == 0
         oo.exo_steady_state = [] ;
     end
     
     z = repmat(ys,1,3);
     z = z(iyr0) ;
-    [resid1,d1,d2,d3] = feval([M.fname '_dynamic'],z,...
-                                     [oo.exo_simul ...
+    [resid1,d1,d2] = feval([M.fname '_dynamic'],z,...
+                              [oo.exo_simul ...
                         oo.exo_det_simul], M.params, dr.ys, 2);
-
-% $$$     hessian = sparse(d2(:,1), d2(:,2), d2(:,3), ...
-% $$$                      size(d1, 1), size(d1, 2)*size(d1, 2));
-% $$$     fy3 = sparse(d2(:,1), d2(:,2), d2(:,3), ...
-% $$$                      size(d1, 1), size(d1, 2)^3);
-
-    options.order = 3;
     
-    nu2 = exo_nbr*(exo_nbr+1)/2;
-% $$$     d1_0 = d1;
-% $$$     gu1 = dr.ghu(dr.i_fwrd_g,:);
-% $$$     guu = dr.ghuu;
-% $$$     for i=1:2
-% $$$         d1 = d1_0 + 0.5*(hessian(:,dr.i_fwrd2a_f)*kron(eye(dr.nd),guu(dr.i_fwrd_g,:)*vSigma_e)+ ...
-% $$$                        fy3(:,dr.i_fwrd3_f)*kron(eye(dr.nd),kron(gu1,gu1)*vSigma_e));
-% $$$     [junk,cols_b,cols_j] = find(lead_lag_incidence(2,dr.order_var));
-% $$$     b = zeros(M.endo_nbr,M.endo_nbr);
-% $$$     b(:,cols_b) = d1(:,cols_j);
-
-% $$$     [dr,info] = dyn_first_order_solver(d1,b,M,dr,options,0);
-        [err,g_0, g_1, g_2, g_3] = k_order_perturbation(dr,M,options);
-        mexErrCheck('k_order_perturbation', err);
-        gu1 = g_1(dr.i_fwrd_g,end-exo_nbr+1:end);
-        guu = unfold(g_2(:,end-nu2+1:end),exo_nbr);
-        d1old = d1;
-        %        disp(max(max(abs(d1-d1old))));
-        %    end
+    if isfield(options,'portfolio') && options.portfolio == 1
+        eq_np = pm.eq_np;
+        
+        d1_np = d1(eq_np,pm.i_d1_np);
+        d2_np = d2(eq_np,pm.i_d2_np);
+
+        M_np = pm.M_np;
+        dr_np = pm.dr_np;
+        
+        [dr_np,info] = dyn_first_order_solver(d1_np,pm.M_np,pm.dr_np,options,0);
+        if info
+            print_info(info,0);
+            return
+        end
+        
+        dr_np = dyn_second_order_solver(d1_np,d2_np,dr_np,pm.M_np,...
+                                        options.threads.kronecker.A_times_B_kronecker_C,...
+                                        options.threads.kronecker.sparse_hessian_times_B_kronecker_C);
+    end
     
-    [junk,cols_b,cols_j] = find(lead_lag_incidence(2,dr.order_var));
+    i_fwrd_f1 = pm.i_fwrd_f1;
+    i_fwrd_f2 = pm.i_fwrd_f2;
+    i_fwrd_f3 = pm.i_fwrd_f3;
+    i_fwrd_g = pm.i_fwrd_g;
+    gu1 = dr_np.ghu(i_fwrd_g,:);
+    ghuu = dr_np.ghuu;
     
-    resid = resid1+0.5*(d1(:,dr.i_fwrd_f)*guu(dr.i_fwrd_g,:)+hessian(:,dr.i_fwrd2_f)*kron(gu1,gu1))*vec(M.Sigma_e);
+    resid = resid1+0.5*(d1(:,i_fwrd_f1)*ghuu(i_fwrd_g,:)+d2(:,i_fwrd_f2)* ...
+                        kron(gu1,gu1))*vec(M.Sigma_e);
 
     if nargout > 1
-        [dr,info] = k_order_pert(dr,M,options,oo);
+        [resid1,d1,d2,d3] = feval([M.fname '_dynamic'],z,...
+                                  [oo.exo_simul ...
+                            oo.exo_det_simul], M.params, dr.ys, 2);
+
+        
+        [a,b,c] = find(d2(eq_np,pm.i_d2_np));
+        d2_np = [a b c];
+        
+        [a,b,c] = find(d3(eq_np,pm.i_d3_np));
+        d3_np = [a b c];
+         
+        options.order = 3;
+        % space holder, unused by k_order_pertrubation
+        dr_np.ys = dr.ys(pm.v_np);
+        nu2 = exo_nbr*(exo_nbr+1)/2;
+        nu3 = exo_nbr*(exo_nbr+1)*(exo_nbr+2)/3;
+        M_np.NZZDerivatives = [nnz(d1_np); nnz(d2_np); nnz(d3_np)];
+        [err,g_0, g_1, g_2, g_3] = k_order_perturbation(dr_np,M_np,options,d1_np,d2_np,d3_np);
+        mexErrCheck('k_order_perturbation', err);
+
+        gu1 = g_1(i_fwrd_g,end-exo_nbr+1:end);
+        ghuu = unfold2(g_2(:,end-nu2+1:end),exo_nbr);
+        ghsuu = get_ghsuu(g_3,size(g_1,2),exo_nbr);
+
+        i_fwrd1_f2 = pm.i_fwrd1_f2;
+        i_fwrd1_f3 = pm.i_fwrd1_f3;
+        n = size(d1,2);
+        d1b = d1 + 0.5*( ...
+            d1(:,i_fwrd_f1)*...
+            d2(:,i_fwrd1_f2)*kron(eye(n),dr_np.ghuu(i_fwrd_g,:)*vec(M.Sigma_e))...
+            + 0.5*d3(:,i_fwrd1_f3)*kron(eye(n),kron(gu1,gu1)*vec(M.Sigma_e)));
+        format short
+        kk1 = [nonzeros(M.lead_lag_incidence(:,1:6)'); ...
+               nnz(M.lead_lag_incidence)+[1; 2]]
+        kk2 = [nonzeros(M.lead_lag_incidence(:,1:6)'); ...
+               nnz(M.lead_lag_incidence)+[3; 4]]
+        format short
+        gu1
+        kron(gu1,gu1)*vec(M.Sigma_e)
+        disp(d1(:,:))
+        disp(d1b(:,:))
+        aa2=d2(:,i_fwrd1_f2)*kron(eye(n),dr_np.ghuu(i_fwrd_g,:)*vec(M.Sigma_e));
+        aa3=d3(:,i_fwrd1_f3)*kron(eye(n),kron(gu1,gu1)*vec(M.Sigma_e));
+        disp(d3(4,7+6*n+6*n*n))
+        disp(d3(4,8+16*n+17*n*n))   %8,17,18
+        disp(d3(4,8+17*n+16*n*n))   %8,17,18
+        disp(d3(4,7*n+17+17*n*n))   %8,17,18
+        disp(d3(4,7*n+18+16*n*n))   %8,17,18
+        disp(d3(4,7*n*n+16*n+18))   %8,17,18
+        disp(d3(4,7*n*n+17+17*n))   %8,17,18
+        pause
+        disp(aa2(:,kk1))
+        disp(aa2(:,kk2))
+        disp(aa3(:,kk1))
+        disp(aa3(:,kk2))
+        [dr,info] = dyn_first_order_solver(d1b,M,dr,options,0);
+        if info
+            print_info(info,0);
+            return
+        end
+        
+        disp_dr(dr,dr.order_var,[]);
+        
     end
 end
 
-function y=unfold(x,n)
+function y=unfold2(x,n)
     y = zeros(size(x,1),n*n);
     k = 1;
     for i=1:n
@@ -526,6 +390,122 @@ function y=unfold(x,n)
             if i ~= j
                 y(:,(j-1)*n+i) = x(:,k);
             end
+            k = k+1;
         end
     end
 end
+
+function y=unfold3(x,n)
+    y = zeros(size(x,1),n*n*n);
+    k = 1;
+    for i=1:n
+        for j=i:n
+            for m=j:n
+                y(:,(i-1)*n*n+(j-1)*n+m) = x(:,k);
+                y(:,(i-1)*n*n+(m-1)*n+j) = x(:,k);
+                y(:,(j-1)*n*n+(i-1)*n+m) = x(:,k);
+                y(:,(j-1)*n*n+(m-1)*n+i) = x(:,k);
+                y(:,(m-1)*n*n+(i-1)*n+j) = x(:,k);
+                y(:,(m-1)*n*n+(j-1)*n+i) = x(:,k);
+                
+                k = k+1;
+            end
+        end
+    end
+end
+
+function pm  = portfolio_model_structure(M,options)
+
+    i_d3_np = [];
+    i_d3_p = [];
+
+    lead_index = M.maximum_endo_lag+2;
+    lead_lag_incidence = M.lead_lag_incidence;
+    eq_tags = M.equations_tags;
+    n_tags = size(eq_tags,1);
+    eq_p = cell2mat(eq_tags(strcmp(eq_tags(:,2), ...
+                                   'portfolio'),1));
+    pm.eq_p = eq_p;
+    pm.eq_np = setdiff(1:M.endo_nbr,eq_p);
+    v_p = zeros(n_tags,1);
+    for i=1:n_tags
+        v_p(i) = find(strncmp(eq_tags(i,3),M.endo_names, ...
+                              length(cell2mat(eq_tags(i,3)))));
+    end
+    if any(lead_lag_incidence(lead_index,v_p))
+        error(['portfolio variables appear in the model as forward ' ...
+               'variable'])
+    end
+    pm.v_p = v_p;
+    v_np = setdiff(1:M.endo_nbr,v_p);
+    pm.v_np = v_np;
+    lli_np = lead_lag_incidence(:,v_np)';
+    k = find(lli_np);
+    lead_lag_incidence_np = lli_np;
+    lead_lag_incidence_np(k) = 1:nnz(lli_np);
+    lead_lag_incidence_np = lead_lag_incidence_np';
+    pm.lead_lag_incidence_np = lead_lag_incidence_np;
+    i_d1_np = [nonzeros(lli_np); nnz(lead_lag_incidence)+(1:M.exo_nbr)'];
+    pm.i_d1_np = i_d1_np;
+    
+    n = nnz(lead_lag_incidence)+M.exo_nbr;
+    ih = reshape(1:n*n,n,n);
+    i_d2_np = ih(i_d1_np,i_d1_np);
+    pm.i_d2_np = i_d2_np(:);
+
+    ih = reshape(1:n*n*n,n,n,n);
+    i_d3_np = ih(i_d1_np,i_d1_np,i_d1_np);
+    pm.i_d3_np = i_d3_np(:);
+
+    M_np = M;
+    M_np.lead_lag_incidence = lead_lag_incidence_np;
+    M_np.lead_lag_incidence = lead_lag_incidence_np;
+    M_np.endo_nbr = length(v_np);
+    M_np.endo_names = M.endo_names(v_np,:);
+    dr_np = struct();
+    dr_np = set_state_space(dr_np,M_np,options);
+    pm.dr_np = dr_np;
+    M_np.var_order_endo_names = M_np.endo_names(dr_np.order_var,:);
+    pm.M_np = M_np;
+    pm.i_fwrd_g = find(lead_lag_incidence_np(lead_index,dr_np.order_var)');    
+
+    i_fwrd_f1 = nonzeros(lead_lag_incidence(lead_index,:));
+    pm.i_fwrd_f1 = i_fwrd_f1;
+    n = nnz(lead_lag_incidence)+M.exo_nbr;
+    ih = reshape(1:n*n,n,n);
+    i_fwrd_f2 = ih(i_fwrd_f1,i_fwrd_f1);
+    pm.i_fwrd_f2 = i_fwrd_f2(:);
+    i_fwrd1_f2 = ih(i_fwrd_f1,:);
+    pm.i_fwrd1_f2 = i_fwrd1_f2(:);
+
+    ih = reshape(1:n*n*n,n,n,n);
+    i_fwrd_f3 = ih(i_fwrd_f1,i_fwrd_f1,i_fwrd_f1);
+    pm.i_fwrd_f3 = i_fwrd_f3(:);
+    i_fwrd1_f3 = ih(i_fwrd_f1,i_fwrd_f1,:);
+    pm.i_fwrd1_f3 = i_fwrd1_f3(:);
+end
+
+function r=ds_static_model(y0,f_h,p0,eq_np,v_np,v_p,endo_nbr,exo_nbr,params)
+    ys = zeros(endo_nbr,1);
+    ys(v_p) = p0;
+    ys(v_np) = y0;
+    r = f_h(ys,zeros(exo_nbr,1),params);
+    r = r(eq_np);
+end
+
+function ghsuu = get_ghsuu(g,ns,nx)
+    nxx = nx*(nx+1)/2;
+    m1 = 0;
+    m2 = ns*(ns+1)/2;
+    kk = 1:(nx*nx);
+    ghsuu = zeros(size(g,1),(ns*nx*nx));
+    
+    for i=1:n
+        j = m1+(1:m2);
+        k = j(end-nxx+1:end);
+        ghsuu(:,kk) = unfold2(g(:,k),nx);
+        m1 = m1+m2;
+        m2 = m2 - (n-i+1);
+        kk = kk + nx*nx;
+    end
+end
\ No newline at end of file
diff --git a/matlab/dynare_solve.m b/matlab/dynare_solve.m
index 72555ed859c44dc0d1741e6ef574244fd600a00a..da0e58921ada2e5c4372ffdfd133ca70cfff2669 100644
--- a/matlab/dynare_solve.m
+++ b/matlab/dynare_solve.m
@@ -74,7 +74,9 @@ if options_.solve_algo == 0
     end
 elseif options_.solve_algo == 1
     nn = size(x,1);
-    [x,info]=solve1(func,x,1:nn,1:nn,jacobian_flag,1,varargin{:});
+    [x,info]=solve1(func,x,1:nn,1:nn,jacobian_flag,1,options_.gstep, ...
+                    options_.solve_tolf,options_.solve_tolx, ...
+                    options_.solve_maxit,options_.debug,varargin{:});
 elseif options_.solve_algo == 2 || options_.solve_algo == 4
     nn = size(x,1) ;
     tolf = options_.solve_tolf ;
@@ -125,14 +127,19 @@ elseif options_.solve_algo == 2 || options_.solve_algo == 4
         if options_.debug
             disp(['DYNARE_SOLVE (solve_algo=2|4): solving block ' num2str(i) ', of size ' num2str(r(i+1)-r(i)) ]);
         end
-        [x,info]=solve1(func,x,j1(r(i):r(i+1)-1),j2(r(i):r(i+1)-1),jacobian_flag, bad_cond_flag, varargin{:});
+        [x,info]=solve1(func,x,j1(r(i):r(i+1)-1),j2(r(i):r(i+1)-1),jacobian_flag, ...
+                        bad_cond_flag, options_.gstep, ...
+                        options_.solve_tolf,options_.solve_tolx, ...
+                        options_.solve_maxit,options_.debug,varargin{:});
         if info
             return
         end
     end
     fvec = feval(func,x,varargin{:});
     if max(abs(fvec)) > tolf
-        [x,info]=solve1(func,x,1:nn,1:nn,jacobian_flag, bad_cond_flag, varargin{:});
+        [x,info]=solve1(func,x,1:nn,1:nn,jacobian_flag, bad_cond_flag, ...
+                        options_.gstep, options_.solve_tolf,options_.solve_tolx, ...
+                        options_.solve_maxit,options_.debug,varargin{:});
     end
 elseif options_.solve_algo == 3
     if jacobian_flag
diff --git a/matlab/sim1_purely_backward.m b/matlab/sim1_purely_backward.m
index b432c4789f2cfb9d970cbf31805c367299747752..beb699880d253338e69620dc824ddef18b6ddf81 100644
--- a/matlab/sim1_purely_backward.m
+++ b/matlab/sim1_purely_backward.m
@@ -34,7 +34,11 @@ function sim1_purely_backward()
         yb = oo_.endo_simul(:,it-1); % Values at previous period, also used as guess value for current period
         yb1 = yb(iyb);
        
-        tmp = solve1(model_dynamic, [yb1; yb], 1:M_.endo_nbr, nyb+1:nyb+M_.endo_nbr, 1, 1, oo_.exo_simul, M_.params, oo_.steady_state, it);
+        tmp = solve1(model_dynamic, [yb1; yb], 1:M_.endo_nbr, nyb+1:nyb+ ...
+                     M_.endo_nbr, 1, 1, options_.gstep, ...
+                     options_.solve_tolf,options_.solve_tolx, ...
+                     options_.solve_maxit,options_.debug,oo_.exo_simul, ...
+                     M_.params, oo_.steady_state, it);
         oo_.endo_simul(:,it) = tmp(nyb+1:nyb+M_.endo_nbr);
     end
     
\ No newline at end of file
diff --git a/matlab/sim1_purely_forward.m b/matlab/sim1_purely_forward.m
index 65a874b5055f035e8c0657712cb135bb994dd64c..795cb25e281c305f242ea56cf21ffc3467bc795c 100644
--- a/matlab/sim1_purely_forward.m
+++ b/matlab/sim1_purely_forward.m
@@ -33,7 +33,11 @@ function sim1_purely_forward()
         yf = oo_.endo_simul(:,it+1); % Values at next period, also used as guess value for current period
         yf1 = yf(iyf);
        
-        tmp = solve1(model_dynamic, [yf; yf1], 1:M_.endo_nbr, 1:M_.endo_nbr, 1, 1, oo_.exo_simul, M_.params, oo_.steady_state, it);
+        tmp = solve1(model_dynamic, [yf; yf1], 1:M_.endo_nbr, 1:M_.endo_nbr, ...
+                     1, 1, options_.gstep, options_.solve_tolf, ...
+                     options_.solve_tolx, options_.solve_maxit, ...
+                     options_.debug,oo_.exo_simul, M_.params, oo_.steady_state, ...
+                     it);
         oo_.endo_simul(:,it) = tmp(1:M_.endo_nbr);
     end
     
\ No newline at end of file
diff --git a/matlab/simul_backward_nonlinear_model.m b/matlab/simul_backward_nonlinear_model.m
index 546990cb5eb656cd581f22117b77937563febcfc..80dec87a2720502e510271791c5947db58c411a7 100644
--- a/matlab/simul_backward_nonlinear_model.m
+++ b/matlab/simul_backward_nonlinear_model.m
@@ -104,6 +104,10 @@ DynareOutput.endo_simul(:,1) = DynareOutput.steady_state;
 for it = 2:sample_size+1
     y(jdx) = DynareOutput.endo_simul(:,it-1); % A good guess for the initial conditions is the previous values for the endogenous variables.
     y(hdx) = y(jdx(iy1));                     % Set lagged variables.
-    y(jdx) = solve1(model_dynamic, y, idx, jdx, 1, 1, DynareOutput.exo_simul, DynareModel.params, DynareOutput.steady_state, it);
+    y(jdx) = solve1(model_dynamic, y, idx, jdx, 1, 1, DynareOptions.gstep, ...
+                    DynareOptions.solve_tolf,DynareOptions.solve_tolx, ...
+                    DynareOptions.solve_maxit,DynareOptions.debug, ...
+                    DynareOutput.exo_simul, DynareModel.params, ...
+                    DynareOutput.steady_state, it);
     DynareOutput.endo_simul(:,it) = y(jdx);
 end
\ No newline at end of file
diff --git a/matlab/solve1.m b/matlab/solve1.m
index 74111276f6468499888ea48c1f86f46792d75f02..97cb5e24e3fab5514f7c29f3010d5aa0c5f06bd7 100644
--- a/matlab/solve1.m
+++ b/matlab/solve1.m
@@ -1,4 +1,4 @@
-function [x,check] = solve1(func,x,j1,j2,jacobian_flag,bad_cond_flag,varargin)
+function [x,check] = solve1(func,x,j1,j2,jacobian_flag,bad_cond_flag,gstep,tolf,tolx,maxit,debug,varargin)
 % function [x,check] = solve1(func,x,j1,j2,jacobian_flag,bad_cond_flag,varargin)
 % Solves systems of non linear equations of several variables
 %
@@ -11,6 +11,12 @@ function [x,check] = solve1(func,x,j1,j2,jacobian_flag,bad_cond_flag,varargin)
 %    jacobian_flag=0: jacobian obtained numerically
 %    bad_cond_flag=1: when Jacobian is badly conditionned, use an
 %                     alternative formula to Newton step
+%    gstep            increment multiplier in numercial derivative
+%                     computation
+%    tolf             tolerance for residuals
+%    tolx             tolerance for solution variation
+%    maxit            maximum number of iterations
+%    debug            debug flag
 %    varargin:        list of arguments following bad_cond_flag
 %    
 % OUTPUTS
@@ -37,18 +43,13 @@ function [x,check] = solve1(func,x,j1,j2,jacobian_flag,bad_cond_flag,varargin)
 % You should have received a copy of the GNU General Public License
 % along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
 
-global M_ options_ fjac  
-
 nn = length(j1);
 fjac = zeros(nn,nn) ;
 g = zeros(nn,1) ;
 
-tolf = options_.solve_tolf ;
-tolx = options_.solve_tolx;
 tolmin = tolx ;
 
 stpmx = 100 ;
-maxit = options_.solve_maxit ;
 
 check = 0 ;
 
@@ -77,7 +78,7 @@ for its = 1:maxit
         fvec = fvec(j1);
         fjac = fjac(j1,j2);
     else
-        dh = max(abs(x(j2)),options_.gstep(1)*ones(nn,1))*eps^(1/3);
+        dh = max(abs(x(j2)),gstep(1)*ones(nn,1))*eps^(1/3);
         
         for j = 1:nn
             xdh = x ;
@@ -89,33 +90,10 @@ for its = 1:maxit
     end
 
     g = (fvec'*fjac)';
-    if options_.debug
+    if debug
         disp(['cond(fjac) ' num2str(cond(fjac))])
     end
-    M_.unit_root = 0;
-    if M_.unit_root
-        if first_time
-            first_time = 0;
-            [q,r,e]=qr(fjac);
-            n = sum(abs(diag(r)) < 1e-12);
-            fvec = q'*fvec;
-            p = e*[-r(1:end-n,1:end-n)\fvec(1:end-n);zeros(n,1)];
-            disp(' ')
-            disp('STEADY with unit roots:')
-            disp(' ')
-            if n > 0
-                disp(['   The following variable(s) kept their value given in INITVAL' ...
-                      ' or ENDVAL'])
-                disp(char(e(:,end-n+1:end)'*M_.endo_names))
-            else
-                disp('   STEADY can''t find any unit root!')
-            end
-        else
-            [q,r]=qr(fjac*e);
-            fvec = q'*fvec;
-            p = e*[-r(1:end-n,1:end-n)\fvec(1:end-n);zeros(n,1)];
-        end     
-    elseif bad_cond_flag && cond(fjac) > 1/sqrt(eps)
+    if bad_cond_flag && rcond(fjac) < sqrt(eps)
         fjac2=fjac'*fjac;
         p=-(fjac2+1e6*sqrt(nn*eps)*max(sum(abs(fjac2)))*eye(nn))\(fjac'*fvec);
     else
@@ -126,7 +104,7 @@ for its = 1:maxit
 
     [x,f,fvec,check]=lnsrch1(xold,fold,g,p,stpmax,func,j1,j2,varargin{:});
 
-    if options_.debug
+    if debug
         disp([its f])
         disp([xold x])
     end