Added GPS to the Stall Detection
Tuned a few parameters for GPS, encoder, and system noises
Kalman filter seems to be able to detect discrepancies between encoders, GPS, and gyroscope!