EKF3: does not recover yaw when GPS yaw becomes available again.

[jmachuca77]

Bug report

Issue details
We have been seeing issues with the yaw on a rover where our only source if yaw is from GPS. I unfortunately don't have a replay log of this issue, and as far as I can tell from the log I have what is happening is that the GPS looses yaw, then the yaw estimate starts drifting. When the GPS yaw comes back, the EKF innovations go crazy and the yaw jumps to a different value. I implemented a failsafe on the companion computer that puts the vehicle into hold when it detects the innovations going out of range so fortunately it catches this.

This code has some of our internal changes but its based on this commit from master [e469699] (May 20th) so it includes #17313 and #17342 already.

Link to the log:
https://www.dropbox.com/s/w5x8f9vnj05q6y9/GPSYawNotRecovered.BIN?dl=0

[rmackay9]

I've added this to the Rover-4.1.0 issues list and so it will at least be tracked and not forgotten. #17116

[IamPete1]

Rover issues list says this has been fixed.