% finding the optimal cost for driving a system of the form:
% X(k+1) = AX(k) + Bu(k)
% from X(0) to the origin while minimizing a quadratic performance of the
% form:
% J = 1/2*sum(X(k)'QX(k) + Ru(k)^2)
% Ref.: Optimal Control by F. Leuis & V. Syrmous, 1995

% set the problem parameters

A = [1 -1; 1 2];
B = [0; 1];
R = 1;
X_0 = [5;5];
r_N = [0;0];

G0_N = 0;
J = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% obtain the controllability grammian

for i = 0:10
    G0_N = G0_N+A^(10-i)*B*R^-1*B'*A'^(10-i);
end

G0_N_inv = inv(G0_N);

% find the optimal control

for i=1:11
    u_opt{i} = R^-1*B'*A'^(10-i)*G0_N_inv*(r_N-A^11*X_0);
end

% compute the optimal cost

for i = 1:11
    J = J+u_opt{i}*u_opt{i};
end

optimal_cost = J