Various fixes with NBV, currently moves in a straight line