Starting the fake position as soon as all the aiding sources stop makes
the local position immediately invalid while we could continue to navigate
for a couple of second with inertial dead-reckoning
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.
Since the horizontal and vertical velocity controllers are now
decoupled, it can be that the horizontal acceleration produced by the
controller is actually greater than the desired one (by design). This
condition would actually make the ARW run "backwards", degrading the
controller performance.
Using the control status flags isn't robust as this part of the code
runs at the EKF update rate while the in_air transition is don at the
prediction rate. It was then likely to miss the transition
fix test by reducing the distance to the new origin: the maximum size of
the local position origin is a cube of 1e6m. If the origin is moved
further than this, the state is clipped to that maximum value
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
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 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 the wind estimator is 1Hz, the
same tuning is obtained with the same values as before.
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
- set local home using global pos and global home
- set local home using GNSS pos and global home
- set global home using global ref of local frame and local home
This is to make clear that the relevant part of the home position
message for navigator is the global one. Local home position isn't
required as everything is done in global coordinates.
The specific home_alt_valid is used when lat/lon are not used
- always try to set local or global home position when possible
- set global home with GNSS position if global pos from EKF isn't
available
- reset home when significantly moved from home before takeoff (checking
lpos or gpos or GNSS)
- reset home on takeoff transition
- reset home on mavlink arm command
- remove "home required accuracy" parameters, rely on validity flags
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
bresch