Vehicle with visual simultaneous localization and mapping system and dynamic outlier rejection functionality

A method for controlling a mobile host having a Visual Simultaneous Localization and Mapping (V-SLAM) system, the method comprising:

• receiving raw input data from a sensor suite of the mobile host;
• determining, via a processor of the V-SLAM system, initial estimates of parameters of the mobile host indicative of a corresponding position of features in a surrounding environment of the mobile host;
• using the input data to estimate poses of the mobile host and three dimensional (3D) coordinates of a plurality of feature map points based on the poses of the mobile host;
• identifying outliers in the feature map points using condition numbers as a predetermined linear algebra property, wherein identifying the outliers includes comparing the feature map points to a plurality of condition number thresholds;
• dynamically filtering out the outliers, using dynamic covariance scaling, to generate a filtered feature map point set;
• and
• transmitting the filtered feature map point set to a navigation system of the mobile host to control a setting thereof.

A vehicle comprising:

• a vehicle body;
• a set of road wheels connected to the vehicle body;
• a navigation system;
• and
• a Visual Simultaneous Localization and Mapping (V-SLAM) system, comprising:
• a sensor suite connected to the vehicle body and operable for sensing and outputting raw input data, the sensor suite including a global positioning system (GPS) receiver, one or more cameras, and an inertial measurement unit (IMU) respectively configured to output, as the raw input data, (i) GPS position data, (ii) multi-frame image data, and (iii) IMU data indicative of a pose of the vehicle;
• and
• a controller in communication with the sensor suite and configured to:
• receive the raw input data from the sensor suite;
• use the raw input data to estimate poses of the vehicle and three dimensional (3D) coordinates of a plurality of feature map points in the multi-frame image data;
• identify outliers in the feature map points using condition numbers as a predetermined linear algebra property;
• dynamically filter out the outliers, via a dynamic covariance scaling process, to generate a filtered feature map point set, including identifying the outliers via comparison of the condition numbers to a plurality of condition number thresholds;
• and
• transmit the filtered feature map point set to the navigation system.