Gazebo Simulation Hector SLAM for position estimation of gps-denied drone


I am using PX4 v1.14.0, with MAVROS, ROS Noetic with a SITL Gazebo Classic simulation.

To simulate GPS-denied environment, I have removed the gps sensor from the sdf file of the iris drone model.

I have added a 2D lidar on the iris drone, and running Hector SLAM to do position estimation.

My px4_config.yaml file has all the tf publish set to false.

I am using a 1D-lidar to estimate the Z distance (height above the ground) the drone is flying. The ground is flat as the drone is flying inside an apartment.

I am publishing the tf frames like the following:

base_link → base_stabilized: I use the IMU information returned by mavros in the hector_imu_attitude_to_tf node

base_stabilized → base_footprint: I simply subscribe to the distance measured by the 1D lidar and publish the z translation.

base_footprint → odom: Subscribing to the /slam_out_pose outputted by the Hector SLAM and removing the z, the pitch and the roll from it. I believe this is like using its own data to feed itself, and thus completely wrong, so I would like help with this please.

odom → map published by hector SLAM.

My Hector mapping config looks like the following.

I believe that the /slam_out_pose published by the Hector SLAM should be fed into mavros as "/mavros/vision_pose/pose”, so that PX4 can take this input to do pose estimation based on Visual Inertial Odometry (VIO), and I am expecting that the values on topics "/mavros/vision_pose/pose” and “/mavros/local_position/pose” would be the same. But this is not happening, and when I command the drone to take off and hold its original X-Y position, the drone takes off and flies off to a random direction.

Please can someone help?

I have not changed any parameters for PX4. I have not found EKF2_AID_MASK in PX4 v1.14.0 to be changed to use VIO.

My question in short is,

How to setup a simulation in Gazebo Classic using PX4 v1.14.0, SITL, iris drone equipped with 2D lidar for X-Y scanning, and 1D lidar with Z scanning, NO GPS, has IMU, to use Hector SLAM to do position estimation?