Some fixes in the PIDs and the stabilization algorithm.