clear all 
clc
%Radkasten : Index k
%Zugaufbau : Index z
%Verschiebungsvektor: q = [xk1, yk1, zk1, xk2, yk2, zk2, xz, yz, zz,
%alphak1, betak1, gammak1, alphak2, betak2, gammak2, alphaz, betaz, gammaz]'

%% Abmessungen
sk = 1.435/2;    %halbe Spurweite
rk = 1.8/2;      %halber Radstand Y25 Drehgestell
Hk = 4.5;        %Zughoehe
                 %Zugaufbau : Index z
sz = sk; %2.8/2; %Zugbreite ICE: 3m;
rz = 1.8/2;      %Vertikale Federn uebereinander
hz = 1.7;        %halbe Zughoehe
Hz = 2*hz;       %4m Zughoehe minus 1m Raddurchmesser 
az = 28.75/2;    %halbe Laenge Mittelwagen ICE4

%% Schleife zum Plotten der Parameterabhaengigkeiten und der Wurzelortskurven

%Parameter fuer die Schleifen
anfangsgeschwindigkeit = 1; 
endgeschwindigkeit = 500; 
anfangsdamp = 1e2;
enddamp = 1e5;
anfangssteifigkeit = 1e2;
endsteifigkeit = 5e6;

%festlegen der Anzahl an ueberprueften Parametern, fuer d_pri und k_pri gleich viele
Schritte = 27; 
Schrittweite_damp = (enddamp-anfangsdamp)/Schritte; 
Schrittweite_stif = (endsteifigkeit-anfangssteifigkeit)/Schritte; 

j=1;
aver_ampl = zeros(Schritte);
Vari_k = zeros(Schritte,1); 
Vari_d = zeros(Schritte,1);

for srt = 0:Schritte
Vari_k(1+srt,1) = anfangssteifigkeit+srt*Schrittweite_stif;
end

%Matrizen, in denen die Ergenisse der EW-Berechnung gespeichert werden
ZR.Mat = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit));
ZR.Mat_real = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit)); 
ZR.Mat_real = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit));

poly.Mat = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit));
poly.Mat_real = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit)); 
poly.Mat_real = zeros(size(endgeschwindigkeit-anfangsgeschwindigkeit));
%3d-Array zum Speichern der Eigenvektoren fuer jede Geschwindigkeit
poly.Array_Ev = zeros(42,84); % hier noch 2D

%%
%for d_pri = anfangsdamp:Schrittweite_damp:enddamp
    f=1; 
 % for   k_pri = anfangssteifigkeit:Schrittweite_stif:endsteifigkeit
 
 for v=anfangsgeschwindigkeit:endgeschwindigkeit

%% Parameter fuer die Berechnung der Kontaktkraefte nach Vorgehen von B.Abdelfattah
a = 45;         %Kraftschlusskoeffizient in Laengsrichtung
b = 116;        % Kraftschlusskoeffizient in Querrichtung
m = 972;        %Masse eines Radsatzes.   
Jz = 484.75; 
Jy = 29.5; 
sp = 2*sk;      %Stuetzweite
r0 = 0.5; 
xi = 15;        %[1/m] Koeffizient der Profilsteifigkeit, =0 fuer kegeliges Profil
gamma = 1/40;   %wirksame Konizizaet
k_pri = 1e6; 
d_pri = 1e3; 

% Vektoren zur Trennung einzenler Freiheitsgrade (siehe Modell 7) hier
% nicht mehr notwendig, da Kraefte nach B.A in Matrixen enthalten sind

%% Simulationsdauer
Simulationsdauer = 12; %[s]

%% Stoerungen
Stoer_FHG_rs = zeros(42,1); 
Stoer_FHG_rs(20,1) = 1;         %yrs12
Stoer_FHG_rs(25,1) = 1;         %yrs34
Stoer_FHG_rs(30,1) = 1;         %yrs56
Stoer_FHG_rs(35,1) = 1;         %yrs78
Stoer_ampl_rs = 0;              %Amplitude der Stoerung
Stoer_FHG_z = zeros(42,1); 
Stoer_FHG_z(8,1) = 1;           %yz
Stoer_ampl_z = 0;               %Amplitude der Stoerung

%% Massen und Traegheiten
mz = 18500;                 %Normale Beladung Wagon ICE4
%mz = 25000;                %Gueterwagen
mr = 972; %1000;            %Masse Radsatz
mk = 4000;                  %Masse Drehgestell
Jka = mk/12*((2*sk)^2+(2*rk)^2); 
Jkb = mk/12*((2*sk)^2+(Hk)^2); 
Jkg = mk/12*((Hk)^2+(2*rk)^2); 
Jza = mz/12*((2*sz)^2+(2*az)^2)+mz*az^2; 
Jzb = Jz; 
Jzg = Jz;  
Jr = Jz;         %Massentraegheit fuer Wanken und Gieren bei Radsaetzen identisch
massenvektor = [mk; mk; mk; mk; mk; mk; mz; mz; mz; Jka; Jkb; Jkg; Jka; Jkb; Jkg; Jza; Jzb; Jzg; mr; mr; mr; Jr; Jr; mr; mr; mr; Jr; Jr; mr; mr; mr; Jr; Jr; mr; mr; mr; Jr; Jr; Jy; Jy; Jy; Jy];
M = diag(massenvektor);
M_inv = inv(M);
m_ges = 2*mk+mz+4*mr;
Gr = (9.81*m_ges/8);        % Radaufstandskraft aus B.Abdelfattah

%% Steifigkeitsmatrizen
%x-Richtung
k.kx11=5.2e7;
k.kx21=5.2e7;
k.kx31=5.2e7;
k.kx41=5.2e7;
k.kx51=5.2e7;
k.kx61=5.2e7;
k.kx71=5.2e7;
k.kx81=5.2e7;
%Sekundaer
k.kx12=4e5;
k.kx22=4e5;
k.kx32=4e5;
k.kx42=4e5;
k.kx52=4e5;
k.kx62=4e5;
k.kx72=4e5;
k.kx82=4e5;

%y-Richtung
%Primaer
k.ky11=7e6;
k.ky21=7e6;
k.ky31=7e6;
k.ky41=7e6;
k.ky51=7e6;
k.ky61=7e6;
k.ky71=7e6;
k.ky81=7e6;

%Sekundaer
k.ky12=4e5;
k.ky22=4e5;
k.ky32=4e5;
k.ky42=4e5;
k.ky52=4e5;
k.ky62=4e5;
k.ky72=4e5;
k.ky82=4e5;

%z-Richtung
%Primaer
k.kz11=1e6;
k.kz21=1e6;
k.kz31=1e6;
k.kz41=1e6;
k.kz51=1e6;
k.kz61=1e6;
k.kz71=1e6;
k.kz81=1e6;

%Sekundaer
k.kz12=4*1e5;
k.kz22=4*1e5;
k.kz32=4*1e5;
k.kz42=4*1e5;
k.kz52=4*1e5;
k.kz62=4*1e5;
k.kz72=4*1e5;
k.kz82=4*1e5;

%% Steifigkeitsmatrix
%Translation
K_trans = zeros(9,9);

%x-Richtung Radkasten 1
K_trans(1,1)=k.kx12+k.kx22+k.kx32+k.kx42+k.kx11+k.kx21+k.kx31+k.kx41;
K_trans(1,7)=-(k.kx12+k.kx22+k.kx32+k.kx42);
K_trans(7,7)=k.kx12+k.kx22+k.kx32+k.kx42+k.kx52+k.kx62+k.kx72+k.kx82;
%x-Richtung Radkasten 2
K_trans(4,4)=k.kx52+k.kx62+k.kx72+k.kx82+k.kx51+k.kx61+k.kx71+k.kx81;
K_trans(4,7)=-(k.kx52+k.kx62+k.kx72+k.kx82);

