% Skript suspension_2.m, in dem ein vereinfachtes Modell
% eines Fahrzeugs simuliert wird
% Arbeitet mit dem Modell suspension2.mdl
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
ct = 5000;    % 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-2*a) -c*(L-a) -c*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-2*a) -k*(L-a) k*a;
    k*(L-2*a) 2*k -k -k;
    -k*a -k k+kt 0;
    k*(L-a) -k 0 k+kt];
% ------- Homogene Lösung
[X, e] = polyeig(K,C,M);   % Eigenwerte
[e, k] = sort(e),
X = X(:,k);
% -------- Homogene Lösung
dt = 0.002;
t = 0:dt:1.5;
%p =[1 1 0 0 0 0 0 0]; % Gewichtungsfaktoren für periodischen Anteil_1
p =[1 1 1 1 0 0 0 0]; % Gewichtungsfaktoren für periodischen Anteil_1
                       % plus Anteil_2
%p =[0 0 1 1 0 0 0 0]; % Gewichtungsfaktoren für periodischen Anteil_2     
%p =[0 0 0 0 1 0 0 0]; % Gewichtungsfaktoren für aperiodischen Anteil_1
% ......
%p =[0 0 0 0 0 0 0 1]; % Gewichtungsfaktoren für aperiodischen Anteil_8

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

% ------- Initialisierung des Simulink-Modells
% Rauigkeit der Fahrbahn
x0v0 = [0 0.002 0 0 0.01 0 0 0]';   % Anfangsbedingungen
dt = 0.01;    Tfinal = 50;
S0 = 2e-8;   % Spektrale Leistungsdichte für die 
             % Anregung in m^2/Hz bei einer Geschwindigkeit von 30 m/s
varianz = S0/dt
v = 30;             % Geschwindigkeit m/s
delay = (L/v);
% -------- Übertragungsfunktionen von Y und Z zu X
Py = [0;0;ct*s+kt;0];    % Anregungen
Pz = [0;0;0;ct*s+kt];
Hy = inv(s^2*M + s*C + K)*Py; % Übertragungsfunktion von Y zu X
Hz = inv(s^2*M + s*C + K)*Pz; % Übertragungsfunktion von Z zu X

[b, a] = tfdata(Hy);    % liefert Zellen
b11y = b{1,:};           % Koeffizienten des Zählers von Y zu Winkel
b12y = b{2,:};           % Koeffizienten des Zählers von Y zu Lage x1
b13y = b{3,:};           % Koeffizienten des Zählers von Y zu Lage x2
b14y = b{4,:};           % Koeffizienten des Zählers von Y zu Lage x3

a11y = a{1,:};           % Koeffizienten des Nenners von Y zu Winkel
                         % Die anderen sind alle gleich
[b, a] = tfdata(Hz);    % liefert Zellen
b11z = b{1,:};           % Koeffizienten des Zählers von Z zu Winkel
b12z = b{2,:};           % Koeffizienten des Zählers von Z zu Lage x1
b13z = b{3,:};           % Koeffizienten des Zählers von Z zu Lage x2
b14z = b{4,:};           % Koeffizienten des Zählers von Z zu Lage x3

a11z = a{1,:};           % Koeffizienten des Nenners von Z zu Winkel
                         % Die anderen sind sind alle gleich
% -------- Aufruf der Simulation
sim('suspension2',[0:dt:100]);

t = y.time;
winkel = y.signals.values(:,1);  % Winkel des Chassis (Masse ms)
x1 = y.signals.values(:,2);      % Lage des Chassis
x2 = y.signals.values(:,3);      % Lage der vorderen Achse
x3 = y.signals.values(:,4);      % Lage der hinteren Achse

figure(2);    clf;
nd = 1:500;
subplot(211), plot(t(nd), winkel(nd));
hold on, plot(t(nd), x1(nd),'r');
title('Winkel und Lagekoordinate des Chassis'); grid on;
xlabel('Zeit in s');    hold off;
subplot(212), plot(t(nd), x2(nd));
hold on, plot(t(nd), x3(nd),'r');
title('Lagekoordinaten der Achsen'); grid on;
xlabel('Zeit in s'); hold off;

% -------- Frequenzgänge mit bode
f = logspace(-1,2,100);
[betrag_x, phase_x, w] = bode(Hy, 2*pi*f);  % Y zu X

figure(3);    clf;
subplot(211), semilogx(f, 20*log10(squeeze(betrag_x)));
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_x));
axis tight;   grid on;
title('Phasengänge für Anregung y');    xlabel('Hz');
legend('Winkel', 'Lage x1', 'Lage x2', 'Lage x3');

[betrag_x, phase_x, w] = bode(Hz, 2*pi*f);  % Z zu X

figure(4);    clf;
subplot(211), semilogx(f, 20*log10(squeeze(betrag_x)));
axis tight;   grid on;
title('Amplitudengänge für Anregung z');    xlabel('Hz');
legend('Winkel', 'Lage x1', 'Lage x2', 'Lage x3');

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