External Vision Position Estimate not working correctly

PX4 Autopilot
1

Hi guys,

I am working on a VPS algorithm for geolocalization, comparing drone feed with satellite image with a mono camera downward facing, I get the estimated latitude/longitude, convert it to NED frame for PX4 and publish it to fmu/in/vehicle_visual_odometry at 28-30Hz. I am simply fusing horizontal position (x,y) data from my algorithm since yaw is very noisy.

After setting up these parameters -

param set EKF2_GPS_CTRL 0
param set EKF2_EV_CTRL 1 (fuse horizontal position only)
param set EKF2_HGT_REF Baro
param set EKF2_EV_DELAY 100
param set EKF2_EV_NOISE_MD 0

I take it off to a certain altitude, and then run the VPS script, and set up all the above parameters, assuming that the yaw will be handled internally by PX4 (IMU, Magnetometer,etc), but the drone starts drifting away and then goes completely haywire, I have attached an image of the same -

image
image1920×1059 340 KB

I am unable to understand the reason behind this. I have attached the function block, which publishes the estimated NED values to the fmu/in/vehicle_visual_odometry topic.

def fast_publish(self):
if self.last_ned is None:
return
north, east, down = self.last_ned
msg = VehicleOdometry()
now_us = int(time.monotonic() * 1e6)

msg.timestamp = now_us
msg.timestamp_sample = now_us
msg.pose_frame = VehicleOdometry.POSE_FRAME_NED
msg.position = [float(north), float(east), float(down)]
msg.q = [1.0, 0.0, 0.0, 0.0]
msg.velocity_frame = VehicleOdometry.VELOCITY_FRAME_NED
msg.velocity = [float('nan'), float('nan'), float('nan')]
msg.angular_velocity = [float('nan'), float('nan'), float('nan')]
msg.position_variance = [2.0, 2.0, 5.0]
msg.orientation_variance = [float('nan'), float('nan'), float('nan')]
msg.velocity_variance = [float('nan'), float('nan'), float('nan')]
msg.quality = 0
self.odom_publisher.publish(msg)

I have also ran a case, keeping GPS enabled and comparing the output from both VPS and GPS, and the normal motion are pretty accurate although yaw is highly drifty as expected.

image
image1507×806 108 KB

Can someone help me out with this

2

Hi
If you have a visual navigation system which provides latitude/longitude I highly recommend to use the auxiliary global position (AGP) interface. This is exactly what it was built for…

Since you’re highly dependent on using the magnetometer for your heading estimate. Make sure you have selected the correct EKF2_MAG_TYPE and performed the mag-calibration on an open field (!).

Let me know if you have further questions, Marco

3

Thank you for your response. I will set up the MAG_TYPE parameter, I might have missed out on that.

Currently, I am running in SITL, so if i publish to the AGP topic, will it get fused with the EKF2 filter. Thanks, Akshay

4

Hi Marco,

Tried using AGP interface and faced few issues -

We took off the drone in SITL, ran our satellite imagery map matching script

Also, we are publishing to /fmu/in/aux_global_position at around 30-33Hz with this function block -

def global_fast_publish(self):
# AUX publisher
msg = AuxGlobalPosition()
now_us = int(time.monotonic() * 1e6)
msg.timestamp = now_us
msg.timestamp_sample = now_us

msg.lat = self.global_lat
msg.lon = self.global_lon
msg.alt = float(0.0)

msg.source = AuxGlobalPosition.SOURCE_VISION
msg.id = 1

msg.eph = 2.0
msg.epv = 4.0

msg.lat_lon_reset_counter = 0

self.aux_gps_pub.publish(msg)
self.get_logger().info(f"AUX PUB → lat={self.global_lat:.6f}, lon={self.global_lon:.6f}")

and then set these params -

param set EKF2_GPS_CTRL 0 # disable real GPS
param set EKF2_AGP0_CTRL 1 # enable horizontal fusion
param set EKF2_HGT_REF Baro # use barometer for height
param set EKF2_MAG_TYPE 1

But everytime, we set the parameters, It goes in land mode. Also, when we run listener aux_global_position, we get never_published

pxh> listener aux_global_position
never published

