SensorOdom returns wrong covariance matrix dimensions in omni wheel case

In the 2D case, the implementation of data.size == 3 is missing (dx, dy, dtheta)