% Estabilizacion del pendulo invertido usando regulador cuadratico
clear; clc; close all;

% Variables
global M m L g;
M = 10;
m = 1;
L = 1;
g = 10;

% Definicion de matrices A y B del sistema linealizado x'=Ax+Bu
global A B;
A = [0 1 0 0;
     0 0 -m*g/M 0; 
     0 0 0 1;
     0 0 (M+m)*g/(L*M) 0];
B = [0;
     1/M;
     0;
     -1/(L*M)];

% Calculo de matriz K usando la funcion lqr
Q = diag([1 1 1 1]);
R = 1;
[K,S,e] = lqr(A,B,Q,R);

% Resolviendo el sistema no lineal x'=f(x,-Kx)
ci = [0.5 0.2 0.4 1.0]; % Condiciones iniciales para [xi xidot theta thetadot]
int = [0 20]; % Intervalo donde se resuelve el sistema
[t,x] = ode45(@(t,x) sistema(t,x,K),int,ci);

% Graficando
figure;
subplot(2,2,1); plot(t,x(:,1)); title('\xi');
subplot(2,2,2); plot(t,x(:,2)); title('\xi^{\prime}');
subplot(2,2,3); plot(t,x(:,3)); title('\theta');
subplot(2,2,4); plot(t,x(:,4)); title('\theta^{\prime}');









