Saves next target in nextIJ and save intermediate results and graphical output.

Each state now save its next target state and intermediate results are
now save in MATLAB workspace in case some error occurs.
Also added graphical output of the pendulum movement.
Deletet more old stuff.