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
- all sources of optical flow publish sensor_optical_flow
- sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow
Co-authored-by: alexklimaj <alex@arkelectron.com>
The noise spectral density, NSD, (square root of power spectral density) is a
continuous-time parameter that makes the tuning independent from the EKF
prediction rate.
NSD corresponds to the rate at which the state uncertainty increases
when no measurements are fused into the filter.
Given that the current prediction rate of EKF2 is 100Hz, the
same tuning is obtained by dividing the std_dev legacy parameter by 10:
nsd = sqrt(std_dev^2 / 100Hz)
The baro observation noise parameter is often over-estimated in order as
a measure to mitigate temporary offsets in the readings due to wind
gusts or poor pressure compensation tuning. The side effect is that the
innovation sequence monitoring based on normalized innovation struggles
to detect an offset in the state because the innovation isn't
statistically significant.
To counter this issue, a simpler check is added to trigger the process
noise boost when the innovation has the same sign for a long period of
time.
Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.
Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op
- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op
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
To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1
To pass from valid to invalid:
- low-passed signed test ratio >= 1
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.
- the estimated mag bias was requiring > 30 seconds of continuous 3d
mag fusion to be reported stable (and saved back to mag cal), this
restores the original intent requiring 30 seconds of accumulated valid
3d mag fusion
- this is a more conservative default if a vehicle isn't set (no land detector running)
- handled horizontal preflight failures in commander when disarmed
rather than overloading xy_valid and v_xy_valid flags
- ekf2 no longer depend on arming or standby status
- 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.
bresch