clear all

%%%%%%% Bekannte Daten %%%%%%%%
g=9.81; %[m/s^2] Erdbeschleunigung

mStab=0.07; %[kg] Masse Stab
mK=0.02; %[kg] Masse Kugel
lges=0.633; %[m] komplette Pendelstablänge mit Kugel
lStab=0.610; %[m] Schwerpunkt Stab
lKugel=lges-0.0125; %[m] Abstand Kugel zum Stabende



rs=0.362; %[m]Länge Drehpunkt zum Schwerpunkt Pendel mit Kugel
mp=mStab+mK; %[kg] Masse Pendel mit Kugel

ur=10; %[kg/s] Reibwert des Pendelwagens

mw=0.5;   %[kg] Masse Pendelwagen

rStab=lges-lStab/2; %[m] Abstand Stab zum gemeinsamen Schwerpunkt
rK=lges-rs-0.0125; %[m] Abstand Kugel zum gemeinsamen Schwerpunkt
%JA[kgm^2] gesamte Trägheit Pendelstange zum gemeinsamen Schwerpunkt
JA=1/12*mStab*lStab^2+mStab*rStab^2+mK*rK^2; 

J=JA+mp*rs^2; %[kgm^2] Trägheit Pendelstab zum Drehpunkt

mges=mp+mw; %Gesamtmasse von Pendel mit Kopf und Pendelwagen

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Zustandsraummodell xp=Ax+Bx  y=Cx+Dx
%A-Systemmatrix
a5=mp*rs*ur/(J*mges-mp^2*rs^2);
a6=mp*rs*g*mges/(J*mges-mp^2-rs^2);

a8= -ur/(J*mges-mp^2*rs^2);
a9= -mp^2*rs^2*g/(J*mges-mp^2*rs^2);

A=[0    1   0   0
   0    a8  a9  0
   0    0   0   1
   0    a5  a6  0];
%B-Eingangsmatrix
a7=J/(J*mges-mp^2*rs^2);
a4= -mp*rs/(J*mges-mp^2*rs^2);

B=[0
   a7
   0
   a4];

%C-Ausgabematrix (2 Reihen da Wagen Posi und Pendelposi output sind)
C=[1 0 0 0
   0 0 1 0];
%D-
D=[0
   0];