Visual-Inertial Odometry

The IMU section ended with a TurtleBot whose IMU-only position estimate was meters off after ten seconds. A camera has the opposite profile. It sees fixed points in the world, so its errors do not accumulate, but it delivers a few frames a second, and a single camera cannot tell how far away a point is. Visual-inertial odometry (VIO) runs both: the IMU predicts the motion between frames, and each frame corrects the prediction and the IMU’s biases. Corke treats the two halves separately, inertial navigation in section 3.4 and visual odometry in section 14.8.3 (Corke, 2023). This section joins them in one filter. This section builds a VIO filter for the IMU inside the D435i. That IMU is a Bosch BMI055 fixed to the camera body (librealsense), so the rotation and offset between the IMU and the camera are known from the factory. The filter is an extended Kalman filter like the one in the EKF section, but its state contains a 3D orientation and the two sensor biases. In this section

R_{WB}

, always written with its frame subscripts, is the rotation from the body frame

\{B\}

to the world frame

\{W\}

R_t

without subscripts is the measurement noise covariance, as in the rest of the chapter.

The D435i IMU

The constants below are the ones the course simulator gives the D435i’s IMU. The white noise comes from the BMI055 noise densities, 0.014 °/s/√Hz for the gyroscope and 150 µg/√Hz for the accelerometer, converted to one sample at 200 Hz. The real camera streams the gyroscope at 200 Hz and the accelerometer at 63 Hz by default (librealsense); the simulator runs both at 200 Hz, and so does this section. Each bias has two parts. A constant part is drawn when the sensor powers on, with a standard deviation of 5 mrad/s for the gyroscope and 0.05 m/s² for the accelerometer. A second part wanders slowly as a first-order Gauss-Markov process, which decays toward zero with a time constant of a few hundred seconds while noise keeps pushing it. These bias values were chosen to make the drift visible within a minute, not taken from a datasheet.

from scipy.spatial.transform import Rotation as Rot

RATE = 200  # Hz
DT = 1 / RATE
G_W = np.array([0.0, 0.0, -9.81])
SIGMA_G, SIGMA_A = 3.5e-3, 2.1e-2  # per-sample white noise, rad/s and m/s^2
BIAS0_G, BIAS0_A = 5e-3, 5e-2  # spread of the bias drawn at power-on
DYN_G, TAU_G = 2e-4, 400.0  # slowly wandering part of the gyro bias
DYN_A, TAU_A = 2e-3, 300.0  # and of the accelerometer bias


def skew(w):
    return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])


def exp_so3(w):
    return Rot.from_rotvec(w).as_matrix()

The scene

The TurtleBot drives the loop of the IMU section, a circle of radius 1.5 m at 0.2 m/s for 60 s, with the camera 20 cm above the floor. The room’s walls are 4 m from the center, and 240 points on them, up to 2.5 m high, form a map that the robot already has. Each camera frame sees some of these points, and the filter knows which map point each pixel belongs to. Building that map and finding those correspondences is the work of SLAM and of the visual SLAM section. This section isolates the fusion of the two sensors. The code below generates the true motion, the true biases and the IMU readings, and then integrates the readings with no correction, as in the IMU section.

def wall_points(n=240, seed=4):
    """Mapped landmarks: points on the walls of an 8 m by 8 m room, up to 2.5 m high."""
    rng = np.random.default_rng(seed)
    side = rng.integers(0, 4, n)
    s = rng.uniform(-4, 4, n)
    z = rng.uniform(0.0, 2.5, n)
    x = np.select([side == 0, side == 1, side == 2], [np.full(n, 4.0), np.full(n, -4.0), s], s)
    y = np.select([side == 0, side == 1, side == 2], [s, s, np.full(n, 4.0)], np.full(n, -4.0))
    return np.stack([x, y, z], 1)


