- 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.
- refactor updateQuaternion() to compute the yaw jacobian directly (respecting the rotation sequence determination)
- fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
- clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
- when at rest continue fusing last static heading with very low variance even if other heading sources are active
As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.
When wind is already estimated, we don't reset the states using airspeed
data, so it could be that the fusion fails if the airspeed isn't
consistent with the filter (test ratio > 1). In this case, don't start
the fusion.
When wind isn't already estimated, the wind states are reset using
airspeed so the fusion can start regardless of the current test ratio.
split the fusion process into:
1. updateAirspeed: computes innov, innov_var, obs_var, ...
2. fuseAirspeed: uses data computed in 1. to generate K, H and fuse the
observation
when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure
At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.
- ekf2: expose dead reckoning as control status flags
- commander:
- add GPS validity check
- in AUTO MISSION if dependent on GPS then a loss of GPS will
- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).
- Separate "baro fault" from "baro intermittent": intermittent is a
temporary failure and should prevent from switching to baro right now,
but "fault" means that it should never be used anymore
- In case of height timeout, check for metrics but not for consistency
as the filter is likely to have diverged already.
