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

Abstract

Fast covariance calculation is required both for SLAM (e.g. in order to solve data association) and for evaluating the information-theoretic term for different candidate actions in belief space planning (BSP). In this paper we make two primary contributions. First, we develop a novel general-purpose incremental covariance update technique, which efficiently recovers specific covariance entries after any change in the inference problem, such as introduction of new observations/variables or re-linearization of the state vector. Our approach is shown to recover them faster than other state-of-the-art methods. Second, we present a computationally efficient approach for BSP in high-dimensional state spaces, leveraging our incremental covariance update method. State of the art BSP approaches perform belief propagation for each candidate action and then evaluate an objective function that typically includes an information-theoretic term, such as entropy or information gain. Yet, candidate actions often have similar parts (e.g. common trajectory parts), which are however evaluated separately for each candidate. Moreover, calculating the information-theoretic term involves a costly determinant computation of the entire information (covariance) matrix which is O(n 3 ) with n being dimension of the state or costly Schur complement operations if only marginal posterior covariance of certain variables is of interest. Our approach, rAMDL-Tree, extends our previous BSP method rAMDL (Kopitkov and Indelman, 2017), by exploiting incremental covariance calculation and performing calculation re-use between common parts of non-myopic candidate actions, such that these parts are evaluated only once, in contrast to existing approaches. To that end, we represent all candidate actions together in a single unified graphical model, which we introduce and call a factor-graph propagation (FGP) action tree. Each arrow (edge) of the FGP action tree represents a sub-action of one (or more) candidate action sequence and in order to evaluate its information impact we require specific covariance entries of an intermediate belief represented by tree's vertex from which the edge is coming out (e.g. tail of the arrow). Overall, our approach has only a one-time calculation that depends on n, while evaluating action impact does not depend on n. We perform a careful examination of our approaches in simulation, considering the problem of autonomous navigation in unknown environments, where rAMDL-Tree shows superior performance compared to rAMDL, while determining the same best actions.Then, the IG of each sub-action is equal to:and sum of these IGs is equal to:

Virtual Maps for Autonomous Exploration of Cluttered Underwater Environments

“…Finally, it is also worth noting that autonomous mobile robot exploration is related to a more broadly-focused body of works that study belief space planning (BSP) in unknown environments. Such works have succeeded in relaxing assumptions often imposed on exploration, such as that of maximum likelihood observations [9], and in achieving more efficient belief propagation and covariance recovery [29]. However, such techniques have thus far been implemented in simple, obstacle-free domains populated with point landmarks.…”

Belief Space Planning for Mobile Robots with Range Sensors using iLQG

“…The major difficulty is that all future measurement sequences have to be rollout in order to determine an optimal policy. A large body of literature [10]- [12] simplifies the problem with maximum likelihood measurement assumption, where only the maximum likelihood measurement sequence is evaluated. Du Toit [13] shows that the assumption introduces artificial information, which makes the resulting control policy less robust if the actual future measurements differ from the ones with maximum likelihood.…”

“…We also compare the proposed method with methods that assume maximum likelihood measurements. The assumption is equivalent to assuming w t+1 = 0 in (12). We refer to this method as M-iLQG in the following.…”

Topological belief space planning for active SLAM with pairwise Gaussian potentials and performance guarantees

“…BSP can be seen as a joint control and estimation problem in which an agent (robot) has to find an optimal control according to a specific task-related objective, which itself has to be estimated while accounting for different sources of uncertainty, for example, due to stochastic sensing, motion or environment. Some interesting instantiations of this problem are active SLAM (e.g., Atanasov et al, 2015; Du et al, 2011; Huang et al, 2005; Indelman et al, 2015; Kim and Eustice, 2014; Kopitkov and Indelman, 2017, 2019; Regev and Indelman, 2017; Stachniss et al, 2004; Valencia et al, 2012), active perception (Bajcsy 1988), sensor deployment and measurement selection (Joshi and Boyd 2009; Bian et al 2006; Hovland and McCarragher 1997), graph reduction, and pruning (Kretzschmar and Stachniss 2012; Carlevaris-Bianco et al 2014).…”

“…In Indelman et al (2015) this challenge was addressed by resorting to the information form and utilizing sparsity; however, calculations still involve expensive access to marginal probability distributions. The rAMDL approach (Kopitkov and Indelman 2017, 2019) performs a one-time calculation of marginal covariance of variables involved in the candidate actions, and then applies an augmented matrix determinant lemma (AMDL) to efficiently evaluate the information-theoretic cost for each candidate action. Nevertheless, that approach still requires recovery of appropriate marginal covariances, the complexity of which depends on state-dimensionality and system sparsity.…”