def truth(radius=1.5, speed=0.2, duration=60.0, height=0.2, seed=0):
    """True motion, true IMU biases, and the IMU readings of a TurtleBot driving a circle."""
    rng = np.random.default_rng(seed)
    w = speed / radius
    t = np.arange(int(duration * RATE) + 1) * DT
    th = np.pi / 2 + w * t
    p = np.stack([radius * np.cos(w * t), radius * np.sin(w * t), np.full_like(t, height)], 1)
    v = np.stack([speed * np.cos(th), speed * np.sin(th), np.zeros_like(t)], 1)
    a = np.stack([-speed * w * np.sin(th), speed * w * np.cos(th), np.zeros_like(t)], 1)
    R = Rot.from_euler("z", th[:, None]).as_matrix()
    om = np.tile([0.0, 0.0, w], (len(t), 1))
    # biases: a constant part drawn at power-on plus a first-order Gauss-Markov wander
    bg, ba = np.empty((len(t), 3)), np.empty((len(t), 3))
    bg0, ba0 = rng.normal(0, BIAS0_G, 3), rng.normal(0, BIAS0_A, 3)
    eg, ea = np.zeros(3), np.zeros(3)
    for k in range(len(t)):
        bg[k], ba[k] = bg0 + eg, ba0 + ea
        eg = np.exp(-DT / TAU_G) * eg + rng.normal(0, DYN_G * np.sqrt(1 - np.exp(-2 * DT / TAU_G)), 3)
        ea = np.exp(-DT / TAU_A) * ea + rng.normal(0, DYN_A * np.sqrt(1 - np.exp(-2 * DT / TAU_A)), 3)
    om_meas = om + bg + rng.normal(0, SIGMA_G, (len(t), 3))
    f_meas = np.einsum("kji,kj->ki", R, a - G_W) + ba + rng.normal(0, SIGMA_A, (len(t), 3))
    return dict(t=t, R=R, p=p, v=v, bg=bg, ba=ba, om_meas=om_meas, f_meas=f_meas,
                om_true=om, f_true=np.einsum("kji,kj->ki", R, a - G_W))


def imu_only(tr, noise=True):
    """Strapdown integration of the IMU readings, with no bias correction."""
    om = tr["om_meas"] if noise else tr["om_true"]
    f = tr["f_meas"] if noise else tr["f_true"]
    R, p, v = tr["R"][0].copy(), tr["p"][0].copy(), tr["v"][0].copy()
    out = [p.copy()]
    for k in range(len(tr["t"]) - 1):
        acc = R @ f[k] + G_W
        p = p + v * DT + 0.5 * acc * DT**2
        v = v + acc * DT
        R = R @ exp_so3(om[k] * DT)
        out.append(p.copy())
    return np.array(out)

tr = truth()
landmarks = wall_points()
p_imu = imu_only(tr)
err_imu = np.linalg.norm(p_imu - tr["p"], axis=1)
print("true gyro bias at the start (mrad/s):", (1e3 * tr["bg"][0]).round(2))
print("true accelerometer bias at the start (m/s^2):", tr["ba"][0].round(3))
print("IMU-only position error after 1, 10, 60 s:", [round(float(err_imu[int(s * RATE)]), 2) for s in (1, 10, 60)], "m")

true gyro bias at the start (mrad/s): [ 0.63 -0.66  3.2 ]
true accelerometer bias at the start (m/s^2): [ 0.005 -0.027  0.018]
IMU-only position error after 1, 10, 60 s: [0.02, 2.49, 116.04] m

The IMU-only estimate is 2.5 m off after 10 s and 116 m off after 60 s. Most of the error comes from the gyroscope bias, whose horizontal components tilt the estimated gravity vector.

The camera measurement

The camera is the D435i’s color camera, with the nominal intrinsics of the real camera from the course’s camera configuration:

f_x = f_y = 617

px and the principal point at (321, 240) in a 640 by 480 image. The camera’s optical frame has

z

forward,

x

right and

y

down, while the robot’s body frame has

x

forward,

y

left and

z

up. The fixed rotation

R_{BC}

maps one to the other. A map point

m

appears at the pixel

p_B = R_{WB}^\top (m - p_{WB}), \qquad (x, y, z)^\top = R_{BC}^\top p_B, \qquad h(R_{WB}, p_{WB}) = \begin{bmatrix} c_x + f_x x / z \\ c_y + f_y y / z \end{bmatrix}.

