%% Comparing the optimal and suboptimal discrete-time LQ-optimal control on a finite horizon

%%
% Based on Example 2.4-1 from Lewis, Frank L., Draguna Vrabie, and Vassilis L. Syrmo. Optimal Control. 3rd ed. John Wiley & Sons, 2012, 
% downloadable from https://lewisgroup.uta.edu/FL%20books/Lewis%20optimal%20control%203rd%20edition%202012.pdf.

%% Defining the model
a=1.05;b=0.01;q=5;r=5;x0=10;sN=5;N=200;

%% Suboptimal solution of Riccati equation for a constant state feedback
Kinf = 9.808;
S_subopt = zeros(1,N+1)
S_subopt(N+1)=sN;
for k=N:-1:1
    S_subopt(k)=q + Kinf^2*r + (a-b*Kinf)^2*S_subopt(k+1);
end

%% Optimal solution to the Riccati equation, including steady state
sinfty = roots([b^2 (r-a^2*r-b^2*q) -q*r]), sinfty = sinfty(1);
kinfty = a*b*sinfty/(r+b^2*sinfty)
aclosed = a-b*kinfty

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

x(1)=x0;
for k=1:N
    u(k)=-K(k)*x(k);
    x(k+1)=a*x(k)+b*u(k);
end

%% Plotting the solutions
k=1:N;
subplot(1,3,1)
plot(k,S(1:end-1),'.-',k,repmat(sinfty,1,length(k)),'.-',k,S_subopt(1:end-1),'.-');
set(gca,'FontSize',12)
%legend('Optimal','Steady-state','Constant-gain suboptimal')
xlabel('k','FontSize',12)
ylabel('s_k','FontSize',12)

subplot(1,3,2)
plot(k,K,'.-',k,repmat(kinfty,1,length(k)),'.-')
set(gca,'FontSize',12)
%legend('Optimal','Steady-state optimal')
xlabel('k','FontSize',12)
ylabel('K_k','FontSize',12)

G = ss(a,b,1,0,'Ts',-1)
Gclosed = feedback(G,kinfty)

subplot(1,3,3)
subplot(1,3,3)
[yinfty,k,xinfty]=initial(Gclosed,x0,k);
plot(k,x(1:end-1),'.-',k,xinfty,'.-')
set(gca,'FontSize',12)
legend('Optimal','Steady-state optimal')
xlabel('k','FontSize',12)
ylabel('x_k','FontSize',12)

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

figure(2)
[yinfty,k,xinfty]=initial(Gclosed,x0,k);
Jopt = 1/2*x(1:end-1).^2.*S(1:end-1);
Jsubopt = 1/2*xinfty.^2.*S_subopt(1:end-1)'; 
plot(k,Jopt,'.-',k,Jsubopt,'.-')
set(gca,'FontSize',12)
legend('Optimal','Steady-state optimal')
xlabel('k','FontSize',12)
ylabel('J_k','FontSize',12)

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