General Purpose Incremental Covariance Update andEfficient Belief Space Planning viaFactor-Graph Propagation Action Tree

# General Purpose Incremental Covariance Update and Efficient Belief Space Planning via Factor-Graph Propagation Action Tree

Dmitry Kopitkov and Vadim Indelman D. Kopitkov is with the Technion Autonomous Systems Program (TASP), Technion - Israel Institute of Technology, Haifa 32000, Israel, dimkak@technion.ac.il . V. Indelman is with the Department of Aerospace Engineering, Technion - Israel Institute of Technology, Haifa 32000, Israel, vadim.indelman@technion.ac.il. This work was supported by the Israel Science Foundation.
###### 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 with 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 , while evaluating action impact does not depend on . 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.

Keywords
Covariance recovery, belief space planning, active SLAM, informative planning, active inference, autonomous navigation

## 1 Introduction

Autonomous operation in unknown or uncertain environments is a fundamental problem in robotics and is an essential part in numerous applications such as autonomous navigation in unknown environments, target tracking, search-and-rescue scenarios and autonomous manufacturing. It requires both computationally efficient inference and planning approaches, where the former is responsible for tracking the posterior probability distribution function given available data thus far, and the latter is dealing with finding the optimal action given that distribution and a task-specific objective. Since the state is unknown and only partially observable, planning is performed in the belief space, where each instance is a distribution over the original state, while accounting for different sources of uncertainty. Such a problem can be naturally viewed as a partially observable Markov decision process (POMDP), which was shown to be computationally intractable and typically is solved by approximate approaches. The planning and decision making problems are challenging both theoretically and computationally. First, we need to accurately model future state belief as a function of future action while considering probabilistic aspects of state sensing. Second, we need to be able to efficiently evaluate utility of this future belief and to find an optimal action, and to do so on-line.

The utility function in belief space planning (BSP) typically involves an information-theoretic term, which quantifies the posterior uncertainty of a future belief, and therefore requires access to the covariance (information) matrix of appropriate variables (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). However, the recovery of specific covariances is computationally expensive in high-dimensional state spaces: while the belief is typically represented in the (square-root) information form to admit computationally efficient updates (Kaess et al., 2012), retrieving the covariance entries requires an inverse of the corresponding (potentially) high-dimensional information matrix. Although sophisticated methods exist to efficiently perform such inverse by exploiting sparsity of square-root information matrix and by reordering state variables for better such sparsity (Kaess et al., 2012), the overall complexity still is at least quadratic w.r.t. state dimension (Ila et al., 2015). Moreover, in case of planning such computation needs to be performed for each candidate action.

The computational efficiency of the covariance recovery and the planning process is the main point of this paper. We develop a novel method to incrementally update covariance entries after any change of the inference problem, as defined next. Moreover, we present a planning algorithm which leverages the key ability of incremental covariance updates and by exploiting action similarity is much faster and yet exact w.r.t. alternative state-of-the-art techniques.

The inference problem can be represented by a set of currently available observations and state variables whose value we are to infer. For example, in a typical SLAM (simultaneous localization and mapping) scenario these variables are the robot poses along a trajectory and landmarks of the environment, while the observations are motion odometry and projection/range measurements. Covariances of the state variables can change as a result of any change in the inference problem, such as introduction of new observations or augmentation of state (e.g. introduction of a new robot pose). Moreover, covariances also depend on current linearization point of the state vector, which in turn can also change after introduction of new observations. In this paper we scrupulously analyze each such possible change in the inference problem and show how covariance entries can be appropriately incrementally updated. Such capability to incrementally update covariance entries is important not only for the inference phase but also for efficiently addressing information-theoretic belief space planning, as we describe next.

BSP is typically solved by determining the best action, given an objective function, from a set of candidate actions while accounting for different sources of uncertainty. Such an approach requires to evaluate the utility of each action from a given set of candidate actions. This evaluation is usually done separately for each candidate action and typically consists of two stages. First, posterior belief for candidate action is propagated and explicit inference is performed. Second, an application-specific objective function is evaluated given candidate action and the corresponding posterior belief. Yet, inference over the posterior belief and evaluation of the objective function can be computationally expensive, especially when the original state is high-dimensional since both parts’ complexity depends on its dimension.

However, in many BSP applications candidate (non-myopic) actions are partially overlapping, i.e. have similar parts. For instance, in a building exploration scenario, candidate actions are trajectories to different locations in a building (see Figure 1) that were provided e.g. by sampling-based motion planning approaches; some of these sampled trajectories will have mutual parts. Typically, these common parts will be evaluated a number of times, as part of evaluation of each action that shares it. Given that we know what are the similar parts between the different candidate actions, it can significantly reduce runtime complexity if we could handle these similar parts only once.

In this paper we present a technique for computation re-use between the candidate actions and exploitation of actions’ similarity, while leveraging the above-mentioned method for incremental covariance updates. We show that such a technique greatly reduces the total decision making runtime. Moreover, we argue that for most cases, explicit inference over the posterior belief is not required and that computation of the objective function can be done efficiently with complexity that is independent of state dimension. In general, the objective function of BSP can contain multiple terms, such as control cost, distance to goal and an information-theoretic term (e.g. entropy, information gain or mutual information). Arguably, in typical settings the control cost and distance to goal can be calculated without explicit inference over the posterior belief, since these terms depend only on linearization point of the state vector. In this paper we show that also the information term does not require an explicit inference over the posterior belief and that action similarity can be efficiently exploited, concluding that BSP problem can be solved without performing time-consuming explicit inference over the posterior belief at all.

