Current recovery strategy is to turn in a circle to the right. But the direction of GPS adjustment at the start may be a better indication of which direction to turn. For example, if the drift is to the east, then the robot should turn whichever direction is closer to east.
Probably save the GPS drift offset at the start. When the cone is lost, add the offset t to the current rover position and take the angle from the rover to that new point as an angle from -pi to +pi. If negative, turn to the right, else turn to the left.
Current recovery strategy is to turn in a circle to the right. But the direction of GPS adjustment at the start may be a better indication of which direction to turn. For example, if the drift is to the east, then the robot should turn whichever direction is closer to east.
Probably save the GPS drift offset at the start. When the cone is lost, add the offset t to the current rover position and take the angle from the rover to that new point as an angle from -pi to +pi. If negative, turn to the right, else turn to the left.