Changing the dead reconning from using the tracks odometry to using a gyro for calculating the change in direction yielded a significant improvement.

The raw telemetry and lidar scans now produce a reasonably reconisable map.

The gryo however suffers continual drift. I've eliminated this when stationary by simply ignoring data from the gyro when the tracks are not turning.

There are also occasional blips of sudden and large magetude change in heading coming ftom the gyro. I intend to try using the calculated change in heading from the tracks odometry to cap the gyro output. This should work as the original problem was that the odometry over estimated the amount of turn due to slip and the gyro only does this occasionally.

I will also be looking at the code i am using to integrate the raw data from the gyro, as it currently has no time compensation and neither the os or java are realtime.