Swerve odometry core logic

[ThomasP850]

Resolves #12

Most of the logic is pretty straightforward. Let me know if anything looks weird.
Key changes

• Pulled @QuackingBob's matrix utils from the robot arm repo
• Updated a lot of the swerve logic to use explicit matrices.
• Created functional swerve odometry

What's left for swerve odometry:

• Test our method of getting chassis angle using the gimbal IMU (Ideally, we should add an IMU to the chassis at some point).
• Publish odometry data wherever algorithms wants it.
• Measure accuracy over set distances, get a general idea of standard deviation with respect to time/distance driven.

[irvingywang]

This code looks pretty good! Could you refactor so that swerve odometry code is contained inside swerve_locomotion.h/c? Otherwise LGTM