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.
- _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>
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
The parameter used to control the maximum dead reckoning time had 'gps' in the parameter name which was confusing because it was used for all measurement types capable of constraining horizontal velocity error growth. The parameter variable has been renamed and the documentation for it improved.
The parameter used to control the maximum time since fusing a measurement before the measurement is considered to be not contributing to aiding had misleading documentation which has been updated.
* terrain_estimator : guard against case where latest range sample is newer than IMU sample
* EKF : control : correct detection of no optical flow fusion over a time period
Terrain validity is determined solely by successful range finder fusion and terrain state initialisation.
A range finder that has been declared faulty requires continuous range finder data fusion requires data to be continuous before the fault status _rng_hgt_faulty can be cleared. This will enforce the requirement for continuous data before fusion can commence.
Eliminate race condition caused by checking for data freshness using time stamps from buffer push instead than buffer pop events.
Consistent use of range data ready and range data fault flags. This achieved by ensuring that _rng_hgt_faulty is set to true for all range data faults, not just data freshness.
Include range data validity requirement in rangeAidConditionsMet() check.
This brings all the range finder data checks (excluding innovation consistency checks) into one place and eliminates the need to perform range checking external to the library.
The hard coded optical flow tilt limit is changed to use the same value as the range finder.
Variable names are changed to make a clear distinction between the max/min values calculated by the stuck range check and the max/min valid values for the sensor.
* EKF angle constants in degrees for readability
* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S
* EKF controlOpticalFlowFusion() use constants and update comments
* EKF controlMagFusion() use angle in degrees
* EKF move earth spin rate to geo and update usage
* EKF: Fix numerical constant error and clean up comments
Comments do not need to contain numerical values when the code makes these clear.
This fixes a error condition that occurs if _time_last_gps is greater than _time_last_imu.
By checking time stamps at the fusion time horizon, we guarantee that this cannot happen because all observations must have a time stamp smaller or equal to _imu_sample_delayed.time_us before they are retrieved from the buffers.
The gyro data accumulation needs to be across the same integration period as the flow sensor. The previous code didn't sample the accumulation until the midpoint of the flow data had fallen behind the fusion time horizon.
This PR changes the optical flow time stamp definition so that flow data is retrieved when the leading edge of the flow accumulation period falls behind the fusion time horizon. This enables the accumulated gyro data to be sampled at the correct time. Fusion is then delayed until the mid sample time has fallen behind the fusion time horizon.
kamilritz
Paul Riseborough
Roman
Mohammed Kabir
Martina
CarlOlsson
Daniel Agar
Anna Dai
Paul Riseborough
jie.zhang
Kabir Mohammed
dakejahl
elia