clear; clc; close all;

A = [ 0,    1,   0,    0;
      9.8,  0,  -9.8,  0;
      0,    0,   0,    1;
     -9.8,  0,  29.4,  0];

B = [ 0,  0;
      1, -2;
      0,  0;
     -2,  5];

C = [1, 0, 0, 0;
     0, 0, 1, 0];

D = zeros(2,2);

sys_ol = ss(A, B, C, D);

poles_ol = pole(sys_ol);
zeros_ol = tzero(sys_ol);
G_ol = tf(sys_ol);

figure;
step(sys_ol);
grid on;

figure;
sigma(sys_ol);
grid on;

Q = diag([100, 10, 100, 10]);
R = diag([1, 1]);
K = lqr(A, B, Q, R);

A_cl = A - B*K;
sys_cl = ss(A_cl, B, C, D);

poles_cl = pole(sys_cl);

figure;
step(sys_cl);
grid on;

figure;
sigma(sys_cl);
grid on;