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.
This patch causes the EKF to update the height of its origin each time it receives a valid GPs message whislt disarmed.
The resultant EKF origin height represents the height of the zero baro alt datum relative to the GPS height datum.
Flight tests have shown that the magnetic field distortion associated with flight from steel structures can extend 3m or higher. To counteract this, a second and final yaw and magnetic field alignment has been added which is activated when the height exceeds 5m for the first time.
Logic used to delay optical flow and airspeed fusion to prevent it occurring on the same time step as magnetometer fusion has been removed. This is no longer required to efficiency improvements made at the firmware level.
Improvements in PX4 firmware have reduced the computational load and mkae the previous practicwe of splitting magnetometer and optical flow fusion across multiple time steps unnecessary and make it possible to perform a covariance prediction prior to fusing data on the same time step. This patch:
1) Ensures that a covariance prediction is always performed prior to fusion of any observation
2) Removes the splitting of magnetometer fusion so that fusion of the X,Y and Z components occurs on the same time time step
3) Removes the splitting of optical flow fusion so that fusion of X and Y components occurs on the same time step
UBlox receivers report an estimate of the speed accuracy that tests show correlates well to speed glitches. Using this to scale the GPS velocity observation noise will reduce the effect of bad GPS velocity data.
If the vehicle moves significantly or the GPS changes position significantly pre-armed, then the GPS glitch logic was being invoked when the first GPs measurements were fused. This patch resets the position to the GPS when the vehicle arms.
Due to the way that gyro calibration is done, the EKF could be effectively not run for up to 30 seconds in extreme cases, making it possible that the GPS would be failed on arming and the copter put into a non-GPS mode.
the longer term solution is to update the gyro calibration so that it does not hold up other processing. the short tyermfix in thsi patch is to look for evidence of a 3D lock in the last received GPS message.
This fixes a bug that meant that once the EKF had started up in a non-GPS mode, it would no longer read the GPS and therefore would never be able to use GPS again until reset.
When Copter arms, the AHRS/EKF may not be run for a few hundred msec depending on conditions. This can cause the arming check to fail the optical flow sensor and place the EKF in a constant position mode.
This additional explicit setting of the constPosMode and constVelMode reduces the likelihood of logic errors being introduced in the future as it places the intended setting of these parameters at arming in the one place. the constVelmode and constPosMode only have one set of conditions each that can trigger these modes in flight, so if these modes are true after arming it will be clear that it was the in-flight condition that triggered.
This ensures the position and velocity measurement status will be set as timed out immediately after use of those measurements is inhibited. This will improve the timeliness of filter status reporting.
It does not make sense to relax the limits on vehicle speed and nav gains just because we have received some invalid flow data. This could make the situation worse if the invalid data was being caused by too much speed.
If we are relying on flow data the vehicle limits should always be applied.
When we are not using GPS measurements, we should not be allowing the GPS glitch logic to reset position states as this can interfere with operation of non GPS modes.
When PV aiding is disabled, then the timeout time reference should not be reset becasue we want the position measurement timeout status to remain true the whole time the measurement is not being used.
Synchronise with covariance prediction to improve numerical stability and accuracy of angle corrections. The 'noise' this produces in the position and velocity estimate is irrelevantbecause these are not used by the control loops during this mode of operation (they are nominally zero anyway).
Prevents possible loss of attitude reference for flights without optical flow and GPS.
The optical flow measurement timeout can reset the velocity states which decouples the position states from IMU errors and therefore significantly reduces the amount of attitude error correction.
This enables the filter to report the last known position after disarm.
The LLH location of the filters NED origin is published and should be logged every time the vehicle is armed to assist with post-flight trajectory reconstruction.
The LLH location of the filters NED origin can be set externally whilst the vehicle is disarmed.
The two state auxiliary EKF has been replaced with a single state filter that only estimates terrain offset. The new filter fuses a optical flow line of sight rate scalar (length of the optical flow LOS rate vector) which provides a terrain offset estimate that is less affected by yaw errors.
Estimation of focal length scale factor error in flight wasn't accurate enough and will be replaced with a pre-flight intrinsic sensor calibration procedure as the scale factor error does not change over time provided the lens assembly is not adjusted.
AP_NavEKF: Remove unwanted printf
Paul Riseborough
Jonathan Challinger
Andrew Tridgell
Randy Mackay
Tom Pittenger
Arthur Benemann
myly10
Grant Morphett