I am working on simulating a copter using Airsim and SITL in a GPS-denied environment. My aim is to use Lidar SLAM for navigation without relying on GPS. Here is the diagram of the system:
I have a 2D lidar attached to the copter in Airsim that provides the lidar scans with 10 Hz. These scans are utilized by the kiss-icp algorithm to provide odometry. Then, the odometry and the scans are processed by the SLAM toolbox to do the mapping and localization.
When the copter is being controlled with the default GPS-enabled settings where lidar is not used for the navigation, kiss-icp odometry provides smooth odometry results and the SLAM toolbox generates the map of the environment as expected. Here is a visualization of it using rviz2 where the green path is the kiss-icp odometry predictions.
However, when I activate the SLAM-based navigation by enabling the following parameters, the control of the drone is not stable.
I see the VISION_POSITION_ESTIMATE messages are generated and sent to the FCU with 10 Hz. I can takeoff the drone and fly it in GUIDED mode with 1 m/s for some time but then it loses control and goes wild. With 2 m/s speed, it goes out of control within less time. A video of what is happening is given below:
- Is a frequency of 10 Hz enough for VISION_POSITION_ESTIMATE?
- Is disabling EK3_SRC1_VELXY and EK3_SRC1_VELZ are okay? If not, how can I provide them with the 2D slam?
- Should I also publish the ODOMETRY mavlink messages to the copter from the kiss-icp? If yes, in which coordinate frame (ENU or NED)?
- What can I do to make control of the quadcopter more stable?
Many thanks for your help.