added files (again), fixed NavLibTest to compile with new interface changes, now tests vel_control