Can you please let us know if we are missing out on anything, or need to set any other specific param or do we have to publish to fmu/in/vehicle_visual_odometry topic as well

5

Did you run micro-xrce-dds-agent udp4 -p 8888 like described here uXRCE-DDS (PX4-ROS 2/DDS Bridge) | PX4 Guide (main) ? Since the uorb topic is not present, I assume there is something wrong in the parsing to uorb…

Also for later: You need to set the EKF2_AGP0_ID in the PX4 params as well.

6

Hi Marco,

I have been following this tutorial -

Got the listener aux_global_position working, but after i disable gps and enable AGP0 Control, QGC goes into land mode.

This is what I have followed -
Terminal 1: I run PX4_GZ_WORLD=aruco make px4_sitl gz_x500_mono_cam_down
Terminal 2: QGC
Terminal 3: MicroXRCEAgent udp4 -p 8888
Terminal 4: ROS_GZ bridge for Camera & IMU

When i run ros2 topic list, I see -
/fmu/in/aux_global_position_v1 to which I publish my lat/lon values via map matching.

This is my code block for publishing the values -

def global_publish(self, lat, lon, sample_us=None):

        msg = AuxGlobalPosition()

now_us = int(time.monotonic() * 1e6)

if sample_us is None:

sample_us = now_us

msg.timestamp = 0

msg.timestamp_sample = int(sample_us)

msg.id = 1

msg.source = 2

msg.lat = float(lat)

msg.lon = float(lon)

msg.alt = float("nan")

msg.eph = 0.322

msg.epv = float("nan")

msg.lat_lon_reset_counter = 0

self.aux_gps_pub.publish(msg)

self.get_logger().info(f"AUX PUB → lat={float(lat):.8f}, lon={float(lon):.8f}")

on listener_aux_global_position, I get -

pxh> listener aux_global_position

TOPIC: aux_global_position
aux_global_position
timestamp: 2922589000 (0.012000 seconds ago)
timestamp_sample: 2922589000 (0 us before timestamp)
lat: 19.135972
lon: 72.912900
alt: nan
eph: 0.32200
epv: nan
id: 1
source: 2
lat_lon_reset_counter: 0

I take off the drone to a certain altitude, until the lat/lon values are published and After setting up these param -

param set EKF2_GPS_CTRL 0
param set EKF2_AGP0_ID 1
param set EKF2_AGP0_CTRL 1
param set EKF2_HGT_REF Baro
param set EKF2_MAG_TYPE 1

QGroundControl goes into land mode indicating no valid global position and local position estimate. Since map matching based estimation is already intensive task, the frequency drops to 10Hz, is that an issue, or should i be publishing to local NED topic as well. Can you help me out here. Thanks

7

okay already one step closer. can you check the uorb topic estimator_aid_src_aux_global_position? You can check the “fused” and “test_ratio” (>1 leads to rejection) field in there to see if the measurement is getting rejected due to “too high innovation while having a low uncertainty”. I assume this is the case since you’re vision pipeline reports an eph of 0.3 which is really small. At least for now, bump the EKF2_AGP0_NOISE to 100, this will decrease the test ratio. For more theory information behind this check out my talk: https://www.youtube.com/watch?v=CMGQJNPiTJg

8

Yes, I have been following your talk, and I also increased the Horizontal standard deviation to 5.0, since my error to gps is around 3-5m and tweaked the following parameters -

param set EKF2_GPS_CTRL 0
param set EKF2_AGP0_ID 1
param set EKF2_AGP0_CTRL 1
param set EKF2_AGP0_NOISE 100
param set EKF2_HGT_REF Baro
param set EKF2_MAG_TYPE 0
param set EKF2_NOAID_TOUT 10000000 (EKF timeout)
param set COM_POS_FS_EPH 10.0 (Increased Error threshold)
param set EKF2_AGP0_GATE 5.0

After GNSS data fusion disabled, the mission still continues, but cs_aux_gpos: False still continues to report False, so I am assuming that it uses EKF dead-reckoning and not my VPS algorithm. Between points 3-4, It encounters slight deviation and goes back to land mode again. Not sure why it rejects my fused VPS data even though I have increased threshold and timeout. I ll tweak some more parameters and let you know. Thanks for your help

image
image1920×1316 392 KB