Add odometry covariance matrices

[jacobperron]

Should do some research into how to properly populate the covariance matrices for robot position and velocity. These can be updated and stored along with the robot Pose and Vel structs:

struct Pose {
    float x;
    float y;
    float yaw;
    float covariance[];
  };

[jacobperron]

The covariances of pose and velocity are now populated based on section 5.2.4 of the book "Introduction to Autonomous Mobile Robots" (Siegwart 2004) (link to PDF)

Still not quite sure if I implemented the math correctly or if the velocity covariance is correct. I am just setting the velocity covariance to the update increment for the pose covariance. At least the pose covariance appears to grow without bound as it should (up to MAX_FLOAT) and the velocity covariance does not.