function [thetahat,fval,g,H] = filterfnc_ar2_H(datmat)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% filterfnc_ar2_H.m
%
% This function reads in a text file containing Tx1 time series
%
% After reading in the text file, the function runs MLE to estimate the
% model parameters.
%
% Optimization done with fmincon (interior-point). 
%
% Output is thetahat which is the vector of parameter estimates:
%
%       thetahat = [mu rho1 rho2 sigw]
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Read in text file
Y=datmat(:,1);

%parameter bounds
lb=[0.0 -0.99 -0.99 1e-3];
ub=[20  2.000 0.999 99.00];

%inital parameter vector
theta0=lb+0.5*(ub-lb);

%Minimize negative log likelihood 
objfnc=@(theta) MLEfnc(Y,theta);
fminconoptions=optimoptions('fmincon','ConstraintTolerance',1e-6,'StepTolerance',1e-6,'OptimalityTolerance',1e-6);
[thetahat,fval,~,~,~,g,H]=fmincon(objfnc,theta0,[],[],[],[],lb,ub,[],fminconoptions);

end

function NLL = MLEfnc(Y,param)
T=size(Y,1);

mu=param(1); 
rho1=param(2); 
rho2=param(3); 
sigw=param(4); 

%impose stationarity
if rho2-rho1>=1 || rho1+rho2>=1 || abs(rho2)>=1
    NLL=Inf;
    return;
end


M = mu*(1-rho1-rho2);

A = [rho1 rho2; 1 0];
B = [sigw; 0];
C = [1 0]; 
D = 0;

%Setting initial state and variance 
S0=mu*ones(2,1); 
P0= eye(2);
    
%Initializing two terms used to compute relevant summations for likelihood
term1=0; term2=0; 
for t=1:T  
    %Predict
    if t==1
        Sttm1 = M + A*S0;
        Pttm1 = A*P0*A' + B*B';
    else 
        Sttm1 = M + A*Stm1tm1;
        Pttm1 = A*Ptm1tm1*A' + B*B';
    end 
     %Update
     H = C*Pttm1*C' + D*D';
     K = (Pttm1*C')/H;
     Stt = Sttm1 + K*(Y(t)-C*Sttm1);
     Ptt = (eye(2)-K*C)*Pttm1;
     
     %Computing terms for likelihood
     term1 = log(det(H)) + term1;
     term2 = (Y(t)-C*Sttm1)'/H*(Y(t)-C*Sttm1) + term2;
     
     %Current update becomes previous period update as we move
     %forward in time
     Stm1tm1=Stt;
     Ptm1tm1=Ptt;
end %t 

LL = - 0.5*T*log(2*pi) - 0.5*term1 - 0.5*term2; 
NLL = - LL; 

end