clc 
clear all
 
%initial conditions%
 
x0 = [pi/2; 0]; %system dynamics
c = 2; 
l = 10; 
xi0 = [0; 0]; %observer on x
xid0 = 0; %observer on the disturbance
simTime = 100; 
%Run simulation%
sim('Pendolo.slx');

%Plot Results%

figure
 
t = 0:10^-3: simTime;

subplot(3,2,1);
 
plot(t, y, 'r', 'LineWidth', 2);
hold on; grid on;
plot(t, v, 'b:', 'LineWidth', 2);
l = title('Output evolutions Vs Reference');
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = xlabel('Time (s)'); 
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = legend('Output', 'Reference');
 set(l,'Interpreter','Latex'); l.FontSize = 14;
 
 subplot(3,2,2);

plot(t, d, 'r', 'LineWidth', 2);
hold on; grid on;
plot(t, xid, 'b:', 'LineWidth', 2);
l = title('Disturbance Vs Estimation');
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = xlabel('Time (s)'); 
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = legend('$d(t)$', '$\hat d(t)$');
 set(l,'Interpreter','Latex'); l.FontSize = 14;
 
subplot(3,2,3);

plot(t, x(1,:), 'r', 'LineWidth', 2);
hold on; grid on;
plot(t, xi(1,:), 'b:', 'LineWidth', 2);
l = title('State Vs Estimation');
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = xlabel('Time (s)'); 
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = legend('$x_1(t)$', '$\hat x_1(t)$');
 set(l,'Interpreter','Latex'); l.FontSize = 14;
 
 subplot(3,2,4);
 
plot(t, x(2,:), 'r', 'LineWidth', 2);
hold on; grid on;
plot(t, xi(2,:), 'b:', 'LineWidth', 2);
l = xlabel('Time (s)'); 
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = legend('$x_2(t)$', '$\hat x_2(t)$');
 set(l,'Interpreter','Latex'); l.FontSize = 14;
 
  subplot(3,2,5);
  
  plot(t, u, 'r', 'LineWidth', 2);
hold on; grid on;
l = title('Control');
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = xlabel('Time (s)'); 
set(l,'Interpreter','Latex'); l.FontSize = 14;
l = legend('$u(t)$');
 set(l,'Interpreter','Latex'); l.FontSize = 14;