The camera sees a point if it lies at least 0.3 m in front of it, within 6 m, and inside the image. The filter uses up to 15 visible points per frame, five frames a second, each with one pixel of noise.

FX = FY = 617.0
CX, CY, WIDTH, HEIGHT = 321.0, 240.0, 640, 480
SIGMA_PX = 1.0
CAM_RATE = 5  # Hz
# camera optical frame (z forward, x right, y down) expressed in the body frame (x forward, y left, z up)
R_BC = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]])


def project(R_wb, p_wb, m):
    """Pixel of world point m, and its Jacobian with respect to the 15-dim error state."""
    p_b = R_wb.T @ (m - p_wb)
    x, y, z = R_BC.T @ p_b
    uv = np.array([CX + FX * x / z, CY + FY * y / z])
    d_uv_d_pc = np.array([[FX / z, 0, -FX * x / z**2], [0, FY / z, -FY * y / z**2]])
    J = np.zeros((2, 15))
    J[:, 0:3] = d_uv_d_pc @ R_BC.T @ skew(p_b)  # orientation error, R = R_hat Exp(dtheta)
    J[:, 3:6] = d_uv_d_pc @ R_BC.T @ (-R_wb.T)  # position error
    return uv, J


def visible(R_wb, p_wb, m):
    p_c = R_BC.T @ (R_wb.T @ (m - p_wb))
    if p_c[2] < 0.3 or np.linalg.norm(p_c) > 6.0:
        return False
    u, v = CX + FX * p_c[0] / p_c[2], CY + FY * p_c[1] / p_c[2]
    return 0 <= u < WIDTH and 0 <= v < HEIGHT

An error-state Kalman filter

An EKF linearizes around its estimate and adds a correction vector to it. That works for position, velocity and biases, but a rotation matrix cannot be corrected by adding to it. The error-state filter separates the two jobs. It keeps a nominal estimate

(R_{WB}, p, v, b_g, b_a)

that it propagates with the full nonlinear model, and a small error vector

\delta x = (\delta\theta, \delta p, \delta v, \delta b_g, \delta b_a) \in \mathbb R^{15}

with covariance

\Sigma_t

. The orientation error

\delta\theta

is a rotation vector applied on the right,

R_{WB}^{\text{true}} = R_{WB} \, e^{[\delta\theta]_\times}

, so it lives in the body frame where the gyroscope measures.

Prediction

For each IMU sample, the filter subtracts the estimated biases and integrates as in the IMU section:

\hat\omega = \tilde\omega - b_g, \quad \hat f = \tilde f - b_a, \quad a = R_{WB} \hat f + g, \quad R_{WB} \leftarrow R_{WB} \, e^{[\hat\omega \delta t]_\times}.

The error grows according to the Jacobian

F

of this step. Its nonzero blocks say that an orientation error rotates with the body, a gyroscope bias error turns into an orientation error, a velocity error turns into a position error, and an orientation error rotates the measured specific force and so becomes a velocity error:

\frac{\partial \delta\theta'}{\partial \delta\theta} = e^{-[\hat\omega\delta t]_\times}, \quad \frac{\partial \delta\theta'}{\partial \delta b_g} = -I\delta t, \quad \frac{\partial \delta p'}{\partial \delta v} = I\delta t, \quad \frac{\partial \delta v'}{\partial \delta\theta} = -R_{WB}[\hat f]_\times \delta t, \quad \frac{\partial \delta v'}{\partial \delta b_a} = -R_{WB}\delta t .

The covariance prediction is

\bar\Sigma_t = F \Sigma_{t-1} F^\top + Q_t

, where

Q_t

holds the white noise of the two sensors and the random walk of the two biases. The filter models each bias as a random walk, while the simulated bias decays slowly toward its power-on value; over one minute the difference is negligible.

Correction

At each camera frame the filter stacks the pixel residuals

z - h

of all the points it sees. Differentiating

h

with

R_{WB} e^{[\delta\theta]_\times} \approx R_{WB}(I + [\delta\theta]_\times)

gives the two nonzero blocks of the measurement Jacobian,

\frac{\partial h}{\partial \delta\theta} = J_\pi R_{BC}^\top [p_B]_\times, \qquad \frac{\partial h}{\partial \delta p} = -J_\pi R_{BC}^\top R_{WB}^\top ,

where

J_\pi

is the

2 \times 3

Jacobian of the pinhole projection. The biases and the velocity have no direct effect on a pixel. The filter learns them only through their correlation with the pose in

\bar\Sigma_t

. The update is the usual Kalman update with

R_t = \sigma_{\text{px}}^2 I

. The estimated error is then folded into the nominal state, the rotation by

R_{WB} \leftarrow R_{WB} e^{[\delta\theta]_\times}

, and reset to zero. The project function returns both the pixel and the Jacobian.

def propagate(x, P, om_m, f_m):
    """IMU prediction of the nominal state and of the 15x15 error covariance."""
    R, p, v, bg, ba = x
    w, f = om_m - bg, f_m - ba
    acc = R @ f + G_W
    F = np.eye(15)
    F[0:3, 0:3] = exp_so3(w * DT).T
    F[0:3, 9:12] = -np.eye(3) * DT
    F[3:6, 6:9] = np.eye(3) * DT
    F[6:9, 0:3] = -R @ skew(f) * DT
    F[6:9, 12:15] = -R * DT
    Qd = np.zeros((15, 15))
    Qd[0:3, 0:3] = np.eye(3) * (SIGMA_G * DT) ** 2
    Qd[6:9, 6:9] = np.eye(3) * (SIGMA_A * DT) ** 2
    Qd[9:12, 9:12] = np.eye(3) * DYN_G**2 * (1 - np.exp(-2 * DT / TAU_G))
    Qd[12:15, 12:15] = np.eye(3) * DYN_A**2 * (1 - np.exp(-2 * DT / TAU_A))
    x = (R @ exp_so3(w * DT), p + v * DT + 0.5 * acc * DT**2, v + acc * DT, bg, ba)
    return x, F @ P @ F.T + Qd


def correct(x, P, z, landmarks):
    """Camera update from the pixels z of mapped landmarks."""
    R, p, v, bg, ba = x
    rows, Hs = [], []
    for zi, m in zip(z, landmarks):
        uv, J = project(R, p, m)
        rows.append(zi - uv)
        Hs.append(J)
    r, H = np.concatenate(rows), np.vstack(Hs)
    Rn = np.eye(len(r)) * SIGMA_PX**2  # pixel measurement noise
    S = H @ P @ H.T + Rn
    K = P @ H.T @ np.linalg.inv(S)
    dx = K @ r
    I_KH = np.eye(15) - K @ H
    P = I_KH @ P @ I_KH.T + K @ Rn @ K.T
    x = (R @ exp_so3(dx[0:3]), p + dx[3:6], v + dx[6:9], bg + dx[9:12], ba + dx[12:15])
    return x, P


def vio(tr, landmarks, seed=1, max_per_frame=15):
    rng = np.random.default_rng(seed)
    x = (tr["R"][0].copy(), tr["p"][0].copy(), tr["v"][0].copy(), np.zeros(3), np.zeros(3))
    P = np.diag(np.r_[np.full(6, 1e-8), np.full(3, 1e-6),
                      np.full(3, BIAS0_G**2 + DYN_G**2), np.full(3, BIAS0_A**2 + DYN_A**2)])
    every = RATE // CAM_RATE
    ps, sig, bgs, bas, n_seen = [x[1]], [np.sqrt(np.diag(P))], [x[3]], [x[4]], []
    for k in range(len(tr["t"]) - 1):
        x, P = propagate(x, P, tr["om_meas"][k], tr["f_meas"][k])
        if (k + 1) % every == 0:
            Rt, pt = tr["R"][k + 1], tr["p"][k + 1]
            seen = [m for m in landmarks if visible(Rt, pt, m)]
            if len(seen) > max_per_frame:
                seen = [seen[i] for i in rng.choice(len(seen), max_per_frame, replace=False)]
            z = [project(Rt, pt, m)[0] + rng.normal(0, SIGMA_PX, 2) for m in seen]
            if seen:
                x, P = correct(x, P, z, seen)
            n_seen.append(len(seen))
        ps.append(x[1]); sig.append(np.sqrt(np.diag(P))); bgs.append(x[3]); bas.append(x[4])
    return dict(p=np.array(ps), sig=np.array(sig), bg=np.array(bgs), ba=np.array(bas), n_seen=np.array(n_seen))

Running VIO

The filter starts from the true pose and velocity, and with both bias estimates at zero, with a covariance equal to the spread of the true biases.

est = vio(tr, landmarks)
err_vio = np.linalg.norm(est["p"] - tr["p"], axis=1)
print(f"landmarks used per camera frame: {est['n_seen'].mean():.1f} on average")
print("VIO position error after 1, 10, 60 s:", [round(float(err_vio[int(s * RATE)]), 4) for s in (1, 10, 60)], "m")
print(f"final accelerometer bias error {np.linalg.norm(est['ba'][-1] - tr['ba'][-1]):.4f} m/s^2 "
      f"(true bias {np.linalg.norm(tr['ba'][-1]):.4f} m/s^2)")
print(f"final gyro bias error {1e3 * np.linalg.norm(est['bg'][-1] - tr['bg'][-1]):.3f} mrad/s "
      f"(true bias {1e3 * np.linalg.norm(tr['bg'][-1]):.3f} mrad/s)")

landmarks used per camera frame: 14.9 on average
VIO position error after 1, 10, 60 s: [0.0038, 0.0062, 0.0062] m
final accelerometer bias error 0.0017 m/s^2 (true bias 0.0314 m/s^2)
final gyro bias error 0.136 mrad/s (true bias 3.444 mrad/s)

Seen from above, the VIO path lies on the true circle, while the IMU-only estimate leaves it within a few seconds.

The VIO error stays below about 2 cm for the whole minute and ends at 6 mm, against 116 m for the IMU alone. Its dashed

2\sigma

line sits just above the error, so the filter’s covariance describes its actual accuracy. Fifteen points are visible in almost every frame, which is far more than the six numbers of a pose need; with fewer points, or a frame where the camera faces a blank wall, the IMU carries the estimate alone until the next useful frame.

The thick lines are the true biases and the thin ones the estimates. Both converge within about 10 s and then track the slow wander of the true biases. At the end the accelerometer bias is known to 0.0017 m/s², against a true bias of 0.031 m/s², and the gyroscope bias to 0.14 mrad/s, against 3.4 mrad/s. The camera never measures a bias. The filter infers them because a wrong bias makes the predicted pose drift in a particular direction, and the next frame shows that drift.

From this filter to VIO in practice

The filter here had a map. A real VIO system builds its landmarks as it goes, by tracking image features over several frames and triangulating them, which is the front end of the visual SLAM section. Systems such as MSCKF keep a short window of past camera poses in the filter state instead of the landmarks, and ORB-SLAM3 optimizes the IMU and camera terms together over a window of keyframes (Campos2020-xm). In the course simulator the same two inputs exist as ROS topics. The camera’s IMU publishes on /camera/imu with the noise and drifting bias described above, in the frame camera_imu_optical_frame, and the color images arrive on /camera/color/image_raw. The simulated camera is smaller than the real one, 320 by 240 pixels with a 60° horizontal field of view, so its focal length is about 277 px and its principal point (160, 120); a filter run in the maze needs those intrinsics. Key references: (Corke, 2023; Campos2020-xm)

References

Corke, P. (2023). Robotics, vision and control: Fundamental algorithms in python.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Visual-Inertial Odometry

The D435i IMU

The scene

The camera measurement

An error-state Kalman filter

Prediction

Correction

Running VIO

From this filter to VIO in practice

References

​The D435i IMU

​The scene

​The camera measurement

​An error-state Kalman filter

​Prediction

​Correction

​Running VIO

​From this filter to VIO in practice

​References

The D435i IMU

The scene

The camera measurement

An error-state Kalman filter

Prediction

Correction

Running VIO

From this filter to VIO in practice

References