PaperlayerView exact revision on arXiv

Source-linked AI summary

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight

Ke Sun, Kartik Mohta, Bernd Pfrommer, Michael Watterson, Sikang Liu, Yash Mulgaonkar, Camillo J. Taylor, Vijay Kumar

arXiv:1712.00036v3cs.RO

TL;DR

MAV VIO must remain accurate and robust while running in real time on constrained onboard computers. The paper proposes S-MSCKF, a filter-based stereo VIO using MSCKF, and reports comparable accuracy with low computational cost, including fast autonomous flight up to 17.5m/s in indoor and outdoor environments.

  • Problem

    MAV applications need VIO that is computationally efficient and robust despite constraints on sensor quality, processor power, and operating conditions.

  • Method

    The paper develops a filter-based stereo VIO using the Multi-State Constraint Kalman Filter.

  • Results

    S-MSCKF maintains similar accuracy while achieving the lowest CPU usage on the fast-flight dataset, and supports flight speeds up to 17.5m/s.

  • Takeaways & Limitations

    S-MSCKF provides robust stereo state estimation with a modest computational budget for aggressive three-dimensional maneuvering and indoor/outdoor flight.

Abstract

from arXiv · show

In recent years, vision-aided inertial odometry for state estimation has matured significantly. However, we still encounter challenges in terms of improving the computational efficiency and robustness of the underlying algorithms for applications in autonomous flight with micro aerial vehicles in which it is difficult to use high quality sensors and pow- erful processors because of constraints on size and weight. In this paper, we present a filter-based stereo visual inertial odometry that uses the Multi-State Constraint Kalman Filter (MSCKF) [1]. Previous work on stereo visual inertial odometry has resulted in solutions that are computationally expensive. We demonstrate that our Stereo Multi-State Constraint Kalman Filter (S-MSCKF) is comparable to state-of-art monocular solutions in terms of computational cost, while providing signifi- cantly greater robustness. We evaluate our S-MSCKF algorithm and compare it with state-of-art methods including OKVIS, ROVIO, and VINS-MONO on both the EuRoC dataset, and our own experimental datasets demonstrating fast autonomous flight with maximum speed of 17.5m/s in indoor and outdoor environments. Our implementation of the S-MSCKF is available at https://github.com/KumarRobotics/msckf_vio.

I. INTRODUCTION

The paper targets accurate, robust, and computationally efficient VIO for MAVs operating with limited onboard sensing and computing resources. It proposes an open-source filter-based stereo approach and evaluates it against established VIO systems on benchmark and fast-flight datasets.

  • Motivation: MAV autonomy requires accurate pose estimation for in-air stabilization, while real-world operation also demands robustness to lighting, texture, and aggressive-motion changes.These requirements arise in GPS-denied and unstructured environments where sensor and payload constraints limit available onboard resources.
  • Motivation: Real-time autonomy requires all software to run onboard on laptop-class hardware without excessive intermittent CPU spikes.VIO must share limited computational resources with other autonomy components.
  • Approach: The proposed S-MSCKF is a filter-based stereo VIO built from MSCKF to improve computational efficiency while retaining stereo robustness.The paper contrasts this design with optimization-based methods and monocular solutions.
  • Contributions: The work presents, to the authors’ knowledge, the first open-source filter-based stereo VIO running onboard on a laptop-class computer without GPU acceleration.The implementation is publicly available, alongside the fast-flight dataset.
  • Contributions: Experiments compare S-MSCKF with OKVIS, ROVIO, and VINS-MONO for accuracy, efficiency, and robustness on EuRoC and FALCON fast-flight data.The comparisons use both a public benchmark and the authors’ experimental platform.

II. RELATED WORK

Related VIO systems differ mainly in whether they use optimization or filtering and whether they use monocular or stereo sensing. The paper motivates S-MSCKF as an efficient stereo filter and describes its bounded-state filter formulation.

  • Existing VIO approaches: Tightly coupled VIO methods are generally divided into optimization-based and filter-based approaches using IMU and image measurements.Optimization methods jointly minimize measurement residuals, whereas filter methods use recursive state estimation.
  • Existing VIO approaches: Filter-based approaches are described as more efficient while achieving accuracy comparable to optimization-based approaches, with FEJ and OC improving consistency.These methods address estimation consistency within the filter framework.
  • Stereo VIO: Stereo and multi-camera VIO systems are less common partly because processing additional images and matching features increases computational cost.Prior stereo systems include both offline and optimization-based implementations, with some requiring powerful CPUs for real-time operation.
  • Filter formulation: The proposed filter state includes IMU pose, velocity, position, sensor biases, camera–body transformation, and N camera states.The formulation uses an error state with a three-dimensional orientation error representation.
  • Filter formulation: Camera states are marginalized after reaching a preset limit to maintain bounded computational complexity.The paper discusses how to choose states for marginalization separately.

