No belief propagation required: Belief space planning in high-dimensional state spaces via factor graphs, the matrix determinant lemma, and re-use of calculation

Abstract

We develop a computationally efficient approach for evaluating the information-theoretic term within belief space planning (BSP), where during belief propagation the state vector can be constant or augmented. We consider both unfocused and focused problem settings, whereas uncertainty reduction of the entire system or only of chosen variables is of interest, respectively. State-of-the-art approaches typically propagate the belief state, for each candidate action, through calculation of the posterior information (or covariance) matrix and subsequently compute its determinant (required for entropy). In contrast, our approach reduces runtime complexity by avoiding these calculations. We formulate the problem in terms of factor graphs and show that belief propagation is not needed, requiring instead a one-time calculation that depends on (the increasing with time) state dimensionality, and per-candidate calculations that are independent of the latter. To that end, we develop an augmented version of the matrix determinant lemma, and show that computations can be re-used when evaluating impact of different candidate actions. These two key ingredients and the factor graph representation of the problem result in a computationally efficient (augmented) BSP approach that accounts for different sources of uncertainty and can be used with various sensing modalities. We examine the unfocused and focused instances of our approach, and compare it with the state of the art, in simulation and using real-world data, considering problems such as autonomous navigation in unknown environments, measurement selection and sensor deployment. We show that our approach significantly reduces running time without any compromise in performance.

Fast Action Elimination for Efficient Decision Making and Belief Space Planning Using Bounded Approximations

“…Approaches that focus on autnomous navigation problems, such as active SLAM, have been also widely examined (e.g. Stachniss et al 2004;Bryson and Sukkarieh 2008;Du et al 2011;Kim and Eustice 2014;Chaves and Eustice 2016;Kopitkov and Indelman 2017).…”

General-purpose incremental covariance update and efficient belief space planning via a factor-graph propagation action tree

“…Recently, we presented a novel approach, rAMDL (Kopitkov and Indelman, 2017), to efficiently calculate entropy and IG for both and cases. This method requires only one-time calculation that depends on dimension n : computation of specific prior marginal (or conditional) covariances.…”

“…This, in turn, can be used to efficiently evaluate the information term of different candidate actions while re-using calculations when possible. Combining our recently-developed rAMDL approach (Kopitkov and Indelman, 2017) with factor-graph propagation (FGP) action tree and incremental covariance update, yields an approach that calculates action impact without explicitly performing inference over the posterior belief, while re-using calculations among different candidate actions.…”

“…The utility function in belief space planning (BSP) typically involves an information-theoretic term, which quantifies the posterior uncertainty of various variables within a future belief. This, in turn, requires access to the marginal covariance (information) matrix of appropriate variables whose uncertainty we wish to account for (Kopitkov and Indelman, 2017). Similarly, covariance of specific variables is also required in the inference phase, for example, in the context of data association (Kaess and Dellaert, 2009).…”

“…To summarize, our main contributions in this article are as follows: (a) we develop an incremental covariance update method to calculate specific covariance entries after any change in the inference problem; (b) we introduce the FGP action tree, that represents all candidate actions in a single hierarchical model and allows us to mutually formulate common parts of candidate actions; (c) we apply an incremental covariance update method to calculate covariance entries from intermediate and posterior beliefs within the FGP action tree, where all calculations that depend on state dimension n are accumulated into a single computational block which is performed only once for all actions, with further per-action computations independent of n ; and (d) we combine the FGP action tree graphical model, the incremental covariance update method. and rAMDL approach (Kopitkov and Indelman, 2017) to yield a new algorithm, rAMDL-Tree , that efficiently solves an information-theoretic BSP problem while handling candidates’ mutual parts only once.…”