01 | VALIDATE THE ESTIMATE AGAINST REALITY

Odometry can produce smooth, believable coordinates while drifting away from the robot’s actual location. Validation compares the estimated pose with an independently measured physical pose.

Measure the same reference point used by the estimator. A corner of the chassis, sensor bracket or arm tip is not interchangeable with that point.

Record the estimate before applying a correction. Resetting to a landmark first and then checking error erases the evidence you wanted to measure.

02 | DISTINGUISH THREE DIFFERENT ERRORS

A robot can miss its target while odometry accurately reports the miss. Conversely, a controller can report arrival because a drifting estimate reached the target even though the robot did not.

Keep all three values where possible: target, estimate and actual measured pose.

03 | WHAT RESETTING POSE MEANS

A pose reset assigns a known X, Y and heading to the selected reference, then captures the current sensor readings as the new baselines.

It does not physically move the robot. It does not make an unknown position known. It also does not require zeroing every sensor.

Use a measured start mark, a deliberately verified alignment or an independent validated position observation. A guess entered into the reset function remains a guess.

04 | RESET STATE AS ONE OPERATION

1. Stop pose-dependent movement and use the estimator’s single owner.
2. Place the reference at a verified pose and keep the robot stationary.
3. Mark the old published estimate invalid.
4. Read a fresh complete sensor snapshot.
5. Set the new field pose and replace every sensor baseline.
6. Publish the new pose, update time and valid status together.

If only X/Y change while old baselines remain, the next update can replay movement from before the reset. If only sensors are zeroed, their next subtraction can generate an artificial jump.

In a concurrent project, serialize reset and update work under one owner or suitable synchronization. Updating several variables from unrelated threads is not an atomic reset.

05 | CALIBRATION, RESET AND CORRECTION ARE DIFFERENT

Changing heading or rotation values on the inertial sensor does not physically turn the robot. A software baseline can map its current accumulated reading to the known field heading without changing that reading.

A mid-route correction needs a design for movement since the observation, measurement uncertainty and controller effects. This lesson uses stationary known-pose resets rather than an automatic fusion algorithm.

06 | CALCULATE POSITION AND HEADING ERROR

Use estimate minus measured pose for signed X/Y error. The position error magnitude is the straight-line distance between them.

def pose_errors(estimated, measured):
    # Both tuples: (x_mm, y_mm, clockwise_heading_deg).
    if not all(math.isfinite(v) for v in estimated + measured):
        raise ValueError("Invalid comparison")
    ex = estimated[0] - measured[0]
    ey = estimated[1] - measured[1]
    position_error = math.sqrt(ex * ex + ey * ey)
    heading_error = (estimated[2] - measured[2] + 180) % 360 - 180
    return ex, ey, position_error, heading_error

Measured pose (500, 500, 90°) and estimate (530, 460, 93°) give X error +30 mm, Y error -40 mm, position error 50 mm and heading error +3°.

Measured heading 358° and estimate 2° give +4°, not -356°. The shortest signed heading comparison handles wrap.

Use tolerances chosen for the task, such as scoring clearance. The code’s 20 mm and 2° acceptance values are examples, not universal odometry standards.

07 | MEASURE DRIFT WITH CONTROLLED TESTS

Returning near the start does not prove the estimate stayed accurate throughout. Errors can cancel. Include intermediate landmarks.

For a stationary test, note duration and load. For moving tests, note route length, speed, turn count and surface conditions.

08 | COMPARE REPEATED TRIALS

Run at least three trials from the same measured start, then compare signed errors as well as their magnitudes.

A consistent bias suggests a systematic issue. Wide scatter suggests variable contact, slip, flex or inconsistent measurement. Both can occur together.

A mean signed X error can be near zero because positive and negative errors cancel. Report mean error magnitude and worst-case error too.

Optionally express position error as a percentage of independently measured travelled distance. State the route and conditions; do not divide by final start-to-end displacement for a closed route.

09 | COMPLETE RESET AND CHECKPOINT DIAGNOSTIC

This sensor-only project supports the TWO and THREE layouts from 06.5. It commands no motors. Retain generated VEX imports and Brain setup above # Begin project code, and avoid duplicate device declarations.

