This commit introduces a quality measure in the range data. It is
used to properly decide whether to initialize the HAGL estimate on
sensor data or MIN_HGT parameter, as well as in the decision of
whether a 'fake' measurement should be published on the ground to
allow for optical flow take-offs.
position/velocity aiding sensor.
Until now, it was alway resetting if the vehicle does not have gps or
external vision. This caused a reset/glitch at every stop (when range data gets
valid)
Simplify calling so that it is only called in two ways:
1) Immediately before 3-axis mag fusion if not called since the last earth field covariance reset so that the earth field declination information can be formed.
2) Immediately after 3-axis mag fusion otherwise.
In the case where the EKF is switching between 3-axis and heading fusion, off-diagonal elements containing the correlation between N,E components of the earth field were being lost on each switch event. These elements contained information about the declination uncertainty and should be preserved.
Ensures that each time the earth field covariance and variance data is reset, that the off-diagonal elements containing earth field declination angle certainty is restored.
Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only.
Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.
EKF waits 10s after GPS signal is lost before setting GPS control status flag to false. As the position information given by the alternative position sources drifts from the last GPS position, the controller over corrects.
With this update, the time horizon until GPS control flag set to false is reduced and only alternative position source is used for estimation.
tested with optical flow as position souce
log from as is https://review.px4.io/plot_app?log=d624af5e-dde4-40ab-ba5b-a693a49f5a36
log with update https://review.px4.io/plot_app?log=13ed6dc3-22dd-43f8-b898-4add41d60439
Previously, the reset of the yaw when climbing above 1.5m was not performed until 3-axis fusion was enabled. This could result in loss of navigation depending on the value of EKF2_MAG_TYPE and the flight profile.
- _flt_mag_align_complete was not set when choosing pure 3D mag fusion.
one effect of this was that the declination used in the filter was not
the one calculated from the magnetic field states.
Signed-off-by: Roman <bapstroman@gmail.com>
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
The parameter used to control the maximum dead reckoning time had 'gps' in the parameter name which was confusing because it was used for all measurement types capable of constraining horizontal velocity error growth. The parameter variable has been renamed and the documentation for it improved.
The parameter used to control the maximum time since fusing a measurement before the measurement is considered to be not contributing to aiding had misleading documentation which has been updated.
* terrain_estimator : guard against case where latest range sample is newer than IMU sample
* EKF : control : correct detection of no optical flow fusion over a time period
Terrain validity is determined solely by successful range finder fusion and terrain state initialisation.
A range finder that has been declared faulty requires continuous range finder data fusion requires data to be continuous before the fault status _rng_hgt_faulty can be cleared. This will enforce the requirement for continuous data before fusion can commence.
Eliminate race condition caused by checking for data freshness using time stamps from buffer push instead than buffer pop events.
Consistent use of range data ready and range data fault flags. This achieved by ensuring that _rng_hgt_faulty is set to true for all range data faults, not just data freshness.
Include range data validity requirement in rangeAidConditionsMet() check.
This brings all the range finder data checks (excluding innovation consistency checks) into one place and eliminates the need to perform range checking external to the library.
The hard coded optical flow tilt limit is changed to use the same value as the range finder.
Variable names are changed to make a clear distinction between the max/min values calculated by the stuck range check and the max/min valid values for the sensor.
kamilritz
Andreas Antener
Jan Brehmer
Nico van Duijn
bresch
Paul Riseborough
Roman
Mohammed Kabir
Martina
CarlOlsson
Daniel Agar
Anna Dai
Paul Riseborough
jie.zhang
Kabir Mohammed
dakejahl