* 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.
* pr-ekfOptFlowFixes:
EKF: fix bug causing height offset when GPS use stops
EKF: Don't reject saturated flow data when it is the only aiding source
EKF: Prevent flow motion check false positives
EKF: Don't assume large position uncertainty when starting optical flow nav
EKF: relax terrain update requirements for continuing optical flow use
EKF: Relax minimum required range finder measurement rate
EKF: relax optical flow on ground motion checks
EKF: range finder aiding logic fixes
EKF: Decouple range finder use criteria checking and selection
EKF: Don't auto select range finder for height when on ground.
EKF: Fix false triggering of optical flow bad motion checks
EKF: update comments
EKF: Don't use optical flow if GPS is good and the vehicle is not using range finder for height
EKF: Stop using EV for yaw when GPS fusion starts
EKF: Add persistence criteria to GPS fail check
EKF: allow GPS fallback if quality bad and alternative aiding available
EKF: always run GPS checks
Kabir Mohammed
Paul Riseborough
Daniel Agar
Philipp Oettershagen
Paul Riseborough
Roman
elia
Mohammed Kabir
Bart Slinger