Localized and Incremental Probabilistic Inference for Large-Scale Networked Dynamical Systems

Abstract

In this article, we present new algorithms for distributed factor graph optimization (DFGO) problems that arise in the probabilistic inference of large-scale networked robotic systems for both batch and real-time problems. First, for the batch DFGO problem, we derive a type of the alternating direction method of multipliers (ADMM) algorithm called the local consensus ADMM (LC-ADMM). The LC-ADMM is fully localized; therefore, the computational effort, communication bandwidth, and memory for each agent scale like <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$o(1)$</tex-math></inline-formula> with respect to the <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">network size</i> . We establish two new theoretical results for the LC-ADMM: 1) exponential convergence when the objective is strongly convex and has a Lipschitz continuous subdifferential and 2) <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$o(1/k)$</tex-math></inline-formula> convergence when the objective is convex and has a unique solution. We also show that the LC-ADMM allows the use of nonquadratic loss functions, such as <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\ell _{1}$</tex-math></inline-formula> -norm and Huber loss. Second, we also develop the incremental DFGO (iDFGO) algorithm for real-time problems by combining the ideas from the LC-ADMM and the Bayes tree. To derive a time-scalable algorithm, we exploit the temporal sparsity of the real-time factor graph and the convergence of the augmented factors of the LC-ADMM. The iDFGO algorithm incrementally recomputes estimates when new factors are added to the graph and is scalable with respect to both network size and time. We validate the LC-ADMM and iDFGO in simulations with examples from multiagent simultaneous localization and mapping and power grids.