(Georgia Institute of Technology, 2012-07)
Kaess, Michael; Williams, Stephen; Indelman, Vadim; Roberts, Richard; Leonard, John J.; Dellaert, Frank
This paper presents a novel algorithm for integrating
real-time filtering of navigation data with full map/trajectory
smoothing. Unlike conventional mapping strategies, the result of
loop closures within the smoother serve to correct the real-time
navigation solution in addition to the map. This solution views
filtering and smoothing as different operations applied within a
single graphical model known as a Bayes tree. By maintaining all
information within a single graph, the optimal linear estimate is
guaranteed, while still allowing the filter and smoother to operate
asynchronously. This approach has been applied to simulated
aerial vehicle sensors consisting of a high-speed IMU and stereo
camera. Loop closures are extracted from the vision system in
an external process and incorporated into the smoother when
discovered. The performance of the proposed method is shown
to approach that of full batch optimization while maintaining
real-time operation.