Visual-Inertial Odometry (VIO) with EKF and Feature Updates

This notebook extends the VIO demo to include visual updates using feature projections and simulates noise in IMU and image measurements.

# Install required packages
!rye add rvc3python spatialmath-python matplotlib numpy

error: Failed to run uv compile   × No solution found when resolving dependencies:

  ╰─▶ Because rvc3-python was not found in the package registry and you

      require rvc3-python, we can conclude that your requirements are

      unsatisfiable.



      hint: An index URL (https://download.pytorch.org/whl/gpu) could not

      be queried due to a lack of valid authentication credentials (403

      Forbidden).

. uv exited with status: exit status: 1

import numpy as np
import matplotlib.pyplot as plt
from spatialmath import SE3
from spatialmath.base import rotz, troty
from roboticstoolbox import CentralCamera
np.random.seed(42)

---------------------------------------------------------------------------
ModuleNotFoundError                       Traceback (most recent call last)
Cell In[2], line 3
      1 import numpy as np
      2 import matplotlib.pyplot as plt
----> 3 from spatialmath import SE3
      4 from spatialmath.base import rotz, troty
      5 from roboticstoolbox import CentralCamera

ModuleNotFoundError: No module named 'spatialmath'

Simulate trajectory, IMU, and landmarks with noise

# Camera intrinsics
cam = CentralCamera(f=0.08, rho=1e-5, pp=[320, 240], name='SimCam')

# Landmarks
n_landmarks = 50
landmarks = np.random.uniform([-1, -1, 3], [1, 1, 5], (n_landmarks, 3)).T

# Trajectory
n_steps = 100
dt = 0.1
trajectory = []
imus = []
for t in range(n_steps):
    x = 0.5 * np.sin(t * 0.1)
    y = 0.5 * np.cos(t * 0.1)
    z = 0.2 * np.sin(t * 0.05) + 3.5
    R = rotz(t * 0.01) @ troty(0.1)
    T = SE3.Rt(R, [x, y, z])
    trajectory.append(T)
    if t > 0:
        dp = (T.t - trajectory[t-1].t) / dt
        omega = np.random.normal(0, 0.01, 3)
        acc = dp + np.random.normal(0, 0.01, 3)
        imus.append((acc, omega))
    else:
        imus.append((np.zeros(3), np.zeros(3)))

Project 3D landmarks to 2D with noise

measurements = []
for T in trajectory:
    uv = cam.project(landmarks, T)
    uv += np.random.normal(0, 1.0, uv.shape)
    measurements.append(uv)

Simple EKF: Prediction + Visual Update

ekf_positions = []
state = np.array([0.0, 0.0, 3.5])
vel = np.zeros(3)
P = np.eye(3) * 0.1
R_meas = np.eye(2) * 1.0
for t in range(n_steps):
    acc, _ = imus[t]
    vel += acc * dt
    state += vel * dt
    P += np.eye(3) * 0.01
    # Visual update with one landmark
    landmark = landmarks[:, 0]
    pred_uv = cam.project(landmark.reshape(3, 1), SE3.Tt(np.eye(3), state))[:, 0]
    meas_uv = measurements[t][:, 0]
    residual = meas_uv - pred_uv
    # Jacobian H (finite difference)
    eps = 1e-5
    H = np.zeros((2, 3))
    for i in range(3):
        delta = np.zeros(3)
        delta[i] = eps
        pred_uv_perturbed = cam.project(landmark.reshape(3, 1), SE3.Tt(np.eye(3), state + delta))[:, 0]
        H[:, i] = (pred_uv_perturbed - pred_uv) / eps
    S = H @ P @ H.T + R_meas
    K = P @ H.T @ np.linalg.inv(S)
    state = state + K @ residual
    P = (np.eye(3) - K @ H) @ P
    ekf_positions.append(state.copy())

Plot Ground Truth vs EKF Trajectory

fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(111, projection='3d')
traj_xyz = np.array([T.t for T in trajectory]).T
ekf_xyz = np.array(ekf_positions).T
ax.plot(*traj_xyz, label='Ground Truth Trajectory')
ax.plot(*ekf_xyz, label='EKF Estimated Trajectory')
ax.scatter(*landmarks, color='red', s=10, alpha=0.5, label='Landmarks')
ax.set_title('VIO with Visual Update and Noise')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.legend()
plt.show()