%% Scalar discrete-time LQ-optimal control on a finite horizon and with free final state

%%
% Based on Example 2.2-2 in 
% Lewis, Frank L., Draguna Vrabie, and Vassilis L. Syrmo. Optimal Control. 3rd ed. John Wiley & Sons, 2012, 
% freely available on the author's web page
% https://lewisgroup.uta.edu/FL%20books/Lewis%20optimal%20control%203rd%20edition%202012.pdf.
% The example is numbered as 2-2-3 in the previous editions.

%% Model of the system and the cost function
a=1.05;
b=0.01;
q=1000;
r=10;
x0=10;
sN=50;
N=350;

%% Solving the discrete-time Riccati equation backwards
% The definition of the solver of recurrent discrete-time Riccati equation
% is elsewhere (in another file).

[K,S]=solve_scalar_discrete_time_riccati(a,b,q,r,sN,x0,N);

%%
% The solver gives not only the solution sequence for the Riccati equation
% but it also provides the optimal state-feedback gains.

%% Plotting the solution of the Riccati equation as well as the corresponding gains and states

k=1:N;
figure(1)
subplot(1,2,1)
plot(k,S(1:end-1),'.-');
set(gca,'FontSize',12)
xlabel('k','FontSize',12)
ylabel('s_k','FontSize',12)

subplot(1,2,2)
plot(k,K,'.-')
set(gca,'FontSize',12)
xlabel('k','FontSize',12)
ylabel('K_k','FontSize',12)

%% Simulation of the closed-loop system with time-varying state-feedback gains

x(1)=x0;                % Mind that Matlab starts indexing from 1.
for k=1:N
    u(k)=-K(k)*x(k);
    x(k+1)=a*x(k)+b*u(k);
end

%% Plotting the simulation responses

figure(2)
k=0:N;
plot(k,x,'.-')
set(gca,'FontSize',12)
xlabel('k','FontSize',12)
ylabel('x_k','FontSize',12)

%print -depsc2 ../lectures/figures/scaopt_outputs.eps
%!epstopdf ../lectures/figures/scaopt_outputs.eps