To that end, we present a new paradigm that represents all candidate (sequence of) actions in a single unified data structure that allows to exploit the similarities between candidate actions while evaluating the impact of each such action. We refer to this structure as factor-graph propagation (FGP) action tree, and show that the developed herein incremental covariance calculation method allows us to compute information gain of the tree’s various parts. 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.

To summarize, our main contributions in this paper are as follows: (a) we develop an incremental covariance update method to calculate specific covariance entries after any change in inference problem; (b) we present factor-graph propagation (FGP) action tree, that represents all candidate actions in single hierarchical model and allows to formulate mutual parts of the actions as a single sub-actions; (c) we apply incremental covariance update method to calculate covariance entries from intermediate and posterior beliefs within the FGP action tree, with complexity independent of state dimension; 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.

This paper is organized as following. In Section 2 we describe the relevant work done in this field. Section 3 contains preliminaries and problem definition. In Section 4, we describe our approaches for incremental covariance recovery (Section 4.1) and information-theoretic BSP problem (Section 4.2). Further, in Section 5 we provide our simulation results that emphasize advantages of the presented herein approaches. Finally, in Section 6 we conclude the discussion about the introduced methods and point out several directions for future research. Additionally, at the end of this paper there is an appendix where we put proofs of several lemmas to improve readability.

## 2 Related Work

In this section we discuss the most relevant work to our approach, starting with computationally efficient covariance calculation and then proceeding to state of the art belief space planning approaches.

#### Computationally Efficient Covariance Recovery in High-Dimensional State Spaces

Fast covariance recovery, under the Gaussian inference setting, is an active research area that has been addressed by several works in the recent years. Naïvely calculating an inverse of a high-dimensional information matrix is prohibitively expensive. However, these calculations can be avoided by exploiting sparsity of the square root information matrix, yielding a recursive method to calculate the required entries (Golub and Plemmons, 1980), which has been recently also proposed by Kaess and Dellaert (Kaess and Dellaert, 2009) within their incremental smoothing and mapping solver. Although such method is faster than a simple inverse of square-root information matrix, the covariances are still calculated from scratch and the complexity depends on state dimension . Moreover, in order to calculate a specific block of covariance matrix, the recursive approach may still need to calculate the entire covariance matrix (with dimensions ) which is very undesirable for high-dimensional state spaces.

More recently, Ila et al. (Ila et al., 2015) introduced an approach to incrementally update covariances after the inference problem was changed. Given specific prior covariance entries that were calculated in previous timestep, their approach efficiently calculates covariance deltas to these entries, which comes out to be much faster than the recursive approach from (Kaess and Dellaert, 2009). Although this approach is similar in spirit to our method of incremental covariance update, it is more limited in the following sense. Its theoretical part deals only with the specific scenario where new observations were introduced to the inference problem, without adding new variables. On the other hand, the mathematical formulation of their approach does not handle the common scenario where the state vector is augmented with new variables, although the simulation part of (Ila et al., 2015) suggests that their approach can also be applicable in this case in practice. We emphasize that this approach is not mathematically sound in the state augmentation case, since such a case involves singular matrices that are assumed to be invertible according to the derivation of (Ila et al., 2015). Moreover, in case of state relinearization, the authors use a recursive method as fallback and calculate covariances from scratch. In contrast, we present a general approach that is mathematically sound and is capable of dealing with any change in the inference problem, including state augmentation and relinearization. Moreover, even though a limited version of incremental covariance update has been developed (Ila et al., 2015), it was not considered within a BSP problem, which is one of our main contributions in this work.

#### Belief Space Planning

As was mentioned above, BSP is an instantiation of a POMDP problem. Optimal solution of POMDP is known to be intractable (Kaelbling et al., 1998) in high-dimensional domains due to curse of dimensionality. Therefore, most of the modern research is focused on approximation methods that solve the planning problem in sub-optimal form with tractable runtime complexity. These approximation methods can be categorized into those that discretize the state/action/measurement space domains and those that act in continuous spaces. Approaches that perform discretization include sampling (Prentice and Roy, 2009; Agha-Mohammadi et al., 2014), simulation (Stachniss et al., 2005) and point-based value iteration (Pineau et al., 2006) methods. Planning approaches that operate in continuous spaces, often also termed as direct trajectory optimization methods, calculate a locally optimal solution given an initial nominal solution using different optimization techniques such as dynamic programming and gradient descent (Indelman et al., 2015; Van Den Berg et al., 2012; Patil et al., 2014; Platt et al., 2010).

Additionally, BSP methods can be separated into those that solve myopic and non-myopic decision making. While myopic approaches, also known as next best view (NBV) approaches in computer vision community (e.g. (Wenhardt et al., 2007; Dunn and Frahm, 2009)), reason about actions taking the system only one step into the future, non-myopic planning (e.g. (Platt et al., 2010; He et al., 2011; Van Den Berg et al., 2012; Kim and Eustice, 2014; Indelman et al., 2015)) deals with sequences of actions taking the system multiple steps into the future. Clearly, for more complex tasks non-myopic methods will perform better as the time period before receiving the reward can be long. Yet, such methods are typically more computationally expensive as more effort is required to consider different probabilistic outcomes along the long planning horizon. In this paper we consider a non-myopic setting and formulate the problem through factor graphs.