%y-Richtung Radkasten 1
K_trans(2,2)=k.ky11+k.ky12+k.ky21+k.ky22+k.ky31+k.ky32+k.ky41+k.ky42;
K_trans(2,8)=-(k.ky12+k.ky22+k.ky32+k.ky42);
K_trans(8,8)=k.ky12+k.ky22+k.ky32+k.ky42+k.ky52+k.ky62+k.ky72+k.ky82;
%y-Richtung Radkasten 2
K_trans(5,5)=k.ky51+k.ky52+k.ky61+k.ky62+k.ky71+k.ky72+k.ky81+k.ky82;
K_trans(5,8)=-(k.ky52+k.ky62+k.ky72+k.ky82);

%z-Richtung Radkasten 1
K_trans(3,3)=k.kz11+k.kz21+k.kz31+k.kz41+k.kz12+k.kz22+k.kz32+k.kz42;
K_trans(3,9)=-(k.kz12+k.kz22+k.kz32+k.kz42);
K_trans(9,9)=k.kz12+k.kz22+k.kz32+k.kz42+k.kz52+k.kz62+k.kz72+k.kz82;
%z-Richtung Radkasten 2
K_trans(6,6)=k.kz51+k.kz61+k.kz71+k.kz81+k.kz52+k.kz62+k.kz72+k.kz82;
K_trans(6,9)=-(k.kz52+k.kz62+k.kz72+k.kz82);

% Aufstellen der Steifikeitsmatrix
K = [K_trans, zeros(9,33);
    zeros(33,42)];

%Rotatorische Steifigkeiten
%Radkasten 1
%Gieren / alpha
K(10,10)=(sk^2)*(k.kx12+k.kx22+k.kx32+k.kx42+k.kx11+k.kx21+k.kx31+k.kx41)+(rk^2)*(k.ky12+k.ky22+k.ky32+k.ky42+k.ky11+k.ky21+k.ky31+k.ky41);
K(10,16)=rk*az*(-k.ky12-k.ky22)+rk*(az-2*rk)*(+k.ky32+k.ky42)+sz*sk*(-k.kx12+k.kx22-k.kx32+k.kx42); 
K(16,16)=az^2*(k.ky12+k.ky22)+(az-2*rk)^2*(k.ky32+k.ky42)+(az-2*rk)^2*(k.ky52+k.ky62)+az^2*(k.ky82+k.ky72)+sz^2*(k.kx12+k.kx22+k.kx32+k.kx42+k.kx52+k.kx62+k.kx72+k.kx82);

%Wanken / beta
K(11,11)=(sk^2)*(k.kz11+k.kz12+k.kz21+k.kz22+k.kz31+k.kz32+k.kz41+k.kz42);
K(11,17)=sk*sz*(-k.kz12-k.kz22-k.kz32-k.kz42);
K(17,17)=(sz^2)*(k.kz12+k.kz22+k.kz32+k.kz42)+(sz^2)*(k.kz52+k.kz62+k.kz72+k.kz82)+(hz^2)*(k.ky12+k.ky22+k.ky32+k.ky42+k.ky52+k.ky62+k.ky72+k.ky82);

%Nicken / gamma
K(12,12)=(rk^2)*(k.kz11+k.kz12+k.kz21+k.kz22+k.kz31+k.kz32+k.kz41+k.kz42);
K(12,18)=rk*az*(-k.kz12-k.kz22)+rk*(az-2*rk)*(k.kz32+k.kz42);
K(18,18)=(az^2)*(k.kz12+k.kz22)+(az-2*rz)^2*(k.kz32+k.kz42)+az^2*(k.kz62+k.kz82)+(az-2*rk)^2*(k.kz52+k.kz72)+hz^2*(k.kx12+k.kx22+k.kx32+k.kx42+k.kx72+k.kx82+k.kx52+k.kx62);

%Radkasten 2
%Gieren / alpha
K(13,13)=(sk^2)*(k.kx52+k.kx62+k.kx72+k.kx82+k.kx51+k.kx61+k.kx71+k.kx81)+(rk^2)*(k.ky51+k.ky61+k.ky71+k.ky81+k.ky52+k.ky62+k.ky72+k.ky82);
K(13,16)=rk*az*(-k.ky72-k.ky82)+rk*(az-2*rk)*(+k.ky52+k.ky62)+sz*sk*(+k.kx72+k.kx82-k.kx52-k.kx62);
%K(16,16)=(Az-2*rk)^2*(k.ky52+k.ky62)+Az^2*(k.ky82+k.ky72);

%Wanken / beta
K(14,14)=(sk^2)*(k.kz51+k.kz52+k.kz61+k.kz62+k.kz71+k.kz72+k.kz81+k.kz82);
K(14,17)=sk*sz*(-k.kz52-k.kz62-k.kz72-k.kz82);
%K(17,17)=(sz^2)*(k.kz52+k.kz62+k.kz72+k.kz82);

%Nicken / gamma
K(15,15)=(rk^2)*(k.kz51+k.kz52+k.kz61+k.kz62+k.kz71+k.kz72+k.kz81+k.kz82);
K(15,18)=rk*az*(-k.kz82-k.kz72)+rk*(az-2*rk)*(+k.kz52+k.kz62);
%K(18,18)=Az*(k.kz62+k.kz82)^2+(Az-2*rk)^2*(k.kz52+k.kz72);

%Nebendiagonalen
K(1,10)=sk*(-k.kx32-k.kx31+k.kx22+k.kx21-k.kx12-k.kx11+k.kx42+k.kx41);
K(1,18)=hz*(-k.kx12-k.kx22+k.kx32+k.kx42);

K(2,10)=rk*(+k.ky41+k.ky42-k.ky11-k.ky12-k.ky21-k.ky22+k.ky31+k.ky32);
K(2,16)=az*(k.ky12+k.ky22)+(az-2*rk)*(k.ky32+k.ky42);
K(2,17)=hz*(-k.ky12-k.ky32+k.ky42+k.ky22);

K(3,11)=sk*(k.kz11+k.kz31-k.kz21-k.kz41+k.kz12+k.kz32-k.kz22-k.kz42);
K(3,12)=rk*(-k.kz31-k.kz41-k.kz32-k.kz42+k.kz11+k.kz21+k.kz12+k.kz22);
K(3,17)=sk*(-k.kz12-k.kz32+k.kz22+k.kz42);
K(3,18)=az*(-k.kz12-k.kz22)+(az-2*rk)*(-k.kz32-k.kz42);

K(4,13)=sk*(-k.kx52+k.kx82+k.kx62-k.kx72-k.kx51+k.kx81+k.kx61-k.kx71);
K(4,18)=hz*(-k.kx82-k.kx72+k.kx52+k.kx62);

K(5,13)=rk*(k.ky71+k.ky72-k.ky61-k.ky62-k.ky51-k.ky52+k.ky81+k.ky82);
K(5,16)=az*(-k.ky82-k.ky72)+(az-2*rk)*(-k.ky62-k.ky52);
K(5,17)=hz*(-k.ky52-k.ky72+k.ky62+k.ky82);

