%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simple ODE system for damped pendulum
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dx_dt = f(t,x)
x1 = x(1);	%theta1
x2 = x(2);	%theta1_dot
x3 = x(3);	%theta2
x4 = x(4);	%theta2_dot
global  g_L
M = [1, 0, 0, 0;  
     0, 2, 0, cos(x1-x3);
     0, 0, 1, 0;
     0, cos(x3-x1), 0, 1];

dx1 = x2;
dx2 = -x4.^2*sin(x1-x3)-2*g_L*sin(x1);
dx3 = x4;
dx4 = x2.^2*sin(x1-x3)-g_L*sin(x3);
F = [ dx1; dx2; dx3; dx4];

dx_dt = inv(M)*F;