An information-theoretic BSP problem seeks for an optimal action that maximally reduces estimation uncertainty of the state vector. Such a problem can be separated into two main cases - unfocused BSP tries to reduce uncertainty of all variables inside the state vector, whereas focused BSP is only interested to reduce uncertainty of a predefined subset (termed as focused variables) of these variables. Typically, the two cases have different best actions, with optimal action from unfocused BSP potentially providing little information about focused variables of focused BSP (see e.g. (Levine and How, 2013)). In both cases, the objective function usually calculates posterior entropy or information gain (of all variables from the state vector or of only focused variables) and may have high computation complexity that depends on state dimension . For instance, the calculation of unfocused posterior entropy usually requires determinant computation of information (covariance) matrix which is in general , and is smaller for sparse matrices as in SLAM problems (Bai et al., 1996). Calculation of focused posterior entropy is even more expensive and requires additional Schur complement computation.

Recently, we presented a novel approach, rAMDL (Kopitkov and Indelman, 2017), to efficiently calculate entropy and information gain for both focused and unfocused cases. This method requires only one-time calculation that depends on dimension - computation of specific prior marginal (or conditional) covariances. Given these prior covariances, rAMDL evaluates information impact of each candidate action independently of state dimension . Such a technique was shown to significantly reduce runtime (by orders of magnitude) compared to standard approaches.

Yet, in most BSP approaches, including our own rAMDL approach, the similarity between candidate actions is not exploited and each candidate is evaluated from scratch. To the best of our knowledge, only the work by Chaves et al. (Chaves and Eustice, 2016) was done in this direction. Their approach performs fast explicit inference over the posterior belief, by constraining variable ordering of the Bayes tree data structure to have candidates’ common variables eliminated first. Still, this approach has its limitations. It explicitly calculates the posterior belief for each action, and though it is done fast, it still requires additional memory to store such posterior beliefs. Further, it does not deal with information-theoretic objective functions whose runtime complexity is usually very expensive, as mentioned above. Moreover, it can only be applied when the SLAM algorithm is implemented using Bayes tree (Kaess et al., 2012), and it was shown to work only for the case where actions are trajectories constrained to have only a single common part.

In contrast, in this paper we develop a BSP technique that re-uses calculations in a general way, by exploiting potentially any number of mutual parts between the candidate actions. It is expressed in terms of factor graphs and can be applied not just for trajectory planning, but for any decision making problem expressed via factor graphs. Moreover, our technique can be implemented independently of a chosen SLAM factor graph optimization infrastructure. We combine several algorithmic concepts together - a unified graphical model FGP action tree, incremental covariance update and rAMDL approach (Kopitkov and Indelman, 2017), and present a BSP solution that does not require explicit inference over the posterior belief while carefully evaluating information impact of each action in an exact way.

## 3 Notations and Problem Formulation

Consider a high-dimensional problem-specific state vector at the current time, where we use the notation ”-” to represent the (a priori) state vector before applying the next action. Depending on the application, can represent robot configuration and poses (optionally also past and current poses), environment-related variables or any other variables of interest. Additionally, consider factors that were added to the inference problem till (and including) current time, where each factor represents a specific measurement model, motion model or prior, and as such involves appropriate state variables .

The joint pdf (probability density function) can be then written as

 P(X−|history)∝nf∏j=1fj(Xj), (1)

where contains all the information gathered by the current time (measurements, controls, etc.).

As common in many inference problems, we will assume that all factors have a Gaussian form:

 fj(Xj)∝exp(−12∥hj(Xj)−rj∥2Σj), (2)

with appropriate model

 rj=hj(Xj)+υj,υj∼N(0,Σj), (3)

where is a known nonlinear function, is zero-mean Gaussian noise and is the expected value of , i.e. . Such a factor representation is a general way to express information about the state. In particular, it can represent a measurement model, in which case, is the observation model, and and are, respectively, the actual measurement and measurement noise. Similarly, it can also represent a motion model. A maximum a posteriori (MAP) inference is the optimization solution of maximizing Eq. (1) w.r.t. state . It can be efficiently calculated (see e.g. Kaess et al. (2012)) such that

 P(X−|history)=N(X⋆−,Σ−)=N−1(η⋆−,Λ−) (4)

where , , and are respectively the current mean vector, covariance matrix, information vector and information matrix (inverse of covariance matrix).

We shall refer to the belief of state at the current time as the prior belief and write

 b[X−]≐P(X−|history). (5)

We now introduce the two problems this paper addresses, along with appropriate notations: general purpose incremental covariance update, and computationally efficient belief space planning. As will be seen in Section 4, our approach to address the latter problem builds upon the solution to the first problem.

#### Problem 1: Covariance Recovery

As mentioned above, in many applications it is mandatory to recover covariance entries of belief . However, typically this belief is represented through its information form ( and ), or the square-root information upper-triangular matrix , with .

Considering a square-root representation, the covariance matrix is and its specific covariance entries can be calculated from entries as (Golub and Plemmons, 1980)

 σii=1rii(1rii−n∑j=i+1,rij≠0rijσji), (6)
 σij=1rii(−j∑k=i+1,rik≠0rikσkj−n∑k=j+1,rik≠0rikσkj). (7)

Note that in order to calculate the upper left covariance entry (), all other covariance entries are required. Therefore, worst case computation (and memory) complexity of this recursive approach is still quadratic in state dimension .

