clear all

A = [0 1; -5 -6];
B = [0; 1];
C = [1 0];
if false
  D = 0;
end

pkg load control

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';

F = place(ss(Aast, Bast, Cast), [-7, -8]);
L = F';

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

u = 1;

i = 1;
for t = sim_time
  if t == 0
    x = [1; 1];
    y = C*x;
    x_1(i) = x(1);
    x_2(i) = x(2);

    xhat = [0; 0];
    yhat = C*xhat;
    xhat_1(i) = xhat(1);
    xhat_2(i) = xhat(2);
  else
    dx = A*x + B*u;
    x = x + dx*dt;
    y = C*x;
    x_1(i) = x(1);
    x_2(i) = x(2);

    dxhat = A*xhat + B*u - L*(yhat - y);
    xhat = xhat + dxhat*dt;
    yhat = C*xhat;
    xhat_1(i) = xhat(1);
    xhat_2(i) = xhat(2);
  end
  i = i + 1;
end

csvwrite("observer_simout.csv", [sim_time', x_1', x_2', xhat_1', xhat_2'])