K(6,14)=sk*(k.kz51-k.kz52-k.kz72+k.kz71-k.kz61+k.kz62-k.kz81+k.kz82);
K(6,15)=rk*(k.kz51+k.kz61-k.kz71-k.kz81+k.kz52+k.kz62-k.kz72-k.kz82);
K(6,17)=sk*(-k.kz52-k.kz72+k.kz62+k.kz82);
K(6,18)=az*(k.kz72+k.kz82)+(az-2*rk)*(+k.kz52+k.kz62);

K(7,10)=sk*(-k.kx22-k.kx32+k.kx12+k.kx42);
%K(7,12)=hz*(k.kx12+k.kx22-k.kx32-k.kx42);%hz falsch, Hoehe Radkasten vernachlaesigt
K(7,13)=sk*(+k.kx72+k.kx62-k.kx82-k.kx52);
%K(7,15)=hz*(-k.kx72-k.kx82+k.kx52+k.kx62);hz falsch, Hoehe Radkasten vernachlaessigt
K(7,16)=sk*(-k.kx22+k.kx12+k.kx32-k.kx42-k.kx52+k.kx62-k.kx72+k.kx82);%? aber null

K(8,10)=rk*(+k.ky12-k.ky42+k.ky22-k.ky32);
K(8,11)=hz*(k.ky12+k.ky32-k.ky22-k.ky42);
K(8,13)=rk*(+k.ky52-k.ky82+k.ky62-k.ky72);
K(8,16)=az*(-k.ky22-k.ky12+k.ky72+k.ky82)+(az-2*rk)*(-k.ky42-k.ky32+k.ky52+k.ky62);
K(8,17)=hz*(-k.ky22-k.ky42-k.ky62-k.ky82+k.ky12+k.ky32+k.ky52+k.ky72);

K(9,11)=sk*(-k.kz12-k.kz32+k.kz22+k.kz42);
K(9,12)=rk*(-k.kz12-k.kz22+k.kz32+k.kz42);
K(9,14)=sk*(-k.kz52-k.kz72+k.kz62+k.kz82);
K(9,15)=rk*(k.kz72+k.kz82-k.kz52-k.kz62);
K(9,17)=sk*(k.kz12+k.kz32+k.kz52+k.kz72-k.kz22-k.kz42-k.kz62-k.kz82);
K(9,18)=az*(k.kz12+k.kz22-k.kz72-k.kz82)+(az-2*rk)*(k.kz32+k.kz42-k.kz52-k.kz62);

K(10,17)=hz*rk*(-k.ky22+k.ky42-k.ky12+k.ky32);
K(11,18)=sk*(az*(-k.kz12+k.kz22)+(az-2*rk)*(-k.kz32+k.kz42));
K(12,17)=sk*rk*(-k.kz12+k.kz32+k.kz22-k.kz42);
K(13,17)=rk*hz*(-k.ky62+k.ky82-k.ky52+k.ky72);
K(14,18)=sk*(az*(k.kz72-k.kz82)+(az-2*rk)*(k.kz52-k.kz62));
K(15,17)=rk*sk*(-k.kz52+k.kz72+k.kz62-k.kz82);
K(16,17)=az*hz*(k.ky12+k.ky32+k.ky52+k.ky72-k.ky22-k.ky42-k.ky62-k.ky82); %Az nicht richtig (Az-2*rk)... aber gleich null
K(17,18)=sz*(az*(+k.kz12-k.kz22-k.kz72+k.kz82)+(az-2*rk)*(k.kz32-k.kz42+k.kz52-k.kz62));

%% Erweiterung um die FHGs der Randsaetze
%Translationen Radsatz 1
K(19,19)=k.kx11+k.kx21;
K(1,19)=-(k.kx11+k.kx21);
K(20,20)=k.ky11+k.ky21;
K(2,20)=-(k.ky11+k.ky21);
K(21,21)=k.kz11+k.kz21;
K(3,21)=-(k.kz11+k.kz21);
%Translation Radsatz 2
K(24,24)=k.kx31+k.kx41;
K(1,24)=-(k.kx31+k.kx41);
K(25,25)=k.ky31+k.ky41;
K(2,25)=-(k.ky31+k.ky41);
K(26,26)=k.kz31+k.kz41;
K(3,26)=-(k.kz31+k.kz41);
%Translation Radsatz 3
K(29,29)=k.kx51+k.kx61;
K(4,29)=-(k.kx51+k.kx61);
K(30,30)=k.ky51+k.ky61;
K(5,30)=-(k.ky51+k.ky61);
K(31,31)=k.kz51+k.kz61;
K(6,31)=-(k.kz51+k.kz61);
%Translation Radsatz 4
K(34,34)=k.kx71+k.kx81;
K(4,34)=-(k.kx71+k.kx81);
K(35,35)=k.ky71+k.ky81;
K(5,35)=-(k.ky71+k.ky81);
K(36,36)=k.kz71+k.kz81;
K(6,36)=-(k.kz71+k.kz81);

%Rotationen Radsatz 1
%alpha
K(22,22)=sk^2*(k.kx11+k.kx21);
K(10,22)=sk^2*(-k.kx11-k.kx21);
%beta
K(23,23)=sk^2*(k.kz11+k.kz21);
K(11,23)=sk^2*(-k.kz11-k.kz21);
%yR1-alphak1
K(10,20)=rk*(k.ky11+k.ky21);
%zR1-betak1
K(11,21)=sk*(k.kz21-k.kz11);
%zR1-gammak1
K(12,21)=rk*(-k.kz11-k.kz21);

%Rotationen Radsatz 2
%alpha
K(27,27)=sk^2*(k.kx31+k.kx41);
K(10,27)=-sk^2*(k.kx31+k.kx41);
%beta
K(28,28)=sk^2*(k.kz31+k.kz41);
K(11,28)=-sk^2*(k.kz31+k.kz41);
%yR2-alphak1
K(10,25)=rk*(-k.ky31-k.ky41);
%zR2-betak1
K(11,26)=sk*(+k.kz41-k.kz31);
%zR2-gammak1
K(12,26)=rk*(k.kz31+k.kz41);

%Rotationen Radsatz 3
%alpha
K(32,32)=sk^2*(k.kx51+k.kx61);
K(13,32)=-sk^2*(k.kx51+k.kx61);
%beta
K(33,33)=sk^2*(k.kz51+k.kz61);
K(14,33)=-sk^2*(k.kz51+k.kz61);
%yR3-alphak2
K(13,30)=rk*(+k.ky51+k.ky61);
%zR3-betak2
K(14,31)=sk*(+k.kz41-k.kz31);
%zR3-gammak2
K(15,31)=rk*(-k.kz51-k.kz61);

%Rotationen Radsatz 4
%alpha
K(37,37)=sk^2*(k.kx71+k.kx81);
K(13,37)=-sk^2*(k.kx71+k.kx81);
%beta
K(38,38)=sk^2*(k.kz71+k.kz81);
K(14,38)=-sk^2*(k.kz71+k.kz81);
%yR4-alphak2
K(13,35)=rk*(-k.ky71-k.ky81);
%zR4-betak2
K(14,36)=sk*(+k.kz81-k.kz71);
%zR4-gammak2
K(15,36)=rk*(+k.kz71+k.kz81);

%K ist symmetrischen -> spiegeln an der Diagonalen
K=K+K'-(eye(42,42).*diag(K));