A. Process Model

The process model propagates the estimated IMU state and its uncertainty from continuous-time dynamics and discrete IMU measurements. New camera states are then augmented from the latest IMU state while covariance propagation maintains the full filter uncertainty.

  • Continuous dynamics: The continuous IMU process model uses bias-corrected angular velocity and acceleration measurements together with gyroscope, accelerometer, and bias random-walk noise.The linearized error dynamics use the quaternion-to-rotation mapping and the skew-symmetric angular-velocity matrix.
  • Discrete propagation: A fourth-order Runge–Kutta integration propagates the estimated IMU state for discrete-time IMU measurements.The uncertainty requires a discrete state-transition matrix and discrete noise covariance.
  • Uncertainty propagation: The discrete noise covariance is computed from the continuous-time system noise covariance before propagating the IMU covariance.The covariance propagation is represented using the partitioned uncertainty of the whole state.
  • Camera-state augmentation: When new images arrive, the filter augments its state with a camera pose computed from the latest IMU state.This operation extends the joint state used by the visual-inertial filter.

B. Measurement Model

The measurement model represents stereo observations from distinct camera poses and estimates feature positions before projecting residuals into a Jacobian null space for filtering.

  • The left and right cameras have distinct poses at each time instance, while the state vector contains only the left-camera pose.
  • Stereo measurements retain four components, removing the need for stereo rectification and allowing observations from different image planes.The state stores the left-camera pose; the right-camera pose is recovered from calibrated extrinsic parameters.
  • The world-frame feature position is estimated by least squares from the current camera-pose estimates, then the measurement model is linearized.
  • Stacking multiple observations of one feature produces a joint measurement relation for the filter update.
  • Because the estimated feature position is correlated with camera poses, the residual is projected into the null space of the measurement Jacobian.This removes the feature-position uncertainty from the residual before a standard EKF update.

C. Observability Constraint

The filter must preserve four unobservable directions in 6-DOF visual-inertial estimation, so the implementation applies an observability-constrained EKF.

  • Four directions—global position and rotation about gravity, or yaw—are unobservable in EKF-based 6-DOF VIO.
  • A naive EKF VIO implementation can gain spurious yaw information because process and measurement steps are linearized at different points.
  • The implementation uses OC-EKF because it is less dependent on accurate initialization than FEJ-EKF and retains camera poses in the state representation compared with robocentric mapping.

D. Filter Update Mechanism

The update mechanism triggers on feature loss or a full pose buffer, then removes camera states frequently using a motion-based keyframe strategy while retaining the latest state.

  • Updates occur when the algorithm loses a feature or the camera-pose buffer reaches its limit.
  • One third of the camera states is marginalized when the buffer is full, following the inherited delayed-update mechanism with real-time modifications.
  • Two camera states are removed every other update, and observations from those states contribute only when the stereo measurements correspond to the same feature.Frequent removal can cause some valid observations to be ignored.
  • A two-way keyframe strategy selects either the second-latest or oldest state for removal based on relative motion, while always retaining the latest state.

E. Image Processing Frontend

The image-processing frontend prioritizes computational efficiency by detecting FAST features and tracking them with KLT optical flow rather than more expensive descriptor-based methods.

  • FAST is used for feature detection because of its efficiency.
  • Existing features are tracked temporally with KLT optical flow.
  • Descriptor-based temporal tracking provides better accuracy but requires substantially more CPU resources for only a small accuracy gain in these experiments.

IV. EXPERIMENTS

