# US 11,994,392 B2 · Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system

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

- Number: US 11,994,392 B2
- Document ID: US11994392B2
- Status: Granted
- Kind: B2
- Application number: 18/045,406
- Filed: 2022-10-10
- Published: 2023-06-22
- Granted: 2024-05-28
- Earliest claimed: 2016-07-22
- 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/US11994392B2

## CPC

- G01C 21/1656: {with passive imaging devices, e.g. cameras}
- G01S 5/16: using electromagnetic waves other than radio waves
- G06F 17/16: Matrix or vector computation {, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization (matrix transposition G06F7/78)}
- G06T 7/277: involving stochastic approaches, e.g. using Kalman filters
- G06T 7/70: Determining position or orientation of objects or cameras (camera calibration G06T7/80)
- 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)
- G01S: RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G06F: ELECTRIC DIGITAL DATA PROCESSING (computer systems based on specific computational models G06N)
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL

## Abstract

A vision-aided inertial navigation system (VINS) implements a square-root multi-state constraint Kalman filter (SR-MSCKF) for navigation. In one example, a processor of a VINS receives image data and motion data for a plurality of poses of a frame of reference along a trajectory. The processor executes an Extended Kalman Filter (EKF)-based estimator to compute estimates for a position and orientation for each of the plurality of poses of the frame of reference along the trajectory. For features observed from multiple poses along the trajectory, the estimator computes constraints that geometrically relate the multiple poses of the respective feature. Using the motion data and the computed constraints, the estimator computes state estimates for the position and orientation of the frame of reference. Further, the estimator determines uncertainty data for the state estimates and maintains the uncertainty data as a square root factor of a covariance matrix.

## Claims

### Claim 1

A system for real-time vision aided inertial navigation, comprising:
a camera configured to capture image data for a plurality of poses of a frame of reference along a trajectory in an environment over a period of time;
an inertial measurement unit (IMU) capable of generating IMU measurements for the frame of reference over the period of time;
and
a set of one or more processors capable of receiving data from the camera and the IMU, wherein the set of one or more processors is also capable of performing steps including:
receiving the image data from the camera and the IMU measurements from the IMU; and
computing, using an estimator, a state vector corresponding to a plurality of poses of the frame of reference along the trajectory, wherein, to compute the state vector, the estimator is configured to:
for features observed at one or more poses, of the plurality of poses along the trajectory, computing one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed; and
updating the state vector, wherein:
the state vector comprises:
a sliding window of predicted state estimates for the system at each of the plurality of poses along the trajectory, and
uncertainty data corresponding to the predicted state estimates, wherein the uncertainty data comprises uncertainty estimates, corresponding to each of the predicted state estimates, and maintained as a Cholesky factor of a covariance matrix; and
updating the state vector comprises updating:
the predicted state estimates according to the IMU measurements, the image data, and the one or more constraints;
the Cholesky factor of the covariance matrix according to the updated predicted state estimates; and
the state vector based, at least in part, on the updated Cholesky factor of the covariance matrix.

### Claim 2 (depends on 1)

The system of claim 1, wherein the estimator maintains the Cholesky factor using a square root form of a Multi-State Constraint Kalman Filter.

### Claim 3 (depends on 1)

The system of claim 1, wherein computing the one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed comprises:
determining feature measurements, from the image data for the plurality of poses of the frame of reference that correspond to the features observed from the one or more poses;
grouping the feature measurements according to the features observed from the one or more poses; and
computing the one or more constraints based on the feature measurements.

### Claim 4 (depends on 1)

The system of claim 1, wherein:
the IMU measurements comprise measurements of angular velocity and measurements of linear acceleration; and
the predicted state estimate for a given pose comprises a position estimate and an orientation estimate.

### Claim 5 (depends on 1)

The system of claim 1, wherein the estimator excludes, from the state vector, predicted state estimates for positions within the environment including the features that were observed from the one or more poses and for which the one or more constraints were computed.

### Claim 6 (depends on 1)

The system of claim 1, wherein the system is integrated within a device selected from the group consisting of a computing device, a tablet computer, a mobile device, a mobile phone, and a robot.

### Claim 7 (depends on 1)

The system of claim 1, wherein the steps further comprise outputting, based on the predicted state estimates, at least one of:
a map of the environment; and
program instructions for navigating the environment.

### Claim 8

