clear all

%% system
A = [0 1; -5 -6];
b = [0; 1];

x0 = [1; 0];

%% free response
sim_time = linspace(0, 6, 1024);

i = 1;
for t = sim_time
  x = expm(A*t)*x0;
  x_1(i) = x(1);
  x_2(i) = x(2);
  i = i + 1;
end

clf
plot(sim_time, x_1, sim_time, x_2)
grid on
xlabel("time [s]"); ylabel("state variable")

%% state feedback control
pkg load control

assert(det(ctrb(A, b)) != 0, "system is NOT controllable")
f = acker(A, b, [-7+2i, -7-2i]);

i = 1;
for t = sim_time
  x = expm((A - b*f)*t)*x0;
  x_1(i) = x(1);
  x_2(i) = x(2);
  i = i + 1;
end

hold on
plot(sim_time, x_1, sim_time, x_2)
legend("free response x_1", "free response x_2", "state feedback x_1", "state feedback x_2")
title("state feedback control")
