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.
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
Motion compensated optical flow rates are supposed to be zeroed if reported flow quality is below the minimum threshold value when on ground.
The comments and logic have been amended to be consistent and make the design intent clearer.
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.
* optical flow: fixed calculation of velocity of the flow sensor relative to
the IMU
- gyroXYZ holds a delta angle and first needs to converted to a gyro rate
Signed-off-by: Roman <bapstroman@gmail.com>
* optical flow: calculate height above the ground with respect to the flow
camera
- the flow camera can be offset from the IMU which needs to be considered
Signed-off-by: Roman <bapstroman@gmail.com>
* estimator interface: fixed comment regarding optical flow sample timestamp
- the timestamp on an optical flow sample corresponds to the trailing
edge of the flow integration period
Signed-off-by: Roman <bapstroman@gmail.com>
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.
* EKF collect_imu take const imu sample and populate buffer
* EKF calculateOutputStates cleanup
* EKF add calculate_quaternion output predictor method
* EKF: update documentation
* EKF: remove unnecessary getter function
* EKF calculateOutputStates only apply dt correction to bias
* EKF pytest assert attitude validity, not update() return
* EKF: correct documentation
* EKF: Do not make attitude validity dependent on yaw alignment status
Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.
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.
High rate optical flow data could make flow fusion to run every major update cycle, resulting in the calculation of bias errors in the body rates used to compensate flow data failing time validity checks and not running. This resulted in a slow drift of the nav solution if bias errors were present in the in the gyro data used for flow sensor motion compensation.
This is required because it can take several seconds for an optical flow sensor operation to be confirmed and there are also many use cases for indoor operation where no optical flow is used.
If magnetometer use is being inhibited because an indoor condition has been detected (optical flow and poor or no GPS) and the optical flow fusion checks fail causing the EKF to stop navigating, we do not want the EKF to start using the magnetometer.
CarlOlsson
Daniel Agar
Paul Riseborough
Paul Riseborough
Hamish Willee
Roman
jie.zhang
Dion Gonano
Kabir Mohammed
dakejahl
elia
Mohammed Kabir
Bart Slinger