Autonomous navigation following a manual flight log

What is the best way to follow a manual flight as closely as possible?
Right now I’m sending ever n points in the manual flight local log (x,y,z, yaw, and velocities) to /mavros/setpoint_raw/local with mavros_msgs::PositionTarget messages.

This roughly works, but between every setpoint, it slows down and speeds up again (I’m guessing due to the PID), and this causes the motion to be super jerky. Is there a better way to do this?

P.S. I’m running all this with gazebo, and not actual hardware, and I eventually want to do this in GPS denied environments, so I’m not sure if I could use the mavros waypoint mission since it requires lat,long,alt.


It seems making your controller lower level as much as possible would be good.
If you’re using multi-rotor model, attitude topic can replace the acceleration command.

In case of GPS denied environment, you might manage to use waypoint via IMU but error would be serious. As long as I know, most of the related works utilize filters or external sensors to mitigate the error.