Swerve Drive Odometry System