EKF updates and bugfixes

This pull request contains a number of  bug fixes and enhancements.

1) Fix to bug that makes it difficult for the filter to reset to the GPS under some flight conditions. This is covered by commits 2e85a6c and a04bbdc. Note, these gave already been merged into Copter 3.2.
2) Improve rejection of bad GPS when satellite count drops below 6. This is covered by commits 9ab45d8, 89b8ea5 and 64252af. Note, these have already been merged into Copter 3.2..