% Modell zum Test verschiedener Integrationsverfahren
% Parameter, Dämpfungs- und Steifigkeitsmatrix aus Modell 7
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;        %Zughöhe
                 %Zugaufbau : Index z
sz = sk; %2.8/2; %Zugbreite ICE: 3m;
rz = 1.8/2;      %Vertikale Federn übereinander
hz = 1.7;        %halbe Zughöhe
Hz = 2*hz;       %4m Zughöhe minus 1m Raddurchmesser 
az = 28.75/2;    %halbe Länge Mittelwagen ICE4

%% Parameter für die Berechnung der Kontaktkräfte nach Vorgehen von B.Abdelfattah
a = 45;%Kraftschlusskoeffizient in Längsrichtung
gamma4 = 116;% Kraftschlusskoeffizient in Querrichtung
m = 972; %Masse eines Radsatzes.  -> muss noch in den nachfolgenden Massen und Trägheiten angepasst werden!!! 
Jz = 484.75; 
Jy = 29.5; 
sp = 2*sk;  %Stützweite
r0 = 0.5; 
xi = 30; %[1/m] Koeffizient der Profilsteifigkeit, =0 für kegeliges Profil
gamma = 1/40; %wirksame Konizizät
k_pri = 1e6; 
d_pri = 1e3; 
v=30; 

%% Massen und Trägheiten
mz = 18500; %Normale Beladung Wagon ICE4
%mz = 25000; %Güterwagen
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; %mz/12*((2*sz)^2+(Hz)^2); 
Jzg = Jz; %mz/12*((Hz)^2+(2*az)^2); 
Jr = Jz;  %(330/12)*(3*(0.2)^2+4*sk^2)+2*((330/12)*(3*0.5^2+0.2^2+330*(2*sk)^2)); %Massenträgheit für Wanken und Gieren bei Radsätzen 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
%Primär
k.kx11=k_pri;
k.kx21=k_pri;
k.kx31=k_pri;
k.kx41=k_pri;
k.kx51=k_pri;
k.kx61=k_pri;
k.kx71=k_pri;
k.kx81=k_pri;

% k.kx11=1e4;
% k.kx21=1e4;
% k.kx31=1e4;
% k.kx41=1e4;
% k.kx51=1e4;
% k.kx61=1e4;
% k.kx71=1e4;
% k.kx81=1e4;
%Sekundär
k.kx12=4*1e5;
k.kx22=4*1e5;
k.kx32=4*1e5;
k.kx42=4*1e5;
k.kx52=4*1e5;
k.kx62=4*1e5;
k.kx72=4*1e5;
k.kx82=4*1e5;

%y-Richtung
%Primär
% k.ky11=1e4;
% k.ky21=1e4;
% k.ky31=1e4;
% k.ky41=1e4;
% k.ky51=1e4;
% k.ky61=1e4;
% k.ky71=1e4;
% k.ky81=1e4;

k.ky11=k_pri;
k.ky21=k_pri;
k.ky31=k_pri;
k.ky41=k_pri;
k.ky51=k_pri;
k.ky61=k_pri;
k.ky71=k_pri;
k.ky81=k_pri;

%Sekundär
k.ky12=1e5;
k.ky22=1e5;
k.ky32=1e5;
k.ky42=1e5;
k.ky52=1e5;
k.ky62=1e5;
k.ky72=1e5;
k.ky82=1e5;

%z-Richtung
%Primär
% k.kz11=1e4;
% k.kz21=1e4;
% k.kz31=1e4;
% k.kz41=1e4;
% k.kz51=1e4;
% k.kz61=1e4;
% k.kz71=1e4;
% k.kz81=1e4;

k.kz11=k_pri;
k.kz21=k_pri;
k.kz31=k_pri;
k.kz41=k_pri;
k.kz51=k_pri;
k.kz61=k_pri;
k.kz71=k_pri;
k.kz81=k_pri;

%Sekundär
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, Höhe Radkasten vernachläsigt
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, Höhe Radkasten vernachlässigt
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);%Vorzeichen umgekehrt? aber null

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 Randsätze
%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);%=0
%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);%=0
%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);%=0
%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);%=0
%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 Kräfte 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*gamma4;
K_nl(25,27)=-2*Gr*gamma4;
K_nl(30,32)=-2*Gr*gamma4;
K_nl(35,37)=-2*Gr*gamma4;
%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;

%% Dämpfungen
%x-Richtung
% d.dx11=1e3; %100
% d.dx21=1e3;
% d.dx31=1e3;
% d.dx41=1e3;
% d.dx51=1e3;
% d.dx61=1e3;
% d.dx71=1e3;
% d.dx81=1e3;

