% linearizace modelu
%                        rovnovazny bod:[¡stavy¡],[¡vstup¡]
parametry_synteza;
u1 = 0.1077e3; % potreba napocitat pro statickou rovnovahu v bode, kolem ktereho linearizujem!!!
u2 = 1.4333e3; % potreba napocitat pro statickou rovnovahu v bode, kolem ktereho linearizujem!!!
[Amat,Bmat,Cmat,Dmat]=linmod('model2lin',[x_stred;y_stred;0;0],[u1;u2]);
%% vyhodnoceni

% riditelnost
R = ctrb(Amat,Bmat);
% pozorovatelnost
O = obsv(Amat,Cmat);
% stabilita: vlastní èísla
lambda = eig(Amat);

if rank(R)==size(Amat,2)
    disp('system je riditelny');
else
    disp('system je neriditelny');
end
if rank(O)==size(Amat,2)
    disp('system je pozorovatelny');
else
    disp('system je nepozorovatelny');
end
if sum(real(lambda)>0)>0
    disp('system je nestabilni');
elseif sum(real(lambda)==0)>0
    disp('system je na hranici stability');
else
    disp('system je stabilni');    
end

figure;step(Amat,Bmat,Cmat,Dmat);
figure;impulse(Amat,Bmat,Cmat,Dmat);
figure;nyquist(Amat,Bmat,Cmat,Dmat);

%% navrh rizeni
% lqr
Q = diag([1;1;1;1]*1e3);
R1 = diag([1;1]);
K = lqr(Amat+10*eye(4),Bmat,Q,R1);
%K = lqr(Amat,Bmat,Q,R1/1000);

figure;step(Amat-Bmat*K,Bmat,Cmat-Dmat*K,Dmat);
figure;impulse(Amat-Bmat*K,Bmat,Cmat-Dmat*K,Dmat);
figure;nyquist(Amat-Bmat*K,Bmat,Cmat-Dmat*K,Dmat);