(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.
(Georgia Institute of Technology, 2012-07)
Indelman, Vadim; Williams, Stephen; Kaess, Michael; Dellaert, Frank
This paper describes a new approach for information
fusion in inertial navigation systems. In contrast to the
commonly used filtering techniques, the proposed approach is
based on a non-linear optimization for processing incoming
measurements from the inertial measurement unit (IMU) and
any other available sensors into a navigation solution. A factor
graph formulation is introduced that allows multi-rate, asynchronous,
and possibly delayed measurements to be incorporated
in a natural way. This method, based on a recently developed
incremental smoother, automatically determines the number of
states to recompute at each step, effectively acting as an adaptive
fixed-lag smoother. This yields an efficient and general framework
for information fusion, providing nearly-optimal state estimates.
In particular, incoming IMU measurements can be processed in
real time regardless to the size of the graph. The proposed method
is demonstrated in a simulated environment using IMU, GPS
and stereo vision measurements and compared to the optimal
solution obtained by a full non-linear batch optimization and to
a conventional extended Kalman filter (EKF).