What’s the new issue in the pose estimation algorithm?

The new issue in the algorithm is doesn’t calculate the correct movement coordinates. It calculates the direction and increases/decreases the x and y coordinates, but not the correct amount.

I think this issue is because lidar noise. Sometimes it detects points as corners which are just noise, but more often the points which are real corners just move around a lot. When it matches the corners and calculates the displacement, it includes the noise in the calculation.

So how will I fix it?

I plan to match find the corner pairs for all consecutive readings, but will not run the ICP algorithm (which calculates displacement). I will match points, which were matched in the previous readings and using that matches after about 5-6 readings I will run the ICP based on the first and the last reading corner point matches. I know this is hard to understand, so here’s the flowchart algorithm: