Added time step parameter to simulation code. Seems to run the control
better. However, the system is still very unstable. Need to make sure
coordinate systems line up.