Patent ID: 9031809
Filing Date: 2015-05-12
Classification: G01C,G01S,G06F,G06K,G06T

Abstract:
1. Apparatus for providing three-dimensional navigation for a node comprising: an inertial measurement unit (IMU), coupled to the node, for providing gyroscope, acceleration and velocity information, collectively IMU information; a ranging unit, coupled to the node, for providing distance information relative to at least one reference node; at least one visual sensor, coupled to the node, for providing images of an environment surrounding the node wherein features are tracked in the images in adjacent frames and combined with the IMU information to produce relative pose measurements; a landmark matcher for generating an error state derived from comparing at least one image to a visual landmark within the environment; a preprocessor, coupled to the inertial measurement unit, the ranging unit, the at least one visual sensor and the landmark matcher for fusing the relative pose measurements, the error state from the landmark matcher, the distance information and the IMU information to generate an error state comprising local and global information; and an error-state predictive filter, coupled to the preprocessor, for processing the error state, which comprises local and global information to produce a continually updated three-dimensional pose of the node, wherein the error-state predictive filter localizes the node by estimating frame to frame information, estimating ranging information and estimating IMU information and integrating the estimates over time when the node fails to localize via the landmark matcher.