Added persistent trajectory generation to the ControlLawTest file. Still plotting the wrong error quaternion