diff --git a/matlab/optimization/penalty_hessian.m b/matlab/optimization/penalty_hessian.m
new file mode 100644
index 000000000..8da7bf6a9
--- /dev/null
+++ b/matlab/optimization/penalty_hessian.m
@@ -0,0 +1,94 @@
+function hessian_mat = penalty_hessian(func,x,penalty,gstep,varargin) % --*-- Unitary tests --*--
+
+% Computes second order partial derivatives with penalty_objective_function
+%
+% INPUTS
+%    func        [string]   name of the function
+%    x           [double]   vector, the Hessian of "func" is evaluated at    x.
+%    penalty     [double]   penalty base used if function fails
+%    gstep       [double]   scalar, size of epsilon.
+%    varargin    [void]     list of additional arguments for "func".
+%
+% OUTPUTS
+%    hessian_mat [double]   Hessian matrix
+%
+% ALGORITHM
+%    Uses Abramowitz and Stegun (1965) formulas 25.3.23 
+% \[
+%     \frac{\partial^2 f_{0,0}}{\partial {x^2}} = \frac{1}{h^2}\left( f_{1,0} - 2f_{0,0} + f_{ - 1,0} \right)
+% \]
+% and 25.3.27 p. 884
+% 
+% \[
+%     \frac{\partial ^2f_{0,0}}{\partial x\partial y} = \frac{-1}{2h^2}\left(f_{1,0} + f_{-1,0} + f_{0,1} + f_{0,-1} - 2f_{0,0} - f_{1,1} - f_{-1,-1} \right)
+% \]
+%
+% SPECIAL REQUIREMENTS
+%    none
+%  
+
+% Copyright (C) 2001-2014 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 ~isa(func, 'function_handle') 
+    func = str2func(func);
+end
+n=size(x,1);
+h1=max(abs(x),sqrt(gstep(1))*ones(n,1))*eps^(1/6)*gstep(2);
+h_1=h1;
+xh1=x+h1;
+h1=xh1-x;
+xh1=x-h_1;
+h_1=x-xh1;
+xh1=x;
+f0=penalty_objective_function(x,func,penalty,varargin{:});
+f1=zeros(size(f0,1),n);
+f_1=f1;
+for i=1:n
+    %do step up
+    xh1(i)=x(i)+h1(i);
+    f1(:,i)=penalty_objective_function(xh1,func,penalty,varargin{:});
+    %do step up
+    xh1(i)=x(i)-h_1(i);
+    f_1(:,i)=penalty_objective_function(xh1,func,penalty,varargin{:});
+    xh1(i)=x(i);%reset parameter
+end
+xh_1=xh1;
+hessian_mat = zeros(size(f0,1),n*n);
+temp=f1+f_1-f0*ones(1,n); %term f_(1,0)+f_(-1,0)-f_(0,0) used later
+for i=1:n    
+    if i > 1  %fill symmetric part of Hessian based on previously computed results      
+        k=[i:n:n*(i-1)];
+        hessian_mat(:,(i-1)*n+1:(i-1)*n+i-1)=hessian_mat(:,k);
+    end     
+    hessian_mat(:,(i-1)*n+i)=(f1(:,i)+f_1(:,i)-2*f0)./(h1(i)*h_1(i)); %formula 25.3.23
+    for j=i+1:n        
+        %step in up direction
+        xh1(i)=x(i)+h1(i);
+        xh1(j)=x(j)+h_1(j);
+        %step in down direction
+        xh_1(i)=x(i)-h1(i);
+        xh_1(j)=x(j)-h_1(j);
+        hessian_mat(:,(i-1)*n+j)=-(-penalty_objective_function(xh1,func,penalty,varargin{:})-penalty_objective_function(xh_1,func,penalty,varargin{:})+temp(:,i)+temp(:,j))./(2*h1(i)*h_1(j)); %formula 25.3.27
+        %reset grid points
+        xh1(i)=x(i);
+        xh1(j)=x(j);
+        xh_1(i)=x(i);
+        xh_1(j)=x(j);
+    end    
+end
+
diff --git a/matlab/optimization/penalty_objective_function.m b/matlab/optimization/penalty_objective_function.m
new file mode 100644
index 000000000..f35e56686
--- /dev/null
+++ b/matlab/optimization/penalty_objective_function.m
@@ -0,0 +1,7 @@
+function [fval,exit_flag,arg1,arg2] = penalty_objective_function(x0,fcn,penalty,varargin)
+    [fval,info,exit_flag,arg1,arg2] = fcn(x0,varargin{:});
+    
+    if info(1) ~= 0
+        fval = penalty + info(2);
+    end
+end
\ No newline at end of file