clear all

g = 9.8; % [m/s^2]
m = 0.3; % [kg]
l = 0.4; % [m]
J = (1/12)*m*l^2; % [kg m^2]
M = 0.7; % [kg]

alpha = (M*l^2 + J)*m + J*M;
A = [0 0 1 0; 0 0 0 1; 0 -((g*l^2*m^2)/alpha) 0 0; 0 (g*l*m*(m + M))/alpha 0 0];
B = [0; 0; (l^2*m + J)/alpha; -((l*m)/alpha)];
C = [1 0 0 1];
if false
  D = 0;
end

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)))
assert(det(obsv(A, C)) != 0, "system is NOT observable")
assert((det(obsv(A, C)) != 0) == (rank(obsv(A, C)) == size(A)(1)))

Aast = A';
Bast = C';
Cast = B';
L = place(ss(Aast, Bast, Cast), [-9, -9, -9, -9])';

Q = eye(4);
R = 1;
F = lqr(A, B, Q, R);

x0 = [0.6; 0.6; 0.6; 0.6];
xhat0 = [0; 0; 0; 0];

xbar0 = [x0; xhat0];
Abar = [A -B*F; L*C A-B*F-L*C];

dt = 0.001; % [s]
sim_time = 0:dt:10;

i = 1;
for t = sim_time
  xbar = expm(Abar*t)*xbar0;
  x_1(i) = xbar(1); x_2(i) = xbar(2); x_3(i) = xbar(3); x_4(i) = xbar(4);
  xhat_1(i) = xbar(5); xhat_2(i) = xbar(6); xhat_3(i) = xbar(7); xhat_4(i) = xbar(8);
  i = i + 1;
end

csvwrite("inv_pendulum_simout.csv", [sim_time', x_1', x_2', x_3', x_4', xhat_1', xhat_2', xhat_3', xhat_4'])
