Blogs

AC3.1 is going out very soon and includes a new safety feature called GPS glitch detection.  There's a wiki page about it but I'd like to highlight how it works because it's near the top of the list of safety issues.  The method itself comes from Craig who apparently used something similar on undersea robots.  The way it works is:

    Step 0: the very first GPS position received is simply "accepted"

    Step 1: when we receive a new message from the GPS we first calculate a "projected position" based on the previously accepted position and velocity assuming that the copter had just flown straight ahead.

    Step 2: we compare the new position to this projected position and if it's within 2m (configurable with the GPSGLITCH_RADIUS parameter) then we 'accept' the new position and we move back to Step #1 and wait for the next update.

    Step 3: if it's outside 2m then we calculate another circle of acceptability which is an estimate of how far the vehicle could have travelled since that last accepted position.  We assume the vehicle can accelerate at 10m/s/s in any direction (value held in the GPSGLITCH_ACCEL param).  So for example if it's been 1sec since the last accepted position, the radius would be 1/2 * 10m/s/s * 1s * 1s = 5m.  This radius starts out small but grows quickly so after 5sec it's 125m, after 10seconds it's 500m.  If the new position still isn't within this radius then we're officially "glitching".  We throw away the GPS position (so we rely only on the inertial nav estimate), we display "Bad GPS Position" on the Mission Planner's HUD and then go back to Step #1 and hope the next sample is better.

     Now after 5 seconds of not having an acceptable GPS position the GPS Failsafe kicks in and the copter Lands or switches to Alt Hold (user configurable using the FS_GPS_ENABLE parameter).

     If on the other hand the GPS position corrects itself (i.e. comes back within the radius) we instantly reset the inertial nav position to this new GPS position.  This reset is important because it saves the inertial nav filter from getting confused about it's velocity and acceleration due to the jump which would lead to the copter flying wonkily even after the glitch has cleared.

     This doesn't mean that GPS glitches are a thing of the past, we still need to get these two parameters (GPSGLITCH_RADIUS and ACCEL) set so they catch most glitches without causing too many false positives but hopefully it's save some copters and this may grow into better algorithsm.

     By the way, before implementating this glitch detection we tried to use the GPSs HDOP values (the GPS's measure of how accurate it's position is) but we found the number trails the actual glitch by up to 2 seconds so by the time the GPS tells you, things have already gone horribly wrong.

All feedback welcome, you can see the code here and a simulated video here which simulates an actual event reported by Garrick Merrill (gps location changed to protect the innocent).  Here's a video showing how quickly the inertial nav position drifts without the GPS.