Use version of addVisionMeasurement that takes stddevs to improve accuracy

[brettle]

You may be able to use the camera resolution and the expected relative pose of the AprilTag to estimate the stddev of the vision measurement so that, for example, when the AprilTag is further away it is given less weight.

I recommend this be a lower priorty item any implementation should be tested before it is merged into master.