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.
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.
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
* EKF: Do not delay reversion to no-aiding mode if parameter initiated
* EKF: Move no-aid reversion resets to helper functions
* EKF: Prevent unwanted fusion of velocity data during no aiding mode
* Revert "EKF: Release flow speed limit with altitude gained"
This reverts commit e70206f74b.
* Revert "fix code style"
This reverts commit 76bf70121c.
* Revert "Reverse the linked list of data_validator_group and maintain a first node"
This reverts commit 32482e7644.
* fix typo
* EKF: use baro if it was reset to baro from ev
* EKF: set vert_pos_reset if resetting to ev hgt
otherwise the position controller will not reset the setpoint -> leading to unwanted altitude changes
When GPS use is gained whilst flying using optical flow data, the sudden release of the speed limit is unannounced to the operator and can cause unexpected acceleration.
This patch releases the speed limit as height is gained, but does not reduce it when the vehicle descends, unless GPS use is lost.
If state errors were large before the reset, then failure to reset the covariance matrix terms can result in incorrect fusion of position and velocity measurements after the reset due to inconsistencies in the covariance matrix.
When performing the initial in-flight mag yaw reset for RW vehicle, do not reset the quaternion states and corresponding variances unless there has been a change in yaw angle large enough to cause problems with navigation.
This is because the state estimates after a reset are more vulnerable to transient sensor errors, so a reset should be avoided if possible.
When performing the initial in-flight magnetic field reset for fixed wing vehicles, resetting the quaternion states and their corresponding covariances should be avoided unless yaw errors are large, because state estimates are vulnerable to transient sensor errors immediately following a reset.
Ensures that a complete reset of velocity and position states will always be performed if yaw has had to be reset using GPS velocity.
Ensures that the yaw_align status cannot be set to false once the filter has aligned.
Paul Riseborough