Fusion with large initial magnetometer biases errors can result in the the NE earth field states reducing in magnitude and effectively flipping sign.
EKF: Move declination state limiting into a separate function
EKF: Limit NE mag states after each 3-axis mag fusion
EKF: Fix bug in mag field strength look-up scale factor
Simplify calling so that it is only called in two ways:
1) Immediately before 3-axis mag fusion if not called since the last earth field covariance reset so that the earth field declination information can be formed.
2) Immediately after 3-axis mag fusion otherwise.
In the case where the EKF is switching between 3-axis and heading fusion, off-diagonal elements containing the correlation between N,E components of the earth field were being lost on each switch event. These elements contained information about the declination uncertainty and should be preserved.
Ensures that each time the earth field covariance and variance data is reset, that the off-diagonal elements containing earth field declination angle certainty is restored.
Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only.
Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.
EKF waits 10s after GPS signal is lost before setting GPS control status flag to false. As the position information given by the alternative position sources drifts from the last GPS position, the controller over corrects.
With this update, the time horizon until GPS control flag set to false is reduced and only alternative position source is used for estimation.
tested with optical flow as position souce
log from as is https://review.px4.io/plot_app?log=d624af5e-dde4-40ab-ba5b-a693a49f5a36
log with update https://review.px4.io/plot_app?log=13ed6dc3-22dd-43f8-b898-4add41d60439
Previously, the reset of the yaw when climbing above 1.5m was not performed until 3-axis fusion was enabled. This could result in loss of navigation depending on the value of EKF2_MAG_TYPE and the flight profile.
- the velocity of the optical flow sensor relative to the IMU expressed in
body frame is the cross product of the angular velocity with the vector
from IMU to the sensor. If we use the angular velocity stored in the flow
sample struct we need to use a negative sign as that angular velocity
follows the opposite sign convention.
Signed-off-by: Roman <bapstroman@gmail.com>
- _flt_mag_align_complete was not set when choosing pure 3D mag fusion.
one effect of this was that the declination used in the filter was not
the one calculated from the magnetic field states.
Signed-off-by: Roman <bapstroman@gmail.com>
The handling of invalid flow data when on ground is performed in controlOpticalFlowFusion() where it is able to handle flow sensors that don't publish gyro data.
Daniel Agar
Paul Riseborough
CarlOlsson
Anna Dai
Paul Riseborough
Hamish Willee
Roman
jie.zhang