%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Uebung 2 - Regelungstechnik
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Aufgabe 2.2: - Lin. Gleichungssystem loesen
%Reset:
clear all
close all
clc

gl1 = [1 , 2 , 4]
gl2 = [2 , 2 , 1]
gl3 = [3 , 2 , 0]

res = [5 ; 4 ; 1]

glsys = [gl1 ; gl2 ; gl3]

% Mit linsolve: (Die Langsamste Methode)
tic
x1 = linsolve(glsys, res)
toc

% Mit inv: (glsys.x=res  =>  x=glsys^(-1).res)
tic
x2 = inv(glsys)*res
toc

% Mit mldivide: (Die Schnellste Methode)
tic
x3 = mldivide(glsys, res)
toc

%Die Fehler der 3 Loesungen unterscheiden sich, die Methode mit Inv
%   ist ungenau.
norm(glsys*x1 - res)
norm(glsys*x2 - res)
norm(glsys*x3 - res)

%% Aufgabe 2.4: - Fourierreihenentwicklung
%Reset:
clear all
close all
clc

A = 10  %Amplitude

x = 0:.025:10;  %Plotbereich
figure          %Ein Grafikobjekt erzeugen
sum = 0

%Im folgenden ein Linienplot
for k=1:100
    sum = sum + A*4/(pi*(2*k-1))*sin((2*k-1)*x);
    pause(0.1)  %0.1s Pausieren
    plot(x,sum,'red','LineWidth',2)
    tit = sprintf('Fourier-Reihenentwicklung Rechteckfunktion: k=%03d', k);
    title(tit, 'FontName', 'Arial', 'FontSize', 14)
    axis([0,10,-1.5*A,1.5*A])
end

%Plot mit Punkten
sum = 0
for k=1:100
    sum = sum + A*4/(pi*(2*k-1))*sin((2*k-1)*x);
    pause(0.1)  %0.1s Pausieren
    plot(x,sum,'.k')
    tit = sprintf('Fourier-Reihenentwicklung Rechteckfunktion: k=%03d', k);
    title(tit, 'FontName', 'Arial', 'FontSize', 14)
    axis([0,10,-1.5*A,1.5*A])
end

%% Aufgabe 2.5: - Funktionsplot
%Reset:
clear all
close all
clc

x = 0:0.25:10;  %Plotbereich
y = 0:0.25:10;

[xx,yy] = meshgrid(x,y);

figure          %Ein Grafikobjekt erzeugen
for t=0:0.25:9.5
    zz=sin(xx*pi/10).*sin(yy*pi/10).*abs(sin(t));
    pause(0.2)  %0.2s Pausieren
    mesh(xx,yy,zz) %Gitterplot
    tit = sprintf('3D-Gitterplot: t=%0.2f', t);
    title(tit, 'FontName', 'Arial', 'FontSize', 14)
    box on
    grid on
    axis([0,10,0,10,0,1])
end

for t=0:0.25:9.5
    zz=sin(xx*pi/10).*sin(yy*pi/10).*abs(sin(t));
    pause(0.2)  %0.2s Pausieren
    surf(xx,yy,zz) %Flaechenplot
    tit = sprintf('3D-Flaechenplot: t=%0.2f', t);
    title(tit, 'FontName', 'Arial', 'FontSize', 14)
    box on
    grid on
    axis([0,10,0,10,0,1])
end

%% Aufgabe 2.7: - Differentialgleichungssystem diskret in Zustandsraumdarst
%Reset:
clear all
close all
clc

Ta = 0.1; %Abtastzeit lt. Angabe
trange = 0:Ta:5;

%Einheitssprung Sigma(t):
u=ones(length(trange),1);

A = [0 1; -4 -3];   %Dynamikmatrix
B = [0;1];          %Eingangsmatrix

x0 = [0;0];         %Anfangsbedingungen (Index=1, matlab start nicht bei 0)           

%Berechnung des DGL-Sys mit dem Euler-Vorwaertsverfahren:
%Als Anfangsbedingung wird x(0) genommen und ausgehend von t=0 zur
%gewuenschten Position (in unserem Fall length(trange)) mit der 
%Schrittweite Ta iteriert.
x=x0(:,1);             % : means, that all elements in the row are selected
for k=2:length(trange)      %length...returns the number of elements...
    x(:,k) = x(:,k-1)+Ta*(A*x(:,k-1)+B*u(k-1)); %2.6 lt. Angabe
