01 | WHY COMBINE SENSORS?

Wheel travel estimates how far the robot has moved. Inertial rotation helps orient that travel in the field. A range to a known wall can provide a position constraint that does not depend on accumulated wheel distance.

Sensor fusion combines complementary evidence through a model. It is not simply averaging every number available.

Start with the odometry ideas from section 06. This guide adds a carefully gated wall correction to that prediction, using a sensor-only practice monitor rather than a new autonomous driving controller.

02 | WHAT EACH SENSOR CONTRIBUTES

Keep raw measurements separate from inferred quantities. A shaft rotation becomes millimetres only after a calibrated conversion. A range becomes a field coordinate only after you identify the wall and account for sensor pose.

03 | KEEP ONE COORDINATE CONVENTION

Use the same convention as the earlier course guides: field +x to the right, +y forward at heading 0°, and headings increasing clockwise. Robot-local sideways is right-positive and forward is forward-positive.

Calibrate signs, scales and offsets before combining measurements. The initial heading must match the physical start orientation. The code uses changes in continuous inertial rotation, anchored to the declared start heading.

04 | PREDICT WITH WHEELS AND INERTIAL ROTATION

For each short update, read forward-wheel travel, sideways-wheel travel and inertial angular change. Correct wheel travel caused solely by rotation around the reference point.

dtheta = radians(current_rotation - previous_rotation)
forward_arc = delta_forward_wheel + forward_wheel_x * dtheta
side_arc = delta_side_wheel - side_wheel_y * dtheta

A forward tracking wheel on the left can roll during a clockwise turn even when the reference point stays still. Removing the offset term prevents that rolling from looking like forward translation.

The starter converts these local arc increments to chords and rotates them using midpoint heading, following 06.5. It keeps an unwrapped internal angle and displays heading modulo 360°.

This is a motion estimate. It still depends on wheel contact, calibrated geometry and an accurate angular increment.

05 | FIRST USE A STRAIGHT WALL EXAMPLE

At heading 0°, the forward sensor looks along +y. If the known wall is at y = 1500 mm, the sensor face is 100 mm ahead of the reference point and the range is 850 mm:

measured_robot_y = 1500 - 850 - 100  # 550 mm

If odometry predicts y = 500 mm, the wall observation differs by +50 mm. That disagreement is the innovation.

06 | INCLUDE HEADING AND SENSOR OFFSET

A slightly angled sensor measures along a ray, not the perpendicular distance to the wall. For a forward-facing sensor, use its heading and both mounting offsets.

# theta is clockwise-positive radians; wall is y = WALL_Y.
measured_y = (WALL_Y
              - (range_mm + sensor_forward_mm) * cos(theta)
              + sensor_right_mm * sin(theta))
hit_x = (estimated_x + sensor_right_mm * cos(theta)
         + (sensor_forward_mm + range_mm) * sin(theta))

The second expression predicts where the ray meets the wall along x. Use it to reject observations outside a verified wall segment or near a corner.

The starter accepts headings only within 8° of +y and applies corrections while measured motion is low. This narrow gate limits geometry and timing sensitivity. An object inside the ray can still impersonate the wall.

07 | CHECK THE INNOVATION BEFORE CORRECTING

Innovation is observed position minus predicted position:

innovation_mm = wall_measured_y - predicted_y

A large discrepancy can come from a wrong wall, an obstruction, wrong offsets, a mistaken starting pose or odometry error. Log and investigate it instead of assuming the range sensor is correct.

A wrong observation within the gate can still be accepted. Verified surroundings and physical testing remain necessary.

08 | APPLY A BOUNDED CORRECTION

For accepted evidence, move the predicted y partway toward the wall observation:

correction = clamp(0.25 * innovation, -20, 20)
estimated_y += correction

For a +50 mm innovation, the correction is +12.5 mm: y changes from 500 to 512.5 mm. The starter changes y only. It keeps x, heading and the previous wheel readings intact.

The gain is an illustrative heuristic. It is not a Kalman gain and the code does not maintain a covariance or statistical confidence interval.