%Zusammenfassen der nichtliearen Kraefte aus B.A. zu K_nl
K_nl = zeros(42);
%y,y
K_nl(20,20)=+2*Gr*xi;
K_nl(25,25)=+2*Gr*xi;
K_nl(30,30)=+2*Gr*xi;
K_nl(35,35)=+2*Gr*xi;
%y,alpha
K_nl(20,22)=-2*Gr*b;
K_nl(25,27)=-2*Gr*b;
K_nl(30,32)=-2*Gr*b;
K_nl(35,37)=-2*Gr*b;
%alpha,y
K_nl(22,20)=+Gr*a*sp*tan(gamma)/r0;
K_nl(27,25)=+Gr*a*sp*tan(gamma)/r0;
K_nl(32,30)=+Gr*a*sp*tan(gamma)/r0;
K_nl(37,35)=+Gr*a*sp*tan(gamma)/r0;
%delta_omea-y_p
K_nl(39,20)=-Gr*a*tan(gamma);
K_nl(40,25)=-Gr*a*tan(gamma);
K_nl(41,30)=-Gr*a*tan(gamma);
K_nl(42,35)=-Gr*a*tan(gamma);

%Zusammenlegen der Systemstifiegkeitsmatrix und der Nl-Steifigkeitsmatrix
K = K+K_nl;

%% Daempfungen
%x-Richtung
d.dx11=1.2e4; %100
d.dx21=1.2e4;
d.dx31=1.2e4;
d.dx41=1.2e4;
d.dx51=1.2e4;
d.dx61=1.2e4;
d.dx71=1.2e4;
d.dx81=1.2e4;

%Sekundaer
d.dx12=5e3;
d.dx22=5e3;
d.dx32=5e3;
d.dx42=5e3;
d.dx52=5e3;
d.dx62=5e3;
d.dx72=5e3;
d.dx82=5e3;

%y-Richtung

d.dy11=1.2*1e4;
d.dy21=1.2*1e4;
d.dy31=1.2*1e4;
d.dy41=1.2*1e4;
d.dy51=1.2*1e4;
d.dy61=1.2*1e4;
d.dy71=1.2*1e4;
d.dy81=1.2*1e4;

%Sekundaer
d.dy12=5e3;
d.dy22=5e3;
d.dy32=5e3;
d.dy42=5e3;
d.dy52=5e3;
d.dy62=5e3;
d.dy72=5e3;
d.dy82=5e3;

%z-Richtung
d.dz11=1.2*1e4;
d.dz21=1.2*1e4;
d.dz31=1.2*1e4;
d.dz41=1.2*1e4;
d.dz51=1.2*1e4;
d.dz61=1.2*1e4;
d.dz71=1.2*1e4;
d.dz81=1.2*1e4;

% Sekundaer
d.dz12=5e3; 
d.dz22=5e3;
d.dz32=5e3;
d.dz42=5e3;
d.dz52=5e3;
d.dz62=5e3;
d.dz72=5e3;
d.dz82=5e3;

%% Daempfungsmatrizen

%Translation
D_trans = zeros(9,9);

%x-Richtung Radkasten 1
D_trans(1,1)=d.dx12+d.dx22+d.dx32+d.dx42+d.dx11+d.dx21+d.dx31+d.dx41;
D_trans(1,7)=-(d.dx12+d.dx22+d.dx32+d.dx42);
D_trans(7,7)=d.dx12+d.dx22+d.dx32+d.dx42+d.dx52+d.dx62+d.dx72+d.dx82;
%x-Richtung Radkasten 2
D_trans(4,4)=d.dx52+d.dx62+d.dx72+d.dx82+d.dx51+d.dx61+d.dx71+d.dx81;
D_trans(4,7)=-(d.dx52+d.dx62+d.dx72+d.dx82);

%y-Richtung Radkasten 1
D_trans(2,2)=d.dy11+d.dy12+d.dy21+d.dy22+d.dy31+d.dy32+d.dy41+d.dy42;
D_trans(2,8)=-(d.dy12+d.dy22+d.dy32+d.dy42);
D_trans(8,8)=d.dy12+d.dy22+d.dy32+d.dy42+d.dy52+d.dy62+d.dy72+d.dy82;
%y-Richtung Radkasten 2
D_trans(5,5)=d.dy51+d.dy52+d.dy61+d.dy62+d.dy71+d.dy72+d.dy81+d.dy82;
D_trans(5,8)=-(d.dy52+d.dy62+d.dy72+d.dy82);

%z-Richtung Radkasten 1
D_trans(3,3)=d.dz11+d.dz21+d.dz31+d.dz41+d.dz12+d.dz22+d.dz32+d.dz42;
D_trans(3,9)=-(d.dz12+d.dz22+d.dz32+d.dz42);
D_trans(9,9)=d.dz12+d.dz22+d.dz32+d.dz42+d.dz52+d.dz62+d.dz72+d.dz82;
%z-Richtung Radkasten 2
D_trans(6,6)=d.dz51+d.dz61+d.dz71+d.dz81+d.dz52+d.dz62+d.dz72+d.dz82;
D_trans(6,9)=-(d.dz52+d.dz62+d.dz72+d.dz82);

% Aufstellen der Daempfungsmatrix
D = [D_trans, zeros(9,33);
    zeros(33,42)];

%Rotatorische Daempfungen
%Radkasten 1
%Gieren / alpha
D(10,10)=(sk^2)*(d.dx12+d.dx22+d.dx32+d.dx42+d.dx11+d.dx21+d.dx31+d.dx41)+(rk^2)*(d.dy12+d.dy22+d.dy32+d.dy42+d.dy11+d.dy21+d.dy31+d.dy41);
D(10,16)=rk*az*(-d.dy12-d.dy22)+rk*(az-2*rk)*(+d.dy32+d.dy42)+sz*sk*(-d.dx12+d.dx22-d.dx32+d.dx42); 
D(16,16)=az^2*(d.dy12+d.dy22)+(az-2*rk)^2*(d.dy32+d.dy42)+(az-2*rk)^2*(d.dy52+d.dy62)+az^2*(d.dy82+d.dy72)+sz^2*(d.dx12+d.dx22+d.dx32+d.dx42+d.dx52+d.dx62+d.dx72+d.dx82);

%Wanken / beta
D(11,11)=(sk^2)*(d.dz11+d.dz12+d.dz21+d.dz22+d.dz31+d.dz32+d.dz41+d.dz42);
D(11,17)=sk*sz*(-d.dz12-d.dz22-d.dz32-d.dz42);
D(17,17)=(sz^2)*(d.dz12+d.dz22+d.dz32+d.dz42)+(sz^2)*(d.dz52+d.dz62+d.dz72+d.dz82)+hz^2*(d.dy12+d.dy22+d.dy32+d.dy42+d.dy52+d.dy62+d.dy72+d.dy82);

%Nicken / gamma
D(12,12)=(rk^2)*(d.dz11+d.dz12+d.dz21+d.dz22+d.dz31+d.dz32+d.dz41+d.dz42);
D(12,18)=rk*az*(-d.dz12-d.dz22)+rk*(az-2*rk)*(d.dz32+d.dz42);
D(18,18)=(az^2)*(d.dz12+d.dz22)+(az-2*rz)^2*(d.dz32+d.dz42)+az^2*(d.dz62+d.dz82)+(az-2*rk)^2*(d.dz52+d.dz72)+hz^2*(d.dx12+d.dx22+d.dx32+d.dx42+d.dx72+d.dx82+d.dx52+d.dx62);