Configure scales, offsets, RESET_POSE and CHECKPOINT_POSE from physical measurements. Start stationary at RESET_POSE. A confirms a reset after returning to that mark; B invalidates; X prints a snapshot; Y logs a checkpoint comparison.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | RESET AND VALIDATION DIAGNOSTIC
# =============================================================================
# Keep generated VEX imports and brain above '# Begin project code'.
# No motors. Avoid duplicate device declarations in Devices.
# TWO: left/forward tracker port 17, sideways tracker port 19, inertial 20.
# THREE: left tracker 17, right tracker 18, sideways tracker 19.
# THREE can optionally compare its heading change against inertial 20.
# Positive tracker readings: forward for parallel wheels, right for side wheel.
import math

LAYOUT = "TWO"                  # "TWO" or "THREE"
CONFIGURED = False
COMPARE_INERTIAL_IN_THREE = False
controller = Controller(PRIMARY)
# A: reset after returning to RESET_POSE; B: invalidate.
# X: print current snapshot; Y: log error at configured CHECKPOINT_POSE.
RESET_POSE = (0.0, 0.0, 0.0)
CHECKPOINT_POSE = (500.0, 500.0, 90.0) # Replace with physical measurement
POSITION_ACCEPT_MM = 20.0             # Example task criterion
HEADING_ACCEPT_DEG = 2.0
LEFT_MM_PER_SENSOR_TURN = 219.44  # Calibrate each combined scale independently
RIGHT_MM_PER_SENSOR_TURN = 219.44
SIDE_MM_PER_SENSOR_TURN = 219.44
LEFT_X_MM = -120.0
RIGHT_X_MM = 120.0
SIDE_Y_MM = -100.0
POLL_MS = 20
PRINT_MS = 200
START_X_MM = 0.0
START_Y_MM = 0.0
START_HEADING_DEG = 0.0
MAX_GAP_MS = 100

left_tracker = Rotation(Ports.PORT17, False)
side_tracker = Rotation(Ports.PORT19, False)
if LAYOUT == "THREE":
    right_tracker = Rotation(Ports.PORT18, False)
use_inertial = LAYOUT == "TWO" or COMPARE_INERTIAL_IN_THREE
if use_inertial:
    inertial = Inertial(Ports.PORT20)

def two_wheel_delta(forward_mm, side_mm, theta_rad, forward_x, side_y):
    values = (forward_mm, side_mm, theta_rad, forward_x, side_y)
    if not all(math.isfinite(v) for v in values):
        raise ValueError("Invalid two-wheel data")
    forward = forward_mm + forward_x * theta_rad
    sideways = side_mm - side_y * theta_rad
    return forward, sideways, theta_rad

def three_wheel_delta(left_mm, right_mm, side_mm, left_x, right_x, side_y):
    values = (left_mm, right_mm, side_mm, left_x, right_x, side_y)
    if not all(math.isfinite(v) for v in values):
        raise ValueError("Invalid three-wheel data")
    separation = right_x - left_x
    if separation <= 0:
        raise ValueError("Right tracking line must be right of left tracking line")
    theta = (left_mm - right_mm) / separation
    # Weighted geometry works even when the reference is not midway.
    forward = (right_x * left_mm - left_x * right_mm) / separation
    sideways = side_mm - side_y * theta
    return forward, sideways, theta


# =============================================================================
class FieldPose:
    def __init__(self, x_mm, y_mm, heading_deg):
        if not all(math.isfinite(v) for v in (x_mm, y_mm, heading_deg)):
            raise ValueError("Invalid starting pose")
        self.x = x_mm
        self.y = y_mm
        self.theta = math.radians(heading_deg)  # Keep unwrapped internally.

    def integrate(self, forward_arc, sideways_arc, dtheta):
        if not all(math.isfinite(v) for v in (forward_arc, sideways_arc, dtheta)):
            raise ValueError("Invalid local increment")
        half = dtheta / 2.0
        # Stable sin(half)/half limit near a straight move.
        factor = (1.0 - half * half / 6.0 if abs(half) < 1e-6
                  else math.sin(half) / half)
        forward_chord = forward_arc * factor
        side_chord = sideways_arc * factor
        midpoint = self.theta + half
        dx = side_chord * math.cos(midpoint) + forward_chord * math.sin(midpoint)
        dy = forward_chord * math.cos(midpoint) - side_chord * math.sin(midpoint)
        new_x, new_y = self.x + dx, self.y + dy
        new_theta = self.theta + dtheta
        if not all(math.isfinite(v) for v in (new_x, new_y, new_theta)):
            raise ValueError("Invalid resulting pose")
        self.x, self.y, self.theta = new_x, new_y, new_theta
        return dx, dy

    def pose(self):
        return self.x, self.y, math.degrees(self.theta) % 360.0


