EKF2: reset position by fusion

[haumarco]

Solved Problem

When the external position reset gets triggered we can now fuse the new position as a "normal" measurement. Therefore the position error (estimate-position on map) gets included in the other states and mainly the wind can be better estimated. For large jumps we still reset the position to ensure we dont induce a really strong wind estimate.
Also adjusted the "set global origin" possibility before takeoff during GNSS denied conditions.

Alternatives

We could also, reset the position and fuse the "measurement" with a high variance. The estimated position would then perfectly align with the sent measurement position but the effects on the other states would not be as harsh for large position jumps (previous estimate / sent measurement).

[dagar]

Could you do this depending on the context (state of the filter, provided accuracy, etc) and then either use it as a correction if you can (with an appropriate gate), otherwise fallback to a hard position reset?

[haumarco]

I added two things:

1. When inertial dead reckoning is true, there was already no aid data for no_aid_timeout_max. Therefore we need to subtract the timeout from the _time_last_horizontal_aiding.

2. In the beginning, the starting condition was satisfied for the fake-pos fusion becaue horizontal aiding time is exceeded. I added the _horizontal_deadreckon_time_exceeded to the continuing condition (now same as starting condition). This does not change the logic since isHorizontalAidingActive directly disables the fake pos as soon as there are any external measurements.

[dagar]

Did you find this was still necessary? I didn't think it was because isTimedOut() is handling when it's zero and now checking the elapsed time without subtraction.

[haumarco]

Define necessary ;)
If we want that valid_timeout_max defines the maximum time where we're using inertial dead reckoning then yes. Because after the last measurement, the system is already dead reckoning with IMU data, but we only label it as such after no_aid_timeout_max, which is correct.

The question is:
Where does one define the starting point of the inertial dead reckoning? After the last sensor-measurement or after the last sensor timed out.

[dagar]

Makes sense, we could probably change it to use the actual timestamps (but it hardly matters).

[dagar]

Needs testing, but otherwise looks good to go from my perspective.