Added a Follow_Traj and GO_HOME states. Hover is initiated only if close enough to targetPose, otherwise follows a trajectory to targetPose