Why Standard Deviations Aren't Enough

Lesson 7's dynamic standard deviation formula correctly assigns lower trust to measurements that are expected to be less accurate. But there's a category of measurement where the error isn't just "larger than usual" — it's catastrophically wrong. A phantom tag detection from a false positive pattern on a banner, or a solvePnP result that puts the robot 8 meters off-field, isn't a slightly inaccurate measurement that should be weighted less. It's a corrupted input that should never reach the Kalman filter at all.

The Kalman filter is designed for Gaussian noise — random errors centered around the correct value. A single outlier 3 meters from the true position is not Gaussian noise. Even with a high standard deviation (low trust), this measurement pulls the filter's estimate meaningfully toward a wrong position. Over several seconds of autonomous, this pull compounds, and the robot's tracked position drifts toward wherever the outliers came from.

The correct architecture is two separate layers: an explicit rejection gate that discards obviously invalid measurements before they reach the Kalman filter, and standard deviation tuning that appropriately weights the measurements that pass the gate. Neither is a substitute for the other.

The Five Rejection Gates

Filter Gate Simulator

Configure the rejection thresholds below and see which detection scenarios are accepted or rejected. The scenarios represent realistic situations from FRC competition — adjust the gate thresholds to find the settings that accept good measurements while rejecting the dangerous ones.

Complete shouldAcceptMeasurement() Implementation

The filter function is a single method that encodes all five gates. It returns true if the measurement should be passed to addVisionMeasurement(), and false if it should be discarded. Every rejection logs its reason — that log is the diagnostic tool you'll need during the inevitable "vision isn't helping" debugging session.

// ── Rejection counters (for SmartDashboard logging) ───────────────────────
private int m_rejectedTagCount  = 0;
private int m_rejectedAmbiguity = 0;
private int m_rejectedDistance  = 0;
private int m_rejectedBoundary  = 0;
private int m_rejectedRotation  = 0;
private int m_accepted          = 0;

/**
 * Returns true if the vision measurement should be passed to the Kalman filter.
 * Logs the rejection reason to SmartDashboard when returning false.
 *
 * @param estimate    The EstimatedRobotPose from PhotonPoseEstimator.update()
 * @param avgDist     Average distance to detected tags (meters)
 * @param gyroRateDegPerSec  Current gyro rotation rate (degrees per second)
 */
private boolean shouldAcceptMeasurement(
    EstimatedRobotPose estimate,
    double avgDist,
    double gyroRateDegPerSec) {

    int numTags = estimate.targetsUsed.size();
    Pose2d pose = estimate.estimatedPose.toPose2d();

    // Gate 1: Require at least one tag ────────────────────────────────────────
    if (numTags == 0) {
        m_rejectedTagCount++;
        SmartDashboard.putString("Vision/LastReject", "NO_TAGS");
        return false;
    }

    // Gate 2: Pose ambiguity (single-tag only) ────────────────────────────────
    // Multi-tag estimates don't have the 2-solution ambiguity problem.
    if (numTags == 1) {
        double ambiguity = estimate.targetsUsed.get(0).getPoseAmbiguity();
        if (ambiguity < VisionConstants.MIN_POSE_AMBIGUITY) {
            m_rejectedAmbiguity++;
            SmartDashboard.putString("Vision/LastReject",
                "LOW_AMBIGUITY (" + String.format("%.2f", ambiguity) + ")");
            return false;
        }
    }

    // Gate 3: Distance cutoff ─────────────────────────────────────────────────
    if (avgDist > VisionConstants.MAX_VISION_DISTANCE_METERS) {
        m_rejectedDistance++;
        SmartDashboard.putString("Vision/LastReject",
            "TOO_FAR (" + String.format("%.1f", avgDist) + "m)");
        return false;
    }

    // Gate 4: Field boundary ──────────────────────────────────────────────────
    if (pose.getX() < 0 || pose.getX() > VisionConstants.FIELD_LENGTH_METERS ||
        pose.getY() < 0 || pose.getY() > VisionConstants.FIELD_WIDTH_METERS) {
        m_rejectedBoundary++;
        SmartDashboard.putString("Vision/LastReject",
            "OUT_OF_FIELD (" + String.format("%.1f", pose.getX()) + ","
            + String.format("%.1f", pose.getY()) + ")");
        return false;
    }

    // Gate 5: Rotation rate ───────────────────────────────────────────────────
    if (Math.abs(gyroRateDegPerSec) > VisionConstants.MAX_ROTATION_RATE_DEG_PER_SEC) {
        m_rejectedRotation++;
        SmartDashboard.putString("Vision/LastReject",
            "ROTATING_TOO_FAST (" + String.format("%.0f", gyroRateDegPerSec) + "°/s)");
        return false;
    }

    // All gates passed
    m_accepted++;
    SmartDashboard.putString("Vision/LastReject", "ACCEPTED");
    return true;
}