# SENSOR SNAPSHOT AND FIELD UPDATE
# =============================================================================
def snapshot():
    left = left_tracker.position(TURNS)
    side = side_tracker.position(TURNS)
    right = right_tracker.position(TURNS) if LAYOUT == "THREE" else 0.0
    if use_inertial:
        if not inertial.installed() or inertial.is_calibrating():
            raise ValueError("Inertial unavailable")
        rotation = inertial.rotation(DEGREES)
    else:
        rotation = 0.0
    if not all(math.isfinite(v) for v in (left, right, side, rotation)):
        raise ValueError("Invalid sensor reading")
    return left, right, side, rotation


def pose_errors(estimated, measured):
    # Both tuples: (x_mm, y_mm, clockwise_heading_deg).
    if not all(math.isfinite(v) for v in estimated + measured):
        raise ValueError("Invalid comparison")
    ex = estimated[0] - measured[0]
    ey = estimated[1] - measured[1]
    position_error = math.sqrt(ex * ex + ey * ey)
    heading_error = (estimated[2] - measured[2] + 180) % 360 - 180
    return ex, ey, position_error, heading_error

class PoseTracker:
    def __init__(self):
        self.pose = None
        self.previous = None
        self.published = (0.0, 0.0, 0.0, 0, False)

    def invalidate(self):
        self.published = (*self.published[:4], False)

    def reset_pose(self, known_pose, sensors, now_ms, stationary_confirmed):
        # One estimator owner only. The caller confirms physical placement.
        # No sensor zeroing and no automatic recalibration here.
        self.invalidate()
        if not stationary_confirmed:
            raise ValueError("Reset requires confirmed stationary placement")
        if not all(math.isfinite(v) for v in sensors):
            raise ValueError("Invalid reset snapshot")
        candidate = FieldPose(*known_pose)
        self.pose = candidate
        self.previous = tuple(sensors)
        self.published = (*candidate.pose(), now_ms, True)

    def update(self, sensors, now_ms):
        if not self.published[4]:
            return False  # Stay invalid until an explicit known-pose reset.
        if now_ms - self.published[3] > MAX_GAP_MS:
            self.invalidate()
            return False
        try:
            if not all(math.isfinite(v) for v in sensors):
                raise ValueError("Invalid snapshot")
            dl = (sensors[0] - self.previous[0]) * LEFT_MM_PER_SENSOR_TURN
            dr = (sensors[1] - self.previous[1]) * RIGHT_MM_PER_SENSOR_TURN
            ds = (sensors[2] - self.previous[2]) * SIDE_MM_PER_SENSOR_TURN
            if LAYOUT == "TWO":
                theta = math.radians(sensors[3] - self.previous[3])
                forward, side, theta = two_wheel_delta(
                    dl, ds, theta, LEFT_X_MM, SIDE_Y_MM)
            else:
                forward, side, theta = three_wheel_delta(
                    dl, dr, ds, LEFT_X_MM, RIGHT_X_MM, SIDE_Y_MM)
            self.pose.integrate(forward, side, theta)
            self.previous = tuple(sensors)
            self.published = (*self.pose.pose(), now_ms, True)
            return True
        except Exception:
            self.invalidate()
            return False


# =============================================================================
# DIAGNOSTIC OPERATOR LOOP: NO MOTORS, NO AUTOMATIC FAULT RESET
# =============================================================================
if LAYOUT not in ("TWO", "THREE"):
    raise ValueError("Choose TWO or THREE")
if not CONFIGURED:
    print("Configure geometry and measured reset/checkpoint poses first.")
