%--------------------------------------------------------------
%Systemzustände definieren
%--------------------------------------------------------------
syms x1;    %Wagenposition
syms x2;    %Wagengeschwindigkeit
syms x3;    %Winkel von Gelenk 1
syms x4;    %Winkelgeschwindigkeit von Winkel1
syms x5;    %Winkel von Gelenk 2
syms x6;    %Winkelgeschwindigkeit von Winkel2
syms x7;    %Winkel von Gelenk 3
syms x8;    %Winkelgeschwindigkeit von Winkel3
syms u;     %Beschleunigung


%--------------------------------------------------------------
% Modellparameter 
%--------------------------------------------------------------
%Pendelarm 1
l12      = 0.323; %Länge zwischen Pendelarm 1 und 2
l1=l12;
ls1     = 0.2121; %Abstand bis zum Schwerpunkt Gelenk 1
lm1=ls1;
mG1     = 0.8083; %Masse Gelenk 1
m1=mG1;
J_G1xx   = 0.013; %Massenträgheitsmoment Gelenk 1
J11zz=J_G1xx;
kG1     = 0.0297; %Reibungskonstante von Gelenk 1
k1=kG1;
%Pendelarm 2
l23      = 0.4190; %Länge zwischen Pendelarm 2 und 3
l2=l23;
ls2     = 0.2929; %Abstand bis zum Schwerpunkt Gelenk 2
lm2=ls2;
mG2     = 1.0040; %Masse Gelenk 2
m2=mG2;
J_G2xx   = 0.0235; %Massenträgheitsmoment Gelenk 2
J22zz=J_G2xx;
kG2     = 0.000; %Reibungskonstante von Gelenk 1
k2=kG2;
%Pendelarm 3
ls3     = 0.2276; %Abstand bis zum Schwerpunkt Gelenk 3
lm3=ls3;
mG3    = 0.5080; %Masse Gelenk 3
m3=mG3;
J_G3xx   = 0.0165; %Massenträgheitsmoment Gelenk 3
J33zz=J_G3xx;
kG3     = 0.0003; %Reibungskonstante von Gelenk 3
k3=kG3; 
g        = 9.8144; %Gravitationskonstante

