%%
%%%%%%%%%%%%%%
%%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,0.01);
[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;
 %Qn =  diag([0.2,0.2]);
 %Qn = [0.02,0.02,0.02,0.02,0.02];
 %Qn = eye(5,5);
 %Qn = 0.002;
 
% [kest,L,P] = kalman(sysd,Qn,Rn);
 M = [2.002 4.3652 6.4523 -1.3621 -12.8523;15.003 10.4562 -32.2659 20.2563 0.3652];

 %[kest,L,P,M] = kalman(sysd,eye(5,5),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.8*Pole_soll);
%[L,S,e]= lqrd(Ad',Cd',Qb,Rb,Nb,0.02) 
%%%%%%%%Beobachter in Kalmanstrucktur:
h = inv(Ad)*L';
Md = Ad-L'*C ; 

%%%%%%%%%%%%%%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);
Pole_soll = eig(Ad+Bd*Kd);

Vw = inv( dd+ (c+dd*Kd) * (inv(eye(5,5)-Ad-Bd*Kd))* Bd );
%%%%%%%%%%%%%%Diagonalnormalnormalform bestimmen:
%w = eig(A);
%[T_DF,H] = eig(A);
%A_DF = inv(T_DF)*A*T_DF;
%B_DF = inv(T_DF)*B;
%C_DF = C*T_DF;
%D_DF = D;

%[num,den] = ss2tf(A,B,C,D);
%H = tf(num,den);
%rlocus(H)

%%%%%%%%%%ReduziertenBeobachter:

A11 = [0.9521 -2.2783 0.0018;0.0098 0.9885 0.0000;-0.0001 -0.0188 0.9874];
A12 = [0.3645 0.0001;0.0018 0.0000;0.0030 0.2131];
A21 = [-0.0000 -0.0001 0.0100;0.0000 0.0009 -0.0997];
A22 = [1.0000 0.0011;-0.0002 0.9892];

B1 = [0;0;0.0095];
B2 = [0;0.0885];

C1 = [0 0 0;0 0 0];
C2 = [1 0;0 1];

P_b = A11-A12*inv(C2)*C1;
Q_b = A12*inv(C2);
R_b = C1*A11+C2*A21-(C1*A12+C2*A22)*inv(C2)*C1;
S_b = (C1*A12+C2*A22)*inv(C2);
B_b = C1*B1+C2*B2;

%Pb_b = [+0.80+0.01i,+0.80-0.01i,+0.97];
Pb_b = [0.85,0.90+0.05i,+0.90-0.05i];
N_b = place(P_b',R_b',Pb_b);
M_b = P_b - N_b'*R_b;
K_b = Q_b + M_b*N_b' - N_b'*S_b;
L_b = B1- N_b'*B_b;

%%%%%%%%%%%%%%%%