EKF: Improve GPS blending; Allow >2 GPSs

[JacobCrabill]

Describe problem solved by this pull request
The GPS blending in EFK2 is hard-coded to 2 GPS receivers, and the failover logic when one GPS fails / loses fix is a bit lacking.

Describe your solution
Made the GPS blending logic within ekf2_main more generic; can now (by changing 2 lines of code) handle blending of an arbitrary number of GPS units. Right now, it's up to 3 GPS units. I also updated the logic behind the selection of a single fallback GPS (in the case that blending has failed) to correctly choose the "best" GPS that is available / hasn't timed out. I welcome feedback on the method I came up with though; I may have missed some edge cases.

Test data / coverage
I have logs from testing somewhere... I'll retest and upload logs soon.

I used a Here2, a Here+, and a Zubax (at the same time) to test that the data was being blended as expected, and that when one GPS dropped out the switch to using other units happened seamlessly.

Side note, #14345 allows you to use the extra compasses and barometers when you have extra UAVCAN GPSs connected 😉

[CarlOlsson]

@JacobCrabill Thanks for your PR!
@bkueng Unfortunately I don't have the allocated time to do an in depth review for this, but before merging results from following tests will need to be supplied (Easiest done using ekf2 replay)

1. Two GPS receivers connected and the first one lose fix -> The ekf_gps_position should instantly jump to the second one
2. Two GPS receivers connected and the first one suddenly reports much lower speed accuracy -> the ekf_gps_position should slowly converge closer to the second one
3. Same thing as (1) and (2) but with three GPS receivers connected and the ekf_gps_position should jump/slowly converge to the weighted average of the two remaining

[dagar]

Related: I'd still like to get GPS blending out of the EKF frontend someday. #13584

[JacobCrabill]

@dagar didn't know you had a PR for that! I was wondering why GPS wasn't treated similarly to other sensors as I was making these changes. I'll take a closer look at that PR; I would be in favor of making my changes on top of that PR instead of this if it's nearly complete.

[dagar]

@dagar didn't know you had a PR for that! I was wondering why GPS wasn't treated similarly to other sensors as I was making these changes. I'll take a closer look at that PR; I would be in favor of making my changes on top of that PR instead of this if it's nearly complete.

Thanks, if you'd be in a position to do a quick test I think we'd be able to make the move fairly quickly.

[dakejahl]

Continued in #14447