* ekf_control: Inhibit mag fusion when field magnitude is large
Move mag inhibition check in separate function
* ekf_control: pull out of functionalities out of controlMagFusion
- yaw abd mag bias observability checks
- mag 3D conditions
- load mag covariances
- set and clear mag control modes
* ekf_control: refactor mag heading/3D start/stop.
Move mag declination, mag 3d and mag heading fusion out of the main function
* ekf_control: extract mag yaw reset and mag declination fusion requirements
* ekf_control: use WMM in isStronMagneticField for mag fusion inhibition
- Correct units of WMM strength table
* ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom)
Also split inAirYawReset from onGroundYawReset
* ekf_control: extract mag automatic selection
- transform if-else into switch-case for parameter fusion type selection
* ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix
flag naming in Test script
* ekf_control: do not run mag fusion if tilt is not aligned.
Reset some variables on ground even if mag fusion is not running yet. It
could be that it runs later so we need to make sure that those variables
are properly set.
* ekf_control: move controlMagFusion and related functions to mag_control.cpp
* ekf control: check for validity of mag strength from WMM and falls back
to average earth mag field with larger gate if not valid
* ekf control: remove evyaw check for mag inhibition
* ekf control: change nested ternary operator into if-else if
* Ekf: create AlphaFilter template class for simple low-pass filtering
0.1/0.9 type low-pass filters are commonly used to smooth data, this
class is meant to abstract the computation of this filter
* ekf control: reset heading using mag_lpf data to avoid resetting on an outlier
fixes ecl issue #525
* ekf control: replace mag_states_only flag with mag_field_disturbed and
add parameter to enable or disable mag field strength check
* ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore
* ekf control: use start/stop mag functions everywhere instead of setting the flag
* ekf control: Run mag fusion depending on yaw_align instead of tilt_align
as there is no reason to fuse mag when the ekf isn't aligned
* AlphaFilter: add test for float and Vector3f
The quaternion to inverse rotation matrix function has been updated so that the rotation it produces is the inverse to that produced by the matrix library and the the inverse of the quaternion is uses. This function is now used to directly calculate an inverse rotation matrix rather than calculating the forward rotation and then transposing it.
clearance. This is the best estimate as we should not rely on a distance
sensor while on the ground. This also helps when the drone is carried
over as it avoids starting with a crazy downward distance for optical
flow scaling.
This removes the check for _rng_hgt_faulty in the decision of publishing
a fake range measurement. The reason for this is that some distance sensors
don't populate the quality flag, even if they are saturated. Hence, if we
are on the ground and not moving, it is safe to publish a fake measurement
of the distance sensor and overwrite the actual sensor data.
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.
* 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.
* EKF: Move optical flow specific state reset to helper functions
* EKF: Ensure loss of optical flow aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference.
If flow data is unavailable for too long - declare optical flow use stopped.
Use consistent time periods for all resets
* EKF: Ensure loss of external vision aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using data as a position.
Don't reset velocity if there is another source of aiding constraining it.
If data is unavailable for too long, declare external vision use stopped.
Use consistent time periods for all resets.
* EKF: Update parameter documentation
Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer
* EKF: make class variable units consistent with documentation
* EKF: Don't reset states when optical flow use commences if using external vision
* EKF: Stop optical flow fusion when on ground if excessive movement is detected.
* EKF: fix terrain estimator vulnerabilities
Reset estimate to sensor value if rejected for 10 seconds
Protect against user motion when on ground.
Fix unnecessary duplication of terrain validity check and separate validity update and reporting.
* EKF: remove unnecessary Info console prints
Optical flow use information can be obtained from the estimator_status.control_mode_flags message
* EKF: fix inaccurate comment
* EKF: remove duplicate calculation from terrain validity accessor function
This is a functionally equivalent. It moves all of the code for the terrain estimator into a single function call from the main filter update, making it clear that it is independent of the main filter.
The tilt compensation being applied previously was based on a flat earth geometric model assuming perfect tilt knowledge which reduces the effect of range errors on height error as the vehicle tilts. however in the real world, variations in terrain gradient and uncertainty in vehicle tilt and sensor alignment tend to increase height error with tilt, so the adjustment of observation variance with tilt has been removed given we do not have a valid mathematical model on which to base it.
Mathieu Bresciani
RomanBapst
Paul Riseborough
Nico van Duijn
Carl Olsson
Daniel Agar
Kabir Mohammed
Paul Riseborough
ChristophTobler
devbharat