Repeatedly blending the same stationary reading would keep pulling the estimate toward it. The example therefore permits only one three-reading correction evaluation per stationary episode. Meaningful measured motion rearms the next episode.

09 | GATE WHEN AND WHERE A WALL READING IS USED

Three sequential readings are a consistency check, not three independent confirmations of truth. They may share systematic error or even the same underlying device sample.

WALL_VERIFIED is a configuration assertion for this controlled lane, not an automatic wall-recognition result. A competition implementation needs a route or region policy that enables wall correction only where the ray and environment are known.

10 | COMPLETE SENSOR-ONLY PRACTICE MONITOR

Use a separate teaching project with no other motion code. Paste below # Begin project code, preserving generated VEX imports and Brain setup. Do not duplicate manual devices in Devices.

Forward tracking wheel: Rotation Sensor 17. Sideways tracking wheel: Rotation Sensor 19. Inertial: 20. Forward Distance Sensor: 16. Both configuration flags start False.

Place the reference point at the declared start pose and keep the robot stationary during disabled calibration. This monitor contains no motor commands and runs only while competition control reports disabled. Any manual movement should be slow and supervised, with mechanisms supported and no active drive program.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | PREDICT AND GATE A WALL CORRECTION
# =============================================================================
# Keep generated VEX imports and Brain setup above '# Begin project code'.
# This is a disabled-mode sensor monitor. It contains NO motor commands.
# Remove other motion code from this teaching project before testing.
import math
forward_tracker = Rotation(Ports.PORT17, False)
side_tracker = Rotation(Ports.PORT19, False)
inertial = Inertial(Ports.PORT20)
front_distance = Distance(Ports.PORT16)
controller = Controller(PRIMARY)

SENSORS_CONFIGURED = False  # Verify signs, scales, offsets and starting pose.
WALL_VERIFIED = False       # Confirm this ray sees the intended fixed practice wall.
FORWARD_MM_PER_TURN = 219.44
SIDE_MM_PER_TURN = 219.44
FORWARD_X_MM = -120.0      # Forward wheel: right-positive lateral offset.
SIDE_Y_MM = -100.0         # Side wheel: forward-positive longitudinal offset.
START_POSE = (500.0, 500.0, 0.0)  # Local practice coordinates; mm, mm, degrees.
WALL_Y_MM = 1500.0         # Known wall perpendicular to +y.
WALL_X_MIN_MM = 0.0
WALL_X_MAX_MM = 2000.0
WALL_EDGE_MARGIN_MM = 100.0
SENSOR_RIGHT_MM = 60.0
SENSOR_FORWARD_MM = 100.0  # Sensor face, not bumper.
WALL_HEADING_TOL_DEG = 8.0
MIN_WALL_RANGE_MM = 200.0
MAX_WALL_RANGE_MM = 1500.0
MAX_INNOVATION_MM = 100.0
MAX_BATCH_SPREAD_MM = 30.0
CORRECTION_GAIN = 0.25
MAX_CORRECTION_MM = 20.0
STILL_LINEAR_MM_S = 30.0
STILL_ANGULAR_DEG_S = 8.0
STILL_MS = 200
MAX_TRACKER_RATE_MM_S = 1500.0
MAX_ROTATION_RATE_DEG_S = 360.0
POLL_MS = 20
MAX_GAP_MS = 100
MAX_READ_SPAN_MS = 20
MONITOR_MS = 20000

def clamp(v, lo, hi):
    return max(lo, min(hi, v))

def angle_error(target, current):
    return (target-current+180)%360-180

