Add covariance matrix to a ros odometry messange