In contrast, an incremental covariance update approach can be applied in order to recover the required covariance entries more efficiently. At each timestep, solving the inference problem for the current belief from Eq. (1) provides MAP estimate and the corresponding covariance or (square root) information matrix. However, at the next step the inference problem changes. To see that, consider the belief at the next timestep which was obtained by introducing new state variables (e.g. new robot poses in SLAM smoothing formulation), with , and by adding new factors (e.g. new measurements, odometry, etc.) where . Additionally, consider the set of variables whose marginal covariance from we are interested in calculating. Note that these variables of interest can contain both old and new variables, with .

Given that we already calculated the required covariance entries from the current belief , in incremental covariance update approach we would like to update these entries after the change in the inference problem (from to ) as:

 ΣM,Yold+=ΣM,Yold−+ΔYold, (8)

where represents the difference between old and new covariance entries. Additionally, in the general case we might be interested in calculating the posterior covariance of new variables of interest , i.e.  , as well as also the cross-covariances between and .

Likewise, also the conditional covariances, from the conditional pdf of one state subset conditioned on another, are required for information-theoretic BSP as was shown in (Kopitkov and Indelman, 2017). Hence, we would also like to develop a similar approach for incremental conditional covariance update.

A limited technique to perform an incremental update of marginal covariances was presented in (Ila et al., 2015). The authors show how to update the covariance entries by downdating the posterior information matrix. Their derivation can be applied for the case where the state vector was not augmented during the change in the inference problem ( is empty). However, that derivation is not valid for the case of state augmentation, which involves zero-padding of prior matrices (described below); such padding yields singular matrices and requires a more delicate handling. Even though their approach is not mathematically sound for the augmentation case, in the simulation part of (Ila et al., 2015) it is insinuated that the approach can also be applied here in practice. Still, the authors clearly declare that their approach does not handle relinearization of the state vector, which can often happen during the change in the inference problem. Further, (Ila et al., 2015) does not consider recovery of conditional covariances. In contrast, we develop a general purpose method that handles incremental (marginal and conditional) covariance updates in all of the above cases in a mathematically sound way.

In Section 4 we categorize the above general change in the inference problem into different sub-cases. Further, we present an approach that carefully handles each such sub-case and incrementally updates covariances that were already calculated before the change in the inference problem, and also computes covariance of newly introduced state variables. As will be shown, the computational complexity of such a method, when applied to a problem where only the marginal covariances need to be recovered (i.e. block diagonal of ), is linear in in the worst case. Furthermore, we will show how our incremental covariance update approach can be also applied to incrementally update conditional covariance entries. Later, this capability will be an essential part in the derivation of our BSP method, rAMDL-Tree.

#### Problem 2: Belief Space Planning

Typically in BSP and decision making problems we have a set of candidate actions from which we need to pick the best action according to a given objective function. As shown in our previous work (Kopitkov and Indelman, 2017), the posterior belief for each action can be viewed as a specific augmentation of the prior factor graph that represents the prior belief (see Figure (a)a). In this paper we shall denote this factor graph by . Each candidate action can add new information about the state variables in form of new factors. Additionally, in specific applications, action can also introduce new state variables into the factor graph (e.g. new robot poses). Thus, similarly to change in inference problem described above for each action we can model the newly introduced state variables denoted by , defining the posterior state vector (after applying the action) as . In a like manner, we denote the newly introduced factors by where .

Therefore, similar to Eq. (1), after applying candidate action , the posterior belief can be explicitly written as

 b[X+]∝b[X−]nnew∏j=1fjnew(Xj+). (9)

Such a formulation is general and supports non-myopic action with any planning horizon, that introduces into the factor graph multiple new state variables and multiple factors with any measurement model. Still, in this paper we assume factors to have a Gaussian form (Eq. (2)).

For the sake of conciseness, in this paper the newly introduced factors and state variables that are added when considering action will be called action ’s increment and denoted as

 I(a)≐{Fnew,Xnew} (10)

The posterior information matrix, i.e. the second moment of the belief , can be written as

 Λ+=Λ−+AT⋅A,Λ+=ΛAug++AT⋅A (11)

where we took the maximum likelihood assumption which considers that the above, a single optimization iteration (e.g. Gauss Newton), sufficiently captures action impact on the belief. Such an assumption is typical in BSP literature (see, e.g. (Platt et al., 2010; Van Den Berg et al., 2012; Kim and Eustice, 2014; Indelman et al., 2015)). The left identity in Eq. (11) is true when is empty, while the right identity is valid for non-empty . The matrix is a noise-weighted Jacobian of newly introduced factors w.r.t. state variables ; is constructed by first augmenting the prior information matrix with zero rows and columns representing the new state variables , as illustrated in Figure 3 (see e.g. (Kopitkov and Indelman, 2017)).

After modeling the posterior information matrix for action , the unfocused information gain (uncertainty reduction of the entire state vector ) can be computed as:

 (12)

where is differential entropy function that measures the uncertainty of input belief, and is a constant that only depends on the dimension of and thus is ignored in this paper. Note that the above unfocused information gain is typically used in applications where the set of new variables, , is empty and so both and have the same dimension. In cases where is not empty (e.g. SLAM smoothing formulation), usually focused information gain is used (see below).

The optimal action is then given by .

For focused BSP problem we would like to reduce uncertainty of only a subset of state variables . When consists of old variables , , we can compute its information gain (IG). Such IG is a reduction of ’s entropy after applying action , where and are prior and posterior beliefs of focused variables . In case consists of newly introduced variables , , the IG function has no meaning as the prior belief does not exist. Instead, we can directly calculate ’s posterior entropy . The IG and entropy functions can be calculated through respectively:

 JFIG(a)=12ln∣∣ΣM,F−∣∣∣∣ΣM,F+∣∣,JFH(a)=dim.const+12ln∣∣ΣM,F+∣∣, (13)

