Inverse sliding-window filters for vision-aided inertial navigation systems

Patent metadata

Field	Value
Publication number	US-9658070-B2
Application number	US-201514796574-A
Country	US
Kind code	B2
Filing date	Jul 10, 2015
Priority date	Jul 11, 2014
Publication date	May 23, 2017
Grant date	May 23, 2017

Abstract

This disclosure describes inverse filtering and square root inverse filtering techniques for optimizing the performance of a vision-aided inertial navigation system (VINS). In one example, instead of keeping all features in the system's state vector as SLAM features, which can be inefficient when the number of features per frame is large or their track length is short, an estimator of the VINS may classify the features into either SLAM or MSCKF features. The SLAM features are used for SLAM-based state estimation, while the MSCKF features are used to further constrain the poses in the sliding window. In one example, a square root inverse sliding window filter (SQRT-ISWF) is used for state estimation.

First claim

The invention claimed is:

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 determines, based on the image data and the IMU data, 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,
wherein the estimator determines the estimates by:
classifying, for each of the poses, each of the features observed at the respective pose into either a first set of the features or a second set of the features,
maintaining a state vector having states for a position and orientation of the VINS and for positions with the environment for the first set of features for a sliding window of two or more of the most recent poses along the trajectory without maintaining states for positions of the second set of features within the state vector, and
applying an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features and compute, in accordance with the constraints, the state estimates within the state vector for the sliding window.

2. The vision-aided inertial navigation system of claim 1 ,
wherein the estimator applies the inverse sliding window filter by, for each update of the state estimates within the state vector, computing a Hessian matrix that excludes at least a portion of the second features set, wherein the Hessian matrix represents a subset of the IMU data and the image along the trajectory, and
wherein the estimator updates the state estimates based on the Hessian.

3. The vision-aided inertial navigation system of claim 2 , further comprising an outlier rejection module,
wherein, upon receiving additional image data or IMU data, the filter computes a covariance from only a subset of an inverse of the Hessian matrix, wherein the covariance represents the amount of uncertainty in the state estimates, and
wherein the outlier rejection module applies the covariance to reject any outliers in the additional image data or IMU data prior to performing an update to the state estimates within the state vector.

4. The vision-aided inertial navigation system of claim 2 , wherein the outlier rejection module applies apply a 2-Point Random sample consensus (RANSAC) technique.

5. The vision-aided inertial navigation system of claim 2 , wherein the estimator applies the inverse sliding window filter by, for each update of the state estimates:
computing, as a covariance, only a subset of the inverse of the Hessian matrix, wherein the covariance represents the amount of uncertainty in the state estimates;
identifying information sources within the estimator based on the covariance, wherein the information sources comprise VINS poses and features; and
marginalizing, based on the identified information sources, a portion of the states to keep a constant-sized state vector comprising the identified VINS poses and features.

6. The vision-aided inertial navigation system of claim 2 , wherein the inverse sliding window filter comprises a square root inverse sliding window filter in which the estimator computes the Hessian matrix as a square root factor of a Hessian matrix that excludes the second features set.

7. The vision-aided inertial navigation system of claim 2 , wherein the estimator classifies the features of the first set of as simultaneous localization and mapping (SLAM) features for computing the state estimates and the second set of features as Multi-state Constraint Kalman Filter (MSCKF) features for computing the constraints.

8. The vision-aided inertial navigation system of claim 5 , wherein the estimator maintains the state vector to have a threshold number of SLAM features and to exclude all of the MSCKF features, where the estimator sets threshold based on current resource utilization within the VINS.

9. The vision-aided inertial navigation system of claim 5 , wherein the estimator uses a modified measurement model for the MSCKF feature measurements that marginalizes the MSCKF features without maintaining state estimates for the MSCKF features in the state vector.

10. The vision-aided inertial navigation system of claim 5 , wherein the Hessian matrix stores terms computed from previous marginalization of the SLAM features for prior poses no longer within the sliding window.

11. The vision-aided inertial navigation system of claim 5 , wherein blocks of the Hessian matrix corresponding to the VINS poses x R and the SLAM feature states l are fully dense due to previous marginalization, while the blocks involving MSCKF feature states f have at least some sparse structures.

12. The vision-aided inertial navigation system of claim 1 , wherein the estimator:
computes a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS;
selects, based on the budget, the first set of features and the second set of features as subsets of all available features; and
computes the state estimate in accordance with the selected features.

13. The vision-aided inertial navigation system of claim 1 , wherein the states within the state vector represent, for each of the poses within the sliding window, an orientation and position of the VINS at a corresponding location along the trajectory.

14. The vision-aided inertial navigation system of claim 1 , wherein the vision-aided inertial navigation system is integrated within a tablet computer, a laptop computer, a mobile phone, a robot or a vehicle.

15. A method comprises:
producing 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;
producing inertial measurement data from an inertial measurement unit (IMU) indicative of motion of the vision-aided inertial navigation system; and
determining estimates for a position and orientation of the vision-aided inertial navigation system for a plurality of poses of the VINS along the trajectory, based on the image data and the IMU data, with a processing unit comprising an estimator,
wherein computing the state estimates comprises:
classifying, for each of the poses, each of the features observed at the respective pose into either a first set of the features or a second set of the features,
maintaining a state vector having states for the position and orientation of the VINS and for positions with the environment for the first set of features for a sliding window of two or more of the most recent poses along the trajectory without maintaining states for positions of the second set of features, and
applying an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features and compute, in accordance with the constraints, the state estimates within the state vector for the sliding window.

16. The method of claim 15 , wherein applying an inverse sliding window filter comprises:
computing a Hessian matrix that excludes the second features set, wherein the Hessi

Assignees

• Univ Minnesota

Inventors

• Roumeliotis Stergios I
• Wu Kejian J

Classifications

• G06T2207/30241

  Trajectory · CPC title

• G06T7/20

  Analysis of motion (motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51) · CPC title

• G06T2207/30244

  Camera pose · CPC title

• G06K9/4604

  Physics · mapped topic

• G01C21/165 Primary

  combined with non-inertial navigation instruments · CPC title