%Radkasten 2
%Gieren / alpha
D(13,13)=(sk^2)*(d.dx51+d.dx52+d.dx61+d.dx62+d.dx71+d.dx72+d.dx81+d.dx82)+(rk^2)*(d.dy51+d.dy61+d.dy71+d.dy81+d.dy52+d.dy62+d.dy72+d.dy82);
D(13,16)=rk*az*(-d.dy72-d.dy82)+rk*(az-2*rk)*(+d.dy52+d.dy62)+sz*sk*(+d.dx72+d.dx82-d.dx52-d.dx62);
%K(16,16)=(Az-2*rk)^2*(k.ky52+k.ky62)+Az^2*(k.ky82+k.ky72);

%Wanken / beta
D(14,14)=(sk^2)*(d.dz51+d.dz52+d.dz61+d.dz62+d.dz71+d.dz72+d.dz81+d.dz82);
D(14,17)=sk*sz*(-d.dz52-d.dz62-d.dz72-d.dz82);
%K(17,17)=(sz^2)*(k.kz52+k.kz62+k.kz72+k.kz82);

%Nicken / gamma
D(15,15)=(rk^2)*(d.dz51+d.dz52+d.dz61+d.dz62+d.dz71+d.dz72+d.dz81+d.dz82);
D(15,18)=rk*az*(-d.dz82-d.dz72)+rk*(az-2*rk)*(+d.dz52+d.dz62);
%K(18,18)=Az*(k.kz62+k.kz82)^2+(Az-2*rk)^2*(k.kz52+k.kz72);

%Nebendiagonalen
D(1,10)=sk*(-d.dx32-d.dx31+d.dx22+d.dx21-d.dx12-d.dx11+d.dx42+d.dx41);
D(1,18)=hz*(-d.dx12-d.dx22+d.dx32+d.dx42);%?

D(2,10)=rk*(+d.dy41+d.dy42-d.dy11-d.dy12-d.dy21-d.dy22+d.dy31+d.dy32);
D(2,16)=az*(d.dy12+d.dy22)+(az-2*rk)*(d.dy32+d.dy42);
D(2,17)=hz*(-d.dy12-d.dy32+d.dy42+d.dy22);

D(3,11)=sk*(d.dz11+d.dz31-d.dz21-d.dz41+d.dz12+d.dz32-d.dz22-d.dz42);
D(3,12)=rk*(-d.dz31-d.dz41-d.dz32-d.dz42+d.dz11+d.dz21+d.dz12+d.dz22);
D(3,17)=sk*(-d.dz12-d.dz32+d.dz22+d.dz42);
D(3,18)=az*(-d.dz12-d.dz22)+(az-2*rk)*(-d.dz32-d.dz42);

D(4,13)=sk*(-d.dx52+d.dx82+d.dx62-d.dx72-d.dx51+d.dx81+d.dx61-d.dx71);
D(4,18)=hz*(-d.dx82-d.dx72+d.dx52+d.dx62);

D(5,13)=rk*(d.dy71+d.dy72-d.dy61-d.dy62-d.dy51-d.dy52+d.dy81+d.dy82);
D(5,16)=az*(-d.dy82-d.dy72)+(az-2*rk)*(-d.dy62-d.dy52);
D(5,17)=hz*(-d.dy52-d.dy72+d.dy62+d.dy82);

D(6,14)=sk*(d.dz51-d.dz52-d.dz72+d.dz71-d.dz61+d.dz62-d.dz81+d.dz82);
D(6,15)=rk*(d.dz51+d.dz61-d.dz71-d.dz81+d.dz52+d.dz62-d.dz72-d.dz82);
D(6,17)=sk*(-d.dz52-d.dz72+d.dz62+d.dz82);
D(6,18)=az*(d.dz72+d.dz82)+(az-2*rk)*(+d.dz52+d.dz62);

D(7,10)=sk*(-d.dx22-d.dx32+d.dx12+d.dx42);
%K(7,12)=hz*(k.kx12+k.kx22-k.kx32-k.kx42);%Hoehe Radkasten
D(7,13)=sk*(+d.dx72+d.dx62-d.dx82-d.dx52);
%K(7,15)=hz*(-k.kx72-k.kx82+k.kx52+k.kx62);Hoehe Radkasten
D(7,16)=sk*(-d.dx22+d.dx12+d.dx32-d.dx42-d.dx52+d.dx62-d.dx72+d.dx82);%? aber null

D(8,10)=rk*(d.dy12-d.dy42+d.dy22-d.dy32);
D(8,11)=hz*(d.dy12+d.dy32-d.dy22-d.dy42);
D(8,13)=rk*(+d.dy52-d.dy82+d.dy62-d.dy72);
D(8,16)=az*(-d.dy22-d.dy12+d.dy72+d.dy82)+(az-2*rk)*(-d.dy42-d.dy32+d.dy52+d.dy62);
D(8,17)=hz*(-d.dy22-d.dy42-d.dy62-d.dy82+d.dy12+d.dy32+d.dy52+d.dy72);

D(9,11)=sk*(-d.dz12-d.dz32+d.dz22+d.dz42);
D(9,12)=rk*(-d.dz12-d.dz22+d.dz32+d.dz42);
D(9,14)=sk*(-d.dz52-d.dz72+d.dz62+d.dz82);
D(9,15)=rk*(d.dz72+d.dz82-d.dz52-d.dz62);
D(9,17)=sk*(d.dz12+d.dz32+d.dz52+d.dz72-d.dz22-d.dz42-d.dz62-d.dz82);
D(9,18)=az*(d.dz12+d.dz22-d.dz72-d.dz82)+(az-2*rk)*(d.dz32+d.dz42-d.dz52-d.dz62);

D(10,17)=hz*rk*(-d.dy22+d.dy42-d.dy12+d.dy32);
D(11,18)=sk*(az*(-d.dz12+d.dz22)+(az-2*rk)*(-d.dz32+d.dz42));
D(12,17)=sk*rk*(-d.dz12+d.dz32+d.dz22-d.dz42);
D(13,17)=rk*hz*(-d.dy62+d.dy82-d.dy52+d.dy72);
D(14,18)=sk*(az*(d.dz72-d.dz82)+(az-2*rk)*(d.dz52-d.dz62));
D(15,17)=rk*sk*(-d.dz52+d.dz72+d.dz62-d.dz82);
D(16,17)=az*hz*(d.dy12+d.dy32+d.dy52+d.dy72-d.dy22-d.dy42-d.dy62-d.dy82); %Az nicht richtig (Az-2*rk)... aber gleich null
D(17,18)=sz*(az*(+d.dz12-d.dz22-d.dz72+d.dz82)+(az-2*rk)*(d.dz32-d.dz42+d.dz52-d.dz62));

%% Erweiterung um die FHGs der Randsaetze
%Translationen Radsatz 1
D(19,19)=d.dx11+d.dx21;
D(1,19)=-(d.dx11+d.dx21);
D(20,20)=d.dy11+d.dy21;
D(2,20)=-(d.dy11+d.dy21);
D(21,21)=d.dz11+d.dz21;
D(3,21)=-(d.dz11+d.dz21);
%Translation Radsatz 2
D(24,24)=d.dx31+d.dx41;
D(1,24)=-(d.dx31+d.dx41);
D(25,25)=d.dy31+d.dy41;
D(2,25)=-(d.dy31+d.dy41);
D(26,26)=d.dz31+d.dz41;
D(3,26)=-(d.dz31+d.dz41);
%Translation Radsatz 3
D(29,29)=d.dx51+d.dx61;
D(4,29)=-(d.dx51+d.dx61);
D(30,30)=d.dy51+d.dy61;
D(5,30)=-(d.dy51+d.dy61);
D(31,31)=d.dz51+d.dz61;
D(6,31)=-(d.dz51+d.dz61);
%Translation Radsatz 4
D(34,34)=d.dx71+d.dx81;
D(4,34)=-(d.dx71+d.dx81);
D(35,35)=d.dy71+d.dy81;
D(5,35)=-(d.dy71+d.dy81);
D(36,36)=d.dz71+d.dz81;
D(6,36)=-(d.dz71+d.dz81);

