% Skript suspension_1.m, in dem ein vereinfachtes Modell
% eines Fahrzeugs simuliert wird
clear;
s = tf('s');

% ------- Parameter des Systems
ms = 300;   % kg
ma = 25;    % kg
I = 225;    % kg m^2
L = 3;      % m
a = 1.7;    % m
%c = 1200;    % N s/m  % laut Graham Kelly Seite 506
%ct = 10000;  % N s/m
c = 1200;     % N s/m
ct = 5000.0; % N s/m

k = 12000;  % N/m
kt = 100000;  % N/m

% ------- Matrizen des Modells
M = [I 0 0 0;0 ms 0 0;0 0 ma 0;0 0 0 ma];

C = [c*(a^2 + (L-a)^2) c*L -c*a c*(L-a);
    c*(L-2*a) 2*c -c -c;
    -c*a -c c+ct 0; 
    c*(L-a) -c 0 c+ct];

K = [k*(a^2+(L-a)^2) k*L -k*a k*(L-a);
    k*(L-2*a) 2*k -k -k;
    -k*a -k k+kt 0;
    k*(L-a) -k 0 k+kt];
% -------- Übertragungsfunktionen von Y und Z
Py = [0;0;ct*s+kt;0];    % Anregungen
Pz = [0;0;0;ct*s+kt];

Hy = inv(s^2*M + s*C + K)*Py;
Hz = inv(s^2*M + s*C + K)*Pz;
Hy1 = minreal(Hy);     Hz1 = minreal(Hz);  % Minimale Realisierung 
% Gleiche Pole- und Nullstellen kürzen

% -------- Sprungantwort
dt = 0.01;                         % Zeitschrittweite
[sy,t] = step(Hy,[0:dt:6]);      % Sprungantwort von Y Anregung

figure(1);   clf;
subplot(211), plot(t,sy(:,1:2));
title('Sprungantwort für Winkel und Lage Masse 1  mit Anregung Y');
xlabel('Zeit in s');    grid on;

nt = length(t);
nd = fix(nt/5);
subplot(212), plot(t(1:nd),sy((1:nd),3:4));
title('Sprungantwort für Lage Masse 2 und Lage Masse 3  mit Anregung Y');
xlabel('Zeit in s');    grid on;    axis tight

[sz,t] = step(Hz,[0:dt:6]);      % Sprungantwort von Z Anregung

figure(2);   clf;
subplot(211), plot(t,sz(:,1:2));
title('Sprungantwort für Winkel und Lage Masse 1   mit Anregung Z');
xlabel('Zeit in s');    grid on;

nt = length(t);
nd = fix(nt/5);
subplot(212), plot(t(1:nd),sz((1:nd),3:4));
title('Sprungantwort für Lage Masse 2 und Lage Masse 3   mit Anregung Z');
xlabel('Zeit in s');    grid on;    axis tight

% -------- Frequenzgänge mit bode
[betrag_y, phase_y, w] = bode(Hy, 2*pi*logspace(-1,2));

figure(3);    clf;
subplot(211), semilogx(w/(2*pi), 20*log10(squeeze(betrag_y)));
axis tight;   grid on;
title('Amplitudengänge für Anregung Y');    xlabel('Hz');
legend('Winkel', 'Lage x1', 'Lage x2', 'Lage x3');

subplot(212), semilogx(w/(2*pi), squeeze(phase_y));
axis tight;   grid on;
title('Phasengänge für Anregung Y');    xlabel('Hz');
legend('Winkel', 'Lage x1', 'Lage x2', 'Lage x3');

% -------- Eigenwerte
[X, e] = polyeig(K,C,M),

% Natürliche Frequenzen
f1 = abs(imag(e(1)))/(2*pi),
f2 = abs(imag(e(3)))/(2*pi),
f3 = abs(imag(e(5)))/(2*pi),
f4 = abs(imag(e(7)))/(2*pi),

% -------- Polynome des Zählers und Nenners
[b, a] = tfdata(Hy);    % liefert Zellen

b11 = b{1,:};           % Koeffizienten des Zählers von Y zu Winkel
b12 = b{2,:};           % Koeffizienten des Zählers von Y zu Lage x1
b13 = b{3,:};           % Koeffizienten des Zählers von Y zu Lage x2
b14 = b{4,:};           % Koeffizienten des Zählers von Y zu Lage x3

a11 = a{1,:};           % Koeffizienten des Nenners von Y zu Winkel
a12 = a{2,:};           % Koeffizienten des Nenners von Y zu Lage x1
a13 = a{3,:};           % Koeffizienten des Nenners von Y zu Lage x2
a14 = a{4,:};           % Koeffizienten des Nenners von Y zu Lage x3

pole = roots(a11),      % Pole der Übertragungsfunktion

% -------- Antwort über lsim
my_sys = tf(b14, a14);    % Für x3 wegen Y
%my_sys = tf(b13, a13);   % Für x2 wegen Y
%my_sys = tf(b12, a12);   % Für x1 wegen Y

% angenähert als digitale Filterung
ny = 10000;     dt = 0.01;
fsig = 0.2;
u = 0.02*cos(2*pi*(0:ny-1)*dt*fsig);
x = lsim(my_sys, u', [0:ny-1]'*dt);

nd = round(10/(fsig*dt));     % Ausschnitt für die Darstellung
figure(4);      clf;
plot((0:nd-1)*dt, u(1:nd));
hold on;     
plot((0:nd-1)*dt,x(1:nd),'r');
title(['Antwort Lage x3; fsig = ',num2str(fsig),' Hz']);
xlabel('Zeit in s');
grid on;     hold off;

% -------- Homogene Lösung
dt = 0.002;
t = 0:dt:1.5;
x0 =[1 1 1 1]';      % Anfangsbedingungen
p = inv([X(:,1)+X(:,2), X(:,3)+X(:,4),...
         X(:,5)+X(:,6), X(:,7)+X(:,8)])*x0; % Gewichtung der Mode

xh = p(1)*(X(:,1)*exp(e(1)*t) + X(:,2)*exp(e(2)*t))+...
     p(2)*(X(:,3)*exp(e(3)*t) + X(:,4)*exp(e(4)*t))+...
     p(3)*(X(:,5)*exp(e(5)*t) + X(:,6)*exp(e(6)*t))+...
     p(4)*(X(:,7)*exp(e(7)*t) + X(:,8)*exp(e(8)*t));
 
figure(5);    clf;
plot(t, xh);
title('Homogene Lösung');
xlabel('Zeit in s');   grid on;
legend('Winkel', 'x1', 'x2', 'x3');



