%%
%%%%%%%%%%%%%%
%%Zustandraumdarstellung mit 5 Zustände:
%x1 = omega_rob
%x2 = winkel_rob
%x3 = omega_welle
%x4 = winkel_welle
%x5 = Strom i
%u = Uein
a11 = -d1_rob/J_rob;
a12 = -c_feder * radius_rob^2 /J_rob;  
a13 = 0;
a14 = c_feder*radius_rob*radius_welle/J_rob;
a15 = 0;
a21 = 1;
a22 = 0; 
a23 = 0;
a24 = 0;
a25 = 0;
a31 = 0;
a32 = +c_feder * radius_welle*radius_rob /J_an;
a33 = -d1_an/J_an;
a34 = -c_feder*radius_welle^2 /J_an;
a35 = Km*ue/J_an;
a41 = 0;
a42 = 0;
a43 = 1;
a44 = 0;
a45 = 0;
a51 = 0;
a52 = 0;
a53 = -Km*ue/L_spule;
a54 = 0;
a55 = -R/L_spule;

A = [a11 a12 a13 a14 a15 ; a21 a22 a23 a24 a25 ; a31 a32 a33 a34 a35 ;...
    a41 a42 a43 a44 a45 ; a51 a52 a53 a54 a55 ];
C = [0 0 0 1 0 ; 0 0 0 0 1];
B = [0;0;0;0;1/L_spule];
D = [0;0];
c = [0 0 0 1 0];
dd = 0;

syscz = ss(A,B,C,D);
sysdz = c2d(syscz,Ts);
[Ad,Bd,Cd,Dd] = ssdata(sysdz);
%%%%%%%%%%%% Steuerkeit und Beobachtbarkeit prüfen

Qb = obsv(A,C);
rank(Qb);
Qs = ctrb(A,B);
det(Qs);

%%%%%%%%Rauschen
%R = 1e-3; 
%Q = diag([0.02,0.02,0.02,0.02,0.02]);
% N = [0.001;0.001;0.001;0.001;0.001];

%BG = [ Bd eye(5,5) ];
 %DH = [ Dd zeros(2,4)];
 %sysd = ss(Ad,BG,Cd,DH,1)
 
%%%%%%%%%Kalmanfilter_Gain
 %Rn = 1e-3;
 Rn =  diag([1e-3,1e-3]);
 %Qn = eye(5,5);
 Qn = 0.001;
 
[kest,L,P,M] = kalman(sysdz,Qn,Rn,0);

 %%%%%%%%Beobachter-Rückführvektor L
Pole_soll = [+0.80,+0.85+0.01i,+0.85-0.01i,0.75-0.05i,0.75+0.05i];
L = place(Ad',Cd',0.7*Pole_soll);
%[L,S,e]= lqrd(Ad',Cd',Qb,Rb,Nb,0.02) 
%%%%%%%%Beobachter in Kalmanstrucktur:
h = inv(Ad)*L';
Md = Ad-L'*Cd ; 

%%%%%%%%%%%%%%Regler
%Kd = place(Ad,Bd,[+0.85+0.05i,+0.85-0.05i,0.90+0.1i,+0.90-0.1i,+0.99]);
%[Kd,S,e]= lqrd(Ad,Bd,Q,R,N,0.01);
Kd = -place(Ad,Bd,Pole_soll);
Kc = -place(A,B,Pole_soll);
%Pole_soll = eig(Ad+Bd*Kd);

Vw = inv( dd+ (c+dd*Kd) * (inv(eye(5,5)-Ad-Bd*Kd))* Bd );

%VVw = -(c*(A+B*Kc)*B)^-1;

%plot(tout,xout)