- refactor all EKF backend output predictor pieces into new OutputPredictor class
- output states are now calculated immediately with new high rate IMU rather than after EKF update
- IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
- move EV yaw and EV position to new state machines
- EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
- new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
- yaw_align now strictly means north (no more rotate external vision aid mask)
- automatic switching between EV yaw, and yaw align north based on GPS quality
- respect new EKF2_EV_CTRL parameter for VPOS usage
- EV hgt rotate EV position before usage (there's often a small offset in frames)
- EV hgt reset use proper EV velocity body frame
- try to keep EV hgt and EV vel state machines consistent
- small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128
- don't store unnecessary IMU copy in class, pass it through where required
- remove custom pi constants
- remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt)
- explicitly initialize everything in header
- apply PX4 code style
- set GPS velocity before yaw estimator update, not after
- in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
During the whole flight, if the difference between the yaw estimate from
EKF2 and the emergency estimator is large and that the GNSS velocity
fusion is failing continuously, immediately reset to the emergency yaw
estimate.
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
- if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
- handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
- isRecent() and isTimedOut() helpers use delayed time
- add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data
Also remove the legacy "range aid" than can be achieved by setting the
height reference to range finder and the range finder control parameter
to "conditional".
Conditional range aiding cal also be set when the height reference isn't
the range finder. This prevents the ratchetting effect due to switching
between references.
Instead of having a single height source fused into the EKF and the
other ones "waiting" for a failure or the primary sensor, fuse all
sources in EKF2 at the same time. To prevent the sources from fighting against each
other, the "primary" source is set as reference and the other ones are
running a bias estimator in order to make all the secondary height
sources converge to the primary one.
If the reference isn't available, another one is automatically selected
from a priority list. This secondary reference keeps its current bias
estimate but stops updating it in order to be the new reference as close
as possible to the primary one.
Daniel Agar