d.dx11=d_pri; %100
d.dx21=d_pri;
d.dx31=d_pri;
d.dx41=d_pri;
d.dx51=d_pri;
d.dx61=d_pri;
d.dx71=d_pri;
d.dx81=d_pri;

%Sekundär
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; % eigentlich 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;

d.dy11=d_pri;
d.dy21=d_pri;
d.dy31=d_pri;
d.dy41=d_pri;
d.dy51=d_pri;
d.dy61=d_pri;
d.dy71=d_pri;
d.dy81=d_pri;

%Sekundär
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;

d.dz11=d_pri;
d.dz21=d_pri;
d.dz31=d_pri;
d.dz41=d_pri;
d.dz51=d_pri;
d.dz61=d_pri;
d.dz71=d_pri;
d.dz81=d_pri;

%Sekundär
d.dz12=5e3; 
d.dz22=5e3;
d.dz32=5e3;
d.dz42=5e3;
d.dz52=5e3;
d.dz62=5e3;
d.dz72=5e3;
d.dz82=5e3;

%% Dämpfungsmatrizen

%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 Dämpfungsmatrix
D = [D_trans, zeros(9,33);
    zeros(33,42)];

%Rotatorische Dämpfungen
%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);%Höhe 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);Höhe 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);
%K(11,17)=sk*hz*(-k.kz12-k.kz32-k.kz22-k.kz42); falsch und schon oben berechnet
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 Randsätze
%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 Dämpfungsanteile aus Rad/Schiene-Kontakt: D_nl
D_nl = zeros(42);

%y_p-y_p 
D_nl(20,20)=+Gr*gamma4*2/v;
D_nl(25,25)=+Gr*gamma4*2/v;
D_nl(30,30)=+Gr*gamma4*2/v;
D_nl(35,35)=+Gr*gamma4*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 Systemdämpfung und Rad/Schiene-Dämpfung
D = D+D_nl;

%% Anregnung
%seitliche Anregung der Zugmasse - z.B. periodischer Seitenwind
amp = zeros(42,1); 
amp(8,1)=0.1; 

%% Umformen des Modells in explizite Form (Ausgangsgleichung -> Zustandsgleichung)
%x_punkt = A*x+B*u
%bisher immer in der Form y = M\K*x+M\D*u in Simulink integriert worden

% Zustandsvektor x = [x_punkt;x]
A = [zeros(size(M)),eye(size(M));-M\K, -M\D];
B = [zeros(size(M));inv(M)];
%% Numerische Integrationsverfahren 

a=0; % Anfangszeit
b=15; % Endzeit
N=10000; % Anzahl Zeitschritte
h=(b-a)/N; % Zeitschritt
%t(1)=a; %erster Zeitschritt = Anfangszeit

% Anfangswerte
%einheitliche Verschiebung aller RS entlang y
y_RS_i = 0.001; 
%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
y(:,1)=[s0;zeros(42,1)]; %erster Wert = Anfangswert
y0=[s0;zeros(42,1)];
y5(:,1)=y(:,1);

