Vision improvements

[minhlpnguyen]

What else should I add? Created a method for filtering individual targets, unsure if it works better

[minhlpnguyen]

@jkleiber @linglejack06 should a filter by target distance be added? this would require writing our own pose estimator class.
A method in Camera.java sorts pose estimations into high uncertainty versus low uncertainty based on average distance and angle from targets; this doesn't weight individual targets differently within a pose estimation but does weight overall poses when considering effect on localization

[aidnem]

Other than the absolute values for the pitch suggestion, this looks fine to me.
Could we also add roll filtering?

[aidnem]

Can we use an absolute value so that if pitch is too positive or too negative it'll reject both?

[aidnem]

Also is there any reason to do this here and not in the filterPhotonPose method (or we could do it in both places)?
I'm not necessarily asking for you to change it, I'm just curious about the reasoning behind putting it here vs. somewhere else.

[minhlpnguyen]

I added adjustments to variance calculating- could you see if it actually makes any logical sense? thank you!

[minhlpnguyen]

Should errors based off of distance be calculated based on distance alone or x and y components of distance?