Thank you for the suggestion! However, I tried to use PID tuning but it seemed to not make much difference.
Plus, the velocity Z set point changes (even though I am sending Vz=0 in offboard mode constantly and the position is staying the same). You can see it in the photo but here is the link if you like: https://review.px4.io/plot_app?log=3e8a8989-76cf-4046-bfc4-f52b38462407 .
I also tried changing the weight of the drone but that seems to not make a difference either.