inverted pendulum - MATLAB CIS540 ASSIGNMENT