MINIMAL STATE AUGMENTATION ALGORITHM FOR STEREO-VISION-BASED NAVIGATION

A method to perform vision based navigation on a moving vehicle. An image processor receives reference images with associated depth information as base frames or search frames. 2D features are detected in base frames and tracked in subsequent search frames. A new base frame is triggered when a number of features or a spatial distribution fall under thresholds. A depth processor reconstructs the depth and 3D position of each of the 2D features. A track manager manages a feature list database mapping the 2D coordinates of each tracked 2D feature with the 3D coordinate of that feature. A state manager constructs a filter state vector with 15 error states propagated at an IMU rate and 6 additional error states corresponding to clones of pose states. A visual updater utilizes the tracked 2D feature coordinates to update the filter state vector. A filter forms residuals and corrects inertial error drift.