EKF variance weighting

This weights the variance "correctly" as we're repeatedly fusing the same GPS measurement to avoid steps on the position estimate. Not flight tested yet, testing on harness in progress.