More documentation on controller
Matlab stuff with more control law simulation, including trajectory generation checking functions