The experiments compare S-MSCKF with established VIO systems on EuRoC and fast-flight datasets, evaluating accuracy, computational load, and robustness. S-MSCKF maintains similar accuracy while reducing CPU usage, but its EuRoC evaluation fails on V2_03_difficult under inconsistent stereo brightness.

  • Experiments: The study compares S-MSCKF with OKVIS, ROVIO, and VINS-MONO on EuRoC and fast-flight datasets, with VINS-MONO loop closure disabled.The fast-flight tests reach maximum speeds of 17.5m/s and use repeated runs for evaluation.
  • A. EuRoC Dataset: On EuRoC, the four approaches show similar accuracy on remaining datasets, while ROVIO has larger error in machine hall sequences.The proposed method does not work properly on V2_03_difficult.
  • A. EuRoC Dataset: Filter-based methods use less CPU than optimization-based methods, favoring onboard real-time applications.In S-MSCKF, approximately 80% of computation comes from the frontend, while the filter uses about 10% of one core at 20Hz.
  • B. Fast Flight Dataset: S-MSCKF achieves the lowest CPU usage on fast-flight datasets while maintaining accuracy similar to the other compared solutions.Fast-flight accuracy is measured using aligned x- and y-direction position RMSE against GPS.
  • B. Fast Flight Dataset: Higher image frequency, resolution, and aggressive motion increase S-MSCKF frontend computation during fast flight.Aggressive flight shortens feature lifetimes, requiring more frequent new feature detection.
  • B. Fast Flight Dataset: Figure 5 reports aligned trajectories, start and goal locations, and speed profiles for the dataset reaching 17.5m/s.The fast-flight experiments command the quadrotor to travel 300m to a waypoint and return.

C. Autonomous Flight in Unstructured Environments

The S-MSCKF is evaluated in a fully autonomous flight through wooded, warehouse, and indoor environments with feature poverty, aggressive maneuvers, and lighting changes. Over a 700m round trip, it achieves approximately 3m final drift, while the implementation is publicly available.

  • C. Autonomous Flight in Unstructured Environments: The autonomous trial combines wooded and warehouse navigation with feature-poor scenes, aggressive maneuvers, and indoor-outdoor lighting changes.The robot searches for a warehouse entrance and target before returning to its starting point.
  • C. Autonomous Flight in Unstructured Environments: 3m final drift over a 700m round-trip trajectory is less than 0.5% of the total traveled distance.State estimation uses only stereo cameras and an IMU; laser measurements are used for mapping.
  • C. Autonomous Flight in Unstructured Environments: The S-MSCKF demonstrates robustness with a modest computational budget during aggressive three-dimensional maneuvering, fast flight up to 17.5m/s, and indoor navigation.The reported scope includes indoor/outdoor transitions and cluttered environments.
  • C. Autonomous Flight in Unstructured Environments: The S-MSCKF implementation is publicly available online.The paper provides an implementation repository for the proposed estimator.
  • C. Autonomous Flight in Unstructured Environments: As a VIO system travels, uncertainty in global position and yaw grows because those directions are unobservable.The authors report that large prior uncertainty can cause estimation jumps or divergence and identify trajectory planning as future work.

APPENDIX

The appendix specifies matrix structures used in the filter formulation, including F and G, the state-augmentation Jacobian J, and chain-rule computation of H_j f_i. It also notes a correction to an earlier equation.

  • APPENDIX: F and G are presented in matrix form for the filter formulation.The supplied passage introduces these matrices but does not state their full semantic roles.
  • APPENDIX: The state augmentation Jacobian J is given in Eq. (3).The appendix introduces J as the Jacobian associated with augmenting the state.
  • APPENDIX: The appendix states that J_I corrects a typo in Eq. (16) of reference.This is identified as a correction to the cited prior formulation.
  • APPENDIX: H_j f_i is computed using the chain rule in Eq. (5).The appendix presents the computation as part of the filter’s Jacobian formulation.

APPENDIX D

The appendix constructs a compact stereo measurement Jacobian using shorthand notation and a left-null-space relation. Because a single stereo measurement has an unobservable direction, it cannot directly update the measurement.

  • APPENDIX D: Short-hand notation from Eq. (8) is used to compactly write the measurement Jacobian in Eq. (7).The notation is introduced before the compact Jacobian expression.
  • APPENDIX D: A four-dimensional vector v is identified as the left null space of H_j.The appendix states this null-space property for the measurement Jacobian.
  • APPENDIX D: The rotation and rank relation imply that v remains in the left null space of H_j.The appendix derives this implication from the stated matrix properties.
  • APPENDIX D: A single stereo measurement cannot be directly used for a measurement update.The null-space result motivates using additional measurements in the filter update.
Loading 1712.00036v3…