# US 10,371,529 B2 · Computational budget estimation for vision-aided inertial navigation systems

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 10,371,529 B2
- Document ID: US10371529B2
- Status: Granted
- Kind: B2
- Application number: 15/601,261
- Filed: 2017-05-22
- Published: 2017-09-14
- Granted: 2019-08-06
- Earliest claimed: 2014-07-11
- Assignees: Regents of the University of Minnesota
- Applicants: Regents of the University of Minnesota
- Inventors: Stergios I. Roumeliotis; Kejian J. Wu
- Web page: https://patentgrep.com/patent/US10371529B2

## CPC

- G01C 21/1656: {with passive imaging devices, e.g. cameras}
- G05D 1/0246: {using a video camera in combination with image processing means}
- 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)}
- G05D 1/0274: {using mapping information stored in a memory device (navigation using map-matching G01C21/30)}
- G06T 7/20: Analysis of motion (motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51)
- G06T 7/70: Determining position or orientation of objects or cameras (camera calibration G06T7/80)
- G06V 20/56: exterior to a vehicle by using sensors mounted on the vehicle
- G06T 2207/30241: Trajectory
- G06T 2207/30244: Camera pose
- G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S)
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING

## 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

### Claim 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 executing 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:
computing a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS,
classifying, for each of the poses and based on the budget, 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
computing state estimates in accordance with the selected features.

### Claim 2 (depends on 1)

The vision-aided inertial navigation system of claim 1, wherein the estimator classifies the features of the first set of features 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 constraints between the poses within the sliding window.

### Claim 3 (depends on 2)

The vision-aided inertial navigation system of claim 2, 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.

### Claim 4 (depends on 2)

The vision-aided inertial navigation system of claim 2, wherein, to classify, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features:
determines that an amount of available processing resources of the VINS has increased;
based on the determination that the amount of available processing resources of the VINS has increased, increases a number of the SLAM features and decreases a number of the MSCKF features.

### Claim 5 (depends on 2)

The vision-aided inertial navigation system of claim 2, wherein, to classify, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features:
determines that an amount of available processing resources of the VINS has decreased;
based on the determination that the amount of available processing resources of the VINS has decreased, decreases a number of the SLAM features and increases a number of the MSCKF features.

### Claim 6 (depends on 2)

The vision-aided inertial navigation system of claim 2, wherein the estimator further classifies the MSCKF features into a third set of features and a fourth set of features, and wherein computing state estimates in accordance with the selected features comprises:
computing state estimates and updated uncertainty data using the third set of features, and
computing state estimates for the fourth set of features without computing updated uncertainty data using the fourth set of features.

### Claim 7 (depends on 6)

The vision-aided inertial navigation system of claim 6, wherein the third set of data comprises Long Features and Old-Short-Features, and the fourth set of data comprises New-Short-Features and Hope Features.

### Claim 8 (depends on 1)

The vision-aided inertial navigation system of claim 1, wherein the estimator applies 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.

### Claim 9 (depends on 8)

The vision-aided inertial navigation system of claim 8, wherein, to apply the inverse sliding window filter, the estimator, for each update of the state estimates within the state vector:
computes 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
updates the state estimates based on the Hessian.

### Claim 10 (depends on 8)

The vision-aided inertial navigation system of claim 8, 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.

### Claim 11 (depends on 1)

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.

### Claim 12

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
computing state 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:
computing a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS;
classifying, for each of the poses and based on the budget, 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
computing state estimates in accordance with the selected features.

### Claim 13 (depends on 12)

The method of claim 12, wherein classifying each of the features observed at the respective pose into either the first set of features or the second set of features comprises classifying the features of the first set of the features as simultaneous localization and mapping (SLAM) features for computing the state estimates and the second set of the features as Multi-state Constraint Kalman Filter (MSCKF) features for computing constraints between the poses within the sliding window.

### Claim 14 (depends on 13)

The method of claim 13, further comprising:
setting a threshold number of SLAM features based on current resource utilization within the VINS; and
maintaining the state vector to have the threshold number of SLAM features and to exclude all of the MSCKF features.

### Claim 15 (depends on 13)

The method of claim 13, wherein classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features comprises:
determining that an amount of available processing resources of the VINS has increased;
based on the determination that the amount of available processing resources of the VINS has increased, increasing a number of the SLAM features and decreases a number of the MSCKF features.

### Claim 16 (depends on 13)

The method of claim 13, wherein classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features comprises:
determining that an amount of available processing resources of the VINS has decreased;
based on the determination that the amount of available processing resources of the VINS has increased, decreasing a number of the SLAM features and increases a number of the MSCKF features.

### Claim 17 (depends on 13)

The method of claim 13, further comprising classifying the MSCKF features into a third set of features and a fourth set of features, and wherein computing state estimates in accordance with the selected features comprises:
computing state estimates and uncertainty data for the third set of features, and
computing state estimates for the fourth set of features without computing uncertainty data for the fourth set of features.

### Claim 18 (depends on 13)

The method of claim 13, wherein the third set of data comprises Long Features and Old-Short-Features, and the fourth set of data comprises New-Short-Features and Hope Features.

### Claim 19 (depends on 12)

The method of claim 12, further comprising:
applying an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features; and
computing, in accordance with the constraints, the state estimates within the state vector for the sliding window.

### Claim 20 (depends on 19)

The method of claim 19, wherein applying the inverse sliding window filter comprises, 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
updating the state estimates based on the Hessian.

### Claim 21 (depends on 19)

The method of claim 19, 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.

### Claim 22 (depends on 12)

The method of claim 12, wherein the VINS is integrated within a tablet computer, a laptop computer, a mobile phone, a robot, or a vehicle.