class PoseEstimate:
    def __init__(self, first):
        self.x, self.y, heading = START_POSE
        self.theta = math.radians(heading)  # Unwrapped clockwise-positive radians.
        self.previous = dict(first)
        self.still_since = None
        self.wall_used = False
        self.wall_batch = []
        self.wall_status = 'NOT_CHECKED'
        self.innovation = None
        self.correction = 0.0
    def predict(self, sample):
        old = self.previous
        dt = (sample['ms']-old['ms'])/1000.0
        if not 0 < dt <= MAX_GAP_MS/1000.0:
            raise ValueError('Prediction update gap')
        df = sample['forward']-old['forward']
        ds = sample['side']-old['side']
        dr = sample['rotation']-old['rotation']
        if (not all(math.isfinite(v) for v in (df, ds, dr))
                or max(abs(df),abs(ds))/dt > MAX_TRACKER_RATE_MM_S
                or abs(dr)/dt > MAX_ROTATION_RATE_DEG_S):
            raise ValueError('Invalid or implausible increment')
        dtheta = math.radians(dr)
        forward_arc = df + FORWARD_X_MM*dtheta
        side_arc = ds - SIDE_Y_MM*dtheta
        half = dtheta/2.0
        factor = 1-half*half/6.0 if abs(half)<1e-6 else math.sin(half)/half
        midpoint = self.theta+half
        forward_chord, side_chord = forward_arc*factor, side_arc*factor
        dx = side_chord*math.cos(midpoint)+forward_chord*math.sin(midpoint)
        dy = forward_chord*math.cos(midpoint)-side_chord*math.sin(midpoint)
        self.x += dx
        self.y += dy
        self.theta += dtheta
        if not all(math.isfinite(v) for v in (self.x,self.y,self.theta)):
            raise ValueError('Invalid prediction')
        self.previous = dict(sample)
        still = (math.hypot(forward_arc,side_arc)/dt <= STILL_LINEAR_MM_S
                 and abs(dr)/dt <= STILL_ANGULAR_DEG_S)
        if still:
            if self.still_since is None:
                self.still_since = sample['ms']
        else:
            self.still_since = None
            self.wall_used = False  # A new stationary episode may use one correction.
            self.wall_batch = []
        return still
    def reject_wall(self, reason):
        self.wall_batch = []
        self.wall_status = reason
        return False
    def correct_wall(self, now, raw_range):
        self.innovation = None
        self.correction = 0.0
        if not WALL_VERIFIED:
            return self.reject_wall('WALL_NOT_VERIFIED')
        if self.still_since is None or now-self.still_since < STILL_MS:
            return self.reject_wall('WAIT_STATIONARY')
        if self.wall_used:
            self.wall_status = 'ALREADY_USED_THIS_STOP'
            return False
        heading = math.degrees(self.theta)
        if abs(angle_error(0,heading)) > WALL_HEADING_TOL_DEG:
            return self.reject_wall('UNSUITABLE_HEADING')
        if (raw_range is None or not math.isfinite(raw_range)
                or not MIN_WALL_RANGE_MM <= raw_range <= MAX_WALL_RANGE_MM):
            return self.reject_wall('RANGE_UNAVAILABLE_OR_OUTSIDE_GATE')
        c,s = math.cos(self.theta),math.sin(self.theta)
        hit_x = self.x+SENSOR_RIGHT_MM*c+(SENSOR_FORWARD_MM+raw_range)*s
        if not WALL_X_MIN_MM+WALL_EDGE_MARGIN_MM <= hit_x <= WALL_X_MAX_MM-WALL_EDGE_MARGIN_MM:
            return self.reject_wall('OUTSIDE_VERIFIED_WALL_SEGMENT')
        measured_y = WALL_Y_MM-(raw_range+SENSOR_FORWARD_MM)*c+SENSOR_RIGHT_MM*s
        innovation = measured_y-self.y
        self.innovation = innovation
        if abs(innovation)>MAX_INNOVATION_MM:
            return self.reject_wall('INNOVATION_TOO_LARGE')
        self.wall_batch.append(measured_y)
        if len(self.wall_batch)<3:
            self.wall_status = 'COLLECTING_THREE'
            return False
        batch = sorted(self.wall_batch)
        self.wall_used = True  # One evaluation per stationary episode, even if noisy.
        self.wall_batch = []
        if batch[-1]-batch[0]>MAX_BATCH_SPREAD_MM:
            self.wall_status = 'BATCH_TOO_NOISY'
            return False
        self.innovation = batch[1]-self.y
        self.correction = clamp(CORRECTION_GAIN*self.innovation,
                                -MAX_CORRECTION_MM,MAX_CORRECTION_MM)
        self.y += self.correction  # Do not overwrite x, heading or wheel baselines.
        self.wall_status = 'CORRECTED_Y'
        return True