%Rotationen Radsatz 1
%alpha
D(22,22)=sk^2*(d.dx11+d.dx21);
D(10,22)=sk^2*(-d.dx11-d.dx21);
%beta
D(23,23)=sk^2*(d.dz11+d.dz21);
D(11,23)=sk^2*(-d.dz11-d.dz21);
%yR1-alphak1
D(10,20)=rk*(+d.dy11+d.dy21);
%zR1-betak1
D(11,21)=sk*(d.dz21-d.dz11);%=0
%zR1-gammak1
D(12,21)=rk*(-d.dz11-d.dz21);

%Rotationen Radsatz 2
%alpha
D(27,27)=sk^2*(d.dx31+d.dx41);
D(10,27)=-sk^2*(d.dx31+d.dx41);
%beta
D(28,28)=sk^2*(d.dz31+d.dz41);
D(11,28)=-sk^2*(d.dz31+d.dz41);
%yR2-alphak1
D(10,25)=rk*(-d.dy31-d.dy41);
%zR1-betak1
D(11,26)=sk*(+d.dz41-d.dz31);%=0
%zR1-gammak1
D(12,26)=rk*(+d.dz31+d.dz41);

%Rotationen Radsatz 3
%alpha
D(32,32)=sk^2*(d.dx51+d.dx61);
D(13,32)=-sk^2*(d.dx51+d.dx61);
%beta
D(33,33)=sk^2*(d.dz51+d.dz61);
D(14,33)=-sk^2*(d.dz51+d.dz61);
%yR3-alphak2
D(13,30)=rk*(+d.dy51+d.dy61);
%zR3-betak2
D(14,31)=sk*(+d.dz61-d.dz51);%=0
%zR3-gammak2
D(15,31)=rk*(-d.dz51-d.dz61);

%Rotationen Radsatz 4
%alpha
D(37,37)=sk^2*(d.dx71+d.dx81);
D(13,37)=-sk^2*(d.dx71+d.dx81);
%beta
D(38,38)=sk^2*(d.dz71+d.dz81);
D(14,38)=-sk^2*(d.dz71+d.dz81);
%yR4-alphak2
D(13,35)=rk*(-d.dy71-d.dy81);
%zR4-betak2
D(14,36)=sk*(+d.dz81-d.dz71);%=0
%zR4-gammak2
D(15,36)=rk*(d.dz71+d.dz81);

%D ist symmetrischen -> spiegeln an der Diagonalen
D=D+D'-(eye(42,42).*diag(D));

%% nichtlineare Daempfungsanteile aus Rad/Schiene-Kontakt: D_nl
D_nl = zeros(42);

%y_p-y_p 
D_nl(20,20)=+Gr*b*2/v;
D_nl(25,25)=+Gr*b*2/v;
D_nl(30,30)=+Gr*b*2/v;
D_nl(35,35)=+Gr*b*2/v;

%alpha_p-alpha_p 
D_nl(22,22)=+Gr*a*sp^2/(2*v);
D_nl(27,27)=+Gr*a*sp^2/(2*v);
D_nl(32,32)=+Gr*a*sp^2/(2*v);
D_nl(37,37)=+Gr*a*sp^2/(2*v);

%delta_omea-delta_omega
D_nl(39,39)=+Gr*a*r0^2/v;
D_nl(40,40)=+Gr*a*r0^2/v;
D_nl(41,41)=+Gr*a*r0^2/v;
D_nl(42,42)=+Gr*a*r0^2/v;

%alpha_p-delta_omega
D_nl(22,39)=-sp*r0*Gr*a/v;
D_nl(27,40)=-sp*r0*Gr*a/v;
D_nl(32,41)=-sp*r0*Gr*a/v;
D_nl(37,42)=-sp*r0*Gr*a/v;

%delta_omega-alpha_p
D_nl(39,22)=-Gr*a*sp*r0/(2*v);
D_nl(40,27)=-Gr*a*sp*r0/(2*v);
D_nl(41,32)=-Gr*a*sp*r0/(2*v);
D_nl(42,37)=-Gr*a*sp*r0/(2*v);

%Zusammenfassen Systemdaempfung und Rad/Schiene-Daempfung
D = D+D_nl;

%% Anfangswerte
%einheitliche Verschiebung aller RS entlang y
y_RS_i = 0.00; 
%einzelne Verschiebungen entlang y
y_RS_1 = 0.00; 
y_RS_2 = 0.00; 
y_RS_3 = 0.00; 
y_RS_4 = 0.00; 

s0 = [0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;y_RS_i+y_RS_1;0;0;0;0;y_RS_i+y_RS_2;0;0;0;0;y_RS_i+y_RS_3;0;0;0;0;y_RS_i+y_RS_4;0;0;0;0;0;0;0];%Auslenkung

[r.rl1,r.rr1] = Berechnung_Kenngroessen2(s0(20,1));
[r.rl2,r.rr2] = Berechnung_Kenngroessen2(s0(25,1));
[r.rl3,r.rr3] = Berechnung_Kenngroessen2(s0(30,1));
[r.rl4,r.rr4] = Berechnung_Kenngroessen2(s0(35,1));
delta_omega_1 = -v/r.rr1+v/r.rl1; 
delta_omega_2 = -v/r.rr2+v/r.rl2;
delta_omega_3 = -v/r.rr3+v/r.rl3;
delta_omega_4 = -v/r.rr4+v/r.rl4;
delta_omega_i = 0.00; 

ein_aus_omega_r = 0; % Anpassung der Anfangsraddrehzahldifferenz an die Verschiebung
v0 = [0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;delta_omega_1*ein_aus_omega_r+delta_omega_i;delta_omega_2*ein_aus_omega_r+delta_omega_i;delta_omega_3*ein_aus_omega_r+delta_omega_i;delta_omega_4*ein_aus_omega_r+delta_omega_i];%Geschwindigkeit

 %% klassische Berechnung der Eigenwerte und Eigenvektoren
[ev,EW.lambda] = eig(K,M);      %Eigenwertproblem: d:Eigenwerte, v:Eigenvektoren
EW.lambda_abs_sort = sort(abs(diag(EW.lambda)));
%ev = ev(:,I); % Eigenvektoren neu zugeordnet, dass zu Reihenfolgen von ev passend
%EigVek_Norm = ev/diag(max(abs(ev))); % Normierung der Eigenvektoren

%% Berechnung der Eigenvektoren inklusive der linearen/nichtlinearen Daempfung mit "polyeig"
[poly.EV_nl,poly.EW_nl] = polyeig(K,D,M);
K_lin = K-K_nl;
D_lin = D-D_nl;
[poly.EV_lin,poly.EW_lin] = polyeig(K_lin,D_lin,M);

 %% Fuer EW-Berechnung mit "polyeig": Speichern der Werte  
%Speichern der EW fuer jede Geschwindigkeit in einer Matrix

