function fx=fx_inv_pendel(t,x)
% fx_inv_pendel
% definering av hoeyresiden i ODE for ulineaer modell av to 
% inverterte pendler som staar paa en vogn. Systemet er 
% stabilisert (regulert) med en LQ-optimal regulator, der
% vi antar at alle tilstander maales.
% Inngangsvariable:
% t - dummy parameter.
% x - tilstandsvektoren av dimensjon 4, dvs. x=[x1;x2;x3,x4]
% der x1 er vinkelutslag pendel 1, x2 er vinkelhastighet for 
% pendel 1, x3 er vinkelutslag pendel 2, x4 er vinkelhastighet for 
% pendel 2.
% Utgangsparametre:
% fx - hoeyresiden i ODE ligningene, dot(x)=fx.
l1=0.7; l2=0.4;
m11=0.044; m22=0.025;
m1l=0.2; m2l=0.2;
%
g=9.81;
m1=m11+m1l;
m2=m22+m2l;
lg1=l1*(m11+2*m1l)/(2*(m11+m1l));
lg2=l2*(m22+2*m2l)/(2*(m22+m2l));
Jt1=m11*(l1^2-3*l1*lg1+3*lg1^2)/3+m1l*(l1-lg1)^2;
Jt2=m22*(l2^2-3*l2*lg2+3*lg2^2)/3+m2l*(l2-lg2)^2;

% lg1=0.637; lg2=0.378; Jt1=6.2147e-3; Jt2=1.2222e-3;

% Lineaer modell for to inverterte pendler
a21=(m1*lg1*g)/(Jt1+m1*lg1^2);
b21=(m1*lg1)/(Jt1+m1*lg1^2);

a43=(m2*lg2*g)/(Jt2+m2*lg2^2);
b41=(m2*lg2)/(Jt2+m2*lg2^2);

fx=zeros(4,1);
G=[168.2771   46.2487 -167.2272  -35.0440];
u=-G*x;                                  % LQ optimal regulator.

ialt=1;
if ialt == 1
 dotv=u;                                 % forenklet modell for vogn.
elseif ialt == 2 
 dotv=-m1*g*tan(x(1))-m2*g*tan(x(3))+u;  % alternativ modell for vogn.
end 

fx(1)=x(2);
fx(2)=a21*sin(x(1)) - b21*cos(x(1))*dotv;
%fx(2)=a21*x(1) - b21*u;
fx(3)=x(4);
fx(4)=a43*sin(x(3)) - b41*cos(x(3))*dotv;
%fx(4)=a43*x(3) - b41*u;
