when on the ground without a position source we would disable the
innovation gate for the barometer. This meant that a single (or small
number of) really bad baro readings would be fused into the EKF,
causing it to destabilise
Fixes#11903
this forces the EKF origin to the GPS alt on a height datum reset if
we have GPS lock. If we don't do this then the reported AMSL alt will
drift over time away from the GPS alt when we reset while on the
ground
All Kalman gain calculations now explicity set gains for deactivated states to zero.
Previous use of loops to set gains to zero have been replaced with more efficient memset operations.
Fix rounding error bug preventing state from updating after initial convergence.
Decouple GPS reference height from published EKf origin height.
Add bitmask parameter to control update and publishing of GPS reference height.
If the GPS receiver selection changes and we are using GPS for height, the vertical position will be reset to the new GPS height measurement.
correct output observer history when doing a GPS height reset
Enables the horizontal speed at which we switch from range finder to alternate to be adjusted.
Does not switch from range finder to alternate based on speed when speed estimate is invalid.
AP_NavEKF3: Implement same maths as PX4/ecl EKF
Replace attitude vector states with quaternions
Remove gyro scale factor states
Add XY accel delta velocity bias estimation
Initial tuning
Add GPS body frame offset compensation
AP_NavEKF3: Fix bugs and consolidate aiding switch logic
Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
AP_NavEKF3: prevent multiple fusion mode changes per filter update
AP_NavEKF3: Update tuning defaults
AP_NavEKF3: Fix bug causing switching in and out of aiding
If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding.
This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed.
An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS.
AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
AP_NavEKF3: Improve switch-over to backup magnetometer
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.
AP_NavEKF3: enable automatic use of range finder height
AP_NavEKF3: Fix bug in handling of invalid range data
AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3:
AP_NavEKF3: Handle yaw jumps due to core switches
AP_NavEKF3: Enable simultaneous GPS and optical flow use
AP_NavEKF3: fix console status reporting
AP_NavEKF3: send messages to mavlink instead of console
This allows the GCS to better handle the display of messages to the user.
AP_NavEKF3: replace deprecated function call
AP_NavEKF3: Compensate for sensor body frame offsets
AP_NavEKF3: Fix bug in median filter code
AP_NavEKF3: save some memory in the position offsets in EKF3
We don't need to copy that vector3f for every sample. A uint8_t does the job
AP_NavEKF3: Add fusion of range beacon data
AP_NavEKF3: Bring up to date with EKF2
AP_NavEKF3: Misc range beacon updates
AP_NavEKF3: Add mising accessors
AP_NavEKF3: remove duplicate include
AP_NavEKF3: Prevent NaN's when accessing range beacon debug data
AP_NavEKF3: Update range beacon naming
AP_NavEKF3: updates
AP_NavEKF3: miscellaneous changes
AP_NavEKF3: misc updates
AP_NavEKF3: misc range beacons updates
AP_NavEKF3: add missing rover default param
The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly.
When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a
single state Bayes estimator,
If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data.
Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
Paul Riseborough
Randy Mackay
Andrew Tridgell
Michael du Breuil
Arjun Vinod
Tom Pittenger
Peter Barker
priseborough
murata
Jonathan Challinger
Dr.-Ing. Amilcar Do Carmo Lucas
Mathieu OTHACEHE