%Eigenwerte (poly) der linearen Daemfung und Steifigkeit sind
%geschwindigkeitsunabhaengig. Geschwindigkeit ist in D_nl enthalten
 clear i
for i = 1:84
    poly.Mat(v,i) = poly.EW_nl(i);
    poly.Mat_real(v,i) = real(poly.EW_nl(i)); %Eintragen des EW aus i-ter Zeile des Vektors in i-te Spalte und v-te Zeite der Matrix
    poly.Mat_imag(v,i) = imag(poly.EW_nl(i));
    i = i+1; 
end
clear i

%Normieren der Eigenvektoren
[poly.max_Value,poly.max_idx] = max(poly.EV_nl);
poly.EV_nl_norm = poly.EV_nl./poly.max_Value;%Indizes der FHGs mit groesster Verschiebung

%Speichern der normierten EV fuer jede Geschwindigkeit in einem 3-dim.-Array
for i =1:84
    poly.Array_Ev(:,i,v) = poly.EV_nl_norm(:,i);
    i = i+1;
end 
clear i

poly.matmax=max(poly.Mat); %groesster EW bei der Variation von v

 %% Analytische Berechnung der Daempfungsmasse und der Quasieigenfrequenzen
C = diag(K);
M_vek = diag(M);
D_hd = diag(D);
Nenner = 2.*sqrt(C.*massenvektor);
Damp_analyt = D_hd./Nenner;
EW.Eigenw = diag(EW.lambda);
EW.Eigenw_abs = abs(EW.Eigenw);
EW.Quasieig = EW.Eigenw.*sqrt(ones(size(C))-Damp_analyt.^2);
Abw_ged = EW.Eigenw-EW.Quasieig; %Differenz von Eigenfrequenzen und Quasieigenfrequenzen

%% Abbruchkriterium
Abbruch = 10.*ones(42,1);

%% Berechnung von Wurzelortskurven (ZRM)
%(entnommen aus Knothe, Kapitel 10) 
%Umformen der Bewegungsgleichng in die Zustandsraumdarstellung
%Es muss sich die explizite Darstellung ergeben, deshalb Vorzeichenwechsel der A_Matrix
%Berechnung der Eigewerte mit der Zustandsraumdarstellung
A_Matrix = [-M\D,-M\K;
            eye(42),zeros(42,42)];

[ZR.Psi,ZR.lambda] = eig(A_Matrix); 
ZR.lambda_diag = diag(ZR.lambda);
ZR.lambda_abs_sort = sort(abs(diag(ZR.lambda)));
ZR.omega_ged = sqrt(ZR.lambda_abs_sort);

%Speichern der EW fuer jede Geschwindigkeit in einer Matrix
for i = 1:84
    ZR.Mat(v,i) = ZR.lambda_diag(i);
    ZR.Mat_real(v,i) = real(ZR.lambda_diag(i));
    ZR.Mat_imag(v,i) = imag(ZR.lambda_diag(i));
    i = i+1; 
end

[ZR.max_Value,ZR.max_idx] = max(ZR.Psi);
ZR.Psi_norm = ZR.Psi./ZR.max_Value; %Indizes der FHGs mit groesster Verschiebung
ZR.matmax=max(ZR.Mat);              %groesster EW des FHGs bei der Variation von v

%% Einbindung Simulink
Simulationsergebnisse = sim('SimModell_acht'); % v = v(t) = v_start+a*t

%% Ermitteln Gieramplitude alpha_RS1 abh. von Parameter 

% localmax_alpha = islocalmax(Simulationsergebnisse.gesamt.Data(:,22)); %Zeitschritte, an denen lokales Maximum des Gierwinkels vorliegt
% for i=1:30 %Abziehen der ersten 30 Maxima, damit eingescchwungener Zustand betrachtet wird
%     localmax_alpha(i,1)=0;
% end
% Value_max_alpha = localmax_alpha.*Simulationsergebnisse.gesamt.Data(:,22); %Amplitudenwerte
% Anz = size(localmax_alpha);
% anz = Anz(1,1)-30; %Anzahl erkannter Amplituden
% average_ampl = sum(Value_max_alpha)/anz; %Durchnittliche Amplitudenhoehe

% figure(4)
% plot(d_pri,average_ampl,'rx');
% title('Durschnittliche Gieramplitude von Radsatz 1, abh. von der Primaerfederung, mit: \xi=15m^{-1}, k_{primaer}=5\cdot 10^5, v=30m/s')
% xlabel('Primaerdaempfung [kg/s]')
% ylabel('Amplitude [m]')
% grid on
% hold on

%Durschnittsamplituden fuer 3d-Plot speichern
%     aver_ampl(j,f) = average_ampl;
%     Vari_d(j) = d_pri; 
%     f=f+1;

    %end, j=j+1 muss nich eigefuegt werden an richtiger Stelle

    
 %% Ende der Geschwindigkeitsvariation 
    v=v+1;
 end % Hier ist das Modell und alle Parameter
 
 %% Zuordnung der EV den EW mit pos. Realteil (ZRM)
 
%Reduzieren auf Realteile
ZR.real = real(ZR.Mat);  
%alle Eitraege Null, nur pos. Realteile 1
for row = 1:500
    for col = 1:84
        if ZR.real(row,col) <= .1 %Grenze, wegen num. Abweichung
            ZR.real(row,col) = 0; 
        else
            ZR.real(row,col) = 1;
        end 
        col = col+1; 
    end
    row = row+1; 
end 

%Zeilenvektor, mit eins in Spalte, wenn dort Realteile groesser 0 vorkommen
ZR.idx_pos_real = zeros(1,84); 
for colu = 1:84
    if sum(ZR.real(:,colu)) > 0 
    ZR.idx_pos_real(1,colu) = 1; 
    else
        ZR.idx_pos_real(1,colu) = 0;
    end
colu = colu+1;    
end

Anzahl_inst_FHG_zrm = sum(ZR.idx_pos_real); %Anzahl instabiler Eigenwerte

%Matrix mit allen instabilen Eigenwert-Geschwindigkeitsvektoren
ZR.Ew_inst = zeros(500,Anzahl_inst_FHG_zrm);
za=1;
for clm = 1:84
    if ZR.idx_pos_real(1,clm) == 1
        ZR.Ew_inst(:,za)=ZR.Mat(:,clm);
        za = za+1;
    end
end    
clear za clm
ZR.Ew_inst_re = real(ZR.Ew_inst);
ZR.Ew_inst_im = imag(ZR.Ew_inst);

%Matrix mit allen Eigenvektoren der Instabilen EWs
ZR.Ev_inst = zeros(84,Anzahl_inst_FHG_zrm);
za=1;
for clm = 1:84
    if ZR.idx_pos_real(1,clm) == 1
        ZR.Ev_inst(:,za)=ZR.Psi_norm(:,clm);
        za = za+1;
    end
end    

%Normieren der Eigenvektoren
for index = 1:Anzahl_inst_FHG_zrm
    ZR.Ev_norm(:,index) = ZR.Ev_inst(:,index)./max(ZR.Ev_inst(:,index)); 
    index = index+1; 
end 
clear index clm za row col colu
%% Zuordnung der EW,EV mit Realteil>0 (Polyeig)

%Reduzieren auf Realteile
poly.real = poly.Mat_real;  
%alle Eitraege Null, nur pos. Realteile 1
for row = 1:500
    for col = 1:84
        if poly.real(row,col) <= .1 %Grenze, wegen num. Abweichung
            poly.real(row,col) = 0; 
        else
            poly.real(row,col) = 1;
        end 
        col = col+1; 
    end
    row = row+1; 
