Tuned the PF planner and added a first level Stuck Detector with timer.

The stuck detector works alright but a better method of detecting
stuckness rather than estimated x or y position variance would be
preferred. Need to see how to detect limit cycles.