where and are prior and posterior marginal covariance matrices of , respectively. Note that in focused BSP the optimal action will be found through or .

To summarize, in order to solve an information-theoretic BSP problem, we are required to calculate IG or entropy (Eqs. (12) and (13)) for each candidate action , and then choose a candidate action with the maximal gain.

## 4 Approach

In this section we present our approaches that efficiently solve the incremental covariance recovery (Section 4.1) and information-theoretic BSP (Section 4.2).

### 4.1 Incremental Covariance Update

In this section we present our technique for efficient update of covariance entries (see Problem 1 in Section 3). In Section 4.1.1 we show how to update marginal covariances of specified variables after new state variables were introduced into the state vector and new factors were added, yet no state relinearization happened during the change in the inference problem. We will show that the information matrix of the entire state belief is propagated through quadratic update form, similarly to Eq. (11). Assuming such quadratic update, we will derive a method to efficiently calculate the change in old covariance entries, to compute the new covariance entries and the cross-covariances between old and new state variables. Further, in Section 4.1.2 we will show that also in the relinearization case the information matrix update has an identical quadratic update form and conclude that our method, derived in Section 4.1.1, can also be applied when some of the state variables were relinearized. Finally, in Section 4.1.3 we will show that also the information matrix of a conditional pdf is updated through quadratic update form and that the same technique from Section 4.1.1 can be applied in order to incrementally update conditional covariance entries. We will show that our approach’s complexity, given the specific prior covariances, does not depend on state dimension .

#### 4.1.1 Update of Marginal Covariance Entries

Consider Problem 1 from Section 3. Consider the belief was propagated from to as described. Yet, let us assume for now that no state relinearization happened (we will specifically handle it in the next section). In this section we show that the posterior covariances of interest can be efficiently calculated as

 ΣM,Y+=f(ΣM,W−), (14)

where is the prior marginal covariance of set and is a transformation function, with calculation complexity that does not depend on state dimension . We derive this function in detail below. The set contains old state variables inside () and is the set of involved variables in the newly introduced factors - variables that appear in ’s models (Eq. (3)). Note that the update of old covariances (Eq. (8)) is only one part of this , as also the computation of covariances for new state variables and cross-covariances between and .

Next, let us separate all possible changes in the inference problem into different cases according to augmented state variables and the newly introduced factors .

If is empty, we will call such a case as not-augmented. This case does not change the state vector () and only introduces new information through new factors. The information matrix in this case can be updated through , where matrix is a noise-weighted Jacobian of newly introduced factors w.r.t. state variables , and ’s height is dimension of all new factors within (see Section 3).

Given is not empty, we will call such a case as rectangular. This case augments the state vector to be and also introduces new information through the new factors. Here the information matrix can be updated through where is a singular matrix that is constructed by first augmenting the prior information matrix with zero rows and columns representing the new state variables , as illustrated in Figure 3; is the posterior state dimension; here will be an matrix.

Finally, for the case when is not empty and total dimension of new factors is equal to the number of newly introduced variables , we will call such a case as squared. Clearly, the squared case is a specific case of the rectangular case, which for instance can represent the new robot poses of candidate trajectory and the new motion model factors. The reason for this specific case to be dealt with in special way is due to the fact that its function is much simpler than function of the more general rectangular case, as we will show below. Thus, when it would be advisable to use function of the squared case.

The summery of the above cases can be found in Table 3.

Next, below we present the function separately for each one of the not-augmented, rectangular and squared cases. Although the function has an intricate form (especially in the rectangular case), all matrix terms involved in it have dimensions , or ; hence, overall calculation of posterior does not depend on state dimension .

###### Lemma 1

For the not-augmented case, the posterior marginal covariance can be calculated as:

 ΣM,Y+=ΣY−−B⋅C−1⋅BT,B≜ΣC−⋅(IA)T,C≜Im+IA⋅ΣI−⋅(IA)T (15)

where , and are parts of prior marginal covariance partitioned through :

 ΣM,W−=(ΣY−ΣC−(ΣC−)TΣI−) (16)

and where consists of ’s columns belonging to involved old variables .

The proof of Lemma 1 is given in Appendix A.1. Note that sets and are not always disjoint. In case these sets have mutual variables, the cross-covariance matrix can be seen just as - partition of prior covariance matrix with rows belonging to and columns belonging to .

###### Lemma 2

For the rectangular case the prior marginal covariance and the posterior marginal covariance have the forms:

 ΣM,W−=(ΣYold−ΣC−(ΣC−)TΣI−) (17)
 ΣM,Y+=(ΣM,Yold+Σ(Yold,Ynew)+(Σ(Yold,Ynew)+)TΣM,Ynew+) (18)

where we partition variables into two subsets and , and where .

Using parts of we can calculate parts of as:

 ΣM,Yold+=ΣYold−−B⋅G−1⋅BT (19)
 ΣM,Ynew+=P(Ynew,:) (20)
 C≜Im+IA⋅ΣI−⋅(IA)T (21)
 P≜[(ATnew⋅C−1⋅Anew)−1](:,Ynew) (22)
 F≜(ATnew⋅Anew)−1 (23)
 K≜Im−Anew⋅F⋅ATnew (24)
 K1≜K⋅IA (25)
 B≜ΣC−⋅KT1 (26)
 G≜Im+K1⋅ΣI−⋅KT1 (27)

