clear;
close all;
clc;


%%Parameter festlegen 
J_R = 0.063;
J_F = 1.35;
c_S = 126;
k_S = 0.03;

%%Matrizen vereinbaren
% Zustandsvektor mit vier Zuständen:
%           Phi_R
%   x =     Phi_R_Punkt 
%           Phi_L 
%           Phi_L_Punkt
A = [0 1 0 0;-c_S/J_R -k_S/J_R c_S/J_R k_S/J_R;0 0 0 1;c_S/J_F k_S/J_F -c_S/J_F -k_S/J_F];
b = [0; 1/J_R; 0; 0];
c = [c_S k_S -c_S -k_S];
%damp(A)

%%symbolische Namen
syms J_R;
syms J_F;
syms c_S;
syms k_S;

%%symbolische Matrizen
Asym = sym('Asym',[4 4]);
Asym(:,:) = 0;
Asym(1,2) = 1;
Asym(2,1) = -c_S/J_R;
Asym(2,2) = -k_S/J_R; 
Asym(2,3) = c_S/J_R; 
Asym(2,4) = k_S/J_R;
Asym(3,4) = 1;
Asym(4,:) = [c_S k_S -c_S -k_S];
Asym

bsym = sym('bsym',[4 1]);
bsym(:,:) = 0;
bsym(2,1) = 1/J_R;
bsym

csym = sym('csym',[1 4]);
csym(:,:) = 0;
csym(1,1) = c_S;
csym(1,2) = k_S;
csym(1,3) = -c_S;
csym(1,4) = -k_S;
csym


%%Zustandsraummodell aufstellen
Antriebsstrang=ss(A,b,c,0)
step(Antriebsstrang)
%Stabilitaet = isstable(Antriebsstrang)
%Eingangs- und Ausgangsnamen des Modells setzen
Antriebsstrang.InputName = {'M_em'};
Antriebsstrang.OutputName = {'M_S'};

%%Analyse des Modells
%Inputs:        1 - M_em 
%Outputs: 1:    1 - M_S
figure
h = bodeplot(Antriebsstrang)
setoptions(h,'PhaseVisible','off','FreqUnits','Hz','Grid','on', 'XLimMode', 'manual', 'XLim', [10^0 10^4], 'YLim', [-200 50]);

%Pol-Nullstellendiagramm
figure
pzmap(Antriebsstrang);
grid;


%%Luenberger Beobachter
A = A;
b = b;
cb = [1 0 0 0];           %es kann nur der Absolutwinkel des Maschinenrotors gemessen werden
Luenberger=ss(A,b,cb,0)   %selbes Modell wie Strecke, nur andere Ausgangsmatrix

Rang_Beobachtbar = rank(obsv(Luenberger))   %Beobachterentwurf möglich, wenn Strecke beobachtbar
Rang_Steuerbar = rank(ctrb(Luenberger))     %Reglerentwurf möglich, wenn Strecke steuerbar

Antriebsstrangpole = pole(Antriebsstrang)        %nur zur Nachvollziehbarkeit im Command Window
Luenbergerpole = 1.1*real(pole(Luenberger))+imag(pole(Luenberger))/i    %Platzieren der Pole links von Strecke und ggfl. schwächen des Imaginärteils (höhere Dämpfung)
                                                                        %hier keine Schwächung liefert besseres Einschwingverhalten der Zustandsgrößen
                                                                        %Realteil nur knapp neben den ursprünglichen polen beseitigt Schwingung un Zustandsgröße 2
                                                                        %gute Einstellung: 1.1*real
Luenbergerpole = [Luenbergerpole(1);Luenbergerpole(2);-1.0;-1.01]       %da bleibende Abweichung zwischen Beobachter und Strecke müssen die in Polstellen Nr. 3 und 4 (in Null beim Antriebsstrang) 
                                                                        %beim Beobachter ebenfalls links von Null platziert werden
                                                                        %weit nach links schieben bringt Schwingung zurück (dafür schnell), zu nahe an imaginärer Achse lässt Zeit bis zum Konvergieren anwachsen
                                                                        %gute Einstellung: -1.0;-1.01 (zwei Polstellen können nicht am selben Ort platziert werden)
% Luenbergerpole = [-500 + 5*i;...
%                    -500 - 5*i;...
%                     -50 + 5*i;...
%                     -50 - 5*i]
lbeob = place(Luenberger.A',Luenberger.c',Luenbergerpole)%.'   %Rückführvektor berechnen
l = lbeob'                                                           %Rückführvektor transponieren

% Luenbergerbeobachter = estim(Luenberger,l)                       %SS-LTI-Modell Beobachter
%                                                                        
% figure
% pzmap(Luenberger,Luenbergerbeobachter)  
%Pol-Nullstellendiagramm
%Luenberger = Antriebsstrang (nur Winkel wird gemessen)
%Luenbergerbeobachter = erzeugtes LTI Modell mit verschobenen Polen
%grid;

% Alternative zum Befehl estim (Berücksichtigung von l in die Systemmatritzen)
Ab=A-l*cb;      %Dynamikmatrix des Beobachters mit neuem Ausgangsvektor (nur Winkel des Rotors wird gemessen)
                %wird nur für das Nullstellendiagramm benötigt, da Struktur in Simulink aufgesplittet
                %würde L in Simulink nicht einzeln zurück geführt, könnte Ab dem ss Modell übergeben werden
figure
pzmap(Luenberger,ss(Ab,[b l],eye(4),0))         
%Pol-Nullstellendiagramm
%Luenberger = Antriebsstrang (nur Winkel des Rotors wird gemessen)
%ss(Ab,b,cb,0) = mit neu berechneter Systemmatrix, Eingängen b und l sowie Einheitsmatrix
%grid;

sim('Luenbergerbeobachter_gesplitted')
figure
plot(gauss)