% linearizace modelu
%                        rovnovazny bod:[¡stavy¡],[¡vstup¡]
[A,B,C,D]=linmod('ModelSimulinkDyn2Lin',[0;0;0;0],0);
%% vyhodnoceni

% riditelnost
R = ctrb(A,B);
% pozorovatelnost
O = obsv(A,C);
% stabilita: vlastní èísla
lambda = eig(A);

if rank(R)==size(A,2)
    disp('system je riditelny');
else
    disp('system je neriditelny');
end
if rank(O)==size(A,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(A,B,C,D);
figure;impulse(A,B,C,D);
figure;nyquist(A,B,C,D);

%% navrh rizeni
% pole placement
P = [-1;-1;-1;-1];
K = acker(A,B,P);

figure;step(A-B*K,B,C-D*K,D);
figure;impulse(A-B*K,B,C-D*K,D);
figure;nyquist(A-B*K,B,C-D*K,D);

% lqr
Q = diag([1;1;1;1]*1e3);
R1 = 1;
K = lqr(A,B,Q,R1);

figure;step(A-B*K,B,C-D*K,D);
figure;impulse(A-B*K,B,C-D*K,D);
figure;nyquist(A-B*K,B,C-D*K,D);

% stavovy pozorovatel - zesileni zpetne vazby pouzijeme z ackermanovy
% formule, zname tim umistni polu
P = [-1;-1;-1;-1]*3;
K = acker(A,B,P);
% nyni navrhneme matici L stavoveho pozorovatele
Pp = [-10;-10;-10;-10];
Lst=acker(A',C',Pp)';


%% animace
figure;
% pocatecni poloha
plot(X0(1),0,'sk');hold on % umoznuje prikreslit dalsi krivky
plot([X0(1) X0(1)+L*sin(X0(3))],[0 L*cos(X0(3))],'k');
% pro animaci
a1 = plot(0,0,'sg');
a2 = plot(0,0,':g');
a3 = plot(0,0,'b');
axis equal
% animace
for kk=1:50:size(x,1)
    set(a1,'Xdata',x(kk,1),'Ydata',0);
    set(a2,'Xdata',[X0(1) x(kk,1)],'Ydata',[0 0]);
    set(a3,'Xdata',[x(kk,1) x(kk,1)+L*sin(x(kk,3))],'Ydata',[0 L*cos(x(kk,3))]);
    drawnow; % vykresli graf
end