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.
This removes a legacy design concept that is no longer required in this filter implementation. Planes will not be armed without EKF aiding and the proposed copter throw mode also requires EKF aiding to be operating.
The other problem with interrupting fusion during the launch is it doesn't reduce the corrections, it just delays them as wen the launch completes, the EKF inertial position estimate is still moving still moved and the corrections are therefore just delayed by the short launch interval.
Thank you to OXINARF for picking up the inconsistency with the previous logic
Change to user adjustable fusion of constant position (as per legacy EKF) instead of constant velocity.
Enable user to specify use of 3-axis magnetometer fusion when operating without aiding.
Don't allow gyro scale factor learning without external aiding data as it can be unreliable
Eliminate the use of horizontal position states during non-aiding operation to make it easier to tune.
Explicitly set the horizontal position associated Kalman gains to zero and the coresponding covariance entries to zero after avery fusion operation.
Make the horizontal velocity observation noise used during non-aiding operation adjustable.
Use a fixed value of velocity noise during initial alignment so that the flight peformance can be tuned without affecting the initial alignment.
The non-GPS mode was not being activated for small height gains - eg indoor flight.
The incorrect innovation consistency check was being applied to the synthetic velocity observations.
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.
Changes generated with:
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
GPS height has been added as a measurement option along with range finder and baro
Selection of the height measurement source has been moved into a separate function
Each height source is assigned its own measurement noise
If GPS or baro alt is not able to be used, it reverts to baro
When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer.
This reduces storage requirements for Copter by 75% or 6KB
It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate.
This means that the EKF filter operations operate at a maximum rate of 100Hz.
The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
Extended GPS loss can result in the earth field states becoming rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
This sets the fusion of the synthetic position and velocity to occur at the same time as the barometer
This makes filter tuning more consistent between GPS and non-GPS useage
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
priseborough
Andrew Tridgell
Mathieu OTHACEHE
Jonathan Challinger
Ricardo de Almeida Gonzaga
Lucas De Marchi
Siddharth Bharat Purohit
Paul Riseborough
Randy Mackay