Extend use of EXTERNAL_POSITION_ESTIMATE data

[priseborough]

Optical navigation technology can provide data in global coordinates, but can also have large variations in processing delay between images which is not catered for by either of the interfaces listed above. Optical navigation can either run on a GCS based system using transmitted camera data or on an onboard companion computer.

The GCS processing case was previously supported in a limited way by the MAV_CMD_EXTERNAL_POSITION_ESTIMATE message. It was developed originally to support infrequent operator position fixes from a GCS with up to 5 seconds latency. These data were used only to reset the position inside the EKF and is not used as observations for state updates. This means that for vehicles doing wind relative dead reckoning, more regular updates could not be used to correct wind and vehicle velocity states for improved dead reckoning accuracy.

Onboard high rate position and attitude data with small and consistent latencies are currently 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 and cannot handle large varying processing delays.

This pull request enables the use of global position estimates from either GCS or onboard companion computers. Latencies of up to 5 seconds can be handled and the processing of these estimates as EKF observations means that vehicle and wind velocity states can be corrected. It also means that innovation consistency checks will be performed which increases protection from error spikes.

GCS based data are supported by extension of the EXTERNAL_POSITION_ESTIMATE command message handling to enable its use with higher rate messages.

The same functionality is also supported for on-board companion computer processing via the GLOBAL_POSITION_SENSOR streaming message.

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.

If the EKF_OPTIONS bit 0 is set (jamming expected option), then if the EKF GPS good to align checks fail and MAV_CMD_EXTERNAL_POSITION_ESTIMATE data is available, GPS use will be discontinued and MAV_CMD_EXTERNAL_POSITION_ESTIMATE data used instead until GPS good to align checks pass. The GPS good to align checks are controlled by the EK3_GPS_CHECK and EK3_CHECK_SCALE parameters.

A basic autotest has been included for the MAV_CMD_EXTERNAL_POSITION_ESTIMATE message.

The following figures are from additional SITL testing using an offboard Gazebo derived vision nav sim using the MAV_CMD_EXTERNAL_POSITION_ESTIMATE message.

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 SITL 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

The following figures are from additional SITL testing using an offboard Gazebo derived vision nav sim using the GLOBAL_POSITION_SENSOR message.

GPS is disabled

Position innovations from the optical navigation position estimates are noisier, but zero mean.

The EKF tracks the position fixes

A position standard deviation of 11.5m was set by the optical nav

Simulared measurements have up to 1150msec of latency and are handled by the EKF.

[peterbarker]

GPS velocity innovations show when GPS data stops.

I'm a bit concerned at the isFlying stuff going to zero here!

[peterbarker]

This obviously does change behaviour for users that don't have the option enabled. Would be nice to have those spelled out.

Considering that the vast majority of our users won't be using this feature, I think we really do need EK3_FEATURE_EXTERNAL_POSITION_FUSION

-----------  -----  ----------  ------  ----  -----  -----  ---
Board        blimp  bootloader  copter  heli  plane  rover  sub
Pixhawk1-1M  440    *           440     448   440    448    440
-----------  -----  ----------  ------  ----  -----  -----  ---

CI must pass; most of the errors will be fixed by adding documentation for your changed/added log message fields.

[priseborough]

I'm a bit concerned at the isFlying stuff going to zero here!

Oops, wrong figure, will fix

[priseborough]

@peterbarker I don't know what's going on with the self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd_int) test that is failing on the 'Denied when armed' test element.

The same test is passing when called previously by self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd)

[priseborough]

@peterbarker The failing CI test.PlaneTests1b: MAV_CMD_PREFLIGHT_CALIBRATION is passing on my machine so don't know what to do to get it to pass.

Edit: looks like it passes sometimes, not always so will investigate further.

Edit: I'm getting these failures when I run the test using master at b996deb

FAILED STEP: test.PlaneTests1b at Fri Jan 23 18:18:46 2026
Failure Summary:
test.PlaneTests1b:
VectorNavEAHRS (Test VectorNav EAHRS support) (GPS status bits did not become good) (see /home/parallels/src/buildlogs/ArduPlane-VectorNavEAHRS.txt) (duration 1.7730586528778076 s)
Deadreckoning (Test deadreckoning support) (Did not get SYS_STATUS after 5.013347864151001 seconds) (see /home/parallels/src/buildlogs/ArduPlane-Deadreckoning.txt) (duration 6.341466426849365 s)
MegaSquirt (test MegaSquirt driver) ([Errno 111] Connection refused) (see /home/parallels/src/buildlogs/ArduPlane-MegaSquirt.txt) (duration 20.448970079421997 s)
Hirth (Test Hirth EFI) ([Errno 111] Connection refused) (see /home/parallels/src/buildlogs/ArduPlane-Hirth.txt) (duration 20.45398497581482 s)
MAV_CMD_PREFLIGHT_CALIBRATION (test MAV_CMD_PREFLIGHT_CALIBRATION mavlink handling) (Expected MAV_RESULT_ACCEPTED got MAV_RESULT_FAILED) (see /home/parallels/src/buildlogs/ArduPlane-MAV_CMD_PREFLIGHT_CALIBRATION.txt) (duration 0.3372013568878174 s)
LoggedNamedValueString (ensure that sent named value strings are logged) (Did not get NAMED_VALUE_STRING after 1.0040805339813232 seconds) (see /home/parallels/src/buildlogs/ArduPlane-LoggedNamedValueString.txt) (duration 2.3505749702453613 s)

[priseborough]

@rishabsingh3003 , @peterbarker , @snktshrma

Following feedback from the MAVLink maintainers I'll rework to use GLOBAL_POSITION_SENSOR for use with onboard sensors when mavlink/mavlink#2419, mavlink/mavlink#2422 and ArduPilot/mavlink#481 are merged

[priseborough]

I've rebased on latest master (no conflicts) and have added support for the GLOBAL_POSITION_SENSOR message. I'll follow up with some test logs.

[rishabsingh3003]

The cov are being reset unconditionally? Is that correct when the new bits in EK3_OPTIONS are set?