% 3)

echo on,
clc,
% wersja 12.04.2013
% najpierw uruchom m-plik SScz01.m
% SScz01
%-------------------------------------
% model w przestrzeni stanu
%-------------------------------------
% dane - macierze - forma kanonicna sterowalna - zmiennych fazowych
A = [0,1,0; 0,0,1; -12,-18.4,-8.2]  % macierz stanu
B = [0; 0; 1]                       % macierz wejścia
C = [8 0 2]                         % macierz wyjścia
D = [0]                             % macierz przejścia
pause
%-------------------------------------
sys = ss(A,B,C,D)               % model przestrzeni stanu
pause
%-------------------------------------
% funkcja eig() - wartości własne
eigA = eig(A)
pause
%-------------------------------------
% funkcja ss2zp()
[zss,bss,wss] = ss2zp(A,B,C,D)
pause
%-------------------------------------
% funkcja tfdata()
[lG,mG] = tfdata(sysG,'v')
pause
%-------------------------------------
[zG,bG,wG] = tf2zp(lG,mG)
pause
%-------------------------------------

% 5)

% odpowiedzi na step()
pause
%t = [0:0.01:12];
[y,t,x] = step(sys); [yc,tc,xc] = step(sysc); yG = step(sysG,t);
%-------------------------------------
disp('!!! Przebiegi czasowej odpowiedzi y(t) i zmiennych stanu x(t) !!!')
disp('!!! Naciśnij ENTER !!!')
pause
%-------------------------------------
figure
subplot(2,2,1), plot(t,y), grid, title('y(t)-ABCD')  %, axis([0,max(t),0,0.4])
subplot(2,2,3), plot(t,x), grid, title('x(t)-ABCD')  %, axis([0,max(t),0,1.1])
subplot(2,2,2), plot(tc,yc), grid, title('y(t)-AcBcCcDc')%, axis([0,max(t),0,0.4])
subplot(2,2,4), plot(tc,xc), grid, title('x(t)-AcBcCcDc')%, axis([0,max(t),0,1.1])
figure
plot(t,y,tc,yc,'x',t,yG,'.'), grid              %, axis([0,max(t),0,0.4])
legend('step()-y-ABCD','step()-yc-AcBcCcDc','step()-yG-G(s)')
pause, close all
%-------------------------------------
% niezerowe warunki początkowe -> x0 = [0.5; 0; 1]
pause
tu=[0:0.1:max(t)]';
x0 = [0.5; 0; 1];
u = ones(length(tu),1);
%u = 2*ones(20,1); u(21:201,1) = 0.5*ones(181,1);
[yu,xu] = lsim(A,B,C,D,u',tu,x0);
%plot(t,u,t,xu)
figure
subplot(2,2,1), plot(t,y), grid, title('y(t) x0=[0;0;0]')  %, axis([0,max(t),0,0.4])
subplot(2,2,3), plot(t,x), grid, title('x(t) x0=[0;0;0]')  %, axis([0,max(t),0,1.1])
subplot(2,2,2), plot(tu,yu), grid, title('yu(t) x0=[0.5;0;1]')%, axis([0,max(t),0,0.4])
subplot(2,2,4), plot(tu,xu), grid, title('xu(t) x0=[0.5;0;1]')%, axis([0,max(t),0,1.1])
figure
plot(t,y,'b',tu,yu,'r'), grid %, axis([0,max(t),0,0.4])
legend('step() - y','step() - yu')
pause, close all
%-------------------------------------

% 6)