where consists of ’s columns belonging to newly introduced variables . Also, we use matrix slicing operator (e.g. ) as it is accustomed in Matlab syntax.

Further, there are two methods to calculate from Eq. (18):

Method 1:

 Σ(Yold,Ynew)+=ΣC−⋅(IA)T⋅[C−1⋅IA⋅ΣI−⋅(IA)T−Im]⋅Anew⋅P (28)

Method 2:

 Σ(Yold,Ynew)+=ΣC−⋅[KT1⋅G−1⋅K1⋅ΣI−−Ik]⋅(IA)T⋅Anew⋅F(:,Ynew) (29)

Empirically we found that method 2 is the fastest option. The proof of Lemma 2 is given in Appendix A.2.

###### Lemma 3

For the squared case the prior marginal covariance and the posterior marginal covariance have the forms:

 ΣM,W−=(ΣYold−ΣC−(ΣC−)TΣI−) (30)
 ΣM,Y+=(ΣM,Yold+Σ(Yold,Ynew)+(Σ(Yold,Ynew)+)TΣM,Ynew+) (31)

where we partition variables into two subsets and , and where .

Using parts of we can calculate parts of as:

 ΣM,Yold+=ΣYold− (32)
 ΣM,Ynew+=Aiv⋅C⋅ATiv (33)
 Σ(Yold,Ynew)+=−ΣC−⋅(IA)T⋅(Aiv)T (34)
 Aiv≜[A−1new](Ynew,:) (35)
 C≜Im+IA⋅ΣI−⋅(IA)T. (36)

We can see that in case of a squared alteration, the covariances of old variables do not change. The proof of Lemma 3 is given in Appendix A.3.

Note that in some applications the inner structure of Jacobian partitions and can be known a priori. In these cases such knowledge can be exploited and the runtime complexity of the above equations can be reduced even more.

#### 4.1.2 Incremental Covariance Update After Relinearization

Till now we have explored scenarios where new information is introduced into our estimation system in a quadratic form via Eq. (11). Such information update is appropriate for planning problems where we take linearization point of existing variables (their current mean vector) and assume to know linearization point of newly introduced variables . However, during the inference process itself, state relinearization can happen and such a quadratic update form is not valid anymore. This is because some factors, linearized with the old linearization point, are removed from the system and their relinearized versions are then introduced. In this case the derived approach to incrementally update posterior covariances cannot be used as it is. In this section we describe the alternative that can be applied after a relinearization event and which is more efficient than state-of-the-art approaches that calculate specific posterior covariances from posterior information matrix from scratch.

Relineariztion may happen when a significantly new information was added into the inference problem and current linearization point of state vector does not optimally explain it anymore. In such cases, iterative optimization algorithms, such as Gauss-Newton, are responsible to update the current linearization point, i.e. to find a more optimal linearization point that better explains the collected so far measurement/motion/prior factors. Conventional approaches re-linearize the entire state vector when new data comes in. On the other hand, incremental optimizer ISAM2 (Kaess et al., 2012) tracks instead the validity of a linearization point of each state variable and re-linearizes only those variables whose change in the linearization point was above a predefined threshold. At each iteration of the nonlinear optimization and for each state variable , ISAM2 finds and, given it is too big (norm of is bigger than the threshold), updates the current estimate of to . In such case, factors involving this state variable need to be relinearized. Clearly, the frequency of such a relinearization event during the inference process depends on the value of the threshold, and can be especially high during, for example, loop-closures in SLAM scenario. Still, in our simulations we have seen that even with a relatively high threshold and small number of loop-closures, relinearization of some small state subset happens almost every second timestep. Thus, in order to accurately track covariances in the general case, while using conventional approaches that re-linearize each time or ISAM2 which re-linearizes only when it is needed, it is very important to know how to incrementally update covariance entries also after the state was relinearized. Below we show that information update of such a relinearization event can be also expressed in a quadratic form; thus, the methods from Section 4.1.1 that incrementally update specific covariance terms can be applied also here.

Denote by the factors that involve any of the variables in . In order to update information of the estimation after relinearization, we would want to remove ’s information w.r.t. old linearization point and to add ’s information w.r.t. the new one. It is not hard to see that posterior information matrix (after relinearization of subset ) can be calculated through

 Λ+=Λ−−AT−⋅A−+AT+⋅A+ (37)

where matrix is a noise-weighted Jacobian of factors w.r.t. old linearization point, and matrix is a noise-weighted Jacobian of factors w.r.t. new linearization point.

Next, using complex numbers the above equation becomes

 Λ+=Λ−+(iAT−AT+)⋅(iA−A+)=Λ−+(iA−A+)T⋅(iA−A+)=Λ−+BT⋅B,B≜(iA−A+). (38)

Note that operator is transpose and not conjugate transpose. Above we see that also here, the information update is quadratic and the update matrix contains terms of old and new Jacobians of factors that were affected by the relinearization event. Therefore, the incremental covariance update described in Section 4.1.1 is also applicable here, making the update of specific covariances much more efficient compared to computation of the covariances from scratch (e.g. through Eqs. (6)-(7)).

More specifically, the update in Eq. (38) is an instance of the not-augmented case from Section 4.1.1. By exploiting the specifics of matrix ’s structure, Lemma 1 can be reduced to:

###### Lemma 4

