×

Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation

  • US 9,709,404 B2
  • Filed: 04/15/2016
  • Issued: 07/18/2017
  • Est. Priority Date: 04/17/2015
  • Status: Active Grant
First Claim
Patent Images

1. A vision-aided inertial navigation system comprising:

  • at least one image source to produce image data along a trajectory of the vision-aided inertial navigation system (VINS) within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the VINS along the trajectory;

    an inertial measurement unit (IMU) to produce IMU data indicative of motion of the vision-aided inertial navigation system; and

    a hardware-based processing unit comprising an estimator that computes, based on the image data and the IMU data, a sliding window of state estimates for at least a position and orientation of the vision-aided inertial navigation system for a plurality of poses of the VINS along the trajectory and respective covariances for each of the state estimates, each of the respective covariances representing an amount of uncertainty in the corresponding state estimate, andwherein the estimator computes the state estimates by;

    classifying the visual features observed at the poses of the VINS within the sliding window into at least a first set of the features and a second set of features as a function of a position within the sliding window for the respective pose from which the respective feature was observed, the second set of features being associated with one or more older poses than the first set of features within the sliding window,applying an extended Kalman filter to update, within the sliding window, each of the state estimates for the VINS and the features using the IMU data and the image data obtained associated with both the first set of features and the second set of features observed from the plurality of poses within the sliding window, andupdating, for each of the state estimates, the respective covariance using the IMU data and the image data associated with the second set of features without using the image data associated with the first set of features.

View all claims
  • 2 Assignments
Timeline View
Assignment View
    ×
    ×