%definieren der expl. DGL
f=@(y,t) A*y+B*amp*k_pri*sin(10*t); %(84x1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   einfaches Euler        E
%   verbessertes Euler     E_v
%   Runge-Kutta 4          RK4
%   Runge-Kutta-Fehlberg   RKF45
%   Euler implizit         E_impl
%   RK implizit            RK4_impl
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Verfahren = 'ode45'; 

switch Verfahren 
    
    case 'ode45'
 %% ode45
odefun=@(yout,tout) A*yout+B*amp*k_pri*sin(10*tout);
tspan = [a,b];
[tout,yout] = ode45(@odefun, tspan, y0);

    case 'ode23s'
%% ode23s
odefun=@(y,t) A*y+B*amp*k_pri*sin(10*t);
tspan = [a:0.0015:b];
[t,y] = ode23s(odefun, tspan, y0);

    case 'E' 
%% Einfaches Euler-Verfahren (einschritt) 
tic
for n=1:N 
t(n+1)=t(n)+h; %Berechnen des nächsten Zeitpunkts
    y(:,n+1)=y(:,n)+h*f(y(:,n),t(n)); % Berechnung des nächsten y werts   
end
T_E = toc; 

    case 'E_v'
%% Verbessertes Eulerverfahren
tic
for n=1:N
    z=y(:,n)+h/2*f(y(:,n),t(n)+h/2);
    y(:,n+1)=y(:,n)+h*f(z,t(n)+h/2);
    t(n+1)=t(n)+h;
end
T_E_v = toc; 

    case 'RK4'
%% Runge_Kutta 4.Ordnung
tic
for n=1:N
    t(n+1)=t(n)+h; 
    kn1 = f(y(:,n),t(n));
    kn2 = f(y(:,n)+0.5*h*kn1,t(n)+0.5*h);
    kn3 = f(y(:,n)+0.5*h*kn2,t(n)+0.5*h);
    kn4 = f(y(:,n)+h*kn3,t(n)+h);
    y(:,n+1)=y(:,n)+(h/6)*(kn1+2*kn2+2*kn3+kn4);
end
T_RK4 = toc;

    case 'RKF45'
%% eigebettetes Runge-Kutta-Fehlberg-Verfahren (ähnlich ode45) 
% aus "Numerik für Naturwissenschaftler und Ingenieure"
% tic
% 
% n=1;                         
% % Sicherheitsfaktoren
% alpha_min = 0.3;
% alpha_max = 1.75;
% beta = 0.925;
% 
% %Fehlertoleranz 
% eps = 1e-13;    
% h_min = 0.00005; 
% h_max = 0.003; 
% h = 0.002;
% t(1) = 0;
% 
% % Aufstellen der Koeffizienten für ein Verfahren mit höherer
% % Konsistenzordnung
% 
% Butcher =   [0,0,0,0,0,0; 
%             0.25,0,0,0,0,0;
%             3/32,9/32,0,0,0,0;
%             1932/2197,-7200/2197,7296/2197,0,1,0;
%             439/216,-8,3680/513,-845/4104,0,0;
%            -8/27,2,-3544/2565,1859/4104,-11/40,0];                          %Butcher-Tableau: Fehlberg mit Ordnungen 4 und 5     
% 
% kn=ones(84,6);      %(FHG x kn1bis6)                                        %Koeffizientenvektor
% 
% gamma4 = [25/216,0,1408/2565,2197/4104,-1/5,0];                             %Gewichtungen niedrigere Ordnung
% gamma5 = [16/135,0,6656/12825,28561/56430,-9/50,2/55]; 
% koefC = [0,0.25,3/8,12/13,1];
% 
% while t(:,n) < b
%     
%        for idx1=1:6
%            for idx2=1:6
%            kn(:,idx1)=f(y(:,n)+Butcher(idx1,idx2)*kn(idx2),t(:,n)+);
%            idx2=idx2+1;
%            end
%            idx1=idx1+1;
%        end
%                                                                 
% c4 = kn*gamma4';                                                            %gemittelte Steigung, niedrigere Ordnung
% c5 = kn*gamma5';                                                            %gemittelte Steigung höhere Ordnung
% 
% %Wert mit beiden Verfahren bestimmen, im ersten Schritt mit h0
% y(:,n+1)=y(:,n)+h*c4;
% y5(:,n+1)=y(:,n)+h*c5;
% 
% % lokalen Fehler berechnen, Hilfsgröße qh ermitteln
% sh(:,n) = norm(y(:,n+1)-y5(:,n+1));
% qh(n) = sh(:,n)/(h*eps); 
% 
% alpha = max(alpha_min,qh(n)^(-1/4));
% alpha = min(alpha_max,alpha);
% h_neu = max(h_min,beta*alpha*h); 
% h_neu = min(h_max,h_neu); 
% 
% if qh(n) <= 1 || h == h_min %dann Schrittweite akzeptieren und zum nächsten Schritt Zeitschritt aktualisieren
%     t(n+1) = t(n)+h;
%     h=min(h_neu,(b-t(n)));
%     n = n+1;        
% else
%     h=h_neu; % Schritt mit neuer Schrittweite wiederholen
% end
%    
% %Speichern der Schrittweite
% H(n,:) = h; 
% % n = n+1;
% end
% T_RKF = toc;

Koef = [0;2/9;1/3;3/4;1;5/6];
Butcher = [0,0,0,0,0;
    2/9,0,0,0,0;
    1/12,1/4,0,0,0;
    -69/128,-243/128,135/64,0,0;
    -17/12,27/4,-27/5,16/15,0;
    65/432,-5/16,13/16,4/27,5/144];
C = [1/9;0;9/20;16/45;1/12;0];
CH = [47/450;0;12/25;32/255;1/30;6/25];
CT = [-1/150;0;3/100;-16/75;-1/20;6/25];

eps = 1e-4; 
n=1;
t(1)=0; 
h_min = 0.0005; 

while t(n) <= b

k1=h*f(y(:,n),t(n)+h*Koef(1));
k2=h*f(y(:,n)+Butcher(2,1)*k1,t(n)+h*Koef(2));
k3=h*f(y(:,n)+Butcher(3,1)*k1+Butcher(3,2)*k2,t(n)+h*Koef(3));
k4=h*f(y(:,n)+Butcher(4,1)*k1+Butcher(4,2)*k2+Butcher(4,3)*k3,t(n)+h*Koef(4));
k5=h*f(y(:,n)+Butcher(5,1)*k1+Butcher(5,2)*k2+Butcher(5,3)*k3+Butcher(5,4)*k4,t(n)+h*Koef(5));
k6=h*f(y(:,n)+Butcher(6,1)*k1+Butcher(6,2)*k2+Butcher(6,3)*k3+Butcher(6,4)*k4+Butcher(6,5)*k5,t(n)+h*Koef(6));

y(:,n+1)=y(:,n)+CH(1)*k1+CH(2)*k2+CH(2)*k3+CH(4)*k4+CH(5)*k5+CH(6)*k6;

Err = abs(CT(1)*k1+CT(2)*k2+CT(3)*k3+CT(4)*k4+CT(5)*k5+CT(6)*k6);

h_neu = norm(0.9*h*(eps/abs(Err)).^0.2);

if max(Err) > eps 
    h = h_neu; 
elseif max(Err) <= eps 
    t(n+1)=t(n)+h;
    h=max(h_min,h_neu); 
    n=n+1; 
else
    t(n+1)=t(n)+h;
    h = h_min;
    n=n+1;
end
H(n)=h; 
end



    case 'E_impl'
%% Implizites einfaches Eulerverfahren
 tic
 for n=1:N 
     t(n+1)=t(n)+h; %Berechnen des nächsten Zeitpunkts
     y(:,n+1) = inv(eye(84)-h*A)*(y(:,n)+h*B*k_pri*sin(10*t(n+1))*amp);
 end
 T_E_impl = toc;
 
    case 'RK4_impl'
%% Implizites Runge-Kutta-Verfahren, 2 stufiges, 4.Ord

Butcher = [0.25,0.25-(sqrt(3)/6);
    0.25+(sqrt(3)/6),0.25];

gamma = [0.5,0.5]';
alpha = [0.5-(sqrt(3)/6);0.5+(sqrt(3)/6)];
kn_alt= zeros(84,2); %84 FHGs, 2 Stuf-Verfahren
 

for n=1:N

%Bestimmen der kn
kn(:,1)=A*(y(:,n)+h*(Butcher(1,1).*kn_alt(:,1)+Butcher(1,2).*kn_alt(:,2))+B*(k_pri*sin(10*(t(n)+alpha(1)*h))*amp));
kn(:,2)=A*(y(:,n)+h*(Butcher(2,1).*kn_alt(:,1)+Butcher(2,2).*kn_alt(:,2))+B*(k_pri*sin(10*(t(n)+alpha(2)*h))*amp));
%Speichern der kn
kn_alt(:,1)=kn(:,1);
kn_alt(:,2)=kn(:,2);
  
t(:,n+1)=t(:,n)+h;
y(:,n+1)=y(:,n)+h*(gamma(1)*kn(:,1)+gamma(2)*kn(:,2));
    
end

end
%% Plotten der Ergebnisse des Euler-Verfahrens
figure(1)
subplot(3,2,1)
plot(t,y(1,:),t,y(2,:),t,y(3,:))
title('Translationen Radkasten 1')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,2) 
plot(t,y(10,:),t,y(11,:),t,y(12,:))
title('Rotationen Radkasten 1')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(3,2,3)
plot(t,y(4,:),t,y(5,:),t,y(6,:))
title('Translationen Radkasten 2')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,4)
plot(t,y(13,:),t,y(14,:),t,y(15,:))
title('Rotationen Radkasten 2')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(3,2,5)
plot(t,y(7,:),t,y(8,:),t,y(9,:))
title('Translationen Zug')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(3,2,6)
plot(t,y(16,:),t,y(17,:),t,y(18,:))
title('Rotationen Zug')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

