Inhibiting gyro bias estimation during the initial tilt alignment speeds alignment.
The calculation of the maxmum state index required has been modified so that it can handle all combinations of inhibited states.
Limiting the maximum state index accessed by all EKF operations result in significant processing reductions when higher index states are not being used.
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.
Copter operation without a magnetometer is limited to constant position and relative position modes only (no GPS or range beacon fusion permitted)
Copter optical flow operation without a magnetometer is permitted.
The ability of planes to takeoff/launch without a magnetometer and align the yaw using the GPS velocity is retained.
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
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.
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.
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
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.
Peter Barker
Andrew Tridgell
Pierre Kancir
priseborough
Tatsuya Yamaguchi
murata
Randy Mackay
Mathieu OTHACEHE
Jonathan Challinger