end 

%Zeilenvektor, mit eins in Spalte, wenn dort Realteile groesser 0 vorkommen
poly.idx_pos_real = zeros(1,84); 
for colu = 1:84
    if sum(poly.real(:,colu)) > 0 
    poly.idx_pos_real(1,colu) = 1; 
    else
        poly.idx_pos_real(1,colu) = 0;
    end
colu = colu+1;    
end

Anzahl_inst_FHG_poly = sum(poly.idx_pos_real); %Anzahl instabiler Eigenwerte

%Matrix mit allen instabilen Eigenwert-Geschwindigkeitsvektoren
poly.Ew_inst = zeros(500,Anzahl_inst_FHG_poly);
za=1;
for clm = 1:84
    if poly.idx_pos_real(1,clm) == 1 %in jeder Spalte gucken, ob eine 1 ist
        poly.Ew_inst(:,za)=poly.Mat(:,clm); % wenn ja, dann nimmt Spalte aus EW-v-Matrix 
        za = za+1;
    end
end    
clear za clm
poly.Ew_inst_re = real(poly.Ew_inst);
poly.Ew_inst_im = imag(poly.Ew_inst);

%Matrix mit allen Eigenvektoren der Instabilen EWs
poly.Ev_inst = zeros(42,Anzahl_inst_FHG_poly);
za=1;
for clm = 1:84
    if poly.idx_pos_real(1,clm) == 1
        poly.Ev_inst(:,za)=poly.EV_nl_norm(:,clm); % !!! Es wird der letzte EV genommen, also hier v = 500
        za = za+1;
    end
end    

%Normieren der Eigenvektoren
for index = 1:Anzahl_inst_FHG_poly
    poly.Ev_norm(:,index) = poly.Ev_inst(:,index)./max(poly.Ev_inst(:,index)); 
    index = index+1; 
end 

%sortieren nach den groessten Eigenvektoren und Zuordnung der FHGs
clear i index
poly.EV_inst_sort = zeros(42,2*Anzahl_inst_FHG_poly);
for i=1:Anzahl_inst_FHG_poly
[werte,index] = sort(real(poly.Ev_inst(:,i)),'descend'); %von gross nach klein sortiert
poly.EV_inst_sort(:,(2*i)-1)=werte; 
poly.EV_inst_sort(:,(2*i))=index; 
index=index+1;
end 
clear i index 

%Erstellen der gleichen Tabelle fuer Zuordnung, aber fuer alle EVn: 
poly.EV_sort = zeros(42,84); 
for i=1:42;
[werte,index] = sort(real(poly.Array_Ev(:,i,500)),'descend'); %von gross nach klein sortiert
poly.EV_sort(:,(2*i)-1)=werte; 
poly.EV_sort(:,(2*i))=index; 
index=index+1;
end 

%% Plotten
%% Surface-Plot: Gierampl. abh. von Daempfung und Steifigkeit
figure(6)
surf(Vari_d,Vari_k,aver_ampl)
title('Durschnittliche Gieramplitude von Radsatz 1 in Abh. der Daempfung und Steifigkeit, \xi=15, v=30m/s')
xlabel('Primaerdaempfung')
ylabel('Primaersteifigkeit')
zlabel('Gieramplitude')

%% Plotten der EW-Zustandsraum
% Plotten der instabilen Freiheitsgrade
% for i = 1:Anzahl_inst_FHG
% figure(i) % Wurzelortskurve fuer die Geschwindigkeitsabhaengigkeit
% plot(ZR.Ew_inst_re(:,i),ZR.Ew_inst_im(:,i),'b.')
% title('Eigenwerte, abh. von der Geschwindigkeit v=(1:500), \xi=15, d_{primaer}=1\cdot 10^2, k_{primaer}=1\cdot 10^6')
% xlabel('\Re{(\lambda_{ZR}(v))}') 
% ylabel('\Im{(\lambda_{ZR}(v))}')
% grid on
% i=i+1;
% end

% figure(3) % Wurzelortskurve fuer die Geschwindigkeitsabhaengigkeit
% plot(ZR.Mat_real(:,20),ZR.Mat_imag(:,20),'b.')
% title('Eigenwerte (ZRM), abh. von der Geschwindigkeit v=(1:500), \xi=15, d_{primaer}=1\cdot 10^2, k_{primaer}=1\cdot 10^6')
% xlabel('\Re{(\lambda_{ZR}(v))}') 
% ylabel('\Im{(\lambda_{ZR}(v))}')
% grid on
% 
% %% Plotten der Eigenwerte "polyeig"
% figure(4) % Wurzelortskurve fuer die Geschwindigkeitsabhaengigkeit mit polyeig
% plot(poly.Mat_real(:,:),poly.Mat_imag(:,:),'b.')
% title('Eigenwerte (polyeig, mit Nl.), abh. von der Geschwindigkeit v=(1:500), \xi=15, d_{primaer}=1\cdot 10^2, k_{primaer}=1\cdot 10^6')
% xlabel('\Re{(\lambda^2(\nu))}') 
% ylabel('\Im{(\lambda^2(\nu))}')
% grid on
% 
% figure(5) %instabile EWn poly
% plot(poly.Ew_inst_re(:,1),poly.Ew_inst_im(:,1),'b.',poly.Ew_inst_re(:,3),poly.Ew_inst_im(:,3),'r.')
% %title('Eigenwerte (polyeig, mit Nl.), abh. von der Geschwindigkeit v=(1:500), \xi=15, d_{primaer}=1\cdot 10^2, k_{primaer}=1\cdot 10^6')
% xlabel('\Re{(\lambda^2(\nu))}') 
% ylabel('\Im{(\lambda^2(\nu))}')
% grid on


%% Orte ueber Zeit
figure(1)
subplot(3,2,1)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,1),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,2),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,3))
title('Translationen Radkasten 1')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,2)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,10),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,11),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,12))
title('Rotationen Radkasten 1')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(3,2,3)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,4),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,5),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,6))
title('Translationen Radkasten 2')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,4)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,13),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,14),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,15))
title('Rotationen Radkasten 2')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(3,2,5)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,7),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,8),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,9))
title('Translationen Zug')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,6)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,16),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,17),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,18))
title('Rotationen Zug')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

%Radsaetze
figure(2); 
sgtitle('Bewegungen der Radsaetze')
subplot(4,3,1)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,19),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,20),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,21))
title('Translationen Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,2)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,22),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,23))
title('Rotationen Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,3)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.Punkt.Data(:,39))
title('Differenz WG Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,4)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,24),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,25),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,26))
title('Translationen Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,5)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,27),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,28))
title('Rotationen Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,6)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.Punkt.Data(:,40))
title('Differenz WG Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,7)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,29),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,30),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,31))
title('Translationen Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,8)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,32),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,33))
title('Rotationen Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,9)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.Punkt.Data(:,41))
title('Differenz WG Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,10)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,34),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,35),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,36))
title('Translationen Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,11)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,37),Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.gesamt.Data(:,38))
title('Rotationen Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,12)
plot(Simulationsergebnisse.gesamt.Time,Simulationsergebnisse.Punkt.Data(:,42))
title('Differenz WG Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

% Phasenportrait
figure(2)
plot(Simulationsergebnisse.Punkt.Data(:,22),Simulationsergebnisse.gesamt.Data(:,22))
title('Phasenportrait y_{punkt},y, Radsatz 1')
xlabel('Geschwindigkeit y_{punkt}')
ylabel('Verschiebung y')