%Radsätze
figure(2); 
sgtitle('Bewegungen der Radsätze')

subplot(4,3,1)
plot(t,y(19,:),t,y(20,:),t,y(21,:))
title('Translationen Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,2)
plot(t,y(22,:),t,y(23,:))
title('Rotationen Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,3)
plot(t,y(39,:))
title('Differenz WG Radsatz 1')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,4)
plot(t,y(24,:),t,y(25,:),t,y(26,:))
title('Translationen Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,5)
plot(t,y(27,:),t,y(28,:))
title('Rotationen Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,6)
plot(t,y(40,:))
title('Differenz WG Radsatz 2')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,7)
plot(t,y(29,:),t,y(30,:),t,y(31,:))
title('Translationen Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,8)
plot(t,y(32,:),t,y(33,:))
title('Rotationen Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,9)
plot(t,y(41,:))
title('Differenz WG Radsatz 3')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')

subplot(4,3,10)
plot(t,y(34,:),t,y(35,:),t,y(36,:))
title('Translationen Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Position [m]')

subplot(4,3,11)
plot(t,y(37,:),t,y(38,:))
title('Rotationen Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Winkel [rad]')

subplot(4,3,12)
plot(t,y(42,:))
title('Differenz WG Radsatz 4')
xlabel('Simulationszeit [s]')
ylabel('Differenz [s^{-1}]')