Trajectory Check
Due to a 'try except' over the whole Error calculation in FDCA controller, it used to calculate the errors correctly.
However cleaning up code resulted in the error that when we have double points in the trajectory it crashes. We should fix this by either changing our CSV files, or with a temporary fix that breaks out or loops throug the double points.