Sensors mean

[Peque]

We currently rely on the last single reading. It would be more reliable to use the mean of the last couple of readings. It would also reduce the noise.

This would also mean that readings are a bit delayed too... But we could workaround this as well by taking into account last measurements speeds (combine sensors and encoders to better estimate position). Kalman filters?

For searching we could use a 5 or 10 mean measurement.