function y=f(x,tau)
    l1=1;l2=1; lt1=0.5;lt2=0.5; % m
    m1=1;m2=1; % kg
    I1=1/3; I2=1/3; % kg * m^2
    g=9.81; % m/s^2
    a=I1+m2*l1^2;
    b=I2;
    c=m2*lt2^2/2;
    d=m2*l1*lt2;
    e=(m1*lt1+m2*l1)*g;
    f=m2*g*lt2;
    h=d+c*cos(x(2));
    MI=inv([a+2*h,h;h,b]);
    csb=c*sin(x(2));
    fsab=f*sin(x(1)+x(2));
    y=[x(3);x(4);MI*[csb*(2*x(3)*x(4)+x(4)^2)+e*sin(x(1))+fsab;csb*x(3)*x(4)+fsab+tau]];
end