def read_sample():
    before = brain.timer.time(MSEC)
    if not inertial.installed() or inertial.is_calibrating():
        raise ValueError('Inertial unavailable')
    forward = forward_tracker.position(TURNS)*FORWARD_MM_PER_TURN
    side = side_tracker.position(TURNS)*SIDE_MM_PER_TURN
    rotation = inertial.rotation(DEGREES)
    if not all(math.isfinite(v) for v in (forward,side,rotation)):
        raise ValueError('Core sensor reading invalid')
    raw = (front_distance.object_distance(MM)
           if front_distance.is_object_detected() else None)
    after = brain.timer.time(MSEC)
    if after-before > MAX_READ_SPAN_MS:
        raise ValueError('Acquisition span too large')
    return {'forward':forward,'side':side,'rotation':rotation,'range':raw,'ms':after}

def monitor():
    settings = (FORWARD_MM_PER_TURN,SIDE_MM_PER_TURN,FORWARD_X_MM,SIDE_Y_MM,
                *START_POSE,WALL_Y_MM,WALL_X_MIN_MM,WALL_X_MAX_MM,
                WALL_EDGE_MARGIN_MM,SENSOR_RIGHT_MM,SENSOR_FORWARD_MM,
                WALL_HEADING_TOL_DEG,MIN_WALL_RANGE_MM,MAX_WALL_RANGE_MM,
                MAX_INNOVATION_MM,MAX_BATCH_SPREAD_MM,CORRECTION_GAIN,
                MAX_CORRECTION_MM,STILL_LINEAR_MM_S,STILL_ANGULAR_DEG_S,
                STILL_MS,MAX_TRACKER_RATE_MM_S,MAX_ROTATION_RATE_DEG_S,
                POLL_MS,MAX_GAP_MS,MAX_READ_SPAN_MS,MONITOR_MS)
    if (not all(math.isfinite(v) for v in settings)
            or min(FORWARD_MM_PER_TURN,SIDE_MM_PER_TURN,MAX_INNOVATION_MM,
                   MAX_BATCH_SPREAD_MM,MAX_CORRECTION_MM,STILL_MS,
                   MAX_TRACKER_RATE_MM_S,MAX_ROTATION_RATE_DEG_S,MONITOR_MS) <= 0
            or not 0 < CORRECTION_GAIN <= 1
            or not 0 < WALL_HEADING_TOL_DEG < 90
            or WALL_EDGE_MARGIN_MM < 0
            or WALL_X_MIN_MM+2*WALL_EDGE_MARGIN_MM >= WALL_X_MAX_MM
            or not 20 <= MIN_WALL_RANGE_MM < MAX_WALL_RANGE_MM <= 2000
            or STILL_LINEAR_MM_S < 0 or STILL_ANGULAR_DEG_S < 0
            or not 0 < POLL_MS < MAX_GAP_MS
            or not 0 < MAX_READ_SPAN_MS <= MAX_GAP_MS):
        return 'INVALID_SETTINGS'
    if competition.is_enabled():
        return 'REQUIRES_DISABLED_MODE'
    started = brain.timer.time(MSEC)
    estimate = PoseEstimate(read_sample())
    log_at = started-200
    wait(POLL_MS,MSEC)  # Ensure a positive interval before the first prediction.
    while True:
        if competition.is_enabled() or controller.buttonB.pressing():
            return 'CANCELLED'
        if brain.timer.time(MSEC)-started >= MONITOR_MS:
            return 'MONITOR_FINISHED'
        sample = read_sample()
        if competition.is_enabled() or controller.buttonB.pressing():
            return 'CANCELLED'
        if sample['ms']-started >= MONITOR_MS:
            return 'MONITOR_FINISHED'
        estimate.predict(sample)
        corrected = estimate.correct_wall(sample['ms'],sample['range'])
        if corrected or sample['ms']-log_at>=200:
            print('pose estimate',round(estimate.x,1),round(estimate.y,1),
                  round(math.degrees(estimate.theta)%360,1),
                  'range',sample['range'],'wall',estimate.wall_status,
                  'innovation',estimate.innovation,'correction',estimate.correction)
            log_at = sample['ms']
        wait(POLL_MS,MSEC)

