Problem
`Test.yawConsistency` was added to `DEFAULT_ENABLED_TESTS` in VisionFilter.java. This test compares the vision-estimated yaw against the gyro yaw within a ±10° tolerance (`yawToleranceRadians`).
Because `yawConsistency` has weight = 1.0, a near-zero score drives the entire weighted geometric mean to ~0 — lower than any practical `minScore` threshold. This means no poses are accepted at all, and lowering `minScore` has no effect.
Root cause
The gyro initializes at 0° on robot boot. If the robot is physically facing any heading other than ~0°, the gyro and vision yaws diverge immediately. At a ~67° error (observed in 2026-03-26 logs, sessions 87–90), `yawConsistency` scores ≈ 0.0 and kills every observation.
Confirmed in logs: 1,669 rejected observations, 0 accepted, across all 4 cameras — even with `minScore` lowered to 0.4.
Why the test exists
The intent is sound: ambiguous single-tag PnP solutions almost always produce incorrect yaw (typically 30–90° off), and the gyro is a reliable independent yaw source. The test is a strong discriminator — once the robot's heading is known.
The gap
The test is only valid after the gyro has been seeded with the field-relative heading, which doesn't happen until the pose estimator has been authoritatively initialized (e.g. by an auto routine). Until then the gyro reads a robot-relative heading (typically 0° on boot), not a field-relative one, making the test meaningless and destructive at startup.
Suggested fixes
- Exclude from `DEFAULT_ENABLED_TESTS` — add it to a separate set used only after the pose has been authoritatively initialized.
- Pass a `poseInitialized` flag into the score call and skip the check until true (similar to how `headingSupplier` is now gated in Vision.java).
- Widen cold-start tolerance (e.g. 45°) and tighten once the pose is known — though this partially defeats the purpose.
Options 1 or 2 are preferable.
Relevant code
- `VisionFilter.java` — `DEFAULT_ENABLED_TESTS`, `yawConsistency` test
- `VisionConstants.java` — `yawTolerance = Degrees.of(10)`
- `Vision.java:180` — `poseInitializedSupplier.getAsBoolean() ? headingSupplier.get() : null`
Problem
`Test.yawConsistency` was added to `DEFAULT_ENABLED_TESTS` in VisionFilter.java. This test compares the vision-estimated yaw against the gyro yaw within a ±10° tolerance (`yawToleranceRadians`).
Because `yawConsistency` has weight = 1.0, a near-zero score drives the entire weighted geometric mean to ~0 — lower than any practical `minScore` threshold. This means no poses are accepted at all, and lowering `minScore` has no effect.
Root cause
The gyro initializes at 0° on robot boot. If the robot is physically facing any heading other than ~0°, the gyro and vision yaws diverge immediately. At a ~67° error (observed in 2026-03-26 logs, sessions 87–90), `yawConsistency` scores ≈ 0.0 and kills every observation.
Confirmed in logs: 1,669 rejected observations, 0 accepted, across all 4 cameras — even with `minScore` lowered to 0.4.
Why the test exists
The intent is sound: ambiguous single-tag PnP solutions almost always produce incorrect yaw (typically 30–90° off), and the gyro is a reliable independent yaw source. The test is a strong discriminator — once the robot's heading is known.
The gap
The test is only valid after the gyro has been seeded with the field-relative heading, which doesn't happen until the pose estimator has been authoritatively initialized (e.g. by an auto routine). Until then the gyro reads a robot-relative heading (typically 0° on boot), not a field-relative one, making the test meaningless and destructive at startup.
Suggested fixes
Options 1 or 2 are preferable.
Relevant code