Change gain matrices to uses given values in controllaw.cpp. Added Velocity control to ControlLawTest.m