Combine Vision Measurement with Odometry

[gokulBa1aji]

1. Add a method to update the SwerveDrivePoseEstimator with a vision-based pose estimate
2. Test with multiple AprilTags-preferably the same tags and the same spacing as on the actual field
3. Ensure that pose-estimation works from multiple angles