par_pendel = [l12 ls1 mG1 J_G1xx kG1 l23 ls2 mG2 J_G2xx kG2 ls3 mG3 J_G3xx kG3 g]; %Parametervektor für Simulinkmodell
%--------------------------------------------------------------
%System mit x_dot=f(x,u)
%--------------------------------------------------------------
f1=[x2;
  u;
  x4;
  ((((2*l12*l23*ls2.^2-2*l12*l23.^2*ls2)*ls3.^2*(x6).^2*sin(2*x7-x5-x3)+(g*l12*l23*ls2-g*l12*l23.^2)*ls3.^2*sin(2*x7-2*x5+x3)+(u*l12*l23*ls2-u*l12*l23.^2)*ls3.^2*cos(2*x7-2*x5+x3)+(g*l12*l23.^2-g*l12*l23*ls2)*ls3.^2*sin(2*x7-2*x5-x3)+(u*l12*l23*ls2-u*l12*l23.^2)*ls3.^2*cos(2*x7-2*x5-x3)+(2*g*l12*l23*ls2-2*g*l12*ls2.^2)*ls3.^2*sin(2*x7-x3)+(2*u*l12*l23*ls2-2*u*l12*ls2.^2)*ls3.^2*cos(2*x7-x3)+(2*l12.^2*ls2.^2-2*l12.^2*l23*ls2)*ls3.^2*(x4).^2*sin(2*x7-2*x3)-2*l12*l23*ls2*ls3.^3*(x8).^2*sin(x7-2*x5+x3)+(4*l12*ls2.^2-2*l12*l23*ls2)*ls3.^3*(x8).^2*sin(x7-x3)-2*g*l12*l23*ls2*ls3.^2*sin(2*x5-x3)-2*u*l12*l23*ls2*ls3.^2*cos(2*x5-x3)+2*l12.^2*l23*ls2*ls3.^2*(x4).^2*sin(2*x5-2*x3)+(2*l12*l23*ls2.^2+2*l12*l23.^2*ls2)*ls3.^2*(x6).^2*sin(x5-x3)+(2*g*l12*ls2.^2-2*g*l12*l23*ls2+2*g*l12*l23.^2)*ls3.^2*sin(x3)+(2*u*l12*ls2.^2-2*u*l12*l23*ls2+2*u*l12*l23.^2)*ls3.^2*cos(x3))*mG2+((-g*l23.^2*ls1*ls3.^2*sin(2*x7-2*x5+x3))-u*l23.^2*ls1*ls3.^2*cos(2*x7-2*x5+x3)+g*l23.^2*ls1*ls3.^2*sin(2*x7-2*x5-x3)-u*l23.^2*ls1*ls3.^2*cos(2*x7-2*x5-x3)+2*g*l23.^2*ls1*ls3.^2*sin(x3)+2*u*l23.^2*ls1*ls3.^2*cos(x3))*mG1+((2*l12*l23*ls3.^2*x8-2*l12*l23*ls3.^2*x6)*cos(2*x7-x5-x3)+(2*l12*l23.^2*ls3*x6-2*l12*l23.^2*ls3*x8)*cos(x7-2*x5+x3)+(2*l12*l23.^2*ls3*x8-2*l12*l23.^2*ls3*x6)*cos(x7-x3)-2*l12*l23*ls3.^2*cos(x5-x3)*x8+2*l12*l23*ls3.^2*x6*cos(x5-x3))*kG3+((2*l12*l23*ls3.^2*x4-2*l12*l23*ls3.^2*x6)*cos(2*x7-x5-x3)+(2*l23.^2*ls3.^2*x4-2*l23.^2*ls3.^2*x6)*cos(2*x7-2*x5)+(2*l12*l23*ls3.^2*x6-2*l12*l23*ls3.^2*x4)*cos(x5-x3)+2*l23.^2*ls3.^2*x6-2*l23.^2*ls3.^2*x4)*kG2+(2*l23.^2*ls3.^2*x4*cos(2*x7-2*x5)-2*l23.^2*ls3.^2*x4)*kG1+2*J_G2xx*l12*l23*ls3.^2*(x6).^2*sin(2*x7-x5-x3)-2*g*J_G2xx*l12*ls3.^2*sin(2*x7-x3)-2*u*J_G2xx*l12*ls3.^2*cos(2*x7-x3)+2*J_G2xx*l12.^2*ls3.^2*(x4).^2*sin(2*x7-2*x3)-2*J_G3xx*l12*l23.^2*ls3*(x8).^2*sin(x7-2*x5+x3)+(4*J_G2xx*l12*ls3.^3+2*J_G3xx*l12*l23.^2*ls3)*(x8).^2*sin(x7-x3)-2*g*J_G3xx*l12*l23.^2*sin(2*x5-x3)-2*u*J_G3xx*l12*l23.^2*cos(2*x5-x3)+2*J_G3xx*l12.^2*l23.^2*(x4).^2*sin(2*x5-2*x3)+(2*J_G2xx*l12*l23*ls3.^2+4*J_G3xx*l12*l23.^3)*(x6).^2*sin(x5-x3)+(2*g*J_G2xx*l12*ls3.^2+2*g*J_G3xx*l12*l23.^2)*sin(x3)+(2*u*J_G2xx*l12*ls3.^2+2*u*J_G3xx*l12*l23.^2)*cos(x3))*mG3.^2+(((-2*g*l12*ls2.^2*ls3.^2*sin(2*x5-x3))-2*u*l12*ls2.^2*ls3.^2*cos(2*x5-x3)+2*l12.^2*ls2.^2*ls3.^2*(x4).^2*sin(2*x5-2*x3)+4*l12*ls2.^3*ls3.^2*(x6).^2*sin(x5-x3)+2*g*l12*ls2.^2*ls3.^2*sin(x3)+2*u*l12*ls2.^2*ls3.^2*cos(x3))*mG2.^2+((4*g*ls1*ls2.^2*ls3.^2*sin(x3)+4*u*ls1*ls2.^2*ls3.^2*cos(x3))*mG1+((2*l12*l23*ls2*ls3*x6-2*l12*l23*ls2*ls3*x8)*cos(x7-2*x5+x3)+((4*l12*ls2.^2-2*l12*l23*ls2)*ls3*x8+(2*l12*l23*ls2-4*l12*ls2.^2)*ls3*x6)*cos(x7-x3)-4*l12*ls2*ls3.^2*cos(x5-x3)*x8+4*l12*ls2*ls3.^2*x6*cos(x5-x3))*kG3+((4*l12*ls2*ls3.^2*x6-4*l12*ls2*ls3.^2*x4)*cos(x5-x3)+4*ls2.^2*ls3.^2*x6-4*ls2.^2*ls3.^2*x4)*kG2-4*ls2.^2*ls3.^2*x4*kG1-2*J_G3xx*l12*l23*ls2*ls3*(x8).^2*sin(x7-2*x5+x3)+(4*J_G3xx*l12*ls2.^2-2*J_G3xx*l12*l23*ls2)*ls3*(x8).^2*sin(x7-x3)-4*g*J_G3xx*l12*l23*ls2*sin(2*x5-x3)-4*u*J_G3xx*l12*l23*ls2*cos(2*x5-x3)+4*J_G3xx*l12.^2*l23*ls2*(x4).^2*sin(2*x5-2*x3)+(4*J_G2xx*l12*ls2*ls3.^2+4*J_G3xx*l12*l23*ls2.^2+4*J_G3xx*l12*l23.^2*ls2)*(x6).^2*sin(x5-x3)+(4*g*J_G2xx*l12*ls3.^2+4*g*J_G3xx*l12*ls2.^2-4*g*J_G3xx*l12*l23*ls2+4*g*J_G3xx*l12*l23.^2)*sin(x3)+(4*u*J_G2xx*l12*ls3.^2+4*u*J_G3xx*l12*ls2.^2-4*u*J_G3xx*l12*l23*ls2+4*u*J_G3xx*l12*l23.^2)*cos(x3))*mG2+((4*g*J_G2xx*ls1*ls3.^2+4*g*J_G3xx*l23.^2*ls1)*sin(x3)+(4*u*J_G2xx*ls1*ls3.^2+4*u*J_G3xx*l23.^2*ls1)*cos(x3))*mG1+((4*J_G2xx*l12*ls3*x8-4*J_G2xx*l12*ls3*x6)*cos(x7-x3)-4*J_G3xx*l12*l23*cos(x5-x3)*x8+4*J_G3xx*l12*l23*x6*cos(x5-x3))*kG3+((4*J_G3xx*l12*l23*x6-4*J_G3xx*l12*l23*x4)*cos(x5-x3)+(4*J_G2xx*ls3.^2+4*J_G3xx*l23.^2)*x6+((-4*J_G2xx*ls3.^2)-4*J_G3xx*l23.^2)*x4)*kG2+((-4*J_G2xx*ls3.^2)-4*J_G3xx*l23.^2)*x4*kG1+4*J_G2xx*J_G3xx*l12*ls3*(x8).^2*sin(x7-x3)+4*J_G2xx*J_G3xx*l12*l23*(x6).^2*sin(x5-x3)+4*g*J_G2xx*J_G3xx*l12*sin(x3)+4*u*J_G2xx*J_G3xx*l12*cos(x3))*mG3+((-2*g*J_G3xx*l12*ls2.^2*sin(2*x5-x3))-2*u*J_G3xx*l12*ls2.^2*cos(2*x5-x3)+2*J_G3xx*l12.^2*ls2.^2*(x4).^2*sin(2*x5-2*x3)+4*J_G3xx*l12*ls2.^3*(x6).^2*sin(x5-x3)+2*g*J_G3xx*l12*ls2.^2*sin(x3)+2*u*J_G3xx*l12*ls2.^2*cos(x3))*mG2.^2+((4*g*J_G3xx*ls1*ls2.^2*sin(x3)+4*u*J_G3xx*ls1*ls2.^2*cos(x3))*mG1+(4*J_G3xx*l12*ls2*x6*cos(x5-x3)-4*J_G3xx*l12*ls2*cos(x5-x3)*x8)*kG3+((4*J_G3xx*l12*ls2*x6-4*J_G3xx*l12*ls2*x4)*cos(x5-x3)+4*J_G3xx*ls2.^2*x6-4*J_G3xx*ls2.^2*x4)*kG2-4*J_G3xx*ls2.^2*x4*kG1+4*J_G2xx*J_G3xx*l12*ls2*(x6).^2*sin(x5-x3)+4*g*J_G2xx*J_G3xx*l12*sin(x3)+4*u*J_G2xx*J_G3xx*l12*cos(x3))*mG2+(4*g*J_G2xx*J_G3xx*ls1*sin(x3)+4*u*J_G2xx*J_G3xx*ls1*cos(x3))*mG1+(4*J_G2xx*J_G3xx*x6-4*J_G2xx*J_G3xx*x4)*kG2-4*J_G2xx*J_G3xx*x4*kG1)/((((2*l12.^2*l23*ls2-2*l12.^2*l23.^2)*ls3.^2*cos(2*x7-2*x5)+(2*l12.^2*l23*ls2-2*l12.^2*ls2.^2)*ls3.^2*cos(2*x7-2*x3)-2*l12.^2*l23*ls2*ls3.^2*cos(2*x5-2*x3)+(2*l12.^2*ls2.^2-2*l12.^2*l23*ls2+2*l12.^2*l23.^2)*ls3.^2)*mG2+(2*l23.^2*ls1.^2*ls3.^2-2*l23.^2*ls1.^2*ls3.^2*cos(2*x7-2*x5))*mG1-2*J_G1xx*l23.^2*ls3.^2*cos(2*x7-2*x5)-2*J_G2xx*l12.^2*ls3.^2*cos(2*x7-2*x3)-2*J_G3xx*l12.^2*l23.^2*cos(2*x5-2*x3)+(2*J_G1xx*l23.^2+2*J_G2xx*l12.^2)*ls3.^2+2*J_G3xx*l12.^2*l23.^2)*mG3.^2+((2*l12.^2*ls2.^2*ls3.^2-2*l12.^2*ls2.^2*ls3.^2*cos(2*x5-2*x3))*mG2.^2+(4*ls1.^2*ls2.^2*ls3.^2*mG1-4*J_G3xx*l12.^2*l23*ls2*cos(2*x5-2*x3)+(4*J_G1xx*ls2.^2+4*J_G2xx*l12.^2)*ls3.^2+4*J_G3xx*l12.^2*ls2.^2-4*J_G3xx*l12.^2*l23*ls2+4*J_G3xx*l12.^2*l23.^2)*mG2+(4*J_G2xx*ls1.^2*ls3.^2+4*J_G3xx*l23.^2*ls1.^2)*mG1+4*J_G1xx*J_G2xx*ls3.^2+4*J_G1xx*J_G3xx*l23.^2+4*J_G2xx*J_G3xx*l12.^2)*mG3+(2*J_G3xx*l12.^2*ls2.^2-2*J_G3xx*l12.^2*ls2.^2*cos(2*x5-2*x3))*mG2.^2+(4*J_G3xx*ls1.^2*ls2.^2*mG1+4*J_G1xx*J_G3xx*ls2.^2+4*J_G2xx*J_G3xx*l12.^2)*mG2+4*J_G2xx*J_G3xx*ls1.^2*mG1+4*J_G1xx*J_G2xx*J_G3xx);
  x6;
  -((((2*l12.^3*ls2-2*l12.^3*l23)*ls3.^2*(x4).^2*sin(2*x7-x5-x3)+(g*l12.^2*l23-g*l12.^2*ls2)*ls3.^2*sin(2*x7-x5-2*x3)+(u*l12.^2*ls2-u*l12.^2*l23)*ls3.^2*cos(2*x7-x5-2*x3)+(g*l12.^2*l23-g*l12.^2*ls2)*ls3.^2*sin(2*x7-x5)+(u*l12.^2*l23-u*l12.^2*ls2)*ls3.^2*cos(2*x7-x5)+(2*l12.^2*l23*ls2-2*l12.^2*l23.^2)*ls3.^2*(x6).^2*sin(2*x7-2*x5)+2*l12.^2*ls2*ls3.^3*(x8).^2*sin(x7+x5-2*x3)+(2*l12.^2*ls2-4*l12.^2*l23)*ls3.^3*(x8).^2*sin(x7-x5)+2*l12.^2*l23*ls2*ls3.^2*(x6).^2*sin(2*x5-2*x3)+(2*l12.^3*ls2+2*l12.^3*l23)*ls3.^2*(x4).^2*sin(x5-x3)+((-g*l12.^2*ls2)-g*l12.^2*l23)*ls3.^2*sin(x5-2*x3)+(u*l12.^2*ls2+u*l12.^2*l23)*ls3.^2*cos(x5-2*x3)+((-g*l12.^2*ls2)-g*l12.^2*l23)*ls3.^2*sin(x5)+((-u*l12.^2*ls2)-u*l12.^2*l23)*ls3.^2*cos(x5))*mG2+((-2*l12*l23*ls1.^2*ls3.^2*(x4).^2*sin(2*x7-x5-x3))+g*l12*l23*ls1*ls3.^2*sin(2*x7-x5-2*x3)-u*l12*l23*ls1*ls3.^2*cos(2*x7-x5-2*x3)+(2*g*l23*ls1.^2-g*l12*l23*ls1)*ls3.^2*sin(2*x7-x5)+(2*u*l23*ls1.^2-u*l12*l23*ls1)*ls3.^2*cos(2*x7-x5)-2*l23.^2*ls1.^2*ls3.^2*(x6).^2*sin(2*x7-2*x5)-4*l23*ls1.^2*ls3.^3*(x8).^2*sin(x7-x5)+2*l12*l23*ls1.^2*ls3.^2*(x4).^2*sin(x5-x3)-g*l12*l23*ls1*ls3.^2*sin(x5-2*x3)+u*l12*l23*ls1*ls3.^2*cos(x5-2*x3)+(g*l12*l23*ls1-2*g*l23*ls1.^2)*ls3.^2*sin(x5)+(u*l12*l23*ls1-2*u*l23*ls1.^2)*ls3.^2*cos(x5))*mG1+((2*l12.^2*ls3.^2*x8-2*l12.^2*ls3.^2*x6)*cos(2*x7-2*x3)+(2*l12.^2*l23*ls3*x8-2*l12.^2*l23*ls3*x6)*cos(x7+x5-2*x3)+(2*l12.^2*l23*ls3*x6-2*l12.^2*l23*ls3*x8)*cos(x7-x5)-2*l12.^2*ls3.^2*x8+2*l12.^2*ls3.^2*x6)*kG3+((2*l12*l23*ls3.^2*x4-2*l12*l23*ls3.^2*x6)*cos(2*x7-x5-x3)+(2*l12.^2*ls3.^2*x4-2*l12.^2*ls3.^2*x6)*cos(2*x7-2*x3)+(2*l12*l23*ls3.^2*x6-2*l12*l23*ls3.^2*x4)*cos(x5-x3)+2*l12.^2*ls3.^2*x6-2*l12.^2*ls3.^2*x4)*kG2+(2*l12*l23*ls3.^2*x4*cos(2*x7-x5-x3)-2*l12*l23*ls3.^2*x4*cos(x5-x3))*kG1-2*J_G1xx*l12*l23*ls3.^2*(x4).^2*sin(2*x7-x5-x3)+2*g*J_G1xx*l23*ls3.^2*sin(2*x7-x5)+2*u*J_G1xx*l23*ls3.^2*cos(2*x7-x5)-2*J_G1xx*l23.^2*ls3.^2*(x6).^2*sin(2*x7-2*x5)+2*J_G3xx*l12.^2*l23*ls3*(x8).^2*sin(x7+x5-2*x3)+((-4*J_G1xx*l23*ls3.^3)-2*J_G3xx*l12.^2*l23*ls3)*(x8).^2*sin(x7-x5)+2*J_G3xx*l12.^2*l23.^2*(x6).^2*sin(2*x5-2*x3)+(2*J_G1xx*l12*l23*ls3.^2+4*J_G3xx*l12.^3*l23)*(x4).^2*sin(x5-x3)-2*g*J_G3xx*l12.^2*l23*sin(x5-2*x3)+2*u*J_G3xx*l12.^2*l23*cos(x5-2*x3)+((-2*g*J_G1xx*l23*ls3.^2)-2*g*J_G3xx*l12.^2*l23)*sin(x5)+((-2*u*J_G1xx*l23*ls3.^2)-2*u*J_G3xx*l12.^2*l23)*cos(x5))*mG3.^2+((2*l12.^2*ls2.^2*ls3.^2*(x6).^2*sin(2*x5-2*x3)+4*l12.^3*ls2*ls3.^2*(x4).^2*sin(x5-x3)-2*g*l12.^2*ls2*ls3.^2*sin(x5-2*x3)+2*u*l12.^2*ls2*ls3.^2*cos(x5-2*x3)-2*g*l12.^2*ls2*ls3.^2*sin(x5)-2*u*l12.^2*ls2*ls3.^2*cos(x5))*mG2.^2+((4*l12*ls1.^2*ls2*ls3.^2*(x4).^2*sin(x5-x3)-2*g*l12*ls1*ls2*ls3.^2*sin(x5-2*x3)+2*u*l12*ls1*ls2*ls3.^2*cos(x5-2*x3)+(2*g*l12*ls1-4*g*ls1.^2)*ls2*ls3.^2*sin(x5)+(2*u*l12*ls1-4*u*ls1.^2)*ls2*ls3.^2*cos(x5))*mG1+((2*l12.^2*ls2*ls3*x8-2*l12.^2*ls2*ls3*x6)*cos(x7+x5-2*x3)+((2*l12.^2*ls2-4*l12.^2*l23)*ls3*x8+(4*l12.^2*l23-2*l12.^2*ls2)*ls3*x6)*cos(x7-x5)-4*l12.^2*ls3.^2*x8+4*l12.^2*ls3.^2*x6)*kG3+((4*l12*ls2*ls3.^2*x6-4*l12*ls2*ls3.^2*x4)*cos(x5-x3)+4*l12.^2*ls3.^2*x6-4*l12.^2*ls3.^2*x4)*kG2-4*l12*ls2*ls3.^2*x4*cos(x5-x3)*kG1+2*J_G3xx*l12.^2*ls2*ls3*(x8).^2*sin(x7+x5-2*x3)+(2*J_G3xx*l12.^2*ls2-4*J_G3xx*l12.^2*l23)*ls3*(x8).^2*sin(x7-x5)+4*J_G3xx*l12.^2*l23*ls2*(x6).^2*sin(2*x5-2*x3)+(4*J_G1xx*l12*ls2*ls3.^2+4*J_G3xx*l12.^3*ls2+4*J_G3xx*l12.^3*l23)*(x4).^2*sin(x5-x3)+((-2*g*J_G3xx*l12.^2*ls2)-2*g*J_G3xx*l12.^2*l23)*sin(x5-2*x3)+(2*u*J_G3xx*l12.^2*ls2+2*u*J_G3xx*l12.^2*l23)*cos(x5-2*x3)+((-4*g*J_G1xx*ls2*ls3.^2)-2*g*J_G3xx*l12.^2*ls2-2*g*J_G3xx*l12.^2*l23)*sin(x5)+((-4*u*J_G1xx*ls2*ls3.^2)-2*u*J_G3xx*l12.^2*ls2-2*u*J_G3xx*l12.^2*l23)*cos(x5))*mG2+(((4*l23*ls1.^2*ls3*x6-4*l23*ls1.^2*ls3*x8)*cos(x7-x5)-4*ls1.^2*ls3.^2*x8+4*ls1.^2*ls3.^2*x6)*kG3+(4*ls1.^2*ls3.^2*x6-4*ls1.^2*ls3.^2*x4)*kG2-4*J_G3xx*l23*ls1.^2*ls3*(x8).^2*sin(x7-x5)+4*J_G3xx*l12*l23*ls1.^2*(x4).^2*sin(x5-x3)-2*g*J_G3xx*l12*l23*ls1*sin(x5-2*x3)+2*u*J_G3xx*l12*l23*ls1*cos(x5-2*x3)+(2*g*J_G3xx*l12*l23*ls1-4*g*J_G3xx*l23*ls1.^2)*sin(x5)+(2*u*J_G3xx*l12*l23*ls1-4*u*J_G3xx*l23*ls1.^2)*cos(x5))*mG1+((4*J_G1xx*l23*ls3*x6-4*J_G1xx*l23*ls3*x8)*cos(x7-x5)+((-4*J_G1xx*ls3.^2)-4*J_G3xx*l12.^2)*x8+(4*J_G1xx*ls3.^2+4*J_G3xx*l12.^2)*x6)*kG3+((4*J_G3xx*l12*l23*x6-4*J_G3xx*l12*l23*x4)*cos(x5-x3)+(4*J_G1xx*ls3.^2+4*J_G3xx*l12.^2)*x6+((-4*J_G1xx*ls3.^2)-4*J_G3xx*l12.^2)*x4)*kG2-4*J_G3xx*l12*l23*x4*cos(x5-x3)*kG1-4*J_G1xx*J_G3xx*l23*ls3*(x8).^2*sin(x7-x5)+4*J_G1xx*J_G3xx*l12*l23*(x4).^2*sin(x5-x3)-4*g*J_G1xx*J_G3xx*l23*sin(x5)-4*u*J_G1xx*J_G3xx*l23*cos(x5))*mG3+(2*J_G3xx*l12.^2*ls2.^2*(x6).^2*sin(2*x5-2*x3)+4*J_G3xx*l12.^3*ls2*(x4).^2*sin(x5-x3)-2*g*J_G3xx*l12.^2*ls2*sin(x5-2*x3)+2*u*J_G3xx*l12.^2*ls2*cos(x5-2*x3)-2*g*J_G3xx*l12.^2*ls2*sin(x5)-2*u*J_G3xx*l12.^2*ls2*cos(x5))*mG2.^2+((4*J_G3xx*l12*ls1.^2*ls2*(x4).^2*sin(x5-x3)-2*g*J_G3xx*l12*ls1*ls2*sin(x5-2*x3)+2*u*J_G3xx*l12*ls1*ls2*cos(x5-2*x3)+(2*g*J_G3xx*l12*ls1-4*g*J_G3xx*ls1.^2)*ls2*sin(x5)+(2*u*J_G3xx*l12*ls1-4*u*J_G3xx*ls1.^2)*ls2*cos(x5))*mG1+(4*J_G3xx*l12.^2*x6-4*J_G3xx*l12.^2*x8)*kG3+((4*J_G3xx*l12*ls2*x6-4*J_G3xx*l12*ls2*x4)*cos(x5-x3)+4*J_G3xx*l12.^2*x6-4*J_G3xx*l12.^2*x4)*kG2-4*J_G3xx*l12*ls2*x4*cos(x5-x3)*kG1+4*J_G1xx*J_G3xx*l12*ls2*(x4).^2*sin(x5-x3)-4*g*J_G1xx*J_G3xx*ls2*sin(x5)-4*u*J_G1xx*J_G3xx*ls2*cos(x5))*mG2+((4*J_G3xx*ls1.^2*x6-4*J_G3xx*ls1.^2*x8)*kG3+(4*J_G3xx*ls1.^2*x6-4*J_G3xx*ls1.^2*x4)*kG2)*mG1+(4*J_G1xx*J_G3xx*x6-4*J_G1xx*J_G3xx*x8)*kG3+(4*J_G1xx*J_G3xx*x6-4*J_G1xx*J_G3xx*x4)*kG2)/((((2*l12.^2*l23*ls2-2*l12.^2*l23.^2)*ls3.^2*cos(2*x7-2*x5)+(2*l12.^2*l23*ls2-2*l12.^2*ls2.^2)*ls3.^2*cos(2*x7-2*x3)-2*l12.^2*l23*ls2*ls3.^2*cos(2*x5-2*x3)+(2*l12.^2*ls2.^2-2*l12.^2*l23*ls2+2*l12.^2*l23.^2)*ls3.^2)*mG2+(2*l23.^2*ls1.^2*ls3.^2-2*l23.^2*ls1.^2*ls3.^2*cos(2*x7-2*x5))*mG1-2*J_G1xx*l23.^2*ls3.^2*cos(2*x7-2*x5)-2*J_G2xx*l12.^2*ls3.^2*cos(2*x7-2*x3)-2*J_G3xx*l12.^2*l23.^2*cos(2*x5-2*x3)+(2*J_G1xx*l23.^2+2*J_G2xx*l12.^2)*ls3.^2+2*J_G3xx*l12.^2*l23.^2)*mG3.^2+((2*l12.^2*ls2.^2*ls3.^2-2*l12.^2*ls2.^2*ls3.^2*cos(2*x5-2*x3))*mG2.^2+(4*ls1.^2*ls2.^2*ls3.^2*mG1-4*J_G3xx*l12.^2*l23*ls2*cos(2*x5-2*x3)+(4*J_G1xx*ls2.^2+4*J_G2xx*l12.^2)*ls3.^2+4*J_G3xx*l12.^2*ls2.^2-4*J_G3xx*l12.^2*l23*ls2+4*J_G3xx*l12.^2*l23.^2)*mG2+(4*J_G2xx*ls1.^2*ls3.^2+4*J_G3xx*l23.^2*ls1.^2)*mG1+4*J_G1xx*J_G2xx*ls3.^2+4*J_G1xx*J_G3xx*l23.^2+4*J_G2xx*J_G3xx*l12.^2)*mG3+(2*J_G3xx*l12.^2*ls2.^2-2*J_G3xx*l12.^2*ls2.^2*cos(2*x5-2*x3))*mG2.^2+(4*J_G3xx*ls1.^2*ls2.^2*mG1+4*J_G1xx*J_G3xx*ls2.^2+4*J_G2xx*J_G3xx*l12.^2)*mG2+4*J_G2xx*J_G3xx*ls1.^2*mG1+4*J_G1xx*J_G2xx*J_G3xx);
  x8;
  ((((2*l12.^2*l23*ls2-2*l12.^2*l23.^2)*ls3.^2*(x8).^2*sin(2*x7-2*x5)+(2*l12.^2*l23*ls2-2*l12.^2*ls2.^2)*ls3.^2*(x8).^2*sin(2*x7-2*x3)+(2*l12.^2*l23.^2*ls2-2*l12.^2*l23*ls2.^2)*ls3*(x6).^2*sin(x7+x5-2*x3)+((-2*l12.^2*l23*ls2.^2)+6*l12.^2*l23.^2*ls2-4*l12.^2*l23.^3)*ls3*(x6).^2*sin(x7-x5)+(g*l12.^2*l23.^2-g*l12.^2*l23*ls2)*ls3*sin(x7-2*x5+2*x3)+(u*l12.^2*l23.^2-u*l12.^2*l23*ls2)*ls3*cos(x7-2*x5+2*x3)+(2*l12.^3*l23*ls2-2*l12.^3*l23.^2)*ls3*(x4).^2*sin(x7-2*x5+x3)+(g*l12.^2*l23.^2-g*l12.^2*l23*ls2)*ls3*sin(x7-2*x5)+(u*l12.^2*l23*ls2-u*l12.^2*l23.^2)*ls3*cos(x7-2*x5)+((-4*l12.^3*ls2.^2)+6*l12.^3*l23*ls2-2*l12.^3*l23.^2)*ls3*(x4).^2*sin(x7-x3)+(2*g*l12.^2*ls2.^2-3*g*l12.^2*l23*ls2+g*l12.^2*l23.^2)*ls3*sin(x7-2*x3)+((-2*u*l12.^2*ls2.^2)+3*u*l12.^2*l23*ls2-u*l12.^2*l23.^2)*ls3*cos(x7-2*x3)+(2*g*l12.^2*ls2.^2-3*g*l12.^2*l23*ls2+g*l12.^2*l23.^2)*ls3*sin(x7)+(2*u*l12.^2*ls2.^2-3*u*l12.^2*l23*ls2+u*l12.^2*l23.^2)*ls3*cos(x7))*mG2+((-2*l23.^2*ls1.^2*ls3.^2*(x8).^2*sin(2*x7-2*x5))-4*l23.^3*ls1.^2*ls3*(x6).^2*sin(x7-x5)+g*l12*l23.^2*ls1*ls3*sin(x7-2*x5+2*x3)+u*l12*l23.^2*ls1*ls3*cos(x7-2*x5+2*x3)-2*l12*l23.^2*ls1.^2*ls3*(x4).^2*sin(x7-2*x5+x3)+(2*g*l23.^2*ls1.^2-g*l12*l23.^2*ls1)*ls3*sin(x7-2*x5)+(u*l12*l23.^2*ls1-2*u*l23.^2*ls1.^2)*ls3*cos(x7-2*x5)-2*l12*l23.^2*ls1.^2*ls3*(x4).^2*sin(x7-x3)+g*l12*l23.^2*ls1*ls3*sin(x7-2*x3)-u*l12*l23.^2*ls1*ls3*cos(x7-2*x3)+(2*g*l23.^2*ls1.^2-g*l12*l23.^2*ls1)*ls3*sin(x7)+(2*u*l23.^2*ls1.^2-u*l12*l23.^2*ls1)*ls3*cos(x7))*mG1+((2*l12.^2*l23*ls3*x8-2*l12.^2*l23*ls3*x6)*cos(x7+x5-2*x3)+(2*l12.^2*l23*ls3*x6-2*l12.^2*l23*ls3*x8)*cos(x7-x5)+(2*l12.^2*l23.^2*cos(2*x5-2*x3)-2*l12.^2*l23.^2)*x8-2*l12.^2*l23.^2*x6*cos(2*x5-2*x3)+2*l12.^2*l23.^2*x6)*kG3+((2*l12.^2*l23*ls3*x4-2*l12.^2*l23*ls3*x6)*cos(x7+x5-2*x3)+(2*l12.^2*l23*ls3*x6-2*l12.^2*l23*ls3*x4)*cos(x7-x5)+(2*l12*l23.^2*ls3*x6-2*l12*l23.^2*ls3*x4)*cos(x7-2*x5+x3)+(2*l12*l23.^2*ls3*x4-2*l12*l23.^2*ls3*x6)*cos(x7-x3))*kG2+(2*l12*l23.^2*ls3*x4*cos(x7-x3)-2*l12*l23.^2*ls3*x4*cos(x7-2*x5+x3))*kG1-2*J_G1xx*l23.^2*ls3.^2*(x8).^2*sin(2*x7-2*x5)-2*J_G2xx*l12.^2*ls3.^2*(x8).^2*sin(2*x7-2*x3)-2*J_G2xx*l12.^2*l23*ls3*(x6).^2*sin(x7+x5-2*x3)+((-4*J_G1xx*l23.^3)-2*J_G2xx*l12.^2*l23)*ls3*(x6).^2*sin(x7-x5)-2*J_G1xx*l12*l23.^2*ls3*(x4).^2*sin(x7-2*x5+x3)+2*g*J_G1xx*l23.^2*ls3*sin(x7-2*x5)-2*u*J_G1xx*l23.^2*ls3*cos(x7-2*x5)+((-2*J_G1xx*l12*l23.^2)-4*J_G2xx*l12.^3)*ls3*(x4).^2*sin(x7-x3)+2*g*J_G2xx*l12.^2*ls3*sin(x7-2*x3)-2*u*J_G2xx*l12.^2*ls3*cos(x7-2*x3)+(2*g*J_G1xx*l23.^2+2*g*J_G2xx*l12.^2)*ls3*sin(x7)+(2*u*J_G1xx*l23.^2+2*u*J_G2xx*l12.^2)*ls3*cos(x7))*mG3.^2+(((2*l12.^2*l23*ls2.^2-2*l12.^2*ls2.^3)*ls3*(x6).^2*sin(x7+x5-2*x3)+(2*l12.^2*ls2.^3-2*l12.^2*l23*ls2.^2)*ls3*(x6).^2*sin(x7-x5)+(g*l12.^2*l23*ls2-g*l12.^2*ls2.^2)*ls3*sin(x7-2*x5+2*x3)+(u*l12.^2*l23*ls2-u*l12.^2*ls2.^2)*ls3*cos(x7-2*x5+2*x3)+(2*l12.^3*ls2.^2-2*l12.^3*l23*ls2)*ls3*(x4).^2*sin(x7-2*x5+x3)+(g*l12.^2*l23*ls2-g*l12.^2*ls2.^2)*ls3*sin(x7-2*x5)+(u*l12.^2*ls2.^2-u*l12.^2*l23*ls2)*ls3*cos(x7-2*x5)+(2*l12.^3*l23*ls2-2*l12.^3*ls2.^2)*ls3*(x4).^2*sin(x7-x3)+(g*l12.^2*ls2.^2-g*l12.^2*l23*ls2)*ls3*sin(x7-2*x3)+(u*l12.^2*l23*ls2-u*l12.^2*ls2.^2)*ls3*cos(x7-2*x3)+(g*l12.^2*ls2.^2-g*l12.^2*l23*ls2)*ls3*sin(x7)+(u*l12.^2*ls2.^2-u*l12.^2*l23*ls2)*ls3*cos(x7))*mG2.^2+(((-4*l23*ls1.^2*ls2.^2*ls3*(x6).^2*sin(x7-x5))+g*l12*l23*ls1*ls2*ls3*sin(x7-2*x5+2*x3)+u*l12*l23*ls1*ls2*ls3*cos(x7-2*x5+2*x3)-2*l12*l23*ls1.^2*ls2*ls3*(x4).^2*sin(x7-2*x5+x3)+(2*g*l23*ls1.^2-g*l12*l23*ls1)*ls2*ls3*sin(x7-2*x5)+(u*l12*l23*ls1-2*u*l23*ls1.^2)*ls2*ls3*cos(x7-2*x5)+(2*l12*l23*ls1.^2*ls2-4*l12*ls1.^2*ls2.^2)*ls3*(x4).^2*sin(x7-x3)+(2*g*l12*ls1*ls2.^2-g*l12*l23*ls1*ls2)*ls3*sin(x7-2*x3)+(u*l12*l23*ls1*ls2-2*u*l12*ls1*ls2.^2)*ls3*cos(x7-2*x3)+((4*g*ls1.^2-2*g*l12*ls1)*ls2.^2+(g*l12*l23*ls1-2*g*l23*ls1.^2)*ls2)*ls3*sin(x7)+((4*u*ls1.^2-2*u*l12*ls1)*ls2.^2+(u*l12*l23*ls1-2*u*l23*ls1.^2)*ls2)*ls3*cos(x7))*mG1+((2*l12.^2*ls2*ls3*x8-2*l12.^2*ls2*ls3*x6)*cos(x7+x5-2*x3)+((2*l12.^2*ls2-4*l12.^2*l23)*ls3*x8+(4*l12.^2*l23-2*l12.^2*ls2)*ls3*x6)*cos(x7-x5)+(4*l12.^2*l23*ls2*cos(2*x5-2*x3)-4*l12.^2*ls2.^2+4*l12.^2*l23*ls2-4*l12.^2*l23.^2)*x8-4*l12.^2*l23*ls2*x6*cos(2*x5-2*x3)+(4*l12.^2*ls2.^2-4*l12.^2*l23*ls2+4*l12.^2*l23.^2)*x6)*kG3+((2*l12.^2*ls2*ls3*x4-2*l12.^2*ls2*ls3*x6)*cos(x7+x5-2*x3)+((4*l12.^2*l23-2*l12.^2*ls2)*ls3*x6+(2*l12.^2*ls2-4*l12.^2*l23)*ls3*x4)*cos(x7-x5)+(2*l12*l23*ls2*ls3*x6-2*l12*l23*ls2*ls3*x4)*cos(x7-2*x5+x3)+((2*l12*l23*ls2-4*l12*ls2.^2)*ls3*x6+(4*l12*ls2.^2-2*l12*l23*ls2)*ls3*x4)*cos(x7-x3))*kG2+((4*l12*ls2.^2-2*l12*l23*ls2)*ls3*x4*cos(x7-x3)-2*l12*l23*ls2*ls3*x4*cos(x7-2*x5+x3))*kG1-2*J_G2xx*l12.^2*ls2*ls3*(x6).^2*sin(x7+x5-2*x3)+((-4*J_G1xx*l23*ls2.^2)+2*J_G2xx*l12.^2*ls2-4*J_G2xx*l12.^2*l23)*ls3*(x6).^2*sin(x7-x5)-2*J_G1xx*l12*l23*ls2*ls3*(x4).^2*sin(x7-2*x5+x3)+2*g*J_G1xx*l23*ls2*ls3*sin(x7-2*x5)-2*u*J_G1xx*l23*ls2*ls3*cos(x7-2*x5)+((-4*J_G1xx*l12*ls2.^2)+2*J_G1xx*l12*l23*ls2-4*J_G2xx*l12.^3)*ls3*(x4).^2*sin(x7-x3)+2*g*J_G2xx*l12.^2*ls3*sin(x7-2*x3)-2*u*J_G2xx*l12.^2*ls3*cos(x7-2*x3)+(4*g*J_G1xx*ls2.^2-2*g*J_G1xx*l23*ls2+2*g*J_G2xx*l12.^2)*ls3*sin(x7)+(4*u*J_G1xx*ls2.^2-2*u*J_G1xx*l23*ls2+2*u*J_G2xx*l12.^2)*ls3*cos(x7))*mG2+(((4*l23*ls1.^2*ls3*x6-4*l23*ls1.^2*ls3*x8)*cos(x7-x5)-4*l23.^2*ls1.^2*x8+4*l23.^2*ls1.^2*x6)*kG3+(4*l23*ls1.^2*ls3*x6-4*l23*ls1.^2*ls3*x4)*cos(x7-x5)*kG2-4*J_G2xx*l23*ls1.^2*ls3*(x6).^2*sin(x7-x5)-4*J_G2xx*l12*ls1.^2*ls3*(x4).^2*sin(x7-x3)+2*g*J_G2xx*l12*ls1*ls3*sin(x7-2*x3)-2*u*J_G2xx*l12*ls1*ls3*cos(x7-2*x3)+(4*g*J_G2xx*ls1.^2-2*g*J_G2xx*l12*ls1)*ls3*sin(x7)+(4*u*J_G2xx*ls1.^2-2*u*J_G2xx*l12*ls1)*ls3*cos(x7))*mG1+((4*J_G1xx*l23*ls3*x6-4*J_G1xx*l23*ls3*x8)*cos(x7-x5)+((-4*J_G1xx*l23.^2)-4*J_G2xx*l12.^2)*x8+(4*J_G1xx*l23.^2+4*J_G2xx*l12.^2)*x6)*kG3+((4*J_G1xx*l23*ls3*x6-4*J_G1xx*l23*ls3*x4)*cos(x7-x5)+(4*J_G2xx*l12*ls3*x4-4*J_G2xx*l12*ls3*x6)*cos(x7-x3))*kG2+4*J_G2xx*l12*ls3*x4*cos(x7-x3)*kG1-4*J_G1xx*J_G2xx*l23*ls3*(x6).^2*sin(x7-x5)-4*J_G1xx*J_G2xx*l12*ls3*(x4).^2*sin(x7-x3)+4*g*J_G1xx*J_G2xx*ls3*sin(x7)+4*u*J_G1xx*J_G2xx*ls3*cos(x7))*mG3+((2*l12.^2*ls2.^2*cos(2*x5-2*x3)-2*l12.^2*ls2.^2)*x8-2*l12.^2*ls2.^2*x6*cos(2*x5-2*x3)+2*l12.^2*ls2.^2*x6)*kG3*mG2.^2+((4*ls1.^2*ls2.^2*x6-4*ls1.^2*ls2.^2*x8)*kG3*mG1+(((-4*J_G1xx*ls2.^2)-4*J_G2xx*l12.^2)*x8+(4*J_G1xx*ls2.^2+4*J_G2xx*l12.^2)*x6)*kG3)*mG2+(4*J_G2xx*ls1.^2*x6-4*J_G2xx*ls1.^2*x8)*kG3*mG1+(4*J_G1xx*J_G2xx*x6-4*J_G1xx*J_G2xx*x8)*kG3)/((((2*l12.^2*l23*ls2-2*l12.^2*l23.^2)*ls3.^2*cos(2*x7-2*x5)+(2*l12.^2*l23*ls2-2*l12.^2*ls2.^2)*ls3.^2*cos(2*x7-2*x3)-2*l12.^2*l23*ls2*ls3.^2*cos(2*x5-2*x3)+(2*l12.^2*ls2.^2-2*l12.^2*l23*ls2+2*l12.^2*l23.^2)*ls3.^2)*mG2+(2*l23.^2*ls1.^2*ls3.^2-2*l23.^2*ls1.^2*ls3.^2*cos(2*x7-2*x5))*mG1-2*J_G1xx*l23.^2*ls3.^2*cos(2*x7-2*x5)-2*J_G2xx*l12.^2*ls3.^2*cos(2*x7-2*x3)-2*J_G3xx*l12.^2*l23.^2*cos(2*x5-2*x3)+(2*J_G1xx*l23.^2+2*J_G2xx*l12.^2)*ls3.^2+2*J_G3xx*l12.^2*l23.^2)*mG3.^2+((2*l12.^2*ls2.^2*ls3.^2-2*l12.^2*ls2.^2*ls3.^2*cos(2*x5-2*x3))*mG2.^2+(4*ls1.^2*ls2.^2*ls3.^2*mG1-4*J_G3xx*l12.^2*l23*ls2*cos(2*x5-2*x3)+(4*J_G1xx*ls2.^2+4*J_G2xx*l12.^2)*ls3.^2+4*J_G3xx*l12.^2*ls2.^2-4*J_G3xx*l12.^2*l23*ls2+4*J_G3xx*l12.^2*l23.^2)*mG2+(4*J_G2xx*ls1.^2*ls3.^2+4*J_G3xx*l23.^2*ls1.^2)*mG1+4*J_G1xx*J_G2xx*ls3.^2+4*J_G1xx*J_G3xx*l23.^2+4*J_G2xx*J_G3xx*l12.^2)*mG3+(2*J_G3xx*l12.^2*ls2.^2-2*J_G3xx*l12.^2*ls2.^2*cos(2*x5-2*x3))*mG2.^2+(4*J_G3xx*ls1.^2*ls2.^2*mG1+4*J_G1xx*J_G3xx*ls2.^2+4*J_G2xx*J_G3xx*l12.^2)*mG2+4*J_G2xx*J_G3xx*ls1.^2*mG1+4*J_G1xx*J_G2xx*J_G3xx)];

