01 | A POSE DESCRIBES ONE POINT ON THE ROBOT

A field pose contains the selected reference point’s X, Y and robot heading. Tracking-wheel geometry from 06.4 provides corrected local movement and a turn increment for each short interval.

Integration means repeatedly adding those interval changes to the previous pose. It is not simply adding raw wheel travel to field Y.

Use the same reference point, distance units and angle convention in the starting pose, sensor offsets and route targets.

02 | KEEP THE CONVENTION VISIBLE

At heading 90 degrees, body forward points field right and body right points field down. Both local components therefore need a heading transformation.

The declared field frame is your map. An inertial reading does not automatically know how that map is oriented.

03 | FOLLOW THE UPDATE PIPELINE

1. Read a consistent set of current sensor values.
2. Subtract previous readings to obtain signed changes.
3. Convert wheel changes to millimetres once.
4. Find angle change and remove wheel-offset rotation.
5. Convert corrected local arc components to midpoint chords.
6. Rotate those components into the field frame.
7. Add field changes and update accumulated heading.
8. Publish one pose snapshot with validity and update time.

The order matters. Correcting offsets after converting raw travel into field coordinates mixes frames and gives the wrong geometry.

04 | ROTATE LOCAL MOVEMENT INTO THE FIELD

For a straight interval at heading h, local forward F and sideways S become:

field_dx = S * math.cos(h) + F * math.sin(h)
field_dy = F * math.cos(h) - S * math.sin(h)

Negative F represents reverse movement. Negative S represents movement to the robot’s left. The same equations handle both signs.

05 | AN INTERVAL WITH TURNING NEEDS A MIDPOINT

During a turn, the robot does not face one direction for the entire interval. Start-only projection points all travel in the old direction; end-only projection points it all in the new direction.

For an approximately constant-curvature interval, use midpoint heading = previous heading + half the angle change.

Corrected local rolling components describe arc travel. Multiply each by sin(θ/2)/(θ/2) to obtain the corresponding midpoint chord component, then apply the field transformation.

For small θ, the factor approaches 1. Frequent updates keep the model useful when curvature changes within a route.

06 | WORK THROUGH A QUARTER-CIRCLE

Start at (0, 0), facing field up. Travel clockwise through 90 degrees on a 400 mm reference-path radius.

1. Forward arc = 400 × π/2 ≈ 628.3 mm.
2. Chord factor = sin(π/4)/(π/4) ≈ 0.9003.
3. Forward chord ≈ 565.7 mm.
4. Midpoint heading = 45 degrees.
5. ΔX ≈ 400 mm and ΔY ≈ 400 mm.
6. New heading = 90 degrees.

Projecting the full 628.3 mm at the ending heading would wrongly place the endpoint at (628.3, 0). Midpoint projection without the chord factor would also overestimate the endpoint.

07 | HANDLE STRAIGHT MOVES AND PURE TURNS

At zero angle change, sin(θ/2)/(θ/2) would appear to divide by zero. Use its limiting value of 1, or a small-angle approximation.

A pure turn about the reference has zero corrected forward and sideways components. X/Y remain unchanged while heading changes. Raw tracking-wheel readings can still be non-zero because of offsets.

A commanded pivot does not prove the chosen reference stayed still. Let measured, corrected translation contribute where the physical movement includes it.

08 | A REUSABLE FIELD-POSE CLASS

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

The class consumes corrected local arc components in millimetres and clockwise angle change in radians. It does not read sensors or command motors.

Heading remains accumulated internally. pose() wraps only the returned display heading into 0 to 360 degrees.

The arc update is exact for its constant body-motion/curvature model over the interval. Real movement, slip and changing curvature make sampling and physical validation essential.

09 | COMPLETE TWO / THREE-WHEEL SENSOR PROJECT

This project extends 06.4 with field integration. TWO uses forward tracker 17, sideways tracker 19 and inertial 20. THREE adds right tracker 18 and derives angle from the parallel wheel difference.

Paste below # Begin project code, retaining generated VEX imports and Brain setup above it. Avoid duplicate device declarations. Set measured scales, offsets and physical starting pose before enabling CONFIGURED.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | TWO / THREE-WHEEL FIELD POSE 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
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

if LAYOUT not in ("TWO", "THREE"):
    raise ValueError("Choose TWO or THREE")
