Proprioception-exteroception fusion

"My joints tell me where I think I am. The world tells me where I actually am. The trick is knowing which one to believe when they disagree, and by exactly how much."

A Self-Localizing AI Agent

This section builds directly on the Kalman family (Chapter 9) and on the probabilistic-fusion algebra of combining two estimates by their inverse covariances (Chapter 49). What is new is that the two streams are not symmetric peers, as in the generic fusion of Chapter 48. One is the motion model and one is the observation, and knowing which is which is the whole design.

Proprioception is the process model

In a Bayesian filter the process model propagates the state forward between measurements and inflates its covariance to reflect growing ignorance. On a robot, that process model is proprioception. A wheeled base integrates wheel encoders and yaw rate into a dead-reckoned pose; a legged robot uses leg odometry, which combines joint encoders (forward kinematics to each foot) with the IMU and a contact estimate: if a foot is planted and not slipping, the base must have moved opposite to the measured foot motion. This kinematic-inertial estimate runs at hundreds of hertz, matches the control loop, and needs no external infrastructure.

The catch is unbounded drift. Every prediction step adds process noise \(Q\), so the covariance grows without a correction:

Here \(\mathbf{u}_k\) is the proprioceptive input (joint velocities, IMU rates) and \(Q_k\) encodes how much you distrust it: foot slip, encoder quantization, and IMU bias all live in \(Q\). This is the same dead-reckoning growth analyzed for inertial navigation in Chapter 24; the legged twist is that contact events make \(Q\) time-varying. A confidently planted stance foot gives tiny process noise; the moment slip is detected, \(Q\) must jump so the filter stops trusting kinematics and leans on exteroception instead.

Exteroception is the correction

Exteroceptive measurements enter as the filter's update step: an absolute, drift-free constraint that pulls the drifting prediction back and shrinks its covariance. A lidar scan-match against a map, a visual-odometry pose, or a detected landmark (Section 57.3) each yields an observation \(\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k\) whose covariance \(R\) says how much to trust it. The Kalman gain then blends prediction and correction in inverse proportion to their uncertainties, the multivariate version of the precision-weighted average from Chapter 49.

Two engineering realities make this harder than the textbook update. First, the two streams live in different frames: joint encoders speak the body frame, a wrist camera speaks the sensor frame, and fusing them demands an accurate extrinsic calibration (the hand-eye transform). A one-degree error in that transform injects a systematic, drift-like bias that no amount of filtering removes, which is why calibration quality caps fusion accuracy. Second, exteroception arrives late: a lidar pose for time \(t\) may land 80 ms after the fact, so it must be applied as a delayed update against the state as it was at \(t\), not the state now. Ignoring that latency is a classic silent bug, and it is the subject of Section 57.6.

The snippet below makes the division of labor concrete: a 2D base estimator dead-reckons on proprioceptive velocity every step and folds in a sparse, noisy exteroceptive position fix when one is available. Watch the covariance trace grow during the blind stretch and collapse at each fix.

import numpy as np

def fuse_step(x, P, u, dt, Q, z=None, R=None, H=np.eye(2)):
    """One predict/update cycle for a 2D base pose.
    u  : proprioceptive velocity (m/s), the process input
    z  : exteroceptive position fix (m) or None if unavailable
    """
    # predict: proprioception drives the motion model
    x = x + u * dt                      # dead-reckon
    P = P + Q * dt                      # covariance grows, so drift

    # update: exteroception corrects, only when present
    if z is not None:
        S = H @ P @ H.T + R             # innovation covariance
        K = P @ H.T @ np.linalg.inv(S)  # Kalman gain
        x = x + K @ (z - H @ x)
        P = (np.eye(2) - K @ H) @ P
    return x, P

rng = np.random.default_rng(0)
x, P = np.zeros(2), np.eye(2) * 0.01
Q, R = np.eye(2) * 0.5, np.eye(2) * 0.04   # trust vision ~ heavily
true = np.zeros(2)
for k in range(30):
    u = np.array([1.0, 0.2])               # commanded velocity
    true = true + u * 0.1
    z = None
    if k % 10 == 9:                         # exteroceptive fix every 1 s
        z = true + rng.normal(0, 0.2, 2)
    x, P = fuse_step(x, P, u + rng.normal(0, 0.3, 2), 0.1, Q, z, R)
    print(f"k={k:2d} trace(P)={np.trace(P):.3f}  err={np.linalg.norm(x-true):.3f}")

The loop above is the skeleton of every classical proprioception-exteroception estimator: the two modalities never enter symmetrically, one is always the predict and the other the update.

Learned fusion and confidence gating

The classical filter gates exteroception with a hand-tuned \(R\) and slip detector. The frontier learns the gate. Instead of hard rules for when the elevation map is trustworthy, a network is trained to blend a proprioceptive state with a (possibly corrupted) exteroceptive one and to down-weight vision automatically when the two disagree in ways that predict a fall. This is the same missing-modality robustness studied for deep multimodal fusion in Chapter 15, specialized to locomotion.

Whether the gate is hand-tuned or learned, calibrating its confidence matters: an over-confident fusion that reports tiny covariance while quietly drifting is more dangerous than an honest noisy one, which is why the calibration and conformal methods of Chapter 18 apply directly to the fused state estimate.

ekf_filter_node:
  ros__parameters:
    frequency: 50.0
    odom0: /wheel/odometry        # proprioceptive: x,y,yaw velocity
    odom0_config: [false,false,false, false,false,false,
                   true,true,false, false,false,true, false,false,false]
    imu0: /imu/data               # proprioceptive: yaw rate + accel
    imu0_config: [false,false,false, false,false,true, ...]
    pose0: /lidar/pose            # exteroceptive: absolute x,y,yaw
    pose0_differential: false

What's Next

In Section 57.5, we turn the disagreement between proprioception and exteroception from a nuisance into a diagnostic: a large, persistent innovation is not just something to down-weight, it is evidence that a sensor has failed. We build on this fusion loop to detect foot slip, a stuck encoder, or a blinded camera, and to recover safely rather than dead-reckon into a wall.