%--------------------------------------------------------------
%Linearisierung und Zustandsrückführung
%--------------------------------------------------------------

%------------------------------Ruhelagen------------------------
xv = {x1,x2,x3,x4,x5,x6,x7,x8,u}; %xv-Zustandsliste
xs000 = {0,0,0,0,0,0,0,0,0};    %xs000 dedeutet:Winkel1 oben, Winkel 2 oben, Winkel 3 oben
xs100 = {0,0,pi,0,0,0,0,0,0};   %xs100 dedeutet:Winkel1 unten, Winkel 2 oben, Winkel 3 oben
xs010 = {0,0,0,0,pi,0,0,0,0};   
xs001 = {0,0,0,0,0,0,pi,0,0};
xs110 = {0,0,pi,0,pi,0,0,0,0};
xs011 = {0,0,0,0,pi,0,pi,0,0};
xs101 = {0,0,pi,0,0,0,pi,0,0};
xs111 = {0,0,pi,0,pi,0,pi,0,0}; 

xs999 = {1,2,3,4,5,6,7,8,1};

x0_pendel = [0 0 0 0 pi 0 0 0]; %Anfangszustand für Simulinkmodell

rl=xs011;
%------------------------------Polvorgabe-----------------------
pol2 =[-2,-2,-1.64,-5.38,-1.04+6.8*1i,-1.04-6.8*1i,-1.35,-8.97]; %Wunscheigenwerte für RL xs010
pol3 =[-2,-1,-1.008+4.56i,-1.008-4.56i,-7.81,-1.03+8.25i,-1.03-8.25i,-4.54]; %Wunscheigenwerte für RL xs011
%----------------------------------------------------------------

x=[x1;x2;x3;x4;x5;x6;x7;x8]; %Zustandsvektor
A=jacobian(f2,x); %f(x,u) nach vektor x ableiten - ergibt Dynamikmatrix
b=jacobian(f2,u); %f(x,u) nach u ableiten ergibt - Eingangsvektor


A_RL = double(subs(A,xv,rl)) %Linearisierte Dynamikmatrix in RL-xs
b_RL = double(subs(b,xv,rl)) %Linearisierter Eingangsvektor in RL-xs
%hier werden die Symbole x1...x9 in double umgewandelt -> die Ruhelage um
%die linearisiert wird, stehen in xv mit den Werten von rl

% -----------------------Ackermann------------------------------
 k1=acker(A_RL,b_RL,pol2) %Zustandsrückführvektor nach Ackermann
% ---------------------------LQR--------------------------------
 Q=eye(8)*3; %Geschwindigkeit mit der die einzelnen Zustände gegen Null getrieben werden 
 R=1; %R=skalar falls ein Eingang;Je größer DiaMatrix desto kleiner Stellgröße -> desto langsamer Regelung
 k2=lqr(A_RL,b_RL,Q,R)