if not CONFIGURED:
    print("Measure scales, offsets and signs before enabling diagnostic.")
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("Scales must be positive and finite")
    if LAYOUT == "THREE" and RIGHT_X_MM <= LEFT_X_MM:
        raise ValueError("Invalid tracking-line separation")
    if use_inertial:
        # Keep stationary; this setup call may block.
        inertial.calibrate()
    previous = snapshot()
    previous_ms = last_print = brain.timer.time(MSEC)
    pose = FieldPose(START_X_MM, START_Y_MM, START_HEADING_DEG)
    # Published tuple: x, y, heading, last update ms, validity.
    # One owner replaces the whole tuple after a successful update.
    published_pose = (*pose.pose(), previous_ms, True)
    print("Estimated field pose active.")
    while True:
        now = brain.timer.time(MSEC)
        if now - previous_ms > MAX_GAP_MS:
            published_pose = (*published_pose[:4], False)
            print("Timing fault: pose invalid; stop and reinitialize.")
            break
        previous_ms = now
        try:
            current = snapshot()
            dl = (current[0] - previous[0]) * LEFT_MM_PER_SENSOR_TURN
            dr = (current[1] - previous[1]) * RIGHT_MM_PER_SENSOR_TURN
            ds = (current[2] - previous[2]) * SIDE_MM_PER_SENSOR_TURN
            inertial_theta = math.radians(current[3] - previous[3])
            if LAYOUT == "TWO":
                forward, sideways, theta = two_wheel_delta(
                    dl, ds, inertial_theta, LEFT_X_MM, SIDE_Y_MM)
            else:
                forward, sideways, theta = three_wheel_delta(
                    dl, dr, ds, LEFT_X_MM, RIGHT_X_MM, SIDE_Y_MM)
            pose.integrate(forward, sideways, theta)
            previous = current
            published_pose = (*pose.pose(), now, True)
            if now - last_print >= PRINT_MS:
                x, y, h, timestamp, valid = published_pose
                print("Estimated X %.1f mm, Y %.1f mm, heading %.1f deg" % (x, y, h))
                last_print = now
        except Exception:
            published_pose = (*published_pose[:4], False)
            print("Feedback/geometry fault: pose invalid; stop.")
            break
        wait(POLL_MS, MSEC)

The inertial comparison setting in THREE only enables additional sensor reading/calibration in this version; wheel-derived θ remains the integration source. Use 06.4’s diagnostic to analyse agreement before choosing a fusion policy.

The example commands no motors. It publishes a tuple containing X, Y, heading, last successful update time and validity. On a fault it retains the last coordinates for diagnosis but marks them invalid.

Separate sensor reads are not atomic. The nominal 20 ms period and 100 ms gap policy are teaching settings, not accuracy guarantees. Calibration may block before the update loop begins.

10 | HEADING WRAP MUST NOT CREATE A FALSE TURN

The TWO layout uses accumulated inertial rotation differences. A turn crossing the displayed 359-to-0 boundary therefore remains a small continuous increment.

THREE derives each angle increment directly from wheel deltas and separation. It also avoids subtracting wrapped display headings.

If another implementation uses heading(), wrap its difference deliberately and accept the assumption that rotation between samples is less than 180 degrees. Display normalization alone is not a measurement algorithm.

Do not reset sensors or recalibrate while the previous baselines are still in use. Reinitialization needs a deliberate known-pose procedure.

11 | TEST GEOMETRY AND SAMPLING TOGETHER

Test both turn directions and normal load. Large gaps can hide a change in motion within an interval even if an arc formula is mathematically correct.

For a tank-drive robot, simulate lateral test cases or use an appropriate fixture rather than forcing its tyres sideways.

12 | USE VALIDITY BEFORE POSITION-BASED CONTROL

A movement routine should check that the pose is valid and sufficiently recent for its task before using it. Read a single published snapshot rather than separate mutable coordinates.

The timestamp records a completed software update, not a guarantee that all sensors acquired perfectly synchronized fresh measurements.

Keep one owner for pose updates and baseline changes. If another task resets the pose without rebasing sensor state, the next increment can create an artificial jump.

Odometry estimates relative movement and accumulates error. Calibration, contact, heading accuracy and independent landmark checks remain necessary. The next guide examines resets and drift validation.

CHALLENGES | BUILD AND CHECK A FIELD ESTIMATE

1. Transform: calculate field increments for positive and negative local movement at cardinal headings.
2. Trace: explain every step from sensor delta to published pose.
3. Calculate: reproduce the quarter-circle endpoint.
4. Simulate: compare one arc update with the same arc divided into 100 updates.
5. Turn: verify a pure rotation changes heading without false translation.
6. Reverse: test signed travel after a turn.
7. Fault: confirm invalid feedback prevents a valid-live pose.
8. Validate: record a closed-route endpoint and measured residual error.

OFFICIAL AND TECHNICAL REFERENCES

• Purdue SIGBots arc-based odometry theory
• VEX accumulated rotation API
• VEX tracking sensor position API

Check coordinate conventions when comparing references. The diagram, implementation and worked examples are independently written learning material. VEX and VEXcode are trademarks of their respective owners.