Method for processing feature measurements in vision-aided inertial navigation

• US 10,247,556 B2
• Filed: 07/23/2014
• Issued: 04/02/2019
• Est. Priority Date: 07/23/2013
• Status: Expired due to Fees

• Current Assignee: Regents of the University of California (University of California)
• Original Assignee: Regents of the University of California (University of California)
• Inventors: Mourikis, Anastasios
• Primary Examiner(s): Patel, Shardul D
• Application Number:
  US14/907,210
• Publication Number:
  US 20160161260A1
• Time in Patent Office: 1,714 Days
• Field of Search: 701468, 701500
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G01C 21/188 for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
• CPC Class Codes: G01S 19/13 Receivers

Abstract

A mobile platform having a camera for outputting image data of features with unknown locations, an inertial measurement unit for outputting inertial measurements of the features, where the coordinates of the features are unknown, a processor, storage and an extended-Kalman filter-based estimator executable on the processor for processing the inertial measurement and features of the image data, where a state vector of the estimator contains a sliding window of states for determining the position and orientation of the mobile platform.

Claims

1. A mobile platform comprising:

a) a camera for outputting image data of features with unknown locations, the features being objects, structures, and points, lines, and planes of objects and/or structures;

b) an inertial measurement unit for outputting inertial measurements of the features, wherein the coordinates of the features are unknown;

c) a processor configured to receive the image data and inertial measurements;

d) storage for storing the image data and the inertial measurements; and

e) an extended-Kalman filter-based estimator executable on the processor for processing the inertial measurement and features of the image data, wherein a state vector of the estimator contains a sliding window of states for determining the position and orientation of the mobile platform the state vector updated by computing and factorizing a rank deficient information matrix representing Fisher information provided by the feature measurements, the computed factor being used as the measurement Jacobian for updating the state vector.

2. The platform of claim 1, wherein the camera is selected from the group consisting of a video camera, a photographic camera, a depth-sensing camera, a time-of-flight depth sensor camera, a structured-light depth-inferring camera and an infrared camera.

3. The platform of claim 2 wherein the camera includes a GPS to provide coordinates of the mobile platform, and wherein the estimator receives the coordinates for processing.

4. The platform of claim 1 further comprising a GPS to provide coordinates of the mobile platform, and wherein the estimator receives the coordinates for processing.

5. The platform of claim 1, wherein the inertial measurement unit comprises one or more gyroscopes, accelerometers, magnetometers, and barometers.

6. A method for tracking the position and orientation of a mobile platform, the method comprising the steps of:

a) receiving at least one image of features from a camera mounted on the mobile platform, the features being objects, structures, and points, lines, and planes of objects and/or structures, wherein the features positions are not known a priori;

b) receiving from an inertial measurement unit mounted on the platform inertial measurements of the features, wherein the coordinates of the features are unknown;

c) processing the at least one image using the feature state information and the inertial measurements using an extended-Kalman filter-based estimator to determine the value of at least one of position, orientation, and velocity and of the mobile platform; and

d) storing the determined value for comparison with new state information; and

(e) associating each sliding window of state vectors to a time instant recording of an image, sensor measurements, or both an image and sensor measurements, the state vector updated by computing and factorizing a rank deficient information matrix representing Fisher information provided by the feature measurements, the computed factor being used as the measurement Jacobian for updating the state vector.

7. The method of claim 6, wherein the sliding window state vectors comprise a current inertial measurement unit state, inertial measurement unit biases, inertial measurement unit scale factors, states of a number of feature points, camera intrinsic parameters, and camera-to-inertial measurement unit extrinsic parameters.

8. The method of claim 6 further comprising the steps of:

storing in storage an estimate of the sliding window of state vectors; and

defining a representation of uncertainty in the estimate, wherein the uncertainty is represented by a matrix of the covariance of the errors in the estimates.

9. The method of claim 8, wherein the sliding window of state vectors comprises platform states corresponding to the time instants that a number of inertial measurements were recorded.

10. The method of claim 9, wherein the platform states are selected from the group consisting of camera, inertial measurement unit positions, orientations, velocities and rotational velocities.

11. The method of claim 9, wherein the measurements comprise images, feature observations or both images and feature observations.

12. The method of claim 11, wherein the extended-Kalman filter-based estimator processes multiple measurements of each feature.

13. The method of claim 12, wherein the measurements of a feature are used to obtain an estimate for the feature'"'"'s location.

14. The method of claim 8 further comprising the steps of:

a) calculating the estimate using an error-minimization method;

b) computing residuals, where the residuals describe the difference between the stored measurements and the measurements predicted by the extended-Kalman filter-based estimator based on the estimates of the stored states;

c) calculating Jacobian matrices of the measurements;

d) calculating a matrix that represents the information from feature measurements for estimating the platform states using residuals and Jacobian matrices; and

e) processing feature measurements without explicitly including estimates of the feature states in the EKF state vector.

15. The method of claim 14, wherein calculating the Jacobian matrices is performed by differentiation, numerically or both differentiation and numerically.

16. The method of claim 15, further comprising the step of premultiplying the Jacobian matrices by a quantity that is an inverse square root of the measurement covariance matrix.

17. The method of claim 16 further comprising the step of processing feature measurements without explicitly including estimates of the feature states in the extended-Kalman filter-based estimator state vector.

18. The method of claim 14 further comprising the steps of:

a) computing a factorization of the matrix;

b) updating the estimator using the computed factor matrix as the measurement Jacobian; and

c) computing an update the estimator'"'"'s state estimate and the representation of its uncertainty using the factor matrix as a correction to the EKF state vector.

19. The method of claim 18, wherein computing the factorization of the matrix is computed incrementally by incorporating each of the features sequentially.