Inverted Pendulum (State Eq.)

State equation:



 Func void diff_eqs(dX, t, x, UY)
   Matrix dX, x, UY;
   Real t;
 {
   Real x1, x2, x3, x4, u, c2, s2, dm;
 
   x1 = x(1); // Position of c.  x2 = x(2); // Angle of pendulum
   x3 = x(3); // Velocity of c.  x4 = x(4); // Angular velocity
   u = UY(1); // Control input
 
   c2=cos(x2); s2=sin(x2); dm = 1+alpha*s2^2;

   dX = [[                          x3                               ]
         [                          x4                               ]
         [(a32*s2*c2 + a33*x3 + a34*c2*x4 + a35*s2*x4^2 + b3*u)/dm   ]
         [(a42*s2 + a43*c2*x3 + a44*x4 + a45*s2*c2*x4^2 + b4*c2*u)/dm]];
 }