For the relinearization case (Eq. (38)), the posterior marginal covariance can be calculated as:

 ΣM,Y+=ΣY−−U⋅UT,U≜ΣC−⋅M,M≜(iM1M2), (39)
 M2≜(IA+)T╱chol[I+IA+⋅ΣI−⋅(IA+)T], (40)
 (41)
 G≜MT2⋅ΣI−⋅(IA−)T, (42)

where , and are parts of the prior marginal covariance partitioned through :

 ΣM,W−=(ΣY−ΣC−(ΣC−)TΣI−) (43)

and where consists of ’s columns belonging to the involved variables ; contains columns of that belong to ; is the identity matrix of an appropriate dimension; represents cholesky decomposition which returns an upper triangular matrix; ”” is the backslash operator from Matlab syntax ().

The proof of Lemma 4 is given in Appendix A.4. While it is mathematically equivalent to Lemma 1, empirically we found that such a formulation is faster and more numerically stable in case of relineariztion.

#### 4.1.3 Incremental Conditional Covariance Update

Above we have seen how to update specific prior marginal covariances given that state’s information update has a quadratic form or . Similarly, we can derive such a method that incrementally updates specific conditional covariances since, as we show below, the update of the conditional information matrix from the conditional probability density function has a similar form.

To prove this statement, let us focus on the not-augmented case where is empty. Define a set of variables , whose posterior conditional covariance , conditioned on an arbitrary disjoint variable set (with ), needs to be updated. Next, let be the set of all state variables that are not in , and note that . The prior information matrix of the prior conditional probability distribution is just a partition of the entire prior information matrix that belongs to columns/rows of variables in . Similarly, the posterior is a partition of . It can be easily shown that

 ΛU|V+=ΛU|V−+(AU)T⋅AU (44)

where is a partition of noise-weighted Jacobian matrix that belong to columns of variables in .

Eq. (44) shows that the conditional probability distribution has a quadratic update, similar to the marginal probability distribution of the entire state vector . Also, note that the required posterior conditional matrix is a partition of the posterior conditional covariance matrix . For better intuition, it can be seen similar to the posterior marginal matrix being a partition of the posterior marginal covariance matrix in the not-augmented case (see Lemma 1). Thus, there exists a function that calculates from , where is the prior conditional covariance matrix of set , conditioned on the set ; here, are the involved variables that are in . Derivation of such a function is trivial, by following the steps to derive function in Section 4.1.1, and is left out of this paper in order to not obscure it with additional complex notations.

A similar exposition can be also shown in the augmented case (i.e.  is not empty), where information update of conditional distribution also has the augmented quadratic form. To summarize, the derived function in Sections 4.1.1 and 4.1.2 can also be used to incrementally update the specific conditional covariances by replacing the prior marginal covariance terms in it with appropriate prior conditional covariances.

#### 4.1.4 Application of Incremental Covariance Update to SLAM

In order to apply our incremental update method in a SLAM setting, we model each change in the inference problem in the form of two separate changes as follows. We consider a specific scenario where at each time step, new robot pose ( is index of time step) and new landmarks are introduced into the state vector . Further, new factors are introduced into the inference system; these factors include one odometry factor between poses and , projection and range factors between the new pose and new landmarks , and finally projection and range factors between and old landmarks. Additionally, in general a subset of old factors (denoted by ) was relinearized as a result of a linearization point change of some old state variables during the inference stage. In case no linearization point change was performed, this set of factors is empty. Note that although we assume above only range and visual measurements, our approach would work for other sensors as well, e.g. in a purely monocular case.

In the first modeled change, we introduce into the inference system all the new state variables ( and ) and their constraining factors ( and ), denoted by and , respectively. It can be shown for this change that the dimension of its newly introduced state variables is equal to the dimension of newly introduced factors . Thus, such change has a form of the squared case (see Table 3) and the updated covariance entries due to this change can be calculated by applying Lemma 3. Also note that after this change all the new state variables are properly constrained, which is essential for the information matrix to remain invertible. Denote this information matrix, i.e. after applying the first change, by :

 ΛM=ΛAugk+ATS⋅AS, (45)

where is the prior information matrix augmented with zero rows/columns for new state variables and is noise-weighted Jacobian of factors .

The remaining parts of the original change in the inference problem are represented by the second change. The posterior information matrix can be updated due to this second change as

 Λk=ΛM+ATO⋅AO−AT−⋅A−+AT+⋅A+, (46)

where is noise-weighted Jacobian of factors ; and are noise-weighted Jacobians of factors w.r.t. old and new linearization points, respectively. The above equation can be rewritten as:

 Λk=ΛM+BT⋅B,B≜⎛⎜⎝iA−A+AO⎞⎟⎠, (47)

and the corresponding covariance matrix can be calculated through Lemma 4, or through Lemma 1 in case there was no relinearization at the current time step, i.e. .

To summarize, any change in the inference problem of our SLAM scenario can be represented as a combination of two fundamental changes - squared (Eq. (45)) followed by (relinearized) not-augmented (Eq. (47)); the information matrix is updated as

 Λk−1⟹ΛM⟹Λk (48)

where can be seen as a logical time step of middle point.

Covariances after the first change can be updated very fast through Lemma 3, since as we saw in Section 4.1.1, the marginal covariances of old variables do not change and only marginal covariances of new variables need to be computed in this case. To do this, we require marginal covariances of involved variables from . Notice that of the first change contains only , whose marginal covariance is available since it was already calculated in the previous time step. Thus, Lemma 3 can be easily applied and the marginal covariances of all state variables at middle point can be efficiently evaluated.

