function [Phi,Q,R,mu] = MLE_ST(Y,Phi,Q,R,mu,S)
%function [Phi,Q,R,mu] = MLE_ST(Y,Phi,Q,R,mu,S)
%Maximum likelihood estimator, modified for Signal + Trend model

% Initialization
n = size(Y,2);
A = [1 1 0 0];
[XF,PF] = KalmanFil(Y,A,Phi,Q,R,mu,S);
LL = loglik(Y,A,Phi,Q,R,mu,S,XF,PF);
difLL = Inf;
iter = 0;

% Iterations
while difLL>1e-3 && iter<100
    % Update estimators
    [X,P,X0,P0,PP] = KalmanSm(A,Phi,Q,R,mu,S,XF,PF);
    S11 = X*X' + sum(P,3);
    S10 = X*[X0 X(:,1:n-1)]' + sum(PP,3);
    S00 = [X0 X(:,1:n-1)]*[X0 X(:,1:n-1)]' + P0 + sum(P(:,:,1:n-1),3);
    Phi = S10*inv(S00);
    Phi = [Phi(1,1) 0 0 0; 0 -1 -1 -1; 0 1 0 0; 0 0 1 0];
    Q = (S11-S10*inv(S00)*S10')/n;
    Q = diag([Q(1,1) Q(2,2) 0 0]);
    R = (Y-A*X)*(Y-A*X)'/n + A*sum(P,3)*A'/n;
    mu = X0;
    % Update log-likelihood
    LL0 = LL;
    [XF,PF] = KalmanFil(Y,A,Phi,Q,R,mu,S);
    LL = loglik(Y,A,Phi,Q,R,mu,S,XF,PF);
    difLL = abs(LL0-LL)/abs(LL0);
    iter = iter + 1;
    disp(sprintf('%s %g','Iter: ',iter,', -2*log(L): ',LL,', Diff: ',difLL))
end