Local position estimation divergence with optical flow

Hello all!

My issue is similar to this post: Divergence of local position estimate with optical flow

I am running a simulation where a drone does not access to gps and instead uses optical flow. At the beginning before optical flow data has began to be fused, velocities are pretty large and random. They eventually even out as optical flow begins to fuse, but my local position estimates still remain diverged.

The post I mentioned says to use LPE, but I don’t think that will fix the problem because the main issue is that its allowed to make such serious estimates even before it starts to use optical flow. Is there a way to reset the position to (0,0), limit the changes before optical flow it used, or just straight up don’t start estimating until optical flow is used?

I’ve attached a plot of some of the logs showing what I mean.