Implement RAIM for April Tags

[jkleiber]

Purpose
The purpose of this addition is to add fault detection and exclusion to our vision pipeline such that erroneous/low confidence AprilTag readings do not enter our position estimate.

The concept is used in GPS navigation, and you can read more about RAIM here: https://en.wikipedia.org/wiki/Receiver_autonomous_integrity_monitoring

The fundamental idea is to reject outlier measurements if multiple navigation references are available. In RAIM, this is related to the number of GPS satellites in view. In our application it is related to the number of AprilTags in View

Project Scope

• Add fault detection and exclusion to AprilTag measurements