clc 
clear all

%All parameters need to be fixed in each block they are used.%

%initial conditions%

x0 = [pi; 0]; %system dynamics
c = pi; 

xi0 = [0; 0]; %observer on x
xid0 = [0; 0]; %observer on the disturbance
simTime = 25; 
eps = 1; 
%Run simulation%
sim('Pendolo2.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(:,1), '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;
 
 