# No autonomous or driver outputs in this monitor project.
def autonomous():
    pass

def driver_control():
    pass

competition = Competition(driver_control,autonomous)
if SENSORS_CONFIGURED and not competition.is_enabled():
    try:
        if not inertial.installed():
            raise ValueError('Inertial missing')
        inertial.calibrate()  # Robot stationary during disabled setup.
        if inertial.is_calibrating() or competition.is_enabled():
            raise ValueError('Calibration or mode changed')
        print('monitor result',monitor())
    except Exception:
        print('CORE_SENSOR_OR_TIMING_FAULT: stop using this estimate')
else:
    print('Verify configuration and remain disabled before monitoring')
while True:
    wait(100,MSEC)

The monitor runs for twenty seconds and reports the pose estimate, range, wall gate result, innovation and applied correction. B or enabled mode ends it. Restart the project at a physically confirmed start pose for another run.

Missing or rejected range evidence leaves the wheel/inertial prediction available. Invalid core feedback, implausible increments or a timing fault ends the monitor. Do not continue using the last printed pose as a current value after exit.

11 | MEASURE TIMING AND KEEP ONE POSE OWNER

Wheel and inertial readings are acquired sequentially, so they are not perfectly simultaneous. The monitor rejects a read span above 20 ms and prediction intervals above 100 ms. These limits are illustrative and need measurement on your setup.

The local timestamp records acquisition completion; it is not a hardware sample timestamp. Low-motion wall gating reduces skew but cannot guarantee that all readings describe exactly the same instant.

Use one owner for prediction and correction. When adding this estimator to autonomous control, update it once per control tick and let the movement action consume the resulting estimate. Do not integrate the same wheel increment again in a second task.

Keep raw close-range stop checks separate from optional pose correction, as in 09.3. A range rejected for localization can still require stopping movement.

12 | KNOW WHAT REMAINS UNOBSERVED

One forward wall measurement constrains the coordinate perpendicular to that wall. It does not measure position along the wall, and this starter does not use it to correct heading.

A second observation of a known perpendicular wall can constrain the other coordinate. Multiple properly modelled range observations may support more elaborate estimation, but their geometry, offsets and identity must be known.

If the robot loses wheel contact or suffers a large pose error, a small correction with an innovation gate may be inadequate. Stop or enter an explicit relocalization procedure rather than forcing readings through the gate.

An extended Kalman filter is a possible later step once the motion model, observation model, uncertainty and tests are understood. Adding a complex filter does not fix an incorrect wall assumption.

13 | COMMON MISTAKES

• Averaging unlike quantities: millimetres and degrees need a shared model.
• Ignoring sensor-face offsets: range begins at the sensor.
• Using a motor encoder as perfect ground travel: account for slip.
• Accepting any detected object as a wall: verify the environment.
• Correcting x and y from one range: respect the observed constraint.
• Resetting wheel baselines after every correction: preserve increment continuity.
• Double-integrating from two tasks: keep one pose owner.
• Calling a heuristic gain statistical confidence: describe what is actually implemented.

CHALLENGES | PREDICT, COMPARE AND CORRECT

1. Predict: calculate a 200 mm forward increment at headings 0° and 90°.
2. Offset: show why pure rotation makes an offset tracking wheel roll.
3. Infer: calculate robot y from a known wall, range and sensor-face offset.
4. Blend: apply the bounded correction to innovations of +50, +90 and +150 mm.
5. Reject: test unavailable range, wrong heading, a corner hit and excessive innovation.
6. Preserve: verify that a y correction changes neither x nor wheel baselines.
7. Compare: record wheel-only prediction against prediction with accepted wall updates.
8. Evaluate: explain how a small obstruction could pass the gate and how your route would prevent it.

OFFICIAL REFERENCES

• Rotation Sensor position and direction API
• Inertial rotation and calibration API
• Distance Sensor detection and range API

The prediction model and wall-correction gates are independently written teaching examples requiring calibrated geometry and physical validation.