Working version. Overshoots badly :( possible causes: 1) Bad Parameters; 2) Low sampling frequency