This patch increases initial gyro bias uncertainty and plane and rover specific process noise to improve the rate of gyro bias learning.
This reduces the likelihood of a navigation failure due to rapid temperature changes in the inertial sensors causing rapid changes in zero rate offset.
The increase in process noise cannot be applied to Copter due to different numerical stability limits arising from the faster update rate.
This ensures that when we start using GPS, that the EKF will be using the correct declination for that location
If declination is not known it defaults to zero
This prevents bad inertial or GPS data combined with the post takeoff heading alignment check used by plane from resulting in earth field states that have an incorrect declination
This patch reworks the in-air transition criteria to reduce the likelihood of false positives and to ensure that there will be enough ground speed to make the heading check work reliably.
This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.
Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail.
This patch introduces the following protections:
1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet)
2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state
3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
Increases magnetometer weighting on yaw corrections when there there is no other aiding to constrain yaw drift.
Prevents switch to GPS if magnetometer data is failing innovation checks which indicates a bad yaw angle
The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement.
This enables this function to be used to set a home position before flight.
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
Ground effect baro errors can cause a spike in height innovation on disarming if ground effect compensation was used during the landing. This causes a transient AHRS fault message if this innovation is outside the pre-arm check limits.
Resetting the vertical position state to the measured height after disarming prevents this.
This estimate is used to offset baro data if we need to switch across from range finder data due to sensor failure. The previous filter coefficients gave a 0.5 seconds time constant on the offset which was too susceptible to baro noise.
Range finder and optical flow data can drop-out and be reliable very close to ground. these patches enable the takeoff to be more relaibly detected and constrain optical flow navigation drift in the first part of takeoff.
Falls back to baro if range finder is unavailable
Adds parameter enabling user to select which height source (baro or range finder) will be used during optical flow nav.
The magnetic field states are reset once at 1.5 metres and again at 5 metres. This height check was using the height at the first arm event as the reference. In the situation where there is baro drift and extgended time between the first arm event and flight, this can cause the magnetic field state to be reset when on the ground. If this happens when flying off a metallic surface, the resultant heading errors can cause sever toilet bowling.
Improve the quality of the GPS required to set an EKF origin
Eliminate repeated update of origin height - origin height updates once when EKF origin is set.
Operation in GPS mode is linked to setting of origin
The maximum time copters can reject GPS has been reduced from 10 to 7 seconds as flight logs have show that inertial dead reckoning with vibration and calibration errors is not good enough to support 10 seconds without aiding.
Paul Riseborough
Andrew Tridgell
Tom Pittenger
Jonathan Challinger
Randy Mackay
myly10