end

%Zum Vergleich mit eigener Abtastzeit
Ta_own = 0.05; %Abtastzeit selbst gewaehlt
trange_own = 0:Ta_own:5;
x_own=x0(:,1);
u=ones(length(trange_own),1);
for k=2:length(trange_own)
    x_own(:,k) = x_own(:,k-1)+Ta_own*(A*x_own(:,k-1)+B*u(k-1));
end

%Zum Vergleich mit eigener Abtastzeit
Ta_own2 = 0.25; %Abtastzeit selbst gewaehlt
trange_own2 = 0:Ta_own2:5;
x_own2=x0(:,1);
u=ones(length(trange_own2),1);
for k=2:length(trange_own2)
    x_own2(:,k) = x_own2(:,k-1)+Ta_own2*(A*x_own2(:,k-1)+B*u(k-1));
end

%Zum Vergleich mit eigener Abtastzeit
Ta_own3 = 0.5; %Abtastzeit selbst gewaehlt
trange_own3 = 0:Ta_own3:5;
x_own3=x0(:,1);
u=ones(length(trange_own3),1);
for k=2:length(trange_own3)
    x_own3(:,k) = x_own3(:,k-1)+Ta_own3*(A*x_own3(:,k-1)+B*u(k-1));
end

%Vergleich mit Control System Toolbox

%Wenn C1, C2 und D wie folgt
C1 = [1;0];           %Ausgangsmatrix fuer x1
C2 = [0;1];           %Ausgangsmatrix fuer x2
C = eye(2);           %Einheitsmatrix

