clear all;
close all;
mw = 4;
mc= 1.5;
L = 1.2;
g=9.81;
a2=mc*g/(mw*L); a4= (mw+mc)*g/(mw*L); b2 =1/mw;
A= [0 0 1 0;0 0 0 1;0 a2 0 0;0 -a4 0 0];
B=[0 0 b2 -b2]';
C=[1 0 0 0];D=0;
n=length(A);
r=size(B,2);
m=size(C,1);
eig(A) % niestabilny bo sa dodatnie i zerowe, uklad typowo niestabilny

tmax= 10;
t=linspace(0,tmax,200);
u=zeros(1,length(t));
C12=[1,0,0,0;0,1,0,0];
x10=[0.1;0;0;0];
y_x1= lsim(A,B,C12,D,u,t,x10)
%plot(t,y_x1); grid

%regulacja stanu - projektowanie regulatora - lokacja biegunow

p = [-1.5 -1.5 -2 -2]';
% tr=4/8=0.5 okolo czas regulacji

%macierz wzmocnien, place nie zadziala bo podwojny biegun
K=acker(A,B,p)            %rank(ctbr(A,B))
Ac=A-B*K;    % eig(Ac) bieguny

% obliczenie N

L=[A B; C D];
P=[zeros(n,m); eye(m)];
nxu=L\P;
N=K*nxu(1:n,:)+ nxu((n+1):(n+m),:);

% odpowiedzi z uwzgl. N
ycx1=lsim(Ac,B*N,C12-D*K,D*N,u,t,x10)  % x10 - wektor stanu poczatkowego
% plot(t,ycx1); grid
%subplot(121);
subplot(221);plot(t,y_x1); grid; title(' uklad otwarty, x10=0.1');
subplot(222); plot(t,ycx1); grid; title(' uklad zamkniety, x10=0.1');

%wyznaczenie transmitancji wzgledem wartosci zadanej ukl. zamknietego
[lw,mw]=ss2tf(Ac,B*N,C-D*K,D*N);
y=step(lw,mw,t);
%transmitancja zakloceniowa
[lz,mz]=ss2tf(Ac,B,C-D*K,D);
yz=step(lz,mz,t);

subplot(223);plot(t,y); grid; title(' uklad zamkniety - odp na wart. zadana');
subplot(224); plot(t,yz); grid; title(' uklad zamkniety - zaklocenie');

close all;
%obserwator
 z(t>=tmax/2)=50;
po=2*p;
H=(acker(A',C',po))';            %macierz wzmocnien korektora
Ao=A-H*C;

%uklad zamkniety  z obserwatorem
Az=[A-B*K,B*K; zeros(n,n) A-H*C];
Bz=[B*N; zeros(n,m)];
Cz=[C; zeros(m,n)];
Dz=D;

nz=length(Az)
Ca=eye(nz);
Da=zeros(nz,r);
y_obs=lsim(Az,Bz,Ca,Da,zeros(size(t)),t,[x10;x10]);  %przebieg dla obserwatora

figure;
x1=y_obs(:,1);
e1=y_obs(:,1+n);
x2=y_obs(:,2);
e2=y_obs(:,n+2);

subplot(221); plot(t,x1,t,x1-e1); grid; title('obserwator - x1'); legend('x1','x1 - uzyskane z obserwatora');
subplot(222); plot(t,x2,t,x2-e2); grid; title('obserwator - x2'); legend('x2','x2 - uzyskane z obserwatora');



subplot(223); plot(t,e1); grid; title('obserwator - e1');
subplot(224); plot(t,e2); grid; title('obserwator - e2');


Ai=[A zeros(n,m); C zeros(m)];
Bi=[B; zeros(m,r)];
pi=[p;min(p)];
kki=acker(Ai,Bi,pi);

k=kki(:,1:n);
ki=kki(:,(n+1):(n+m));

%uklad zamkniety z calkowaniem bez obserwatora
Aci=[A-B*k -B*ki; C zeros(m)];
Bci=[B*N; -eye(m)];
Cci= [C zeros(m) ];
Dci = 0;

[lwi,mwi] = ss2tf(Aci,Bci,Cci,Dci)

%odpowiedz skokowa
figure;
ywi=step(lwi,mwi,t);
 plot(t,ywi); grid;

 tmax=2*tmax;
 t=linspace(0,tmax,400);   %generuje liczby w danym zakresie
 z=zeros(size(t));
 z(t>=tmax/2) = 50;
 figure;
 subplot(121);
 hold on;
 for kNi=[ki ki 0; N 0 N]
     Aci= [A-B*k -B*kNi(1); C zeros(m)];
     Bci=[B*kNi(2); -eye(m)];
     [lwi,mwi]=ss2tf(Aci,Bci,Cci,Dci);
     ywi=step(lwi,mwi,t);
     yzi=lsim(Aci,[B;zeros(m,r)],Cci,Dci,z,t);
     plot(t,ywi+yzi); legend('N+I','tylko I','tylko N');
 end
hold off;
grid;

%e0=[0.2 0 0 0
e0=[0 0 0 0]';
xexio0=[e0;e0;zeros(m,1)];
subplot(122); hold on

for kNi =[ki ki 0;N 0 N]
    Acoi=[A-B*k B*k -B*kNi(1)
         zeros(n) A-H*C zeros(n,m)
         C zeros(m,n+m)];
     Bcoi=[B*kNi(2); zeros(n,m); -eye(m)];
     Ccoi=[C zeros(m,n+m)];
     Dcoi=0;
     ywoi=lsim(Acoi,Bcoi,Ccoi,Dcoi,ones(size(t)),t,xexio0);
     yzoi=lsim(Acoi,[B;zeros(n,r);zeros(m,r)],Ccoi,Dcoi,z,t);
     % yzoi=lsim(Acoi,[B;B;zeros(m,r)],Ccoi,Dcoi,z,t);
     plot(t,ywoi+yzoi);
end
hold off; grid;