// ── Updated periodic() calling the filter ─────────────────────────────────
@Override
public void periodic() {
    Optional<EstimatedRobotPose> estimateOpt = m_poseEstimator.update();

    estimateOpt.ifPresent(estimate -> {
        int    numTags = estimate.targetsUsed.size();
        double avgDist = estimate.targetsUsed.stream()
            .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm())
            .average().orElse(0);
        double gyroRate = m_drive.getGyroRateDegPerSec();

        if (shouldAcceptMeasurement(estimate, avgDist, gyroRate)) {
            Matrix<N3, N1> stdDevs = computeVisionStdDevs(avgDist, numTags);
            m_drive.addVisionMeasurement(
                estimate.estimatedPose.toPose2d(),
                estimate.timestampSeconds,
                stdDevs);
        }
    });

    // Log rejection statistics every 50 loops (~1 second)
    if ((Timer.getFPGATimestamp() * 50) % 50 < 1) {
        SmartDashboard.putNumber("Vision/Reject/TagCount",  m_rejectedTagCount);
        SmartDashboard.putNumber("Vision/Reject/Ambiguity", m_rejectedAmbiguity);
        SmartDashboard.putNumber("Vision/Reject/Distance",  m_rejectedDistance);
        SmartDashboard.putNumber("Vision/Reject/Boundary",  m_rejectedBoundary);
        SmartDashboard.putNumber("Vision/Reject/Rotation",  m_rejectedRotation);
        SmartDashboard.putNumber("Vision/Accepted",          m_accepted);
    }
}

Adding Filter Constants to VisionConstants

// ── Measurement rejection thresholds ─────────────────────────────────────

// Gate 2: Minimum pose ambiguity score to accept a single-tag estimate.
// Ambiguity ranges 0.0–1.0; higher = more confident single solution.
// Below this threshold, the two solvePnP solutions are too similar to
// distinguish reliably. Start at 0.2; raise if phantom poses occur.
public static final double MIN_POSE_AMBIGUITY = 0.2;

// Gate 3: Maximum average tag distance to accept a measurement.
// Beyond this, position error exceeds useful correction threshold.
// Lower this at Week 1 events; raise once tuning is stable.
public static final double MAX_VISION_DISTANCE_METERS = 5.0;

// Gate 5: Maximum gyro rotation rate to accept a measurement.
// Above this angular velocity, frame exposure blurs tag corners
// enough to corrupt solvePnP. 540°/s = 1.5 full rotations/second,
// which is extreme for most FRC maneuvers.
public static final double MAX_ROTATION_RATE_DEG_PER_SEC = 540.0;

Reading Rejection Logs to Debug Vision Problems

The rejection counters tell you exactly where your vision pipeline is losing measurements. Here's how to interpret each counter during a post-match review:

• High Reject/Ambiguity: Many single-tag frames where the algorithm can't distinguish the two solvePnP solutions. Most commonly caused by viewing tags at long range or steep angles. Consider adding a second camera or moving the existing camera higher to see tags more directly.
• High Reject/Distance: Robot is frequently too far from tags for useful measurements. Either the field layout doesn't provide close-range tags on your scoring path, or your autonomous spending too much time far from tags. Consider tightening the autonomous path to bring the robot closer to scoring-area tags.
• Any Reject/Boundary: A measurement produced a pose outside the field. This is a red flag — it means something upstream is seriously wrong (false positive detections, wrong calibration, incorrect robot-to-camera transform, or wrong field layout). Investigate and fix the root cause rather than treating the boundary check as a long-term solution.
• High Reject/Rotation: Robot is spinning faster than the gate allows during moments when tags are visible. This is expected if your autonomous includes spin-in-place maneuvers near scoring structures. If rotation rejection is preventing useful measurements during a specific auto phase, consider pausing vision fusion during that phase.
• Low Accepted relative to total loops: Vision rarely produces accepted measurements. Check that the coprocessor is running (detection counter increments), that the camera faces tags during the scenario you're testing, and that none of the thresholds are set too aggressively.

When to Tighten or Relax Each Gate

The default thresholds from this lesson are conservative starting points. Competition conditions vary, and the right settings depend on your robot's specific capabilities and your game's field layout.

• Tighten ambiguity threshold (raise MIN_POSE_AMBIGUITY toward 0.5): if phantom poses are occurring at moderate distance. This accepts fewer single-tag measurements but makes each one more reliable.
• Relax distance cutoff (raise MAX_VISION_DISTANCE_METERS toward 7–8 m): only if you have a high-resolution camera (1920×1080 or better) with a long focal length that can reliably detect corners at that distance. Verify by measuring actual distance error at the extended range.
• Tighten distance cutoff (lower toward 3–4 m): if you're consistently getting vision-induced noise in the 4–5 m range during a specific autonomous phase. Sometimes accepting only close-range measurements produces better overall accuracy than accepting all measurements.
• Relax rotation rate gate (raise toward 720°/s or disable): if your autonomous includes fast spin sequences near scoring structures and you want vision to continue correcting during those spins. Only relax this after verifying that measurements during spin are actually accurate for your specific camera setup.
• Add a multi-tag requirement (set min tags to 2): for the approach phase of a precision scoring autonomous when you know multiple tags will be visible. This provides cleaner corrections at the cost of no vision feedback during single-tag portions of the path.

🔌 System Check

Knowledge Check