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
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.
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
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.
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
The terrain offset solution status is usable for a short period of time without state updates so a timeout has been added which prevents the rapid changes in solution status due to short duration sensor read errors.
Paul Riseborough
Randy Mackay
Jonathan Challinger
Andrew Tridgell
Grant Morphett