clc
close all
clear all

A =[ 0       1   0       0;
    -0.3421  0   6.592   0.031;
     0       0   0       1;
     18.898  0  -0.344  -1.713];
B = [0;    -0.0633;   0;   3.4960];
C =  eye(4);
D =  zeros(4,1);
sys = ss(A, B, C, D);

binstr = {'NE', 'ANO'};
fprintf('Riditelny: %s\n', binstr{(rank(A) == rank(ctrb(A, B))) + 1});
fprintf('Pozorovatelny: %s\n', binstr{(rank(A) == rank(obsv(A, C))) + 1});

n = size(B, 1);
Ai = [A zeros(n,1); -C(1, :) 0];
Bi = [B; 0];
Ci = [C zeros(n,1)];
Br = [0; 0; 0; 0; 1];
sysi = ss(Ai,Bi,Ci,D);

Ts = 0.01;
sysid = c2d(sysi, Ts);
disp(pole(sysid));

plplac = [0.95 0.9 0.85 0.8 0.75];
K = place(sysid.A, sysid.B, plplac);
Ki = K(n + 1);
Kr = K(1 : n);

ritf = ss(Ai - Bi * K, Br, Ci, D);
step(ritf);