Apart from these a flow deck is attached and the overall flying weight is 42g.

Now the CF seems to be unstable when flying. Basically it can maintain the stability well enough when hovering. It becomes unstable and crash when I try to change the moving direction using set_hover_point command. At first I thought this as a problem with the flow deck since there were not enough patterns on the floor. But the problem is same with good patterns and lighting conditions. Most of the times it topples upside down when crashing. As per my knowledge I think the cg has moved higher with the new upgrades.

What can I do to increase the stability ? Would it be enough tuning PIDs to overcome this problem?

There is no proper method to identify the out of range readings in VL53L1X. When an obstacle is not within the range, sensor gives random values. That's the reason to mark the cells as occupied at unnecessary places.

Are you plotting the points with respect to the height to make a 3D plot?

The biggest problem is the pose correction with a limited number of sensors. Are you using any kind of pose correcting method or just plotting the points around the estimated position from EKF?

Ok, thanks for the info. I haven't looked that much into the limiting but it sounds interesting. For the plot in the image I limited the distance to 2 m.

Yes, it's plotted in 3D but I was at the same height (the boards around wasn't very high). But I'm hoping to get some time trying to fly in an environment where I get go a bit up and down. I've cleaned up the example a bit and posted it here.

I haven't tried any correction, I'm just plotting the points around the estimated position from the EKF.

The problem is that the error accumulates with the distance when there is no pose correction. Currently I'm working on several methods for pose correction. The hardest part is to correct the pose using a limited number of ToF sensors. I will update the thread about the progress.

Apart from that have you tried to maintain a constant altitude over obstacles with different heights?