% postać kanoniczna: "modal" i "companion"
% "modal" - macierz Am zawiera tylko elementy rzeczywiste
% "companion" - forma kanoniczna obserwowalna
% funcja canon()
[Am,Bm,Cm,Dm] = canon(A,B,C,D,'modal')
%[Amx,Bmx,Cmx,Dmx] = canon(Ac,Bc,Cc,Dc,'modal')
pause
%-------------------------------------
[Amc,Bmc,Cmc,Dmc] = canon(A,B,C,D,'companion')
%[Amcx,Bmcx,Cmcx,Dmcx] = canon(Ac,Bc,Cc,Dc,'companion')
% Ac' - odpowiada Amc - tu tak jest, ale dla Acx=inv(P1)*Amc*P1
% Acx'=Amc, ale A'=Amc
pause
%-------------------------------------
% macierz transformacyjna T = inv(P)
% nowe współrzędne stanu z = inv(P)*x = T*x
[Am,Bm,Cm,Dm,Tm] = canon(A,B,C,D,'modal')
%[Amx,Bmx,Cmx,Dmx,Tmx] = canon(Ac,Bc,Cc,Dc,'modal')
%[sysm,Tm] = canon(sys,'modal')
[Amc,Bmc,Cmc,Dmc,Tmc] = canon(A,B,C,D,'companion')
%[Amcx,Bmcx,Cmcx,Dmcx,Tmcx] = canon(Ac,Bc,Cc,Dc,'companion')
pause
%-------------------------------------
% porównanie wartości własnych
[eig(Ac), eig(A), eig(Am), eig(Amc)]
pause
%-------------------------------------

% 7)

% transformacja do nowego układu współrzędnych stanu
P  = inv(Tm);
Az = inv(P)*A*P;
Bz = inv(P)*B;
Cz = C*P;
Dz = D;
pause
%-------------------------------------
Pmc  = inv(Tmc);
Azmc = inv(Pmc)*A*Pmc;
Bzmc = inv(Pmc)*B;
Czmc = C*Pmc;
Dzmc = D;
pause
%-------------------------------------
% porównanie wartości własnych
[eig(A), eig(Am), eig(Amc), eig(Az), eig(Azmc)]
pause
%-------------------------------------
% transformacja tożsamościowa (modalna) - diagonalizacja A
% Ad = macierz.wekt.wł.A^(-1)*A*mac.wek.wł.A -> macierz A będzie diagonalna 
% Bd = inv(macierz.wekt.wł.A)*B
% Cd = C*macierz.wekt.wł.A
% Dd = D
pause
%-------------------------------------
% M macierz wektorów własnych
% Ad macierz wartosci własnych (diagonalna)
[M Ad] = eig(A)                                        
pause
%-------------------------------------
Bd = inv(M)*B    % transformowana macierz wejścia
Cd = C*M         % transformowana macierz wyjścia
Dd = D           % transformowana macierz przejścia
pause
%-------------------------------------
% eigAd = eig(Ad)
[eig(Az), eig(Azmc), eig(Ad), eig(inv(M)*A*M)]
pause
%-------------------------------------
% funkcja ss2tf()
[numGm,denGm]   = ss2tf(Am,Bm,Cm,Dm);
[numGmc,denGmc] = ss2tf(Amc,Bmc,Cmc,Dmc);
[numGd,denGd]   = ss2tf(Ad,Bd,Cd,Dd);
%-------------------------------------
printsys(numGm,denGm)
printsys(numGmc,denGmc)
printsys(numGd,denGd)
pause
%-------------------------------------
% funkcja roundaf() - zaokrąglenie
numGm = roundaf(numGm); denGm = roundaf(denGm);
numGmc = roundaf(numGmc); denGmc = roundaf(denGmc);
numGd = roundaf(numGd); denGd = roundaf(denGd);
printsys(numGm,denGm)
printsys(numGmc,denGmc)
printsys(numGd,denGd)
pause
%-------------------------------------
% odwrócenie zmiennych - przejście między Ac i A, 
% Amc->AmcP1 - zamiana kolumn
% AmcP1=inv(P1)*Amc*P1
P1=[0 0 1; 0 1 0; 1 0 0]
AcP1=inv(P1)*Ac*P1
BcP1=inv(P1)*Bc
CcP1=Cc*P1
DcP1=Dc
eigAcP1=eig(AcP1)
pause
%-------------------------------------

% 8)

% sterowalność
Ster = ctrb(A,B)
rzad_Mc = rank(Ster)
pause
%-------------------------------------
% obserwowalność
Obser = obsv(A,C)
rzad_Mo = rank(Obser)
pause
%-------------------------------------