% Skript suspension_3.m, in dem ein vereinfachtes Modell
% eines Fahrzeugs mit Zufallsanregung simuliert wird
% Arbeitet mit dem Modell suspension3.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);

% ------- 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 alle gleich
% -------- Aufruf der Simulation
sim('suspension3',[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(1);    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;
hold off;
subplot(212), plot(t(nd), x2(nd));
hold on, plot(t(nd), x3(nd),'r');
title('Lagekoordinatenn der Achsen'); grid on;
hold off;

% -------- Spektrale Leistungsdichten
fs = 1/dt;
h = spectrum.welch;
Htheta = psd(h, winkel, 'Fs', fs);
Hx1    = psd(h, x1, 'Fs', fs);
Hx2    = psd(h, x2, 'Fs', fs);
Hx3    = psd(h, x3, 'Fs', fs);

Htheta = Htheta.data;    Hx1 = Hx1.data; 
Hx2 =    Hx2.data;       Hx3 = Hx3.data; 
nx = length(Htheta);

figure(2);     clf;
plot((0:nx-1)*fs/(2*(nx-1)), 10*log10(Htheta),'*');
hold on;
plot((0:nx-1)*fs/(2*(nx-1)), 10*log10(Hx1),'.');
plot((0:nx-1)*fs/(2*(nx-1)), 10*log10(Hx2),'x');
plot((0:nx-1)*fs/(2*(nx-1)), 10*log10(Hx3),'o');
title('Spektrale Leistungsdichten');
xlabel('Hz');    ylabel('dB m^2/Hz');    grid on;
legend('Winkel \theta', 'x1','x2','x3');
hold off;


