%
% Eigenwertvorgabe (inkl. Beobachter)
%
% x ist nicht messbar
%


close all
clear all
clc

A=[0 1 0;0 0 1;0 1 -1];
b=[0;0;1]; 
c=[1 0 0];
P=ss(A,b,c,0);


% Zustandsregler

% ü=10%, tr=2
d=0.6; wn=1; 

% Soll-Üfkt (dom. Polpaar)
s=tf('s');
Ts=wn*wn/(s*s+2*d*wn*s+wn*wn);

% gewählte Eigenwerte

s1=-d*wn+j*wn*sqrt(1-d*d);
s2=-d*wn-j*wn*sqrt(1-d*d);
s3=-10;

k=acker(A,b,[s1 s2 s3]);
V=-1/(c*inv(A-b*k)*b);

% Zustandsbeobachter

% Eigenwerte der Fehlerdynamik
s1=-5.5;
s2=-5.5;
s3=-5.5;
l=acker(A',c',[s1 s2 s3])';


AA=[A -b*k;l*c A-l*c-b*k];
bb=[b*V;b*V];
cc=[c 0*c];

T=ss(AA,bb,cc,0);

T1=ss(AA,bb,[1 0 0 0 0 0],0);
T2=ss(AA,bb,[0 1 0 0 0 0],0);
T3=ss(AA,bb,[0 0 1 0 0 0],0);

Td1=ss(AA,bb,[0 0 0 1 0 0],0);
Td2=ss(AA,bb,[0 0 0 0 1 0],0);
Td3=ss(AA,bb,[0 0 0 0 0 1],0);

t=0:0.05:10;
u=ones(1,length(t));
x0=[0.3;1;-1;0;0;0];

y=lsim(T,u,t,x0);
ys=step(Ts,t);

plot(t,y),grid
hold on
plot(t,ys,'r');
hold off

legend('real','ideal');

x1=lsim(T1,u,t,x0);
xd1=lsim(Td1,u,t,x0);

x2=lsim(T2,u,t,x0);
xd2=lsim(Td2,u,t,x0);

x3=lsim(T3,u,t,x0);
xd3=lsim(Td3,u,t,x0);

figure

plot(t,x1),grid
hold on
plot(t,xd1,'--');

plot(t,x2,'r');
plot(t,xd2,'r--');

plot(t,x3,'g');
plot(t,xd3,'g--');

legend('x1','xd1','x2','xd2','x3','xd3');








