(Georgia Institute of Technology, 2014)
Williams, Stephen; Indelman, Vadim; Kaess, Michael; Roberts, Richard; Leonard, John J.; Dellaert, Frank
We present a parallelized navigation architecture that is capable of
running in real-time and incorporating long-term loop closure constraints
while producing the optimal Bayesian solution. This architecture splits
the inference problem into a low-latency update that incorporates new
measurements using just the most recent states (filter), and a high-latency
update that is capable of closing long loops and smooths using all past
states (smoother). This architecture employs the probabilistic graphical
models of Factor Graphs, which allows the low-latency inference and high-latency inference to be viewed as sub-operations of a single optimization performed within a single graphical model. A specific factorization of the
full joint density is employed that allows the different inference operations
to be performed asynchronously while still recovering the optimal solution produced by a full batch optimization. Due to the real-time, asynchronous
nature of this algorithm, updates to the state estimates from the high-latency smoother will naturally be delayed until the smoother calculations
have completed. This architecture has been tested within a simulated
aerial environment and on real data collected from an autonomous ground
vehicle. In all cases, the concurrent architecture is shown to recover the
full batch solution, even while updated state estimates are produced in
real-time.
(Georgia Institute of Technology, 2013-08)
Indelman, Vadim; Williams, Stephen; Kaess, Michael; Dellaert, Frank
This paper presents a new approach for high-rate information fusion in modern
inertial navigation systems, that have a variety of sensors operating at different
frequencies. Optimal information fusion corresponds to calculating the maximum a posteriori estimate over the joint probability distribution function (pdf) of all states, a computationally-expensive process in the general case. Our approach consists of two key components, which yields a flexible, high-rate,
near-optimal inertial navigation system. First, the joint pdf is represented using
a graphical model, the factor graph, that fully exploits the system sparsity and provides a plug and play capability that easily accommodates the addition and removal of measurement sources. Second, an efficient incremental inference
algorithm over the factor graph is applied, whose performance approaches
the solution that would be obtained by a computationally-expensive batch optimization
at a fraction of the computational cost. To further aid high-rate
performance, we introduce an equivalent IMU factor based on a recently developed
technique for IMU pre-integration, drastically reducing the number of
states that must be added to the system. The proposed approach is experimentally
validated using real IMU and imagery data that was recorded by a ground
vehicle, and a statistical performance study is conducted in a simulated aerial scenario. A comparison to conventional fixed-lag smoothing demonstrates that our method provides a considerably improved trade-off between computational complexity and performance.