function optimal1(N)

% find the control law u{i}=-K{i}*X{i}+KV{i}*V{i+1}
% for the system of the form:
% X(k+1) = AX(k) + Bu(k)
% such that y(k) = X(k,1) + X(k,2) tracks r(k) with an LQ tracking function
% where:
% r(k)=0.9^(k)+tan(k)*(k)
% 
% Ref.: Optimal Control by F. Leuis & V. Syrmous, 1995

% set the problem parameters

A=[0 1;0 0 ];
B=[0;1];
C=[1 1];
R=1;
Q=10;

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

S{N}=[0 0;0 0];
V{N}=[0;0];

for i=1:N-1
    
    r(N-i)=0.9^(N-i)+tan(N-i)*(N-i);
    
    K{N-i}=inv([B'*S{N-i+1}*B+R])*B'*S{N-i+1}*A;
    S{N-i}=A'*S{N-i+1}*(A-B*K{N-i})+C'*Q*C;
    V{N-i}=(A-B*K{N-i})'*V{N-i+1}+C'*Q*r(N-i);
    KV{N-i}=inv([B'*S{N-i+1}*B+R])*B';
        
end

X{1}=[0;0];

for i=1:N-1
    
    u{i}=-K{i}*X{i}+KV{i}*V{i+1};
    X{i+1}=A*X{i}+B*u{i};
    y(i)=X{i}(1)+X{i}(2);
    
end

t_domain=[1:1:N-1];
plot(t_domain,y,'r',t_domain,r,'b')
title('true signal (blue) and tracking signal (red) behaviour')

steady_state_gain_K=K{N-2}
steady_state_gain_KV=KV{N-2}