Real-time pose estimation system using inertial and feature measurements

Abstract

A hybrid estimator system using visual and inertial sensors for real-time pose tracking on devices with limited processing power using at least one processor, a memory, a storage and communications through a protocol and one or more than one software module for a hybrid estimator, real-time algorithm selection to process different measurements, statistical learning for these characteristics to compute the expected device computing cost of any strategy for allocating measurements to algorithms, and algorithm selection based on the statistical learning module.

What is claimed is: 
     
         1 . A hybrid estimator system using sensors for real-time pose tracking of moving platforms, the system comprising:
 a) a device comprising at least one processor, a memory, a storage and communications through a protocol; and   b) one or more than one module, communicatively coupled to each other, comprising code executable on the processor for
 1) a hybrid estimator, comprising a plurality of algorithms for processing measurements; and 
 2) selecting the constituent algorithms of the hybrid estimator used to process each measurement. 
   
     
     
         2 . The system of  claim 1 , where a module comprising non-statutory instructions for learning the statistical characteristics of the measurements is used. 
     
     
         3 . The system of  claim 1 , where the constituent algorithm selection is based on the learning module. 
     
     
         4 . The system of  claim 1 , where the system gathers information to compute the expected cost of strategies for processing measurements by different algorithms to reduce the computational requirements. 
     
     
         5 . The system of  claim 1 , where the system solves, in real time, an optimization problem with an objective function representing the expected computation time to identify the preferred strategy. 
     
     
         6 . The system of  claim 1 , where the hybrid estimator estimates a moving platform's trajectory using inertial measurements and the observations of features by one or more than one sensors. 
     
     
         7 . The system of  claim 1 , where the hybrid estimator estimates a moving platform's trajectory using inertial measurements and the observations of features by a visual sensor. 
     
     
         8 . The system of  claim 1 , where the system comprises non-transitory instructions to process each of the available feature measurements. 
     
     
         9 . The system of  claim 8 , where the feature measurements can be processed by including a description of the features as variables to be estimated in the hybrid estimator. 
     
     
         10 . The system of  claim 8 , where the feature measurements can be processed by using obtained observations in order to derive constraints, directly used for updating the pose estimates of the moving object. 
     
     
         11 . The system of  claim 1 , where the system comprises non-transitory instructions to improve the accuracy of the pose estimates, reduce the computational requirements of the estimator or both improve the accuracy of the pose estimates and reduce the computational requirements of the estimator during system operation to reduce the processing requirements. 
     
     
         12 . The system of  claim 1 , where the hybrid estimator comprises non-transitory instructions to determine which of the plurality of methods is to be used for processing each of the feature measurements to reduce the computational requirements of the estimator and to adapt the system to the characteristics of the environment, the trajectory of the moving object, and the availability of processing resources. 
     
     
         13 . The system of  claim 1 , where the hybrid estimator further comprises instructions for a module that determines the number of features that should be extracted from raw sensor data in order to adjust to the availability of computational resources. 
     
     
         14 . The system of  claim 1 , where the hybrid estimator comprises non-transitory instructions to construct linearized approximations of the nonlinear mathematical models that describe the motion and the sensor measurements in order to compute a description of the uncertainty of the estimates. 
     
     
         15 . The system of  claim 14 , where the linearization points are selected to preserve the system's observability properties. 
     
     
         16 . The system of  claim 15 , where a unique estimate of certain states is used in order to compute the linearization matrices for all measurement equations that involve each of the states. 
     
     
         17 . The system of  claim 15  where the estimates of one or more than one state is used to compute the linearization matrices and can be modified by equal amounts, to reduce linearization inaccuracy while preserving the system's observability properties. 
     
     
         18 . The system of  claim 1 , where the hybrid estimator comprises non-transitory instructions to implement a hybrid extended Kalman filter. 
     
     
         19 . The system of  claim 1 , where the hybrid extended Kalman filter comprises an extended-Kalman filter algorithm that includes feature states in the state vector, and a sliding-window extended-Kalman-filter algorithm that includes states of the mobile platform in the state vector. 
     
     
         20 . The system of  claim 19 , where the size of the sliding window is selected to reduce the computational cost of the hybrid extended Kalman filter. 
     
     
         21 . The system of  claim 1 , where the hybrid estimator module determines the choice of algorithm to process each individual feature depending on the distribution of the feature track lengths of features. 
     
     
         22 . The system of  claim 21 , where the hybrid estimator module determines the optimal strategy for processing the feature measurements by solving a one-variable optimization problem using the information above. 
     
     
         23 . The system of  claim 1 , where the hybrid estimator module processes all available measurements without loss of localization information. 
     
     
         24 . The system of  claim 1 , where the plurality of algorithms must have bounded computational complexity, irrespective of the duration of the trajectory. 
     
     
         25 . The system of  claim 24 , where the plurality of algorithms employ an extended Kalman filter. 
     
     
         26 . The system of  1 , where the hybrid estimator includes an extended Kalman filter-sliding-window iterative minimization algorithm, where the state vector contains a current IMU state as well as representations of the feature positions. 
     
     
         27 . The system of  claim 26 , where the features that leave the field of view are removed from the state vector leaving only the currently observed ones, to keep the computations bounded. 
     
     
         28 . The system of  claim 26 , where the hybrid estimator comprises other extended-Kalman-filter algorithms that maintain a sliding window of camera poses in the state vector, and use the feature observations to apply probabilistic constraints between these poses. 
     
     
         29 . The system of  claim 26 , where the hybrid estimator comprises a multistate-constraint Kalman filter. 
     
     
         30 . The system of  claim 26 , where the algorithm used to process feature measurements is selected to have the lowest computational cost. 
     
     
         31 . The system of  claim 26 , where the hybrid filter is a combination of both the extended Kalman filter-sliding-window iterative minimization and the multistate-constraint Kalman filter algorithms. 
     
     
         32 . The system of  claim 26 , where the hybrid filter is a filter whose state vector contains the current IMU state, m camera poses, and s k  features, and determines whether a feature will be processed using the multistate-constraint Kalman filter algorithm, or whether it will be included in the state vector and processed using the extended Kalman filter-sliding-window iterative minimization algorithm. 
     
     
         33 . A method for using a hybrid estimator using visual and inertial sensors for real-time pose tracking on devices with limited processing power, the method comprising the steps of:
 a) determining a method to be used by the system for processing each of the feature measurements;   b) adapting the system to the characteristics of the environment, the trajectory of the moving object, and the availability of processing resources;   c) determining the number of features to be extracted from raw sensor data in order to adjust to the availability of computational resources; and   d) constructing linearized approximations of the nonlinear mathematical models that describe the motion and the sensor measurements.   
     
     
         34 . A method for making a hybrid estimator using visual and inertial sensors for real-time pose tracking on devices with limited processing power, the method comprising the steps of:
 a) providing a device comprising at least one processor, a memory, a storage and communications through a protocol; and   b) providing one or more than one software module, communicatively coupled to each other, comprising code executable on the processor for:
 1) a hybrid estimator; 
 2) real-time algorithm selection to process different measurements; 
 3) statistical learning for these characteristics to compute the expected device computing cost of any strategy for allocating measurements to algorithms; and 
 4) algorithm selection based on the statistical learning module. 
   
     
     
         35 . The method of  claim 34  further comprising the steps of:
 a) propagating the state vector and covariance matrix using the IMU readings; 
 b) determining when camera measurements and features are available; 
 c) augmenting the state vector with the latest camera pose; 
 d) determine if the features are to be processed using an multistate-constraint Kalman filter algorithm; where if the features are to be processed the perform the following steps:
 1) calculating a residual and Jacobian matrix for each feature to be processed; 
 2) performing a Mahalanobis gating test; and 
 3) forming a residual vector and a Jacobian matrix using all features that passed the gating test; 
 
 e) computing residuals and measurement Jacobian matrices, and form the residual {tilde over (z)} k  and Jacobian matrix H k  for features that are included in the state vector; 
 f) updating the state vector and covariance matrix using the residual {tilde over (z)} k  and Jacobian matrix H k ; and 
 g) initialize features tracked in all images of the sliding window; 
 h) updating state management by performing the following steps:
 1) removing sliding-window iterative minimization features that are no longer tracked; and 
 2) changing the anchor pose for sliding-window iterative minimization features anchored at the oldest pose; and 
 
 i) removing oldest camera poses from the state vector. 
 
     
     
         36 . The method of  claim 35  further comprising the steps of:
 a) analyzing the computations needed for the hybrid estimator; and 
 b) calculating the number of floating-point operations per update of the hybrid algorithm.