INVERSE SLIDING-WINDOW FILTERS FOR VISION-AIDED INERTIAL NAVIGATION SYSTEMS

摘要

This disclosure describes inverse filtering and square root inverse filtering techniques for optimizing the performance of a vision-aided inertial navigation system (VINS). In one example, instead of keeping all features in the system's state vector as SLAM features, which can be inefficient when the number of features per frame is large or their track length is short, an estimator of the VINS may classify the features into either SLAM or MSCKF features. The SLAM features are used for SLAM-based state estimation, while the MSCKF features are used to further constrain the poses in the sliding window. In one example, a square root inverse sliding window filter (SQRT-ISWF) is used for state estimation.

主权项

1. A vision-aided inertial navigation system comprising:
at least one image source to produce image data along a trajectory of the vision-aided inertial navigation system (VINS) within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the VINS along the trajectory; an inertial measurement unit (IMU) to produce IMU data indicative of motion of the vision-aided inertial navigation system; and a hardware-based processing unit comprising an estimator that determines, based on the image data and the IMU data, estimates for at least a position and orientation of the vision-aided inertial navigation system for a plurality of poses of the VINS along the trajectory, wherein the estimator determines the estimates by:
classifying, for each of the poses, each of the features observed at the respective pose into either a first set of the features or a second set of the features,maintaining a state vector having states for a position and orientation of the VINS and for positions with the environment for the first set of features for a sliding widow of two or more of the most recent poses along the trajectory without maintaining states for positions of the second set of features within the state vector, andapplying an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features and compute, in accordance with the constraints, the state estimates within the state vector for the sliding window.