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

• US 9,658,070 B2
• Filed: 07/10/2015
• Issued: 05/23/2017
• Est. Priority Date: 07/11/2014
• Status: Active Grant

• Current Assignee: Regents of The University of Minnesota (University of Minnesota)
• Original Assignee: Regents of The University of Minnesota (University of Minnesota)
• Inventors: Roumeliotis, Stergios I., Wu, Kejian J.
• Primary Examiner(s): Black, Thomas G
• Assistant Examiner(s): HUYNH, LUAT T
• Application Number:
  US14/796,574
• Publication Number:
  US 20160327395A1
• Time in Patent Office: 683 Days
• Field of Search: 701500, 701501, 701468, 701472, 34235731, 34235734, 34235732, 34235765
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06T 2207/30241 Trajectory
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/20 Analysis of motion motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80
• CPC Class Codes: G06V 20/56 exterior to a vehicle by using sensors mounted on the vehicle

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.

Claims

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 Hessian matrix represents a subset of the IMU data and the image along the trajectory based on a budget computed by the VINS; and

computing the state estimates based on the Hessian.

17. The method of claim 16, further comprising:

responsive to receiving additional image data or IMU data, computing a covariance from only a subset of an inverse of the Hessian matrix of the inverse sliding window filter, wherein the covariance represents the amount of uncertainty in the state estimates within the state vector, and

utilizing the covariance within an outlier rejection process 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.

18. The method of claim 16, wherein applying an inverse sliding window filter comprises:

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; and

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.

19. The method of claim 16, wherein the applying an inverse sliding window filter comprises applying a square root inverse sliding window filter (SQRT-ISWF) in which the Hessian matrix is computed as a square root factor of a Hessian matrix that excludes the second features set.

20. The method in claim 16, further comprising classifying 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) for computing the constraints.

21. The method in claim 20, further comprising maintaining the state vector to have a threshold number of SLAM features, where the estimator sets threshold based on current resource utilization within the VINS.

22. The method in claim 20, further comprising using 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.

23. The method in claim 20, wherein the Hessian matrix stores terms computed from previous marginalization of the SLAM features for prior poses no longer within the sliding window.

24. The method in claim 20, wherein blocks of the Hessian matrix correspond to 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 some sparse structures.

25. The method in claim 15, further comprising:

computing a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS,

selecting the first set of features and the second set of features from a subset of all available features determined based on the budget, and

computing the state estimate in accordance with the selected features.

26. The method in claim 15, wherein the states within the state vector representing, for each of the poses within the sliding window, an orientation and position.

27. The method of claim 15, wherein the sliding window comprises a square root inverse sliding window filter (SQRT-ISWF).