To update all marginal covariances after the second change (through Lemma 1 or Lemma 4) we require marginal covariance of involved variables (in factors and of the second change) from covariance matrix . Moreover, we will require cross-covariances from between variables and the rest of the variables, as can be seen from the equations of the lemmas. Thus, we require entire columns from that belong to . These columns can be easily calculated at time (from prior covariance matrix ) and propagated to middle point by applying Lemma 3. The specific columns (belonging to some state subset ) of matrix can be efficiently calculated through two backsubstitution operations:

 V≜RTk−1╲IY,Σ(:,Y)k−1=Rk−1╲V, (49)

where are columns from the identity matrix that belong to variables in and ”” is the Matlab’s backsubstitution operator with being identical to solving linear equations for .

The other alternative for this 2-stage incremental covariance update is to use Lemma 2 for the rectangular inference change as follows. The posterior information matrix can be calculated in one step as:

 Λk=ΛAugk+ATS⋅AS+ATO⋅AO−AT−⋅A−+AT+⋅A+=ΛAugk+BT⋅B,B≜⎛⎜ ⎜ ⎜⎝ASiA−A+AO⎞⎟ ⎟ ⎟⎠. (50)

Such a change has a form of the rectangular case (see Table 3); therefore, the updated covariance entries and the marginal covariances of new state variables can be calculated by applying Lemma 2. Note that operator within the lemma is transpose and not conjugate transpose. Similarly to the above 2-stage method, the rectangular case will also require entire columns from that belong to old involved variables. This can be done here in the same way through Eq. (49).

We evaluate the above methods in our SLAM simulation in Section 5.1 and show their superiority over other state-of-the-art alternatives.

### 4.2 Information-Theoretic Belief Space Planning

In this section we develop a new approach that, based on the derived above incremental covariance update method, efficiently solves the information-theoretic BSP problem defined in Problem 2 from Section 3. Given a set of candidate actions, the proposed paradigm exploits common aspects among different actions for efficient BSP in high-dimensional state spaces. Each (non-myopic) action gives rise to a posterior belief that can be represented by an appropriate factor graph. In many applications different candidate actions will share some newly introduced factors and state variables (their factor graph increments). For example, two trajectory candidates that partially share their navigation path, will introduce the same factors for this mutual trajectory part (see Figure (a)a). The posterior factor graphs of these candidate actions therefore have common parts, in terms of factor and variable nodes, and in addition all of these factor graphs start from the belief at the current time.

Our proposed paradigm saves computation time by identifying the common parts in these posterior factor graphs, and switching to a unified graphical model that we introduce, the factor-graph propagation (FGP) action tree, which represents gradual construction of posterior factor graphs from the current factor graph. For instance, in Figures (b)b and (c)c two different FGP action trees are depicted. Both lead to the same posterior beliefs of candidate actions, yet one of them can be evaluated more efficiently, as will be explained in Section 4.2.2. Given such a graphical model, we develop efficient method to evaluate information impact of each candidate action in unified way. As we show, this method requires specific covariance entries for the intermediate beliefs that are represented by the tree’s vertices, which we calculate by our incremental covariance recovery method (see Section 4.1) with computational complexity that does not depend on state dimension (see Section 4.2.3). Further, we avoid posterior belief propagation and calculation of determinants of huge matrices for each candidate action by using the aforementioned incremental covariance update and the rAMDL method from (Kopitkov and Indelman, 2017). Moreover, we evaluate candidates’ common parts only once instead of considering these parts separately for each of the candidates.

Determining the best topology of the FGP action tree, given the individual factor graphs for different candidate actions, is by itself a challenge that requires further research. In this paper we consider one specific realization of this concept, by examining the problem of motion planning under uncertainty and using the structure of the candidate trajectories for FGP action tree construction (see Section 4.2.2). In the results reported in Section 5 we consider scenario of autonomous exploration in unknown environment where such tree topology allows us to reduce computation time twice compared to baseline approaches.

#### 4.2.1 rAMDL Approach

In our recently-developed approach, rAMDL (Kopitkov and Indelman, 2017), the information-theoretic costs (12) and (13) are evaluated efficiently, without explicit inference over posterior beliefs for different actions and without calculating determinants of large matrices. As rAMDL is an essential part of our approach presented herein, below we provide a concise summary for the sake of completeness of the current paper. For a more detailed review of rAMDL the reader is referred to (Kopitkov and Indelman, 2017).

In (Kopitkov and Indelman, 2017) we showed that the information impact of action (Eqs. (12) and (13)) is a function of prior covariances for the subset that contains variables involved in new factors of , and of matrix that contains non-zero columns of the noise-weighted Jacobian matrix . Given the prior covariances of , such a function can be calculated very fast, with complexity independent of state dimension. Thus, in rAMDL we first calculate the required prior covariances for all candidate actions as a one-time, yet still expensive, calculation, after which we efficiently evaluate information impact of each candidate action. The main structure of the rAMDL approach is shown in Algorithm 1.

In particular, for the case where is empty, the unfocused IG from Eq. (12) can be calculated as

 JIG(a)=12ln∣∣Im+IA⋅ΣM,IX−⋅(IA)T∣∣, (51)

where is the prior marginal covariance of variables.

In case is empty and we want to calculate focused IG of focused variables in (see Eq. (13), left), it can be calculated through

 (52)

where denotes the involved variables that are unfocused, is the prior conditional covariance of conditioned on , and is a partition of with columns that belong to variables in