Added reference trajectory generation; started control unit tests