Gate yawConsistency on pose initialization to fix cold-start vision rejection

[nlaverdure]

Closes #181

Summary

• Rename poseInitialized → poseAsserted in Drive, Vision, Robot for clarity
• Vision takes BooleanSupplier poseAssertedSupplier + Supplier<Rotation2d> headingSupplier; heading is passed to scoreObservation as null until activation, causing yawConsistency to skip
• Dual-path activation for yawConsistency:
  1. poseAsserted = true — set by auto routine via Drive.setPose()
  2. visionAcceptedCount >= yawConsistencyMinAccepted (default 10) — vision self-bootstraps without an auto routine (teleop-only matches, practice)
• Remove stale comment claiming FMS connection initializes the pose (it does not)

Log validation (VACHE Q54)

Analysis of akit_26-03-22_14-57-55_vache_q54.wpilog:

• Pre-auto (disabled): All 4 cameras see tags at 5–7 m, exceeding tagDistanceTolerance = 4.0 m. Zero observations pass. visionAcceptedCount stays at 0. The poseAsserted path (auto) remains the primary activation mechanism for competition starting positions.
• Post-auto (poseAsserted = true): First accepted pose at t=151.16 s (1.16 s into auto). 10th accepted pose at t=151.93 s (~1.9 s into auto). Scores uniformly 0.65–1.00, all above minScore = 0.65. Pose trajectory smooth — no jumps or outliers.
• Threshold conclusion: yawConsistencyMinAccepted = 10 is well-chosen. Accepts arrive dense (~5–8/s once in range) and the pose is stable by the 10th accepted observation. The threshold would be reached in ~2 s in any scenario where observations pass the distance/ambiguity filters.

Test plan

• Verify vision poses are accepted immediately after robot boot (before any auto routine) when tags are within 4 m
• Verify yawConsistency still rejects ambiguous PnP solutions after pose is activated
• Confirm RobotPosesAccepted is non-empty during auto (poseAsserted path)
• Confirm vision self-bootstraps (visionAcceptedCount path) in a teleop-only run with robot placed near tags

🤖 Generated with Claude Code