Unstable altitude and yaw performance in an indoor 3D slam copter

I’ve been testing a copter using 2 OS1-16 3D lidars with ROS for indoor navigation purposes and I’m experiencing very unstable performance in Loiter and Guided mode flights. The copter largely overshoots altitude targets in Guided mode missions, and the altitude values reported by MP are all over the place. It also keeps yawing in Loiter hover without any pilot input and in Guided mode as well, when the yaw exceeds ~90deg, the copter then starts spinning and the pilot has to take over to land the copter.

The drone has unusual dimensions (388mm width, 480mm height, 14" props and weight of 4.8Kg) so at first I suspected that the weight might be causing the issues but we tried to fly it using GPS indoor and we didn’t experience any of those issues. And looking at the vision position in the logs and comparing it to the target position from the Guided mode missions, it seems that the positioning from ROS is stable and fairly accurate. So I’m not sure how to explain the unstable EKF behavior. I attached the logs for one of the guided mode flights (the target positions are sent by a companion computer, for some reason guided appears in the log as HOLD) and one of the loiter flights. I’d appreciate if anyone can share any insight.

guided and loiter