1) Do not switch to a core until its states have been updated.
2) Distinguish between mandated switches required for health failure and optional switches required due to innovation checks failing.
3) Apply hyseresis to innovation check levels
RC_Channel: To nullptr from NULL.
AC_Fence: To nullptr from NULL.
AC_Avoidance: To nullptr from NULL.
AC_PrecLand: To nullptr from NULL.
DataFlash: To nullptr from NULL.
SITL: To nullptr from NULL.
GCS_MAVLink: To nullptr from NULL.
DataFlash: To nullptr from NULL.
AP_Compass: To nullptr from NULL.
Global: To nullptr from NULL.
Global: To nullptr from NULL.
This can improve position hold performance where it is not practical to have the IMU located at the centroid.
Although this enables the effect of IMU position offsets to be corrected, users will still need to be instructed to place the IMU as close to the vehicle c.g. as practical as correcting for large offsets makes the velocity estimates noisy.
Correction requires the body rates averaged across the flow sensor sampling interval. This data has been added to the sensor buffer.
The body rate data from the flow sensor driver does not contain the Z component, so an equivalent value sampled from the navigation IMU has been used instead.
The variable omegaAcrossFlowTime has been moved out of the class and into the only function that uses it.
A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter.
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
Revert "AP_NavEKF2: Fix bug in published yaw reset value found during code review"
commit 175faf1e41.
Revert "AP_NavEKF2: use a struct for all yaw step class variables"
commit 77fad065d1.
Partially revert "AP_NavEKF2: Handle yaw jumps due to core switches"
commit 885bfd1b4e.
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.
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. other wise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.
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.
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.
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.
The filter status logic calculations were being repeated every time the get function was called.
The logic is now updated once per filter update step and a separate get function added
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.
Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter.
1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed
2) The velocity states were not being reset
3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
Remember the mag bias and earth field states learned during flight when the vehicle lands.
This improves performance for vehicles that do multiple flight on one power cycle
Provide consistent overshoot of 5% across a wider range of time constants and prevent selection of larger time constants causing 'ringing' in the position and velocity outputs.
Fixes a problem observed in a flight log where rapid temperature change caused the accel bias to change faster than the EKF could keep up.
This allows the bias to be learned faster but with acceptable level of noise in the estimate
IMU data was being corrected before being used by the co-variance prediction, whereas the delta angles and velocities in the derivation were supposed to be uncorrected.
This patch creates separate variable for the corrected data
Magnetometer bias states will subject to larger errors early in flight before flight motion makes the offsets observable and the state variances reduce.
Adds a check on state variances.
Replaces the parameter check with a check of the actual filter fusion method being used.
Allow different process noise to be set for body (sensor bias) and earth field states.
This allows a stable magnetometer bias estimate to be available at end of flight whilst still allowing for external magnetic anomalies during landing.
Adjust default values to give stable mag bias learning and fast learning of external anomalies.
Automatically use the highest gain consistent with a 5% overshoot to minimise RMS tracking errors.
Provide an alternative correction method for the position and velocity states that allows the user to specify the time-constant. This can be used to fine tune the output observer for for platform specific sensor errors and control loop sensitivity estimation noise.
The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe.
A check for yaw errors due to a ground based magnetic anomaly has been introduced.
The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
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.
priseborough
Randy Mackay
murata
Andrew Tridgell
Mathieu OTHACEHE
Jonathan Challinger
Francisco Ferreira