%Es handelt sich um kein System mit direktem Durchgriff, daher D=0.
D = 0;               %Durchschaltmatrix
sys = ss(A,B,C',D);  %generiert Kontiunierliches System

dsys = c2d(sys,Ta,'zoh');       %generiert Diskretes System
dstep = step(dsys,trange(end));  %Sprungantwort

x1_euler_steps=(x(1,:))';
x1_own_euler_steps=(x_own(1,:))';
x1_own2_euler_steps=(x_own2(1,:))';
x1_own3_euler_steps=(x_own3(1,:))';

x2_euler_steps=(x(2,:))';
x2_own_euler_steps=(x_own(2,:))';
x2_own2_euler_steps=(x_own2(2,:))';
x2_own3_euler_steps=(x_own3(2,:))';

%In der Folgenden Matrix sind die Werte bei t=5s zu sehen: (in der linken
%Spalte sind die Ta, in der mittleren Spalte stehen die zugehoerigen Werte
%und in der rechten Spalte steht die Differenz zum richtigen Ergebnis)
res_x1_5s = [0 dstep(end,1) 0;Ta x1_euler_steps(end) x1_euler_steps(end)-dstep(end,1);Ta_own x1_own_euler_steps(end) x1_own_euler_steps(end)-dstep(end,1);Ta_own2 x1_own2_euler_steps(end) x1_own2_euler_steps(end)-dstep(end,1);Ta_own3 x1_own3_euler_steps(end) x1_own3_euler_steps(end)-dstep(end,1)];
res_x2_5s = [0 dstep(end,2) 0;Ta x2_euler_steps(end) x2_euler_steps(end)-dstep(end,2);Ta_own x2_own_euler_steps(end) x2_own_euler_steps(end)-dstep(end,2);Ta_own2 x2_own2_euler_steps(end) x2_own2_euler_steps(end)-dstep(end,2);Ta_own3 x2_own3_euler_steps(end) x2_own3_euler_steps(end)-dstep(end,2);];

%Die Indizes von 1sek
i1s = (length(trange)-1)/5+1;
i1s_own = (length(trange_own)-1)/5+1;
i1s_own2 = (length(trange_own2)-1)/5+1;
i1s_own3 = (length(trange_own3)-1)/5+1;
res_x1_1s = [0 dstep(i1s,1) 0;Ta x1_euler_steps(i1s) x1_euler_steps(i1s)-dstep(i1s,1);Ta_own x1_own_euler_steps(i1s_own) x1_own_euler_steps(i1s_own)-dstep(i1s,1);Ta_own2 x1_own2_euler_steps(i1s_own2) x1_own2_euler_steps(i1s_own2)-dstep(i1s,1);Ta_own3 x1_own3_euler_steps(i1s_own3) x1_own3_euler_steps(i1s_own3)-dstep(i1s,1);];
res_x2_1s = [0 dstep(i1s,2) 0;Ta x2_euler_steps(i1s) x2_euler_steps(i1s)-dstep(i1s,2);Ta_own x2_own_euler_steps(i1s_own) x2_own_euler_steps(i1s_own)-dstep(i1s,2);Ta_own2 x2_own2_euler_steps(i1s_own2) x2_own2_euler_steps(i1s_own2)-dstep(i1s,2);Ta_own3 x2_own3_euler_steps(i1s_own3) x2_own3_euler_steps(i1s_own3)-dstep(i1s,2);];

%res_x1_5s_disp = gallery('uniformdata',[5 3],0);
disp('x1 bei 1 sekunde:')
disp(dataset({res_x1_1s, 'Ta', 'Wert', 'Differenz'}))
disp('x2 bei 1 sekunde:')
disp(dataset({res_x2_1s, 'Ta', 'Wert', 'Differenz'}))
disp('x1 bei 5 sekunden:')
disp(dataset({res_x1_5s, 'Ta', 'Wert', 'Differenz'}))
disp('x2 bei 5 sekunden:')
disp(dataset({res_x2_5s, 'Ta', 'Wert', 'Differenz'}))


figure                %Ein Grafikobjekt erzeugen
subplot(2,1,1)      %2 Zeilen, 1 Spalte, an die 1. Pos.
stepplot(sys(1),trange(end))
hold on
plot(trange,dstep(:,1),'b.')
plot(trange_own,x1_own_euler_steps, 'g.')
plot(trange_own2,x1_own2_euler_steps, 'k.')
plot(trange_own3,x1_own3_euler_steps, 'c.')
plot(trange,x1_euler_steps, 'r.')
hold off
subplot(2,1,2)      %2 Zeilen, 1 Spalte, an die 2. Pos.
stepplot(sys(2),trange(end))
hold on
plot(trange,dstep(:,2),'b.')
plot(trange_own,x2_own_euler_steps, 'g.')
plot(trange_own2,x2_own2_euler_steps, 'k.')
plot(trange_own3,x2_own3_euler_steps, 'c.')
plot(trange,x2_euler_steps, 'r.')
hold off
leg_sys = 'Kont. - Control System Toolbox';
leg_dstep = sprintf('Diskret - Control System Toolbox Ta=%0.2fs', Ta);
leg_euler = sprintf('Euler-Vorwaertsverfahren Ta=%0.2fs',Ta);
leg_euler_own = sprintf('Euler-Vorwaertsverfahren Ta=%0.2fs',Ta_own);
leg_euler_own2 = sprintf('Euler-Vorwaertsverfahren Ta=%0.2fs',Ta_own2);
leg_euler_own3 = sprintf('Euler-Vorwaertsverfahren Ta=%0.2fs',Ta_own3);
legend(leg_sys, leg_dstep, leg_euler, leg_euler_own, leg_euler_own2, leg_euler_own3,'Location', 'SouthEast')
axis([0, 5, 0, 0.55])

%Stabilitaet:
%Explizite Verfahren wie der Vorwaerts Euler haben das Problem, dass sie 
%mit zu grossen Abtastperioden zunehmend ungenauer und instabiler werden.
%Eine Alternative waeren impliziten Einzelschrittverfahren, mit denen 
%groessere Abtastperioden moeglich sind, da sie oftmals groessere 
%Stabilitaetsbereiche besitzen.
%
%Betrachtet man die die Abweichungen bei 5sek., sieht man, dass diese sich
%immer weiter verringern, je kleiner die Abtastrate gewaehlt wird.
%
%Bzgl. der Stabilitaet kann gesagt werden:
%x(0)=[0 0], x(k)=x(k-1)+Ta(A *x(k-1)+b*1) 
%   -> x(1)=Ta*b, x(2)=Ta*b+Ta(A*Ta*b+b)=Ta*b(1+A*Ta+1)=Ta*b(2+A*Ta)
%   -> x(3)=Ta*b(3*3A*Ta+Ta^2A^2)
%   -> x(4)=Ta*b(4+6A*Ta+4Ta^2A^2+Ta3A^3)
%   -> x(5)=Ta*b(5+10A*Ta+10*A^2Ta^2+5Ta^3A^3+Ta^4A^4)
%   -> ...
% Damit dieses Verfahren Stabil ist, muss gelten, dass |A*Ta|<1, ansonsten
%   wuerden die x(k) immer groesser werden.
%
%Durch eine kleinere Abtastzeit, wird die Differentialgleichung besser
%approximiert. Bei starken Richtungsaenderungen kann mit diesem
%Eulerverfahren bei zu grossen Abtastzeiten nur schwer mitgehalten werden.
%% Aufgabe 2.8: - Nichtlin. DGL Numerisch loesen
%Reset:
clear all
close all
clc

%Die folgende DGL
%d^3 x/dt^3 + cos(x)^2*d^2 x/dt^2 + dx/dt+e^(-x)*u=0
%
%wird durch einfuehren von Folgenden Vars:
%x1=x             --> dx1/dt=dx/dt=x2
%x2=dx/dt         --> dx2/dt=d^2x/dt^2=x3
%x3=d^2x/dt^2     --> dx3/dt=d^3x/dt^3
%
%umgewandelt in ein DGL-Sys 1. Ordnung:
%d x1/dt=x2
%d x2/dt=x3
%d x3/dt=-(cos(x1)^2 x3 + x2 + e^(-x1)*u)
%
%das in die aequivalente Integraldarstellung gebracht werden kann: (fuer
%details siehe 2.10 in der Angabe) wobei x1,0; x2,0; x3,0 die AB darstellen
%x1=x1,0+int[0 to t](x2)dtau
%x2=x2,0+int[0 to t](x3)dtau
%x3=x3,0-int[0 to t](cos(x1)^2 x3 + x2 + e^(-x1)*u)dtau
x1_0=0
x2_0=0
x3_0=0

%Es ist nur der Plot von x1 relevant, denn das ist die Loesung der DGL.
%% Aufgabe 2.9: - Zweimasseschwinger
%Reset:
clear all
close all
clc
% Fuer nicht linearisiertes Modell siehe sfunc_bsp9_m.m

%Linearisertes Modell:
%parameter (siehe Tabelle 2.2)
m1 = 5;             %Masse 1[kg]
m2 = 1;             %Masse 2[kg]
s10 = 0.35;         %Anfangsposition Masse 1[m]
s120 = 0.5;         %Anfangsposition Masse 2[m]
d1 = 3;             %Daempfer 1[Ns/m]
d12 = 0.15;         %Daempfer 2[Ns/m]
c1 = 50;            %Federkonstante 1[N]
c12 = 5;            %Federkonstante 2[N]
F1R = 30;           %Ruhelage u
F2R = 0;            %Ruhelage v

%Ruhelage vom Linearisierten Modell (benoetigt fuer sfunc_bsp9_m.m):
%(siehe Maple file: 1.12 (Ruhelage))
s1R = asinh((F2R+F1R)/c1)*s10+s10;
s2R = asinh((F2R+F1R)/c1)*s10+asinh(F2R/c12)*s120+s10+s120;
w1R = 0;
w2R = 0;
x0_sfunc=[s1R,w1R,s2R,w2R];%Anfangszustand 

x0_lin=[0,0,0,0];

%benoetigt fuer simulink bloecke:
uR = [F1R;F2R];
yR = asinh(F2R/c12)*s120 + s120;

%siehe AA, BB, CC, DD von Maple file: 1.12
a21 = -(c1*sqrt((F1R^2+2*F1R*F2R+F2R^2+c1^2)/c1^2)*s120+c12*sqrt((F2R^2+c12^2)/c12^2)*s10)/(s10*s120*m1);
a22 = -(d1+d12)/m1;
a23 = c12*sqrt((F2R^2+c12^2)/c12^2)/(s120*m1);
a24 = d12/m1;
a41 = c12*sqrt((F2R^2+c12^2)/c12^2)/(s120*m2);
a42 = d12/m2;
a43 = -c12*sqrt((F2R^2+c12^2)/c12^2)/(s120*m2);
a44 = -d12/m2;
b21 = -1/m1 ;
b42 = -1/m2 ;

A = [0 1 0 0; a21 a22 a23 a24; 0 0 0 1; a41 a42 a43 a44];
B = [0 0; b21 0; 0 0; 0 b42];
C = [-1 0 1 0]; %Achtung! bereits transponiert
D = [0 0];

linsys = ss(A, B, C, D) %nicht C' (siehe oben)
step(linsys, 10)