Classical Visual-Inertial Odometry on the EuRoC MAV Dataset

Tech stack: Python, NumPy, OpenCV, Extended Kalman Filter, FAST + Lucas-Kanade, Two-Point RANSAC, Levenberg-Marquardt Optimization
A from-scratch Python implementation of the Stereo Multi-State Constraint Kalman Filter (S-MSCKF) — a tightly-coupled, filter-based VIO system that fuses stereo camera observations with IMU measurements for 6-DoF state estimation. Built the complete pipeline from IMU propagation and feature tracking through to the MSCKF measurement update, and benchmarked it against millimeter-accurate Vicon ground truth on the EuRoC Micro Aerial Vehicle dataset. Serves as the classical baseline to the team's deep-learning DeepVIO work.
Highlights

Implemented the full S-MSCKF pipeline: IMU propagation via 4th-order Runge-Kutta, JPL-quaternion error-state formulation, and a sliding window of up to 20 camera poses to keep the state dimension bounded regardless of the number of tracked features.
Built the visual front-end from scratch — FAST corner detection on a 4×5 grid for spatial uniformity, Lucas-Kanade KLT tracking with IMU-predicted rotation initialization, stereo matching along epipolar lines, and two-point RANSAC outlier rejection.
Implemented Levenberg-Marquardt feature triangulation with inverse-depth parameterization and a sufficient-motion baseline check.
Implemented the MSCKF measurement update with left-nullspace projection to marginalize feature positions out of the state, plus chi-squared gating and Joseph-form covariance update for numerical stability.
Carried out full quantitative evaluation: ATE, RPE vs. sub-trajectory length, rotation/translation error over time, scale drift analysis, and Umeyama SE(3) alignment.

Results (EuRoC MH_01_easy, 2862 frames, 73.04 m path)

3D ATE RMSE: 0.1025 m (≈ 0.14% of trajectory length).
Rotation RMSE: 2.46° overall (Roll 1.98°, Pitch 0.95°, Yaw 1.12°) — bounded throughout the sequence due to continuous visual correction of gyroscope drift.
Scale drift: 0.10, with Umeyama scale = 1.000 — confirming metric scale is correctly recovered from the stereo baseline.
Sub-linearly growing relative pose error consistent with a tightly-coupled filter operating without loop closure.