angle- and delta velocity bias variance
- the contribution of process noise per iteration for these states can be so
small that it gets lost if using standard floating point summation
Signed-off-by: Roman <bapstroman@gmail.com>
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.
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.
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.
* 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.
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.
Remove duplicate checking for dead reckoning and consolidate into a single function.
Use separate timers to check for start of dead reckoning and check when dead reckoning has been performed for too long for the nav solution to be valid.
Allow the timeout for validity reporting to be adjusted externally.
Separate external reporting of dead reckoning from internal checks.
* EKF: Move optical flow specific state reset to helper functions
* EKF: Ensure loss of optical flow aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference.
If flow data is unavailable for too long - declare optical flow use stopped.
Use consistent time periods for all resets
* EKF: Ensure loss of external vision aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using data as a position.
Don't reset velocity if there is another source of aiding constraining it.
If data is unavailable for too long, declare external vision use stopped.
Use consistent time periods for all resets.
* EKF: Update parameter documentation
Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer
* EKF: make class variable units consistent with documentation
* EKF: Don't reset states when optical flow use commences if using external vision
* EKF: Stop optical flow fusion when on ground if excessive movement is detected.
* EKF: fix terrain estimator vulnerabilities
Reset estimate to sensor value if rejected for 10 seconds
Protect against user motion when on ground.
Fix unnecessary duplication of terrain validity check and separate validity update and reporting.
* EKF: remove unnecessary Info console prints
Optical flow use information can be obtained from the estimator_status.control_mode_flags message
* EKF: fix inaccurate comment
* EKF: remove duplicate calculation from terrain validity accessor function
kamilritz