else:
    scales = (LEFT_MM_PER_SENSOR_TURN, RIGHT_MM_PER_SENSOR_TURN,
              SIDE_MM_PER_SENSOR_TURN)
    if not all(math.isfinite(v) and v > 0 for v in scales):
        raise ValueError("Invalid scales")
    if LAYOUT == "THREE" and RIGHT_X_MM <= LEFT_X_MM:
        raise ValueError("Invalid separation")
    if use_inertial:
        # Place robot stationary at RESET_POSE before starting the project.
        # Calibration may block; do not move during it.
        inertial.calibrate()
    tracker = PoseTracker()
    tracker.reset_pose(RESET_POSE, snapshot(), brain.timer.time(MSEC), True)
    previous_a = controller.buttonA.pressing()
    previous_b = controller.buttonB.pressing()
    previous_x = controller.buttonX.pressing()
    previous_y = controller.buttonY.pressing()
    print("A known-pose reset; B invalidate; X snapshot; Y checkpoint log")
    while True:
        now = brain.timer.time(MSEC)
        a = controller.buttonA.pressing()
        b = controller.buttonB.pressing()
        x = controller.buttonX.pressing()
        y = controller.buttonY.pressing()
        try:
            if b and not previous_b:
                tracker.invalidate()
                print("Pose explicitly invalidated.")
            elif a and not previous_a:
                # A is the operator's confirmation, not a stationary detector.
                # First return physically to RESET_POSE and stop movement.
                tracker.reset_pose(RESET_POSE, snapshot(), now, True)
                print("Pose and baselines reset together:", tracker.published)
            else:
                tracker.update(snapshot(), now)
            if x and not previous_x:
                print("x,y,h,last_update_ms,valid:", tracker.published)
            if y and not previous_y:
                published = tracker.published
                if not published[4] or now - published[3] > MAX_GAP_MS:
                    print("Checkpoint rejected: invalid/stale pose")
                else:
                    # Stop physically at the measured checkpoint before Y.
                    errors = pose_errors(published[:3], CHECKPOINT_POSE)
                    passed = (errors[2] <= POSITION_ACCEPT_MM
                              and abs(errors[3]) <= HEADING_ACCEPT_DEG)
                    print("estimate", published[:3], "measured", CHECKPOINT_POSE,
                          "ex,ey,pos_err,heading_err", errors, "pass", passed)
        except Exception:
            tracker.invalidate()
            print("Read/reset fault: pose invalid; investigate before resetting.")
        previous_a, previous_b, previous_x, previous_y = a, b, x, y
        wait(POLL_MS, MSEC)

A and Y are operator confirmations. The code does not detect physical placement or stationary state. Only press them after stopping at the configured measured pose.

A timing or read fault keeps the estimate invalid until a deliberate reset. Old coordinates remain available for diagnosis but are not presented as live-valid data.

The checkpoint is a configured fixed measurement, not a target inferred from motor commands. For another landmark, enter its independently measured coordinates in a new test configuration.

10 | CHECK RESET BEHAVIOUR IN SIMULATION

Start with non-zero sensor readings, move, then reset to a different known pose using the current readings. Feed the same snapshot again: the pose should not jump.

Next feed a small forward change: only that new movement should affect the new pose. Repeat after crossing the displayed heading boundary.

Test invalid reset readings and an unconfirmed stationary reset. The published state should remain invalid, rather than claiming a successful correction.

In a hardware project, coordinate reinitialization with all controllers that use pose. A controller must not keep acting on a stale target error while its reference changes.

11 | DIAGNOSE PATTERNS BEFORE RETUNING

Test with the normal robot load and mechanism positions. Revalidate after changing mounts, wheel tyres, gearing or chassis geometry.

Separate software model checks from physical accuracy evidence. A quarter-circle calculation passing in Python cannot establish that a real wheel rolled without slip.

12 | SET A VALIDATION STANDARD FOR YOUR TASK

Write a measurable requirement before testing: maximum position error, maximum heading error, route conditions, repeated-trial success rate and fault behaviour.

For example, a controlled alignment task might require all three test runs to finish within a measured clearance margin. The chosen limits should come from the mechanism and route, not from whichever results the estimator happens to produce.

Keep configuration, logs and physical measurements with the saved code. An estimate becomes useful when its accuracy and failure conditions are known.

CHALLENGES | RESET, MEASURE AND VALIDATE

1. Compare: distinguish estimate error from target-arrival error using one measured endpoint.
2. Calculate: find position magnitude and wrapped heading error for a checkpoint.
3. Reset: simulate non-zero baselines and verify no replayed movement.
4. Fault: confirm an invalid reset leaves the pose invalid.
5. Measure: record straight, turn and closed-route trials at intermediate landmarks.
6. Repeat: report mean magnitude, worst-case error and signed bias.
7. Diagnose: propose one evidence-based configuration change.
8. Specify: write an acceptance standard for a real autonomous task.

OFFICIAL REFERENCES

• VEX inertial calibration and numerical heading/rotation changes
• VEX tracking sensor position and resets

The reset manager, validation method and worked examples are independently written learning material. VEX and VEXcode are trademarks of their respective owners.