clear;
clc
%% System Parameters
A=[0.995 0.099;-0.099 0.985];
n=length(A);
B=[0;0];
X0=[1;0];
N=100;
Q=[2 0;0 1];
R=10;
Jx_quadratic=zeros(n,n);
Jx_lyap=zeros(n,n);
%%%%%%%%%%%%%%%%%%%%%%%%%% Part - 2 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Plyap=dlyap(A,Q); % Solve discrete-time Lyapanov equations
Xk=zeros(2,N);
Xk(:,1)=X0;
for i=2:N
Xk(:,i)=A*Xk(:,i-1);
end
figure(1)
subplot(2,2,1)
hold on
stairs(0:N-1,Xk(1,:),'LineWidth',2)
title('Exercise 4 - Part 2 Infinite Horizon cost-to-go')
xlabel('Sample (N)')
ylabel('X_{1}')
grid on
subplot(2,2,2)
hold on
stairs(0:N-1,Xk(2,:),'LineWidth',2)
xlabel('Sample (N)')
ylabel('X_{2}')
grid on
for j=1:N
Jx_quadratic(j,j)=Xk(:,j)'*Q*Xk(:,j);
Jx_lyap(j,j)=Xk(:,j)'*Plyap*Xk(:,j);
end
subplot(2,2,3)
stairs(0:N-1,diag(Jx_quadratic(:,:)),'LineWidth',2)
xlabel('Sample (N)')
ylabel('Quadratic Cost Function')
grid on
hold on
subplot(2,2,4)
stairs(0:N-1,diag(Jx_lyap(:,:)),'LineWidth',2)
xlabel('Sample (N)')
ylabel('Lyapanov Cost Function')
grid on
hold on
COST_quadratic=trace(Jx_quadratic(:,:));
COST_LYAPANOV=trace(Jx_lyap(:,:));
disp(['The Cost Function With ARE Method for X0 ' mat2str(X0) ' is ' num2str(COST_quadratic) ' and With Lyapanov Method is ' num2str(Jx_lyap(1,1))]);
