clear all

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

pkg load control
assert(det(ctrb(A, B)) != 0, "system is NOT controllable")
assert((det(ctrb(A, B)) != 0) == (rank(ctrb(A, B)) == size(A)(1)))

x0 = [1; 0];

%% LQR
sim_time = linspace(0, 6, 1024);

Q = [1 0; 0 1]; R = 1;

clf
for weight = [1, 10, 100, 1000]
  Q(1,1) = weight;
  [G, X, L] = lqr(A, B, Q, R);

  i = 1;
  for t = sim_time
    x = expm((A - B*G)*t)*x0;
    x_1(i) = x(1);
    x_2(i) = x(2);
    i = i + 1;
  end
  plot(sim_time, x_1, sim_time, x_2)
  hold on
end
grid on
legend("x_1", "x_2"); xlabel("time [s]"); ylabel("state")
title("Q_1_1 = 1, 10, 100, 1000")

pause()

%% LQR
Q = [1 0; 0 1]; R = 1;

clf
for weight = [1, 10, 100, 1000]
  Q(2,2) = weight;
  [G, X, L] = lqr(A, B, Q, R);

  i = 1;
  for t = sim_time
    x = expm((A - B*G)*t)*x0;
    x_1(i) = x(1);
    x_2(i) = x(2);
    i = i + 1;
  end
  plot(sim_time, x_1, sim_time, x_2)
  hold on
end
grid on
legend("x_1", "x_2"); xlabel("time [s]"); ylabel("state")
title("Q_2_2 = 1, 10, 100, 1000")

pause()

%% LQR
Q = [1 0; 0 1]; R = 1;

clf
for weight = [1, 0.1, 0.01, 0.001]
  R = weight;
  [G, X, L] = lqr(A, B, Q, R);

  i = 1;
  for t = sim_time
    x = expm((A - B*G)*t)*x0;
    x_1(i) = x(1);
    x_2(i) = x(2);
    i = i + 1;
  end
  plot(sim_time, x_1, sim_time, x_2)
  hold on
end
grid on
legend("x_1", "x_2"); xlabel("time [s]"); ylabel("state")
title("R = 1, 0.1, 0.01, 0.001")
