Cannot tune a 250 UAV to fly in offboard

Here are my logs:

I have a 250 mm UAV that flies indoor and I have to say that it’s the hardest UAV to tune that I have ever tested. It’s holding the position but drifts and oscillates around setpoint. It flies relatively well in stablize or altctl, so I assume that MC_* parameters are OK and the problem is in MPC_XY_.
Do you have any tips or ideas how to improve it’s performance?

This map explains my problem:

Are you sure that the issue is the position control tuning and not the estimation? Could it be that there are delays in your estimation which leads to the poor performance?

@JulianOesThat may be a problem. I have estimated the delays to be around 180-200 ms behind IMU readings. Vibrations and oscillations of the UAV in air generates some additional noise but I was hoping for the EKF2 to filer it. Unfortunately I don’t have a system to read the ground truth and I really don’t know how to improve it.

@JulianOes And what about the altitude? I have a Lidar set as primary sensor but cannot make it hold altitude when flying in offboard:

Is that OK result or do it need more tuning? Bigger platforms performed way better, even on default parameters.

It could also be a vibration issue looking at your accel data. Which Pixhawk or Pixracer are you using? And how is it mounted to the frame?

It’s a Pixfalcon mounted on metal plate and fairly stiff foam.

I have compared local_position.z with current_distance from lidar and it clearly shows that the local position is not strictly following readings from LIDAR. If I enable terrain following in MPC_ALT_MODE it should hold the altitude based on the ground sensor, right? And if I fly in offboard then I have to take care of it myself by calculating appropriate setpoint, right?

Edit: I changed to velocity setpoint and can say that It’s definitely a vibration problem:
The velocity estimation is wrong. It sometimes thinks that it’s flying up when the distance sensor is showing that it’s actually hovering or going down. These brief moments generates oscillations from the graph above.

@JulianOes it doesn’t look that bad for me:

I’m not sure how to help except suggestion to mount the Pixhawk on vibration damping foam. Or maybe @Paul_Riseborough has input.

Ok, thank you for your help. I will try to improve vibroisolation and further tune parameters.