wd=invp(t,w);

function wd=invp(t,w);
% inverted pendulum on a cart.
% This function simulates the dynamics of an inverted pendulum on  cart.
% The states are:
% w(1)           position of the trolley,                 x
% w(2)           its derivative,                          x_dot
% w(3)           angle of the pendulum from the vertical  th
% w(4)           its derivative                           th_dot
% The array K determines the external force u=-K*w applied to the cart
% and must be defined and declared global in the calling function
% The physical laws that define the system are:
%
% (M+m)      x_dd + ml cos(th) th_dd = ml  sin(th) (th_d)^2 + F
% ml cos(th) x_dd + m l^2      th_dd = mgl sin(th)

% --- define parameters of the system
g     = 9.81; % gravity acceleration, m/s^2
l     = 0.5;  % length of the pendulum, m
M     = 1.0;  % mass of the cart, Kg
m     = 0.1;  % mass of the pendulum
% --- compute external force
% if you are using a version of MATLAB prior to 4.0
% please delete next line

global  K;                 
F     =-K*w;

% --- compute second derivatives x_dd and th_dd
x     = w(1);
x_d   = w(2);
th    = w(3);
th_d  = w(4);
G     = [(M+m)           m*l*cos(th);
          m*l*cos(th)    m*(l^2)    ];
H     = [m*l*(sin(th))*(th_d)^2 + F  ;
         m*g*l*sin(th)              ];
Drv   = G\H;                 % [x_dd; th_dd]
[m,n] = size(w); wd=zeros(m,n);
wd(1) = w(2);
wd(2) = Drv(1);
wd(3) = w(4);
wd(4) = Drv(2);