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.
Non user adjustable parameters are now declared as 'const' in the header.
The _ prefix has been removed from non user adjustable tuning parameters.
We use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
For consistency some signed integer type declarations have been changed to unsigned where appropriate.
Required to prevent acquisition of GPS mid flight causing unwanted change in position and velocity
A distinction has been mad between the arm and disarm transition and the decision to use position hold mode (formerly static mode)
When regaining GPS after a timeout, an offset is applied when fusing GPS velocity so that GPS velocity and position data as fused by the EKF is kinematically consistent.
This velocity offset is also accounted for when fusing air data so that wind estimates are not corrupted when the GPS position offset is being pulle back to zero.
The intended behaviour is that the EKF position will be pulled back to the GPS position at a rate of 5m/s for planes and 1 m/s for copters. This avoids large deviations in trajectory when GPS is regained.
1) Un-used public methods to report height and position drifting have been removed
2) A time-out has been added to the airspeed innovation consistency check so that if we are relying on airspeed to constrain velocity drift, a filter divergence or other fault that causes the airspeed to be continually rejected will trigger a change in health status.
3) A timeout of velocity, position or height measurements does not cause a filter fault to be reported. Timeouts can be due to sensor errors and do not necessarily indicate that the filter has failed.
4) Time-outs of various measurements are used to present a consolidated bitmask which inidicates which parts of the solution can be used, eg attitude, height, velocity, relative position, absolute position, etc.
This provides the calling vehicle software the abiity to request the EKF to not use GPS.
An integer is returned that indicates the type of operation available:
0 = request rejected (request will only be accepted if the EKF is in static mode, eg pre-armed)
1 = request accepted, attitude, vertical velocity and position estimates available
2 = request accepted, attitude, height rate, height, horizontal velocity and relative position estimates available
Paul Riseborough
Jonathan Challinger
Andrew Tridgell
Randy Mackay
Grant Morphett