A method for operating a real-time vision-aided inertial navigation system (VINS), comprising:
receiving image data for a plurality of poses of a frame of reference along a trajectory in an environment over a period of time;
receiving IMU measurements for the frame of reference over the period of time; and
computing a state vector corresponding to a plurality of poses of the frame of reference along the trajectory, wherein, computing the state vector comprises:
for features observed at one or more poses, of the plurality of poses along the trajectory, computing one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed; and
updating the state vector, wherein:
the state vector comprises:
a sliding window of predicted state estimates for the VINS at each of the plurality of poses along the trajectory, and
uncertainty data corresponding to the predicted state estimates, wherein the uncertainty data comprises uncertainty estimates, corresponding to each of the predicted state estimates, and maintained as a Cholesky factor of a covariance matrix; and
updating the state vector comprises updating:
the predicted state estimates according to the IMU measurements, the image data, and the one or more constraints;
the Cholesky factor of the covariance matrix according to the updated predicted state estimates; and
the state vector based, at least in part, on the updated Cholesky factor of the covariance matrix.

### Claim 9 (depends on 8)

The method of claim 8, wherein the Cholesky factor of the covariance matrix is maintained using a square root form of a Multi-State Constraint Kalman Filter.

### Claim 10 (depends on 9)

The method of claim 9, wherein computing the one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed comprises:
determining feature measurements, from the image data for the plurality of poses of the frame of reference that correspond to the features observed from the one or more poses;
grouping the feature measurements according to the features observed from the one or more poses; and
computing the one or more constraints based on the feature measurements.

### Claim 11 (depends on 8)

The method of claim 8, wherein:
the IMU measurements comprise measurements of angular velocity and measurements of linear acceleration; and
the predicted state estimate for a given pose comprises a position estimate and an orientation estimate.

### Claim 12 (depends on 8)

The method of claim 8, wherein state estimates for positions within the environment are excluded from the state vector when they include the features:
that were observed from the one or more poses; and
for which the one or more constraints were computed.

### Claim 13 (depends on 8)

The method of claim 8, further comprising outputting, based on the predicted state estimates, at least one of:
a map of the environment; and
program instructions for navigating the environment.

### Claim 14

A non-transitory, computer-readable medium comprising instructions that, when executed, are configured to cause a processor of a vision-aided inertial navigation system (VINS) to:
receive image data for a plurality of poses of a frame of reference along a trajectory in an environment over a period of time;
receive IMU measurements for the frame of reference over the period of time; and
compute a state vector corresponding to a plurality of poses of the frame of reference along the trajectory, wherein, computing the state vector comprises:
for features observed at one or more poses, of the plurality of poses along the trajectory, compute one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed; and
update the state vector, wherein:
the state vector comprises:
a sliding window of predicted state estimates for the VINS at each of the plurality of poses along the trajectory, and
uncertainty data corresponding to the predicted state estimates, wherein the uncertainty data comprises uncertainty estimates, corresponding to each of the predicted state estimates, and maintained as a Cholesky factor of a covariance matrix; and
updating the state vector comprises updating:
the predicted state estimates according to the IMU measurements, the image data, and the one or more constraints;
the Cholesky factor of the covariance matrix according to the updated predicted state estimates; and
the state vector based, at least in part, on the updated Cholesky factor of the covariance matrix.

### Claim 15 (depends on 14)

The non-transitory, computer-readable medium of claim 14, wherein the processor maintains the Cholesky factor of the covariance matrix using a square root form of a Multi-State Constraint Kalman Filter.

### Claim 16 (depends on 15)

The non-transitory, computer-readable medium of claim 15, wherein computing the one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed comprises:
determining feature measurements, from the image data for the plurality of poses of the frame of reference that correspond to the features observed from the one or more poses;
grouping the feature measurements according to the features observed from the one or more poses; and
computing the one or more constraints based on the feature measurements.

### Claim 17 (depends on 14)

The non-transitory, computer-readable medium of claim 14, wherein:
the IMU measurements comprise measurements of angular velocity and measurements of linear acceleration; and
the predicted state estimate for a given pose comprises a position estimate and an orientation estimate.

### Claim 18 (depends on 14)

The non-transitory, computer-readable medium of claim 14, wherein state estimates for positions within the environment are excluded from the state vector when they include the features:
that were observed from the one or more poses; and
for which the one or more constraints were computed.

### Claim 19 (depends on 14)

The non-transitory, computer-readable medium of claim 14, further comprising outputting, based on the predicted state estimates, at least one of:
a map of the environment; and
program instructions for navigating the environment.