C++ AngleMean tests don't account for angles being weighted

[connorworley]

The implementation of AngleMean varies between C++ and Java.

C++: https://github.com/wpilibsuite/allwpilib/blob/2cb171f6f5bfc01f852234e1a1cc9646936f153c/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h#L97-L102

Java: https://github.com/wpilibsuite/allwpilib/blob/4d9ff7643318f2bd7eb7530c1b70106d0875e800/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java#L101-L102

Both are supposed to be based on code from https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter.ipynb (see definition of state_mean) but the C++ version fails to weight the angular state variable.

#4168 contains a fix, but the issue was not caught by AngleMean's tests as they don't seem to meaningfully check for weighting (applying the fix also does not cause unmodified tests to fail). A test should be added that checks for weighting.