Extend use of EXTERNAL_POSITION_ESTIMATE data #31738
Open
+211
−30
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
The difference between receive and measurement time is important for debugging this interface.
…urce AP_NavEKF3: Fix setLatLng GPS handover logic AP_NavEKF3: Ensure GPS is declared bad if fix status less than 3D AP_NavEKF3: fix comment typo AP_NavEKF3: Allow a longer gap between setLatLng before unblocking bad GPS AP_NavEKF3: Fix public vs internal origin bug affecting setLatLng offset AP_NavEKF3: Make learning of setLatLng offset a parameter option AP_NavEKF3: Reset to GPS if prior setLatLng data suspect AP_NavEKF3: Rework switch between setlatLng and GPS AP_NavEKF3: Don't use stale data AP_NavEKF3: Improve robustness to manouevre induced prediction errors AP_NavEKF3: Provide dal.log_SetLatLng with data received time stamp
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
The MAV_CMD_EXTERNAL_POSITION_ESTIMATE data is currently used to reset the position inside the EKF and is not used as a measurement. It was developed originally to support infrequent position fixes from a GCS with up to 5 seconds latency.
High rate position and attitude data with small and consistent latencies are supported via the VISION_POSITION_ESTIMATE, GLOBAL_VISION_POSITION_ESTIMATE and VICON_POSITION_ESTIMATE messages which the EKF processes using the writeExtNavData interface. This is limited to local coordinates.
Optical navigation technology can provide data in global coordinates, but can also have large variations in processing delay between images this is not catered for by either of the interfaces listed above.
This pull request extends the EXTERNAL_POSITION_ESTIMATE message handling to enable its use with higher rate messages as an alternative position measurement source. This means that states other than position, such as vehicle and wind velocity will also be corrected. It also means that innovation consistency checks will be performed which increases protection from error spikes.
The EK3_OPTIONS parameter is used to enable and configure the feature. Setting bit 3 enables the feature and MAV_CMD_EXTERNAL_POSITION_ESTIMATE data will be treated as a measurement. This bit is off by default. Setting bit 4 causes the offset between the MAV_CMD_EXTERNAL_POSITION_ESTIMATE to be learned when GPS quality is sufficient to pass the same checks used for EKF alignment. This offset is then applied to the data when required for navigation. This option is useful if the MAV_CMD_EXTERNAL_POSITION_ESTIMATE message is derived from an odometry technique that drifts over time.
A basic autotest has been included.
Additional SITL testing using an offboard Gazebo derived vision nav sim has also been performed.
The following plots show the Copter vehicle type test with GPS turned off halfway through the run.
The position innovations get noisy when the GPS stops as the EKF starts using the MAV_CMD_EXTERNAL_POSITION_ESTIMATE data.
GPS velocity innovations show when GPS data stops.
The simulated measurements have significant lag and jitter which are handled by the EKF
A SITLr test was performed using a fixed wing model
Measurements had high jitter and latency
The EKF handles the larger position fix errors in this application - the fallover from GPS to using vision nav is obvious