Turtlebot3 2D LIDAR pose estimation

Question

I am trying to implement point to point ICP using a Turtlebot3 as part of a state estimation stack. Here are the sensor specs: http://www.robotis.us/360-laser-distance-sensor-lds-01-lidar/

Currently the transformations returned by my algorithm are short of what they should be - e.g. a 30cm movement in the x direction is detected as a translation of around 7cm. The same goes for rotations. I am struggling to find the issue(s) with my code; I am also wondering whether this LIDAR is suitable for such a purpose.

My initial guess for the transformation is provided by the state output by a kalman filter that is currently only running the prediction step. I have carefully verified that these estimates are very close to the true transformation in my short tests.

When I transform the LIDAR scan into [x,y] coordinates in the sensor reference frame, if the range of the point is less than the minimum range, I set it to the minimum range and likewise for the maximum range. I don't think this is the issue, but putting it here in case.

Here is my pseudo-code for ICP:

ICP(src, dst, guess):

    src = transform_points(src, guess)
    transform = guess

    for i in range MAX_ITERATIONS:
        // Returns the indicies of the coordinates in dst that are closest to each point in src  
        // and the distances between pairs
        dists, idxs = find_nearest_neighbour(src, dst)

        // Remove outliers
        src = src[dists <= 0.02]
        idxs = idxs[dists <= 0.02]
        dst = dst[idxs]

        tr = compute_transform(src, dst)
        
        src = transform_points(src, tr)

        // Update transform with dot product
        transform = tr * transform

    return transform

I have tried playing with the value of MAX_ITERATIONS, introducing a change in error termination condition and absolute error condition.

This way of removing outliers is quite crude, but without it, the robot's pose as computed by applying the transformation returned by ICP will diverge even if the robot is not moving. The transform is computed with around 250 out of the original 360 points.

Compute transform uses the closed form solution utilising centroids and SVD of cross-covariance matrix. I noticed that the first time this is called in the ICP loop (i.e. the iteration straight after the guess transform was applied), the transform returned "undoes" most of what the guess did to the src coordinates. This is the python code for the function in case this is the issue

def compute_transform(self, src, dst):
        src_centroid = np.mean(src, axis=0)
        dst_centroid = np.mean(dst, axis=0)
        s = src - src_centroid
        d = dst - dst_centroid
        ccovar = (d.T @ s) / dst.shape[0]
        u, _, v = np.linalg.svd(ccovar)
        # Ensure we get a proper rotation without a reflection:
        v[-1, :] *= np.linalg.det(u) * np.linalg.det(v)
        r = u @ v.T
        t = dst_centroid - r.T @ src_centroid
        return np.c_[r, t]

I am hoping someone might be able to spot any mistakes / suggest where to look / tell me that what I'm trying to do isn't possible as I've spent a good few weeks trying to debug my code to no avail. Thank you very much for your time.