3D trajectory tracking of underactuated non-minimum phase underwater vehicles