ICS: Incremental Constrained Smoothingfor State Estimation

Paloma Sodhi1, Sanjiban Choudhury2, Joshua G. Mangelson1, and Michael Kaess1

Abstract— A robot operating in the world constantly receivesinformation about its environment in the form of new measure-ments at every time step. Smoothing-based estimation methodsseek to optimize for the most likely robot state estimate using allmeasurements up till the current time step. Existing methodssolve for this smoothing objective efficiently by framing theproblem as that of incremental unconstrained optimization.However, in many cases observed measurements and knowledgeof the environment is better modeled as hard constraintsderived from real-world physics or dynamics. A key challengeis that the new optimality conditions introduced by the hardconstraints break the matrix structure needed for incrementalfactorization in these incremental optimization methods.

Our key insight is that if we leverage primal-dual methods,we can recover a matrix structure amenable to incrementalfactorization. We propose a framework ICS that combinesa primal-dual method like the Augmented Lagrangian withan incremental Gauss Newton approach that reuses previ-ously computed matrix factorizations. We evaluate ICS on aset of simulated and real-world problems involving equalityconstraints like object contact and inequality constraints likecollision avoidance.

I. INTRODUCTION

Localization and state estimation are increasingly formu-lated as smoothing problems since the use of such meth-ods tends to lead to increased efficiency and accuracy [1].Typically, the smoothing objective is formulated as a MAPinference problem over a graph whose nodes are the un-known state variables and edges encode sensor measurementinformation. For Gaussian models, MAP inference results ina sparse, unconstrained nonlinear least squares optimization[2]. Such problems can be solved incrementally [3, 4], i.e. asnew measurements arrive at each time step, without havingto re-solve from scratch. However, in many cases observedmeasurements and knowledge about the environment wouldbe better modeled as hard constraints in the optimization.These can either be constraints imposed by physics, such asrobots must not violate dynamics or teleport through objects,or these can be constraints imposed by certain sensors, suchas contact measurements. We address the problem of satis-fying such constraints within the framework of incrementaloptimization to allow for efficient and precise inference frommeasurements arriving at each time step.

1P. Sodhi, J. Mangelson and M. Kaess are with The Robotics In-stitute, Carnegie Mellon University, Pittsburgh, PA, USA. {psodhi,jmangels, kaess}@cs.cmu.edu

2 S. Choudhury is with School of Computer Science and Engineering,University of Washington, Seattle, WA, USA. sanjibac@cs.uw.edu

This work was partially supported by the Office of Naval Research underaward N00014-16-1-2103.

Drift in estimate violates obstacles

Drift in estimate violates contact

Enforce equality

constraints

Enforce inequality constraints

(b)(a)

Fig. 1. (a) Mobile robot navigating down a corridor: Enforcing inequalityconstraints would restrict the space of possible robot trajectory estimates tothose not in collision. (b) Robot arm manipulating an object with uncertainpose: Enforcing equality constraints would restrict the space of possibleposes to those in contact with robot gripper.

Consider the problem of a robot manipulating an objectwhose pose is uncertain (Fig. I(b)). There are two primaryways to sense object pose: one using a noisy and oftenoccluded vision system, and another using a precise contactsensor placed on the arm. Under a typical unconstrainedoptimization framework, contact sensor measurements wouldbe incorporated as high confidence cost priors, or an equiv-alent of penalty terms in the optimization [5]. However,this need not guarantee satisfaction of these terms, whichdepends on ratio of their weights relative to other cost termsin the objective. Too disproportionate a ratio can lead toill-conditioning, causing numerical issues. Moreover, it isnot straightforward to incorporate inequality constraints inthe unconstrained framework—e.g. a robot moving down acorridor must lie within corridor boundaries even in presenceof a drifting state estimate indicating otherwise (Fig. I(a)).

The unconstrained incremental optimization framework isan incremental Gauss-Newton method that relies on updatingsparse matrix factorizations of the Jacobians efficiently, with-out needing to re-factorize at each time step [3]. These oper-ations are no longer possible when constraints, i.e., Karush-Kuhn-Tucker (KKT) conditions [6] are introduced. Our keyidea is to leverage primal-dual methods and work around thislimitation by splitting the optimization in two separate steps.The primal step retains the structure of unconstrained non-linear least squares, amenable to incremental Gauss Newtonmethods. The dual step is simply a gradient ascent step whichis incremental in nature. As a result, constraints are preciselysatisfied without having to recompute solutions from scratchevery time step.

We propose a framework ICS for doing incremental con-strained smoothing. ICS combines a primal-dual method like

the Augmented Lagrangian [7] with an incremental GaussNewton approach that reuses previously computed matrixfactorizations [3]. ICS works as follows: at each time step t,the robot receives new measurements and constraints, it thenincrementally updates state estimates for times 1 : t reusingmatrix factorizations from previous step and performing al-ternating primal-dual updates to convergence at current step.Hence, at any given time, it maintains a solution consistentwith all measurements received so far without having to re-solve for all constraints. Our main contributions are:

1) Introduce incremental constrained smoothing problem.2) Propose a primal dual framework with incremental

Gauss Newton steps for solving the problem.3) Provide empirical evaluation against baselines for state

estimation applications.

II. RELATED WORK

Simultaneous Localization and Mapping (SLAM) problemhas been well studied for several decades [1]. Early methodswere based on the well-known extended Kalman Filter [8, 9].However, such filtering-based methods had difficulty scalingdue to the curse of dimensionality [10, 11]. Eventually itwas recognized that by leveraging inherent sparsity, it waspossible to solve SLAM as a smoothing problem tractablyas a large but sparse nonlinear optimization over the entirerobot trajectory [12, 13]. While this enables handling a largenumbers of poses, solving it as a batch optimization requiresfactorizing a large Jacobian matrix composed of all previousmeasurements every new time step and is dependent onhaving a good linearization point. To address this, Kaesset al. [3] proposed incremental smoothing and mapping(iSAM) that solved this problem incrementally by updatingthe existing factorization with new measurements as opposedto factorizing from scratch, enabling real-time optimization.

Above methods formulate the problem as a MAP inferenceover a factor graph resulting in an unconstrained nonlinearoptimization problem. Within this framework, measurementsare incorporated as weighted terms in overall cost that pe-nalizes deviation of predicted measurements from observedvalues [2]. In many cases, however, observed measure-ments or knowledge about the environment can be bettermodeled as hard constraints in the optimization problem.Some recent methods [14–17] have been proposed towardsincorporating hard constraints into the factor graph frame-work. Cunningham et al. [14] perform hybrid elimination tosolve a system of equations with mixed unconstrained andequality constraint factors. Ta et al. [15] too solve for mixedunconstrained and equality constraint factors by utilizing aspecial QR factorization within a SQP framework. Jimenezet al. [17] extend to inequality constraints by treating activeinequalities as equality constraints in an active set. Choud-hary et al. [16] solve for a distributed constrained objectiveusing ADMM within a factor graph framework.

All these methods [14–17], however, are formulated forsolving a batch optimization objective. It isn’t clear howthese can extend to incremental solutions wherein matrixfactorizations from previous time steps are to be reused

to avoid re-solving from scratch. Another set of relatedbatch optimization techniques utilize Lagrangian duality forcertifying optimality in SLAM solutions [18, 19]. A relatedwork by Bai et al. [20] formulates incremental SLAM asa cycle/constraint selection problem finding an optimal setof consistent transformations. This differs from our use ofincremental denoting reuse of matrix factorizations fromprevious time steps.

III. PROBLEM FORMULATION

Maximum a posteriori (MAP) inference for localization(or SLAM) problems with Gaussian noise models is equiv-alent to solving a nonlinear least-squares problem [2]. Fora factor graph, MAP inference involves maximizing productof all factor graph potentials, that is,

x̂ = argmaxx

m∏i=1

φi(x) (1)

Assuming that φi(x) are all Gaussian factors corrupted byzero-mean, normally distributed noise,

φi(x) ∝ exp

{−1

2||fi(x)− zi||2Σi

}⇒ x̂ = argmin

x

1

2

m∑i=1

||fi(x)− zi||2Σi

(2)

where, fi(x) is a likelihood function predicting the expectedmeasurement given the state, zi is the actual measurement,and || · ||Σi

is the Mahalanobis distance with measurementcovariance Σi.

A. Unconstrained nonlinear least-squares

The optimization objective in Eq. 2 is an unconstrainednonlinear least-squares minimization. For general nonlinearfunctions fi(x), this objective is non-convex. However, ifwe have a reasonable initial guess available, we can usenonlinear optimization methods like Gauss-Newton (GN)to converge to a global minimum where gradient of theoptimization objective equals 0. GN proceeds by linearizingmeasurement functions fi(·) at each iteration using a first-order Taylor expansion as,

fi(x) = fi(x0 + δx) ≈ fi(x0) + Fiδx (3)

where, x0 is the linearization point, Fi = ∂fi(x)∂x

∣∣∣x0

is the

measurement Jacobian, and δx = x − x0 the state updatevector. Substituting the Taylor expansion back into Eq. 2,

δx∗ = argminδx

m∑i=1

||Fiδx−(zi − fi(x0)

)||2Σi

= argminδx

1

2

m∑i=1

||Aiδx− bi||22

= argminδx

1

2||Aδx− b||22

(4)

where,

Ai = Σ−1/2i Fi , bi = Σ

−1/2i

(zi − fi(x0

i ))

(5)

⇢2<latexit sha1_base64="I23ERclYwAwwHz1KaI9Bp5N4Av8=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIviqexWQY8FLx4r2A9ol5JNs21sNlmSrFCW/gcvHhTx6v/x5r8xbfegrQ8GHu/NMDMvTAQ31vO+UWFtfWNzq7hd2tnd2z8oHx61jEo1ZU2qhNKdkBgmuGRNy61gnUQzEoeCtcPx7cxvPzFtuJIPdpKwICZDySNOiXVSq6dHql/rlyte1ZsDrxI/JxXI0eiXv3oDRdOYSUsFMabre4kNMqItp4JNS73UsITQMRmyrqOSxMwE2fzaKT5zygBHSruSFs/V3xMZiY2ZxKHrjIkdmWVvJv7ndVMb3QQZl0lqmaSLRVEqsFV49joecM2oFRNHCNXc3YrpiGhCrQuo5ELwl19eJa1a1b+s1u6vKvXzPI4inMApXIAP11CHO2hAEyg8wjO8whtS6AW9o49FawHlM8fwB+jzB0HgjtQ=</latexit>

R(t�1)<latexit sha1_base64="97d4J+UwOcBQXjUAo+K0/qYJ6yM=">AAAB8HicbVBNSwMxEJ2tX7V+VT16CVahHiy7VdBjwYvHKvZD2rVk02wbmmSXJCuUpb/CiwdFvPpzvPlvTNs9aPXBwOO9GWbmBTFn2rjul5NbWl5ZXcuvFzY2t7Z3irt7TR0litAGiXik2gHWlDNJG4YZTtuxolgEnLaC0dXUbz1SpVkk78w4pr7AA8lCRrCx0v3tQ1o2p97JpFcsuRV3BvSXeBkpQYZ6r/jZ7UckEVQawrHWHc+NjZ9iZRjhdFLoJprGmIzwgHYslVhQ7aezgyfo2Cp9FEbKljRopv6cSLHQeiwC2ymwGepFbyr+53USE176KZNxYqgk80VhwpGJ0PR71GeKEsPHlmCimL0VkSFWmBibUcGG4C2+/Jc0qxXvrFK9OS/VjrI48nAAh1AGDy6gBtdQhwYQEPAEL/DqKOfZeXPe5605J5vZh19wPr4Bl56PhQ==</latexit>

d(t�1)<latexit sha1_base64="6IGTucMB2MIsC1gFGszG7fU3I/s=">AAAB8HicbVBNSwMxEM36WetX1aOXYBHqwbJbBT0WvHisYD+kXUs2m21Dk+ySzApl6a/w4kERr/4cb/4b03YP2vpg4PHeDDPzgkRwA6777aysrq1vbBa2its7u3v7pYPDlolTTVmTxiLWnYAYJrhiTeAgWCfRjMhAsHYwupn67SemDY/VPYwT5ksyUDzilICVHsLHrALn3tmkXyq7VXcGvEy8nJRRjka/9NULY5pKpoAKYkzXcxPwM6KBU8EmxV5qWELoiAxY11JFJDN+Njt4gk+tEuIo1rYU4Jn6eyIj0pixDGynJDA0i95U/M/rphBd+xlXSQpM0fmiKBUYYjz9HodcMwpibAmhmttbMR0STSjYjIo2BG/x5WXSqlW9i2rt7rJcx3kcBXSMTlAFeegK1dEtaqAmokiiZ/SK3hztvDjvzse8dcXJZ47QHzifP7JCj5M=</latexit>

(a) Step 1 (b) Step 2

while <!converged>

R(t)<latexit sha1_base64="z5X89k5GENXGVQSK/mLnmB5piMo=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoMQL2E3CnoMePEYxTwgWcPsZJIMmZ1dZnqFsOQjvHhQxKvf482/cZLsQRMLGoqqbrq7glgKg6777eTW1jc2t/LbhZ3dvf2D4uFR00SJZrzBIhnpdkANl0LxBgqUvB1rTsNA8lYwvpn5rSeujYjUA05i7od0qMRAMIpWat0/pmU8n/aKJbfizkFWiZeREmSo94pf3X7EkpArZJIa0/HcGP2UahRM8mmhmxgeUzamQ96xVNGQGz+dnzslZ1bpk0GkbSkkc/X3REpDYyZhYDtDiiOz7M3E/7xOgoNrPxUqTpArtlg0SCTBiMx+J32hOUM5sYQyLeythI2opgxtQgUbgrf88ippViveRaV6d1mqkSyOPJzAKZTBgyuowS3UoQEMxvAMr/DmxM6L8+58LFpzTjZzDH/gfP4AuYqPDw==</latexit>

d(t)<latexit sha1_base64="PN2/GgiNshq2qrUs5pf29vDFRr8=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSLUS0mqoMeCF48V7Ae0sWy2m3bpZhN2J0IJ/RFePCji1d/jzX/jts1BWx8MPN6bYWZekEhh0HW/nbX1jc2t7cJOcXdv/+CwdHTcMnGqGW+yWMa6E1DDpVC8iQIl7ySa0yiQvB2Mb2d++4lrI2L1gJOE+xEdKhEKRtFK7cFjVsGLab9UdqvuHGSVeDkpQ45Gv/TVG8QsjbhCJqkxXc9N0M+oRsEknxZ7qeEJZWM65F1LFY248bP5uVNybpUBCWNtSyGZq78nMhoZM4kC2xlRHJllbyb+53VTDG/8TKgkRa7YYlGYSoIxmf1OBkJzhnJiCWVa2FsJG1FNGdqEijYEb/nlVdKqVb3Lau3+qlwneRwFOIUzqIAH11CHO2hAExiM4Rle4c1JnBfn3flYtK45+cwJ/IHz+QPVPo8h</latexit>

G(t)<latexit sha1_base64="4ZUB6+i1EMZ/r0EOMGhm8SHBZl8=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoMQL2E3CnoMeNBjBPOAZA2zk0kyZHZ2mekVwpKP8OJBEa9+jzf/xkmyB00saCiquunuCmIpDLrut5NbW9/Y3MpvF3Z29/YPiodHTRMlmvEGi2Sk2wE1XArFGyhQ8nasOQ0DyVvB+Gbmt564NiJSDziJuR/SoRIDwShaqXX7mJbxfNorltyKOwdZJV5GSpCh3it+dfsRS0KukElqTMdzY/RTqlEwyaeFbmJ4TNmYDnnHUkVDbvx0fu6UnFmlTwaRtqWQzNXfEykNjZmEge0MKY7MsjcT//M6CQ6u/VSoOEGu2GLRIJEEIzL7nfSF5gzlxBLKtLC3EjaimjK0CRVsCN7yy6ukWa14F5Xq/WWpRrI48nACp1AGD66gBndQhwYwGMMzvMKbEzsvzrvzsWjNOdnMMfyB8/kDqJyPBA==</latexit>

g(t)<latexit sha1_base64="+aoanZRhkgMz9AtbG1GJYxzBlks=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSLUS0mqoMeCF48V7Ae0sWy2m3bpZhN2J0IJ/RFePCji1d/jzX/jts1BWx8MPN6bYWZekEhh0HW/nbX1jc2t7cJOcXdv/+CwdHTcMnGqGW+yWMa6E1DDpVC8iQIl7ySa0yiQvB2Mb2d++4lrI2L1gJOE+xEdKhEKRtFK7eFjVsGLab9UdqvuHGSVeDkpQ45Gv/TVG8QsjbhCJqkxXc9N0M+oRsEknxZ7qeEJZWM65F1LFY248bP5uVNybpUBCWNtSyGZq78nMhoZM4kC2xlRHJllbyb+53VTDG/8TKgkRa7YYlGYSoIxmf1OBkJzhnJiCWVa2FsJG1FNGdqEijYEb/nlVdKqVb3Lau3+qlwneRwFOIUzqIAH11CHO2hAExiM4Rle4c1JnBfn3flYtK45+cwJ/IHz+QPZ3I8k</latexit>

Givens rotations

H(t)<latexit sha1_base64="7imNoQVvwacI2So1Z1dtqzmpmgY=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoMQL2E3CnoMeMkxgnlAEsPsZDYZMju7zPQKYclHePGgiFe/x5t/4yTZgyYWNBRV3XR3+bEUBl3328ltbG5t7+R3C3v7B4dHxeOTlokSzXiTRTLSHZ8aLoXiTRQoeSfWnIa+5G1/cjf3209cGxGpB5zGvB/SkRKBYBSt1K4/pmW8nA2KJbfiLkDWiZeREmRoDIpfvWHEkpArZJIa0/XcGPsp1SiY5LNCLzE8pmxCR7xrqaIhN/10ce6MXFhlSIJI21JIFurviZSGxkxD33aGFMdm1ZuL/3ndBIPbfipUnCBXbLkoSCTBiMx/J0OhOUM5tYQyLeythI2ppgxtQgUbgrf68jppVSveVaV6f12qkSyOPJzBOZTBgxuoQR0a0AQGE3iGV3hzYufFeXc+lq05J5s5hT9wPn8AqiaPBQ==</latexit>

h(t)<latexit sha1_base64="VQnELN/KYf2CYBqfAs+yK96GUys=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSLUS0mqoMeCF48V7Ae0sWy2m3bpZhN2J0IJ/RFePCji1d/jzX/jts1BWx8MPN6bYWZekEhh0HW/nbX1jc2t7cJOcXdv/+CwdHTcMnGqGW+yWMa6E1DDpVC8iQIl7ySa0yiQvB2Mb2d++4lrI2L1gJOE+xEdKhEKRtFK7dFjVsGLab9UdqvuHGSVeDkpQ45Gv/TVG8QsjbhCJqkxXc9N0M+oRsEknxZ7qeEJZWM65F1LFY248bP5uVNybpUBCWNtSyGZq78nMhoZM4kC2xlRHJllbyb+53VTDG/8TKgkRa7YYlGYSoIxmf1OBkJzhnJiCWVa2FsJG1FNGdqEijYEb/nlVdKqVb3Lau3+qlwneRwFOIUzqIAH11CHO2hAExiM4Rle4c1JnBfn3flYtK45+cwJ/IHz+QPbZo8l</latexit>

G(t�1)<latexit sha1_base64="2ShCZ5G02Vd9OYzAQ4sJA9+gtXk=">AAAB8HicbVBNSwMxEJ31s9avqkcvoUWoB8tuFfRY8KDHCvZD2rVk02wbmmSXJCuUpb/CiwdFvPpzvPlvTNs9aOuDgcd7M8zMC2LOtHHdb2dldW19YzO3ld/e2d3bLxwcNnWUKEIbJOKRagdYU84kbRhmOG3HimIRcNoKRtdTv/VElWaRvDfjmPoCDyQLGcHGSg83j2nZnHmnk16h5FbcGdAy8TJSggz1XuGr249IIqg0hGOtO54bGz/FyjDC6STfTTSNMRnhAe1YKrGg2k9nB0/QiVX6KIyULWnQTP09kWKh9VgEtlNgM9SL3lT8z+skJrzyUybjxFBJ5ovChCMToen3qM8UJYaPLcFEMXsrIkOsMDE2o7wNwVt8eZk0qxXvvFK9uyjVilkcOTiGIpTBg0uowS3UoQEEBDzDK7w5ynlx3p2PeeuKk80cwR84nz+GAI94</latexit>

g(t�1)<latexit sha1_base64="ReKZKx7hvIEVmG9zVBARgFrKQ0k=">AAAB8HicbVBNSwMxEJ2tX7V+Vb0IXoJFqAfLbhX0WPDisYL9kHYt2TTbhibZJckKZemv8OJBEa/+HG/+G9N2D9r6YODx3gwz84KYM21c99vJrayurW/kNwtb2zu7e8X9g6aOEkVog0Q8Uu0Aa8qZpA3DDKftWFEsAk5bwehm6reeqNIskvdmHFNf4IFkISPYWOlh8JiWzbl3NukVS27FnQEtEy8jJchQ7xW/uv2IJIJKQzjWuuO5sfFTrAwjnE4K3UTTGJMRHtCOpRILqv10dvAEnVqlj8JI2ZIGzdTfEykWWo9FYDsFNkO96E3F/7xOYsJrP2UyTgyVZL4oTDgyEZp+j/pMUWL42BJMFLO3IjLEChNjMyrYELzFl5dJs1rxLirVu8tS7SiLIw/HcAJl8OAKanALdWgAAQHP8ApvjnJenHfnY96ac7KZQ/gD5/MHtH6Pjg==</latexit>

H(t�1)<latexit sha1_base64="LwtLV86VYoQrN6+plqv14EX/Lts=">AAAB8HicbVBNSwMxEJ31s9avqkc9BItQD5bdKuix4KXHCvZD2rVk02wbmmSXJCuUpb/CiwdFvPpzvPlvTNs9aOuDgcd7M8zMC2LOtHHdb2dldW19YzO3ld/e2d3bLxwcNnWUKEIbJOKRagdYU84kbRhmOG3HimIRcNoKRrdTv/VElWaRvDfjmPoCDyQLGcHGSg+1x7RkLrzzSa9QdMvuDGiZeBkpQoZ6r/DV7UckEVQawrHWHc+NjZ9iZRjhdJLvJprGmIzwgHYslVhQ7aezgyfozCp9FEbKljRopv6eSLHQeiwC2ymwGepFbyr+53USE974KZNxYqgk80VhwpGJ0PR71GeKEsPHlmCimL0VkSFWmBibUd6G4C2+vEyalbJ3Wa7cXRWrJ1kcOTiGUyiBB9dQhRrUoQEEBDzDK7w5ynlx3p2PeeuKk80cwR84nz+Fvo9z</latexit>

h(t�1)<latexit sha1_base64="r8kXZSsVNNEa+E5vV+MtXWZu/jo=">AAAB8HicbVBNSwMxEJ2tX7V+VT3qIViEerDsVkGPBS8eK9gPadeSTbNtaJJdkqxQlv4KLx4U8erP8ea/MW33oK0PBh7vzTAzL4g508Z1v53cyura+kZ+s7C1vbO7V9w/aOooUYQ2SMQj1Q6wppxJ2jDMcNqOFcUi4LQVjG6mfuuJKs0ieW/GMfUFHkgWMoKNlR6Gj2nZnHtnk16x5FbcGdAy8TJSggz1XvGr249IIqg0hGOtO54bGz/FyjDC6aTQTTSNMRnhAe1YKrGg2k9nB0/QqVX6KIyULWnQTP09kWKh9VgEtlNgM9SL3lT8z+skJrz2UybjxFBJ5ovChCMToen3qM8UJYaPLcFEMXsrIkOsMDE2o4INwVt8eZk0qxXvolK9uyzVjrM48nAEJ1AGD66gBrdQhwYQEPAMr/DmKOfFeXc+5q05J5s5hD9wPn8Atz6Pkw==</latexit>

partial Jacobian

partial Jacobian

new measurements

new equality constraints

new inequality constraints

wT<latexit sha1_base64="kWFm4rsN8AN+9do6/mbXjK51L0o=">AAAB83icbVDLSsNAFL2pr1pfVZe6GCyCq5JUQZcFNy4r9AVNLJPppB06mYSZiVJCf8ONC0Xc+jPu/BsnaRbaemDgcM693DPHjzlT2ra/rdLa+sbmVnm7srO7t39QPTzqqiiRhHZIxCPZ97GinAna0Uxz2o8lxaHPac+f3mZ+75FKxSLR1rOYeiEeCxYwgrWRXDfEeuIH6dP8oT2s1uy6nQOtEqcgNSjQGla/3FFEkpAKTThWauDYsfZSLDUjnM4rbqJojMkUj+nAUIFDqrw0zzxH50YZoSCS5gmNcvX3RopDpWahbyazjGrZy8T/vEGigxsvZSJONBVkcShIONIRygpAIyYp0XxmCCaSmayITLDERJuaKqYEZ/nLq6TbqDuX9cb9Va15WtRRhhM4gwtw4BqacAct6ACBGJ7hFd6sxHqx3q2PxWjJKnaO4Q+szx9SQJG/</latexit>

�<latexit sha1_base64="15ljRBvV6YHkKl7jo0dYR7dExaI=">AAAB7XicbVDLSgNBEOyNrxhfUY96GAyCp7AbBT0GvHiMYB6QLGF2MpuMmccyMyuEJf/gxYMiXv0fb/6Nk2QPmljQUFR1090VJZwZ6/vfXmFtfWNzq7hd2tnd2z8oHx61jEo1oU2iuNKdCBvKmaRNyyynnURTLCJO29H4dua3n6g2TMkHO0loKPBQspgRbJ3U6g2xELhfrvhVfw60SoKcVCBHo1/+6g0USQWVlnBsTDfwExtmWFtGOJ2WeqmhCSZjPKRdRyUW1ITZ/NopOnfKAMVKu5IWzdXfExkWxkxE5DoFtiOz7M3E/7xuauObMGMySS2VZLEoTjmyCs1eRwOmKbF84ggmmrlbERlhjYl1AZVcCMHyy6ukVasGl9Xa/VWlfprHUYQTOIMLCOAa6nAHDWgCgUd4hld485T34r17H4vWgpfPHMMfeJ8/fMuO8w==</latexit>

y<latexit sha1_base64="JV7Xy/uzB1dRgpSgdbMk4Yu6p5M=">AAAB6HicbVBNS8NAEJ3Ur1q/qh71sFgETyWpgh4LXjy2YGuhDWWznbRrN5uwuxFK6C/w4kERr/4kb/4bt20O2vpg4PHeDDPzgkRwbVz32ymsrW9sbhW3Szu7e/sH5cOjto5TxbDFYhGrTkA1Ci6xZbgR2EkU0igQ+BCMb2f+wxMqzWN5byYJ+hEdSh5yRo2VmpN+ueJW3TnIKvFyUoEcjX75qzeIWRqhNExQrbuemxg/o8pwJnBa6qUaE8rGdIhdSyWNUPvZ/NApObfKgISxsiUNmau/JzIaaT2JAtsZUTPSy95M/M/rpia88TMuk9SgZItFYSqIicnsazLgCpkRE0soU9zeStiIKsqMzaZkQ/CWX14l7VrVu6zWmleV+mkeRxFO4AwuwINrqMMdNKAFDBCe4RXenEfnxXl3PhatBSefOYY/cD5/AN2djNs=</latexit>

R(t)<latexit sha1_base64="LQNGq8XBY/xjG6Je/EEw3+uon98=">AAAB7nicbVA9TwJBEJ3DL8Qv1FKLjcQEG3KHJlqS2FiiESEBJHvLHmzY27vszpmQCz/CxkJjbP09dv4bF7hCwZdM8vLeTGbm+bEUBl3328mtrK6tb+Q3C1vbO7t7xf2DBxMlmvEGi2SkWz41XArFGyhQ8lasOQ19yZv+6HrqN5+4NiJS9ziOeTekAyUCwShaqXn3mJbxbNIrltyKOwNZJl5GSpCh3it+dfoRS0KukElqTNtzY+ymVKNgkk8KncTwmLIRHfC2pYqG3HTT2bkTcmqVPgkibUshmam/J1IaGjMOfdsZUhyaRW8q/ue1EwyuuqlQcYJcsfmiIJEEIzL9nfSF5gzl2BLKtLC3EjakmjK0CRVsCN7iy8vkoVrxzivV24tS7TiLIw9HcAJl8OASanADdWgAgxE8wyu8ObHz4rw7H/PWnJPNHMIfOJ8/uFaPCw==</latexit>

R(t)T

<latexit sha1_base64="RRRDNIbfEpNkLgBRjmeeM5UW5SY=">AAAB8nicbVBNS8NAEN34WetX1aMeFotQLyWpgh4LXjxW6RekadlsN+3STTbsToQS8jO8eFDEq7/Gm//GbZuDtj4YeLw3w8w8PxZcg21/W2vrG5tb24Wd4u7e/sFh6ei4rWWiKGtRKaTq+kQzwSPWAg6CdWPFSOgL1vEndzO/88SU5jJqwjRmXkhGEQ84JWAk97GfphW4zPrNbFAq21V7DrxKnJyUUY7GoPTVG0qahCwCKojWrmPH4KVEAaeCZcVeollM6ISMmGtoREKmvXR+coYvjDLEgVSmIsBz9fdESkKtp6FvOkMCY73szcT/PDeB4NZLeRQnwCK6WBQkAoPEs//xkCtGQUwNIVRxcyumY6IIBZNS0YTgLL+8Stq1qnNVrT1cl+tneRwFdIrOUQU56AbV0T1qoBaiSKJn9IreLLBerHfrY9G6ZuUzJ+gPrM8f3taQ3Q==</latexit>

G(t)T

<latexit sha1_base64="IObaYiXch7TjSIIz7Vk5xBBhg1Y=">AAAB8nicbVBNS8NAEN34WetX1aMeFotQLyWpgh4LHvRYoV+QpmWz3bRLN9mwOxFKyM/w4kERr/4ab/4bt20O2vpg4PHeDDPz/FhwDbb9ba2tb2xubRd2irt7+weHpaPjtpaJoqxFpZCq6xPNBI9YCzgI1o0VI6EvWMef3M38zhNTmsuoCdOYeSEZRTzglICR3Pt+mlbgMus3s0GpbFftOfAqcXJSRjkag9JXbyhpErIIqCBau44dg5cSBZwKlhV7iWYxoRMyYq6hEQmZ9tL5yRm+MMoQB1KZigDP1d8TKQm1noa+6QwJjPWyNxP/89wEglsv5VGcAIvoYlGQCAwSz/7HQ64YBTE1hFDFza2YjokiFExKRROCs/zyKmnXqs5VtfZ4Xa6f5XEU0Ck6RxXkoBtURw+ogVqIIome0St6s8B6sd6tj0XrmpXPnKA/sD5/AM28kNI=</latexit>

H(t)T

<latexit sha1_base64="n/Cfdb/mYcthTpWtwL3cR1Q4fso=">AAAB8nicbVBNS8NAEN34WetX1aMeFotQLyWpgh4LXnqs0C9I07LZbtqlm03YnQgl5Gd48aCIV3+NN/+N2zYHbX0w8Hhvhpl5fiy4Btv+tjY2t7Z3dgt7xf2Dw6Pj0slpR0eJoqxNIxGpnk80E1yyNnAQrBcrRkJfsK4/fZj73SemNI9kC2Yx80IyljzglICR3MYgTStwnQ1a2bBUtqv2AnidODkpoxzNYemrP4poEjIJVBCtXceOwUuJAk4Fy4r9RLOY0CkZM9dQSUKmvXRxcoavjDLCQaRMScAL9fdESkKtZ6FvOkMCE73qzcX/PDeB4N5LuYwTYJIuFwWJwBDh+f94xBWjIGaGEKq4uRXTCVGEgkmpaEJwVl9eJ51a1bmp1h5vy/WLPI4COkeXqIIcdIfqqIGaqI0oitAzekVvFlgv1rv1sWzdsPKZM/QH1ucPz0qQ0w==</latexit>

d(t)<latexit sha1_base64="XVM75XirEIgtXmXAe/HYZLn8N+M=">AAAB8HicbVBNSwMxEJ2tX7V+VT3qIViEeim7VdBjwYvHCrZW2rVks2kbmmSXJCuUZX+FFw+KePXnePPfmLZ70NYHA4/3ZpiZF8ScaeO6305hZXVtfaO4Wdra3tndK+8ftHWUKEJbJOKR6gRYU84kbRlmOO3EimIRcHofjK+n/v0TVZpF8s5MYuoLPJRswAg2VnoIH9O0as6yrF+uuDV3BrRMvJxUIEezX/7qhRFJBJWGcKx113Nj46dYGUY4zUq9RNMYkzEe0q6lEguq/XR2cIZOrRKiQaRsSYNm6u+JFAutJyKwnQKbkV70puJ/Xjcxgys/ZTJODJVkvmiQcGQiNP0ehUxRYvjEEkwUs7ciMsIKE2MzKtkQvMWXl0m7XvPOa/Xbi0rjOI+jCEdwAlXw4BIacANNaAEBAc/wCm+Ocl6cd+dj3lpw8plD+APn8wecDZAp</latexit>

y<latexit sha1_base64="JV7Xy/uzB1dRgpSgdbMk4Yu6p5M=">AAAB6HicbVBNS8NAEJ3Ur1q/qh71sFgETyWpgh4LXjy2YGuhDWWznbRrN5uwuxFK6C/w4kERr/4kb/4bt20O2vpg4PHeDDPzgkRwbVz32ymsrW9sbhW3Szu7e/sH5cOjto5TxbDFYhGrTkA1Ci6xZbgR2EkU0igQ+BCMb2f+wxMqzWN5byYJ+hEdSh5yRo2VmpN+ueJW3TnIKvFyUoEcjX75qzeIWRqhNExQrbuemxg/o8pwJnBa6qUaE8rGdIhdSyWNUPvZ/NApObfKgISxsiUNmau/JzIaaT2JAtsZUTPSy95M/M/rpia88TMuk9SgZItFYSqIicnsazLgCpkRE0soU9zeStiIKsqMzaZkQ/CWX14l7VrVu6zWmleV+mkeRxFO4AwuwINrqMMdNKAFDBCe4RXenEfnxXl3PhatBSefOYY/cD5/AN2djNs=</latexit>

(<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

⇢1<latexit sha1_base64="3KowpCv0oRPSCe5hNd+ZHbNbHbo=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIvgqexWwR4LXjxWsB/QLiWbZtvYbLIkWaEs/Q9ePCji1f/jzX9j2u5BWx8MPN6bYWZemAhurOd9o8LG5tb2TnG3tLd/cHhUPj5pG5VqylpUCaW7ITFMcMlallvBuolmJA4F64ST27nfeWLacCUf7DRhQUxGkkecEuukdl+P1cAflCte1VsArxM/JxXI0RyUv/pDRdOYSUsFMabne4kNMqItp4LNSv3UsITQCRmxnqOSxMwE2eLaGb5wyhBHSruSFi/U3xMZiY2ZxqHrjIkdm1VvLv7n9VIb1YOMyyS1TNLloigV2Co8fx0PuWbUiqkjhGrubsV0TDSh1gVUciH4qy+vk3at6l9Va/fXlUY9j6MIZ3AOl+DDDTTgDprQAgqP8Ayv8IYUekHv6GPZWkD5zCn8Afr8AUXGjuU=</latexit> G(t)

<latexit sha1_base64="8Fy2QxgKEMeqIp+tRCCt+ZgDPGU=">AAAB7nicbVA9SwNBEJ2LXzF+RS21WAxCbMJdFLQMWGgZwZhAEsPeZi9Zsrd37M4J4ciPsLFQxNbfY+e/cZNcoYkPBh7vzTAzz4+lMOi6305uZXVtfSO/Wdja3tndK+4fPJgo0Yw3WCQj3fKp4VIo3kCBkrdizWnoS970R9dTv/nEtRGRusdxzLshHSgRCEbRSs2bx7SMZ5NeseRW3BnIMvEyUoIM9V7xq9OPWBJyhUxSY9qeG2M3pRoFk3xS6CSGx5SN6IC3LVU05Kabzs6dkFOr9EkQaVsKyUz9PZHS0Jhx6NvOkOLQLHpT8T+vnWBw1U2FihPkis0XBYkkGJHp76QvNGcox5ZQpoW9lbAh1ZShTahgQ/AWX14mD9WKd16p3l2UasdZHHk4ghMogweXUINbqEMDGIzgGV7hzYmdF+fd+Zi35pxs5hD+wPn8AadojwA=</latexit>

g(t)<latexit sha1_base64="e5f47Q2H0TT+d8zauRid4rUeEk4=">AAAB7nicbVDLSgNBEOyNrxhfUY96GAxCvITdKOgx4MVjBPOAZA2zk0kyZHZ2mekVwpKP8OJBEa9+jzf/xkmyB00saCiquunuCmIpDLrut5NbW9/Y3MpvF3Z29/YPiodHTRMlmvEGi2Sk2wE1XArFGyhQ8nasOQ0DyVvB+Hbmt564NiJSDziJuR/SoRIDwShaqTV8TMt4Me0VS27FnYOsEi8jJchQ7xW/uv2IJSFXyCQ1puO5Mfop1SiY5NNCNzE8pmxMh7xjqaIhN346P3dKzq3SJ4NI21JI5urviZSGxkzCwHaGFEdm2ZuJ/3mdBAc3fipUnCBXbLFokEiCEZn9TvpCc4ZyYgllWthbCRtRTRnahAo2BG/55VXSrFa8y0r1/qpUO83iyMMJnEEZPLiGGtxBHRrAYAzP8ApvTuy8OO/Ox6I152Qzx/AHzucP2KiPIA==</latexit>

max

(<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

(<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

(<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

(<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

0 , (<latexit sha1_base64="PUShaRw80u8ojux4CR20UTSzaRw=">AAAB6HicbVDLTgJBEOzFF+IL9ehlIjHhRHbRRI4kXjxCIo8ENmR2aGBkdnYzM2tCNnyBFw8a49VP8ubfOMAeFKykk0pVd7q7glhwbVz328ltbe/s7uX3CweHR8cnxdOzto4SxbDFIhGpbkA1Ci6xZbgR2I0V0jAQ2Ammdwu/84RK80g+mFmMfkjHko84o8ZKzfKgWHIr7hJkk3gZKUGGxqD41R9GLAlRGiao1j3PjY2fUmU4Ezgv9BONMWVTOsaepZKGqP10eeicXFllSEaRsiUNWaq/J1Iaaj0LA9sZUjPR695C/M/rJWZU81Mu48SgZKtFo0QQE5HF12TIFTIjZpZQpri9lbAJVZQZm03BhuCtv7xJ2tWKd12pNm9K9VoWRx4u4BLK4MEt1OEeGtACBgjP8ApvzqPz4rw7H6vWnJPNnMMfOJ8/a0WMpg==</latexit>

H(t)<latexit sha1_base64="0eA7RzvUNbTKimV6fMwgmfy3xy8=">AAAB7nicbVDLSgNBEOz1GeMr6lEPg0GIl7AbBT0GvOQYwTwgWcPsZJIMmZ1dZnqFsOQjvHhQxKvf482/cZLsQRMLGoqqbrq7glgKg6777aytb2xubed28rt7+weHhaPjpokSzXiDRTLS7YAaLoXiDRQoeTvWnIaB5K1gfDfzW09cGxGpB5zE3A/pUImBYBSt1Ko9piW8nPYKRbfszkFWiZeRImSo9wpf3X7EkpArZJIa0/HcGP2UahRM8mm+mxgeUzamQ96xVNGQGz+dnzslF1bpk0GkbSkkc/X3REpDYyZhYDtDiiOz7M3E/7xOgoNbPxUqTpArtlg0SCTBiMx+J32hOUM5sYQyLeythI2opgxtQnkbgrf88ippVsreVblyf12snmVx5OAUzqEEHtxAFWpQhwYwGMMzvMKbEzsvzrvzsWhdc7KZE/gD5/MHqPKPAQ==</latexit>

h(t)<latexit sha1_base64="0YH1gOfjpiB+MQbMVG9NPo0JLgk=">AAAB7nicbVDLSgNBEOyNrxhfUY96GAxCvITdKOgx4MVjBPOAZA2zk0kyZHZ2mekVwpKP8OJBEa9+jzf/xkmyB00saCiquunuCmIpDLrut5NbW9/Y3MpvF3Z29/YPiodHTRMlmvEGi2Sk2wE1XArFGyhQ8nasOQ0DyVvB+Hbmt564NiJSDziJuR/SoRIDwShaqTV6TMt4Me0VS27FnYOsEi8jJchQ7xW/uv2IJSFXyCQ1puO5Mfop1SiY5NNCNzE8pmxMh7xjqaIhN346P3dKzq3SJ4NI21JI5urviZSGxkzCwHaGFEdm2ZuJ/3mdBAc3fipUnCBXbLFokEiCEZn9TvpCc4ZyYgllWthbCRtRTRnahAo2BG/55VXSrFa8y0r1/qpUO83iyMMJnEEZPLiGGtxBHRrAYAzP8ApvTuy8OO/Ox6I152Qzx/AHzucP2jKPIQ==</latexit>

�xj<latexit sha1_base64="OdbUrK5RG8CtBKQ8YwtvOkhyMZs=">AAAB83icbVDLSgNBEOyNrxhfUY96GAyCp7AbBT0GvHiMYB6QXcPs7CQZM/tgplcMS37DiwdFvPoz3vwbJ8keNLGgoajqprvLT6TQaNvfVmFldW19o7hZ2tre2d0r7x+0dJwqxpsslrHq+FRzKSLeRIGSdxLFaehL3vZH11O//ciVFnF0h+OEeyEdRKIvGEUjuW7AJVLydJ89THrlil21ZyDLxMlJBXI0euUvN4hZGvIImaRadx07QS+jCgWTfFJyU80TykZ0wLuGRjTk2stmN0/IqVEC0o+VqQjJTP09kdFQ63Hom86Q4lAvelPxP6+bYv/Ky0SUpMgjNl/UTyXBmEwDIIFQnKEcG0KZEuZWwoZUUYYmppIJwVl8eZm0alXnvFq7vajUj/M4inAEJ3AGDlxCHW6gAU1gkMAzvMKblVov1rv1MW8tWPnMIfyB9fkDBieRjg==</latexit>

�xj<latexit sha1_base64="OdbUrK5RG8CtBKQ8YwtvOkhyMZs=">AAAB83icbVDLSgNBEOyNrxhfUY96GAyCp7AbBT0GvHiMYB6QXcPs7CQZM/tgplcMS37DiwdFvPoz3vwbJ8keNLGgoajqprvLT6TQaNvfVmFldW19o7hZ2tre2d0r7x+0dJwqxpsslrHq+FRzKSLeRIGSdxLFaehL3vZH11O//ciVFnF0h+OEeyEdRKIvGEUjuW7AJVLydJ89THrlil21ZyDLxMlJBXI0euUvN4hZGvIImaRadx07QS+jCgWTfFJyU80TykZ0wLuGRjTk2stmN0/IqVEC0o+VqQjJTP09kdFQ63Hom86Q4lAvelPxP6+bYv/Ky0SUpMgjNl/UTyXBmEwDIIFQnKEcG0KZEuZWwoZUUYYmppIJwVl8eZm0alXnvFq7vajUj/M4inAEJ3AGDlxCHW6gAU1gkMAzvMKblVov1rv1MW8tWPnMIfyB9fkDBieRjg==</latexit>

�xj<latexit sha1_base64="OdbUrK5RG8CtBKQ8YwtvOkhyMZs=">AAAB83icbVDLSgNBEOyNrxhfUY96GAyCp7AbBT0GvHiMYB6QXcPs7CQZM/tgplcMS37DiwdFvPoz3vwbJ8keNLGgoajqprvLT6TQaNvfVmFldW19o7hZ2tre2d0r7x+0dJwqxpsslrHq+FRzKSLeRIGSdxLFaehL3vZH11O//ciVFnF0h+OEeyEdRKIvGEUjuW7AJVLydJ89THrlil21ZyDLxMlJBXI0euUvN4hZGvIImaRadx07QS+jCgWTfFJyU80TykZ0wLuGRjTk2stmN0/IqVEC0o+VqQjJTP09kdFQ63Hom86Q4lAvelPxP6+bYv/Ky0SUpMgjNl/UTyXBmEwDIIFQnKEcG0KZEuZWwoZUUYYmppIJwVl8eZm0alXnvFq7vajUj/M4inAEJ3AGDlxCHW6gAU1gkMAzvMKblVov1rv1MW8tWPnMIfyB9fkDBieRjg==</latexit>

vj<latexit sha1_base64="qlRkkhsdP6vSCmRKH1iDUBgvXH0=">AAAB7HicbVBNTwIxEJ3FL8Qv1KMeGomJJ7KLJnok8eIRExdIYCXd0oVKt920XRKy4Td48aAxXv1B3vw3FtiDgi+Z5OW9mczMCxPOtHHdb6ewtr6xuVXcLu3s7u0flA+PmlqmilCfSC5VO8Saciaob5jhtJ0oiuOQ01Y4up35rTFVmknxYCYJDWI8ECxiBBsr+ePH7GnaK1fcqjsHWiVeTiqQo9Erf3X7kqQxFYZwrHXHcxMTZFgZRjidlrqppgkmIzygHUsFjqkOsvmxU3RulT6KpLIlDJqrvycyHGs9iUPbGWMz1MveTPzP66QmugkyJpLUUEEWi6KUIyPR7HPUZ4oSwyeWYKKYvRWRIVaYGJtPyYbgLb+8Spq1qndZrd1fVeqneRxFOIEzuAAPrqEOd9AAHwgweIZXeHOE8+K8Ox+L1oKTzxzDHzifPxg7jsA=</latexit>

uj<latexit sha1_base64="TSZ6gS02pMWiTUakW8FOE0/t+1M=">AAAB7nicbVBNS8NAEJ3Ur1q/qh71sFgETyVRQY8FLx4r2A9oa9lsN+3azSbsToQS8iO8eFDEq7/Hm//GbZuDtj4YeLw3w8w8P5bCoOt+O4WV1bX1jeJmaWt7Z3evvH/QNFGiGW+wSEa67VPDpVC8gQIlb8ea09CXvOWPb6Z+64lrIyJ1j5OY90I6VCIQjKKVWslDmj5mWb9ccavuDGSZeDmpQI56v/zVHUQsCblCJqkxHc+NsZdSjYJJnpW6ieExZWM65B1LFQ256aWzczNyapUBCSJtSyGZqb8nUhoaMwl92xlSHJlFbyr+53USDK57qVBxglyx+aIgkQQjMv2dDITmDOXEEsq0sLcSNqKaMrQJlWwI3uLLy6R5XvUuqud3l5XacR5HEY7gBM7AgyuowS3UoQEMxvAMr/DmxM6L8+58zFsLTj5zCH/gfP4A3XCPyw==</latexit>

vj�1<latexit sha1_base64="DUOrbYVSNV2Y6qricwAlc4rIABs=">AAAB7nicbVDLSgNBEOz1GeMr6lEPg0HwYtiNgh4DXjxGMA9I1jA76U3GzM4uM7OBsOQjvHhQxKvf482/cfI4aGJBQ1HVTXdXkAiujet+Oyura+sbm7mt/PbO7t5+4eCwruNUMayxWMSqGVCNgkusGW4ENhOFNAoENoLB7cRvDFFpHssHM0rQj2hP8pAzaqzUGD5mTxfeuFMouiV3CrJMvDkpwhzVTuGr3Y1ZGqE0TFCtW56bGD+jynAmcJxvpxoTyga0hy1LJY1Q+9n03DE5s0qXhLGyJQ2Zqr8nMhppPYoC2xlR09eL3kT8z2ulJrzxMy6T1KBks0VhKoiJyeR30uUKmREjSyhT3N5KWJ8qyoxNKG9D8BZfXib1csm7LJXvr4qVk3kcOTiGUzgHD66hAndQhRowGMAzvMKbkzgvzrvzMWtdceYzR/AHzucP9IqPMg==</latexit>

uj�1<latexit sha1_base64="7UTD7O512P44RmjY4R942jEJMUI=">AAAB7nicbVDLSgNBEOyNrxhfUY96GAyCF8NuFPQY8OIxgnlAsobZSW8yZnZ2mZkVwpKP8OJBEa9+jzf/xsnjoIkFDUVVN91dQSK4Nq777eRWVtfWN/Kbha3tnd294v5BQ8epYlhnsYhVK6AaBZdYN9wIbCUKaRQIbAbDm4nffEKleSzvzShBP6J9yUPOqLFSM33IHs+9cbdYcsvuFGSZeHNSgjlq3eJXpxezNEJpmKBatz03MX5GleFM4LjQSTUmlA1pH9uWShqh9rPpuWNyapUeCWNlSxoyVX9PZDTSehQFtjOiZqAXvYn4n9dOTXjtZ1wmqUHJZovCVBATk8nvpMcVMiNGllCmuL2VsAFVlBmbUMGG4C2+vEwalbJ3Ua7cXZaqx/M48nAEJ3AGHlxBFW6hBnVgMIRneIU3J3FenHfnY9aac+Yzh/AHzucP8wCPMQ==</latexit>

Eq. 15a

Eq. 15b

Eq. 12b

Eq. 12c

vj�1

<latexit sha1_base64="/76/icVvX6ddeoYiQO+S+kfqxk0=">AAAB7nicbVDLSgNBEOz1GeMr6tHLYBS8GHYloseAF48RzAOSGGYnvcmY2dllZjYQlnyEFw+KePV7vPk3TpI9aGJBQ1HVTXeXHwuujet+Oyura+sbm7mt/PbO7t5+4eCwrqNEMayxSESq6VONgkusGW4ENmOFNPQFNvzh7dRvjFBpHskHM46xE9K+5AFn1FipMXpMny68SbdQdEvuDGSZeBkpQoZqt/DV7kUsCVEaJqjWLc+NTSelynAmcJJvJxpjyoa0jy1LJQ1Rd9LZuRNyZpUeCSJlSxoyU39PpDTUehz6tjOkZqAXvan4n9dKTHDTSbmME4OSzRcFiSAmItPfSY8rZEaMLaFMcXsrYQOqKDM2obwNwVt8eZnUL0teuXR1Xy5WTrM4cnAMJ3AOHlxDBe6gCjVgMIRneIU3J3ZenHfnY9664mQzR/AHzucP+DyPPg==</latexit>

uj�1

<latexit sha1_base64="cj999SNZORKm6mvJoDUuQ8Mp5Lg=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBqPgxbArET0GvHiMYB6QxDA7mU3GzM4uM71CWPIRXjwo4tXv8ebfOEn2oIkFDUVVN91dfiyFQdf9dnIrq2vrG/nNwtb2zu5ecf+gYaJEM15nkYx0y6eGS6F4HQVK3oo1p6EvedMf3Uz95hPXRkTqHscx74Z0oEQgGEUrNZOH9PHcm/SKJbfszkCWiZeREmSo9YpfnX7EkpArZJIa0/bcGLsp1SiY5JNCJzE8pmxEB7xtqaIhN910du6EnFqlT4JI21JIZurviZSGxoxD33aGFIdm0ZuK/3ntBIPrbipUnCBXbL4oSCTBiEx/J32hOUM5toQyLeythA2ppgxtQgUbgrf48jJpXJS9SvnyrlKqnmRx5OEIjuEMPLiCKtxCDerAYATP8ApvTuy8OO/Ox7w152Qzh/AHzucP9rKPPQ==</latexit>

Fig. 2. Overview of set of operations involved at every new time step t of our ICS framework. (a) Step 1 updates to the most recent matrix factorizationR(t) and Jacobians G(t), H(t) based on new measurements and new equality, inequality constraints arriving at time step t. (b) Step 2 performs theprimal-dual updates iteratively until convergence to ensure that δx∗ satisfies the equality, inequality constraints within a threshold.

and, A, b are constructed by collecting all Ai, bi togetherinto one giant matrix, vector respectively. Once the linearsystem is solved, x0 is updated according to x0 ← x0 + δx∗

and we iterate until convergence criteria are met.

B. Constrained nonlinear least-squares

Consider now the same least-squares objective as in Eq. 2but now subject to a set of nonlinear equality and inequalityconstraints,

x̂ =argminx

1

2

m∑i=1

||fi(x)− zi||2Σi

s.t., gj(x) = 0 , j = 1, . . . p

hk(x) ≤ 0 , k = 1, . . . q

(6)

Linearizing measurement functions fi(·), gj(·), hk(·),about a linearization point x0 similar to Eq. 3 would resultin a subproblem of form,

δx∗ =argminδx

1

2

m∑i=1

||Aδx− b||22

s.t., Gδx+ g(x0) = 0

Hδx+ h(x0) ≤ 0

(7)

where, A, b are constructed by collecting all Ai, bi togethersimilar to the unconstrained case in Eqs. 4, 5. G ∈ Rp×n,H ∈ Rq×n here are jacobians of equality, inequality con-straints respectively, and g(·) ∈ Rp, h(·) ∈ Rq are equality,inequality constraint evaluations at linearization point x0.

IV. APPROACH

We want to solve these optimization problems incremen-tally, i.e. as measurements and constraints arrive. Section IV-A illustrates how to solve the unconstrained subproblem inEq. 4 incrementally based on Givens rotations [3]. However,the constrained subproblem in Eq. 7 presents an additionalchallenge due to additional linearized equality/inequalityconstraints. It no longer suffices to set the gradient to 0,instead, the KarushKuhnTucker (KKT) conditions [6] needto be satisfied. In Section IV-B, we show how to do this ina manner that allows us to use Givens rotation to facilitateincremental updates.

A. Incremental unconstrained optimization

The unconstrained objective in Eq. 4 is solved as,

∇δx1

2||Aδx− b||2 = 0

⇒ ATAδx = AT b(8)

Eq. 8 is referred to as the normal equations. Since A istypically sparse for localization (or SLAM) problems, we cansolve these normal equations efficiently using sparse matrixfactorization like QR decomposition [2, 3]. That is, factoringm×n matrix A with m ≥ n as Q

[R 0

]T, and substituting

this factorization into the normal equations in Eq. 8,[RT 0

]QTQ

[R0

]δx =

[RT 0

]QT b

⇒ RTRδx =[RT 0

] [de

]⇒ Rδx = d

(9)

where Q is a m×m orthogonal matrix with QTQ = I , R isa n× n upper triangular matrix, and d ∈ Rn, e ∈ R(m−n).

To solve this incrementally, when a new measurementat time step t arrives, it is more efficient to modify theprevious time step factorization R(t−1) directly by QR-updating instead of updating and refactoring entire matrixA again [3]. Adding a new measurement row wT andcorresponding RHS γ at time t yields a new system thatis not yet in the correct factorized form,

A(t) ←[R(t−1)

wT

], b(t) ←

[d(t−1)

γ

](10)

To convert A(t) into correct factorized form R(t), Givensrotations [3] are applied to zero out the new rows wT

corresponding to new measurements at time step t. RHS ofEq. 10 is simultaneously updated with the same rotationsto obtain d(t). This Givens rotations based updating ofthe previous time step factorization R(t−1) provides thebasis of efficient incremental solutions for the unconstrainedproblem [3]. In general, the maximum number of Givensrotations needed for adding a new measurement row is n.However, since both R(t−1), wT are sparse, only a constantnumber of Givens rotations are needed.

B. Incremental constrained optimizationFor solving the constrained nonlinear least-squares ob-

jective in Eq. 7 it is no longer sufficient to only satisfygradient of the quadratic objective becoming zero (Eq. 8).Instead we must construct the Lagrangian and ensure theKKT conditions are satisfied at optimality. However, directlysolving KKT equations does not retain the form in Eq. 9amenable to incremental factorization. Instead, we rely onprimal-dual methods that essentially split the optimization intwo: a primal step and a dual step. Specifically, we use theaugmented Lagrangian/method of multipliers [7]. We startby writing out the Lagrangian for Eq. 7,

L(δx, v, u) =1

2||Aδx− b||22 + vT

(Gδx+ g(x0)

)+

uT(Hδx+ h(x0)

)(11)

where, v ∈ Rp, u ∈ Rq are the dual variables associatedwith equality, inequality constraints respectively.

The augmented Lagrangian also includes additional aug-mentation terms (for improved robustness) in the Lagrangianof the form ρ1

2 ||Gδx + g(x0)||22 and ρ22 ||Hδx + h(x0)||22

corresponding to equality and inequality constraints respec-tively. For simplicity of notations, we will absorb theseadditional quadratic augmentation terms within ||Aδx− b||22itself. That is, similar to the derivation in Eq. 5, the overallA, b matrix, vector will now additionally include rows Ai, biwith Ai =

√ρ12 G, bi = −

√ρ12 g(x0) and Ai =

√ρ22 H ,

bi = −√

ρ22 h(x0).

To satisfy KKT conditions at optimality, augmentedLagrangian solves a maximin dual problem of the formmaxu≥0,v minδx L(δx, v, u). It solves this problem itera-tively [7]: first, solving for δx by minimizing the Lagrangiantreating v, u as constants, followed by maximizing resultantdual function treating δx as constant. This iterative updateprocess at each iteration j can be expressed as,

δxj = argminδx L(δx, vj−1, uj−1) (12a)

vj = vj−1 + ρ1(Gδxj + g(x0)) (12b)

uj = max(0, uj−1 + ρ2(Hδxj + h(x0))

)(12c)

where, Eq. 12a is the primal update and Eqs. 12b, 12c arethe dual updates. The dual updates are simply projectedgradient ascent steps for maximizing the dual functionmaxu≥0,v g(v, u) := maxu≥0,v L(δxj , v, u). This step isincremental in nature. ρ1, ρ2 are the ascent step sizes thatappeared earlier as coefficients of the augmentation terms.Note that the dual function g(v, u) requires inequality dualsto be u ≥ 0. This ensures that g(v, u) always lower boundsthe primal objective, and that maximizing g(v, u) approachesthe optimal primal objective [6].

Having split the optimization in Eq. 12, the primal updatecan now be computed using Givens rotations similar toSection IV-A. Expanding Eq. 12a we have

∇δxL(δx, vj−1, uj−1) = 0

⇒ ATAδxj −AT b+GT vj−1 +HTuj−1 = 0

⇒ ATAδxj = AT b−GT vj−1 −HTuj−1

(13)

Algorithm 1 ICS: Incremental Constrained Smoothingfor t = 1 to T doZ1:t ← Z1:t−1 ∪ Zt

if !(batch) then[R(t), d(t)] = givensUpdate(x(t−1),Zt, R

(t−1), d(t−1))[G(t), g(t)] = partialJacobian(x(t−1),Zt, G

(t−1), g(t−1))[H(t), h(t)] = partialJacobian(x(t−1),Zt, H

(t−1), h(t−1))[δx∗, v∗, u∗] = primalDual(R(t), d(t), G(t), g(t), H(t), h(t), v(t-1), u(t-1))x(t) ← x(t−1) + δx∗

v(t) ← v∗, u(t) ← u∗

elsewhile <!converged> do[R(t), d(t)] = fullQR(x(t−1),Z1:t)[G(t), g(t)] = fullJacobian(x(t−1),Z1:t)[H(t), h(t)] = fullJacobian(x(t−1),Z1:t)[δx∗, v∗, u∗] = primalDual(R(t), d(t), G(t), g(t), H(t), h(t), v(t-1), u(t-1))x(t) ← x(t−1) + δx∗

v(t) ← v∗, u(t) ← u∗

end whileend if

end for

Function: primalDual(R(t), d(t), G(t), g(t), H(t), h(t), v(t−1), u(t−1))Initialize: j ← 1, v0 ← v(t−1), u0 ← u(t−1)

while <!converged> doR(t)T y = G(t)T vj−1 +H(t)T uj−1

R(t)δxj = d(t) − yvj ← vj−1 + ρ1(G(t)δxj + g(t))

uj ← max(

0, uj−1 + ρ2(H(t)δxj + h(t)))

end whilereturn δxj , vj , uj

This form is similar to the normal equations in Eq. 8. SinceA is sparse, we can apply QR factorization to solve Eq. 13.That is, factoring m×n matrix A with m ≥ n as Q

[R 0

]T,

and substituting this factorization in Eq. 13,

[RT 0]QTQ

[R0

]δxj = [RT 0]QT b−GT vj-1 −HTuj-1

⇒ RTRδxj =[RT 0

] [de

]−GT vj−1 −HTuj−1

⇒ Rδxj = d−R−T (GT vj−1 +HTuj−1)(14)

Eq. 14 can hence be solved as an iterative two-step process,a forward substitution followed by a back substitution, i.e.,

RT y = (GT vj−1 +HTuj−1) (15a)

Rδxj = d− y (15b)

This differs from the original normal equations solution thatinvolved only a single back substitution (Eq. 9).

Figure 2 provides an overview of the set of operationsinvolved at every new time step t of ICS. We need to updatethe factorization R(t) and Jacobians G(t), H(t) once everytime step (Step 1), followed by solving for the primal, dualupdates iteratively until convergence (Step 2).

Algorithm 1 summarizes the overall incremental con-strained smoothing method. It also shows periodic batchsteps that computes matrix factorization from scratch. Thesesteps may be triggered whenever we want to re-linearize paststates or update variable ordering to retain sparsity in thefactorization [3]. For simplicity, dual ascent step sizes ρ1, ρ2

are shown fixed in the algorithm. In practice, we use adaptivestep sizes for improved convergence. We make use of simple

x0<latexit sha1_base64="SIOScugtW9b30hoB+smUihPNn18=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120i7dbMLuRiyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEsG1cd1vZ2V1bX1js7BV3N7Z3dsvHRw2dZwqhg0Wi1i1A6pRcIkNw43AdqKQRoHAVjC6mfqtR1Sax/LBjBP0IzqQPOSMGivdP/XcXqnsVtwZyDLxclKGHPVe6avbj1kaoTRMUK07npsYP6PKcCZwUuymGhPKRnSAHUsljVD72ezUCTmzSp+EsbIlDZmpvycyGmk9jgLbGVEz1IveVPzP66QmvPYzLpPUoGTzRWEqiInJ9G/S5wqZEWNLKFPc3krYkCrKjE2naEPwFl9eJs1qxbuoVO8uy7XTPI4CHMMJnIMHV1CDW6hDAxgM4Ble4c0Rzovz7nzMW1ecfOYI/sD5/AECdI2F</latexit>

x1<latexit sha1_base64="eJJD70EOSNbB8rT7oA6aCMX6epg=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qnn9Uplt+LOQJaJl5My5Kj3Sl/dfszSiCtkkhrT8dwE/YxqFEzySbGbGp5QNqID3rFU0YgbP5udOiFnVumTMNa2FJKZ+nsio5Ex4yiwnRHFoVn0puJ/XifF8NrPhEpS5IrNF4WpJBiT6d+kLzRnKMeWUKaFvZWwIdWUoU2naEPwFl9eJs1qxbuoVO8uy7XTPI4CHMMJnIMHV1CDW6hDAxgM4Ble4c2Rzovz7nzMW1ecfOYI/sD5/AED+I2G</latexit>

xt<latexit sha1_base64="XQ7hpb+3LdUsybffkry4asE4tRg=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qmHvVLZrbgzkGXi5aQMOeq90le3H7M04gqZpMZ0PDdBP6MaBZN8UuymhieUjeiAdyxVNOLGz2anTsiZVfokjLUthWSm/p7IaGTMOApsZ0RxaBa9qfif10kxvPYzoZIUuWLzRWEqCcZk+jfpC80ZyrEllGlhbyVsSDVlaNMp2hC8xZeXSbNa8S4q1bvLcu00j6MAx3AC5+DBFdTgFurQAAYDeIZXeHOk8+K8Ox/z1hUnnzmCP3A+fwBphI3J</latexit>

x2<latexit sha1_base64="Z/ONRLPdhlIh4ZQRFMlKagYdANk=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qlX7ZXKbsWdgSwTLydlyFHvlb66/ZilEVfIJDWm47kJ+hnVKJjkk2I3NTyhbEQHvGOpohE3fjY7dULOrNInYaxtKSQz9fdERiNjxlFgOyOKQ7PoTcX/vE6K4bWfCZWkyBWbLwpTSTAm079JX2jOUI4toUwLeythQ6opQ5tO0YbgLb68TJrVindRqd5dlmuneRwFOIYTOAcPrqAGt1CHBjAYwDO8wpsjnRfn3fmYt644+cwR/IHz+QMFfI2H</latexit>

<latexit sha1_base64="GBhiExX3bXxBH/XYcDyUkLowLJ8=">AAAB+nicdVBLTwIxGOziC/G16FEPjcTE06YLiHAj8eIRE3kksCHdUqCh213broas/BQvHjTGq7/Em//GLmCiRidpOpn5vnQ6fsSZ0gh9WJmV1bX1jexmbmt7Z3fPzu+3VBhLQpsk5KHs+FhRzgRtaqY57USS4sDntO1PLlK/fUulYqG41tOIegEeCTZkBGsj9e18zw/5QE0DcyU9Tm9mfbuAnFoNld0KRM4ZQsVKzRBUKlYrFeg6aI4CWKLRt997g5DEARWacKxU10WR9hIsNSOcznK9WNEIkwke0a6hAgdUeck8+gyeGGUAh6E0R2g4V79vJDhQaTozGWA9Vr+9VPzL68Z6WPUSJqJYU0EWDw1jDnUI0x7ggElKNJ8agolkJiskYywx0aatnCnh66fwf9IqOm7JKV6VC/WjZR1ZcAiOwSlwwTmog0vQAE1AwB14AE/g2bq3Hq0X63UxmrGWOwfgB6y3T26OlKg=</latexit>

<latexit sha1_base64="GBhiExX3bXxBH/XYcDyUkLowLJ8=">AAAB+nicdVBLTwIxGOziC/G16FEPjcTE06YLiHAj8eIRE3kksCHdUqCh213broas/BQvHjTGq7/Em//GLmCiRidpOpn5vnQ6fsSZ0gh9WJmV1bX1jexmbmt7Z3fPzu+3VBhLQpsk5KHs+FhRzgRtaqY57USS4sDntO1PLlK/fUulYqG41tOIegEeCTZkBGsj9e18zw/5QE0DcyU9Tm9mfbuAnFoNld0KRM4ZQsVKzRBUKlYrFeg6aI4CWKLRt997g5DEARWacKxU10WR9hIsNSOcznK9WNEIkwke0a6hAgdUeck8+gyeGGUAh6E0R2g4V79vJDhQaTozGWA9Vr+9VPzL68Z6WPUSJqJYU0EWDw1jDnUI0x7ggElKNJ8agolkJiskYywx0aatnCnh66fwf9IqOm7JKV6VC/WjZR1ZcAiOwSlwwTmog0vQAE1AwB14AE/g2bq3Hq0X63UxmrGWOwfgB6y3T26OlKg=</latexit>

<latexit sha1_base64="GBhiExX3bXxBH/XYcDyUkLowLJ8=">AAAB+nicdVBLTwIxGOziC/G16FEPjcTE06YLiHAj8eIRE3kksCHdUqCh213broas/BQvHjTGq7/Em//GLmCiRidpOpn5vnQ6fsSZ0gh9WJmV1bX1jexmbmt7Z3fPzu+3VBhLQpsk5KHs+FhRzgRtaqY57USS4sDntO1PLlK/fUulYqG41tOIegEeCTZkBGsj9e18zw/5QE0DcyU9Tm9mfbuAnFoNld0KRM4ZQsVKzRBUKlYrFeg6aI4CWKLRt997g5DEARWacKxU10WR9hIsNSOcznK9WNEIkwke0a6hAgdUeck8+gyeGGUAh6E0R2g4V79vJDhQaTozGWA9Vr+9VPzL68Z6WPUSJqJYU0EWDw1jDnUI0x7ggElKNJ8agolkJiskYywx0aatnCnh66fwf9IqOm7JKV6VC/WjZR1ZcAiOwSlwwTmog0vQAE1AwB14AE/g2bq3Hq0X63UxmrGWOwfgB6y3T26OlKg=</latexit>

x2<latexit sha1_base64="Z/ONRLPdhlIh4ZQRFMlKagYdANk=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qlX7ZXKbsWdgSwTLydlyFHvlb66/ZilEVfIJDWm47kJ+hnVKJjkk2I3NTyhbEQHvGOpohE3fjY7dULOrNInYaxtKSQz9fdERiNjxlFgOyOKQ7PoTcX/vE6K4bWfCZWkyBWbLwpTSTAm079JX2jOUI4toUwLeythQ6opQ5tO0YbgLb68TJrVindRqd5dlmuneRwFOIYTOAcPrqAGt1CHBjAYwDO8wpsjnRfn3fmYt644+cwR/IHz+QMFfI2H</latexit>

x0<latexit sha1_base64="SIOScugtW9b30hoB+smUihPNn18=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120i7dbMLuRiyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEsG1cd1vZ2V1bX1js7BV3N7Z3dsvHRw2dZwqhg0Wi1i1A6pRcIkNw43AdqKQRoHAVjC6mfqtR1Sax/LBjBP0IzqQPOSMGivdP/XcXqnsVtwZyDLxclKGHPVe6avbj1kaoTRMUK07npsYP6PKcCZwUuymGhPKRnSAHUsljVD72ezUCTmzSp+EsbIlDZmpvycyGmk9jgLbGVEz1IveVPzP66QmvPYzLpPUoGTzRWEqiInJ9G/S5wqZEWNLKFPc3krYkCrKjE2naEPwFl9eJs1qxbuoVO8uy7XTPI4CHMMJnIMHV1CDW6hDAxgM4Ble4c0Rzovz7nzMW1ecfOYI/sD5/AECdI2F</latexit>

x1<latexit sha1_base64="eJJD70EOSNbB8rT7oA6aCMX6epg=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qnn9Uplt+LOQJaJl5My5Kj3Sl/dfszSiCtkkhrT8dwE/YxqFEzySbGbGp5QNqID3rFU0YgbP5udOiFnVumTMNa2FJKZ+nsio5Ex4yiwnRHFoVn0puJ/XifF8NrPhEpS5IrNF4WpJBiT6d+kLzRnKMeWUKaFvZWwIdWUoU2naEPwFl9eJs1qxbuoVO8uy7XTPI4CHMMJnIMHV1CDW6hDAxgM4Ble4c2Rzovz7nzMW1ecfOYI/sD5/AED+I2G</latexit>

xt<latexit sha1_base64="XQ7hpb+3LdUsybffkry4asE4tRg=">AAAB6nicbVBNS8NAEJ34WetX1aOXxSp4KkkV9Fjw4rGi/YA2lM120y7dbMLuRCyhP8GLB0W8+ou8+W/ctjlo64OBx3szzMwLEikMuu63s7K6tr6xWdgqbu/s7u2XDg6bJk414w0Wy1i3A2q4FIo3UKDk7URzGgWSt4LRzdRvPXJtRKwecJxwP6IDJULBKFrp/qmHvVLZrbgzkGXi5aQMOeq90le3H7M04gqZpMZ0PDdBP6MaBZN8UuymhieUjeiAdyxVNOLGz2anTsiZVfokjLUthWSm/p7IaGTMOApsZ0RxaBa9qfif10kxvPYzoZIUuWLzRWEqCcZk+jfpC80ZyrEllGlhbyVsSDVlaNMp2hC8xZeXSbNa8S4q1bvLcu00j6MAx3AC5+DBFdTgFurQAAYDeIZXeHOk8+K8Ox/z1hUnnzmCP3A+fwBphI3J</latexit>

=<latexit sha1_base64="Twk672gqDuZ/7FM2jTndBPPWG1w=">AAAB9XicdVBLSwMxGMzWV62vqkcvoUXwtGTX2vUiFLx4rGAf0K4lm03b0Gx2SbJKWfo/vHhQxKv/xZv/xmxbQUUHQoaZ7yOTCRLOlEbowyqsrK6tbxQ3S1vbO7t75f2DtopTSWiLxDyW3QArypmgLc00p91EUhwFnHaCyWXud+6oVCwWN3qaUD/CI8GGjGBtpNt+EPNQTSNzZRezQbmK7DPPqzt1iGwHuS5yDUFeDaEadGw0RxUs0RyU3/thTNKICk04VqrnoET7GZaaEU5npX6qaILJBI9oz1CBI6r8bJ56Bo+NEsJhLM0RGs7V7xsZjlQezUxGWI/Vby8X//J6qR6e+xkTSaqpIIuHhimHOoZ5BTBkkhLNp4ZgIpnJCskYS0y0KapkSvj6KfyftF3bObXd61q1UVnWUQRHoAJOgAM80ABXoAlagAAJHsATeLburUfrxXpdjBas5c4h+AHr7RNAGpLo</latexit> =<latexit sha1_base64="Twk672gqDuZ/7FM2jTndBPPWG1w=">AAAB9XicdVBLSwMxGMzWV62vqkcvoUXwtGTX2vUiFLx4rGAf0K4lm03b0Gx2SbJKWfo/vHhQxKv/xZv/xmxbQUUHQoaZ7yOTCRLOlEbowyqsrK6tbxQ3S1vbO7t75f2DtopTSWiLxDyW3QArypmgLc00p91EUhwFnHaCyWXud+6oVCwWN3qaUD/CI8GGjGBtpNt+EPNQTSNzZRezQbmK7DPPqzt1iGwHuS5yDUFeDaEadGw0RxUs0RyU3/thTNKICk04VqrnoET7GZaaEU5npX6qaILJBI9oz1CBI6r8bJ56Bo+NEsJhLM0RGs7V7xsZjlQezUxGWI/Vby8X//J6qR6e+xkTSaqpIIuHhimHOoZ5BTBkkhLNp4ZgIpnJCskYS0y0KapkSvj6KfyftF3bObXd61q1UVnWUQRHoAJOgAM80ABXoAlagAAJHsATeLburUfrxXpdjBas5c4h+AHr7RNAGpLo</latexit> =<latexit sha1_base64="Twk672gqDuZ/7FM2jTndBPPWG1w=">AAAB9XicdVBLSwMxGMzWV62vqkcvoUXwtGTX2vUiFLx4rGAf0K4lm03b0Gx2SbJKWfo/vHhQxKv/xZv/xmxbQUUHQoaZ7yOTCRLOlEbowyqsrK6tbxQ3S1vbO7t75f2DtopTSWiLxDyW3QArypmgLc00p91EUhwFnHaCyWXud+6oVCwWN3qaUD/CI8GGjGBtpNt+EPNQTSNzZRezQbmK7DPPqzt1iGwHuS5yDUFeDaEadGw0RxUs0RyU3/thTNKICk04VqrnoET7GZaaEU5npX6qaILJBI9oz1CBI6r8bJ56Bo+NEsJhLM0RGs7V7xsZjlQezUxGWI/Vby8X//J6qR6e+xkTSaqpIIuHhimHOoZ5BTBkkhLNp4ZgIpnJCskYS0y0KapkSvj6KfyftF3bObXd61q1UVnWUQRHoAJOgAM80ABXoAlagAAJHsATeLburUfrxXpdjBas5c4h+AHr7RNAGpLo</latexit>

(a) (b)

Fig. 4. Factor graphs for (a) Simulated Contact Dataset (b) Simulated/Real-world Navigation Dataset. Factors shown are prior (green), odometry (blue),equality constraints (red) and inequality constraints (orange).

adaptation scheme where: (a) ρj = ρj−1 × g if constraintviolation decreases by less than a minimum change factor,or (b) ρj = ρj−1 × 1/r if constraint violation increases.

V. RESULTS AND EVALUATION

We evaluate ICS qualitatively and quantitatively on metricslike constraint violations, estimation accuracies and runtimeperformance against an incremental unconstrained optimiza-tion approach like iSAM [3]. We implemented ICS withinthe iSAM C++ library. All evaluations are done on a laptopwith an Intel Core i7-7820HQ 2.9GHz processor.

A. Datasets and Baselines

We evaluate our approach on the following three datasets:(1) Simulated Contact, (2) Simulated Navigation, and (3)Real-world Navigation. For the simulated contact datasetwe enforce equality constraints like object contact, whilefor simulated and real-world navigation datasets we enforceinequality constraints like collision avoidance.

Fig. 4(a),(b) shows factor graphs for Simulated Contactand Simulated/Real-world Navigation datasets respectively.A factor graph is a bipartite graph with: variable nodesx ∈ V (empty circles) and factor nodes φ ∈ U (filledcircles) [13]. In Fig. 4(a) state variables xt =

[ptx p

ty p

tθ

]Tmodel the 2D pose of a circular object at time t beingpushed by a circular gripper. There are two measurements:object odometry and contact sensor on the gripper. We modelcontact measurements as equality constraint factors thatconstrain the squared distance between object and gripper.

In Fig. 4(b) state variables xt =[ptx p

ty p

tθ

]Tmodels the

2D pose a mobile robot navigating inside a building. Thereis one measurement: the robot odometry. We additionallyhave inequality constraint factors that are piece-wise linearconstraints corresponding to boundaries of the corridor thatthe robot is in currently. For now, we extract these corridorconstraints manually using the floor plan for both simulatedand real-world navigation datasets, and assume that we haveaccess to these constraints on the back-end.

We compare performance of ICS against an incrementalunconstrained optimization framework like iSAM [3]. Forcomparison, equality/inequality constraint factors in ICS areincorporated as regular unconstrained cost factors in iSAM.We don’t include loop closure factors in any of the datasetsfor simplicity of analysis. However, these can be incorporatedin both frameworks as additional binary factors to the graph.

Gaussian noise with σodom = 0.008, 0.294, 0.942 areadded to odometry measurements for datasets 1, 2, 3 respec-tively. The convergence criteria for alternating primal dualupdates is set to ||Gδx + g||2 ≤ ε1 for equality constraintsand ||Hδx+h||2 ≤ ε2 for inequality constraints that are notsatisfied (i.e. > 0). ε1 is set as 10−6 (dataset 1), ε2 as 10−4

(datasets 2,3). Maximum allowed primal-dual iterations areset to 100. ρ1 = 500 for dataset 1, and ρ2 = 20, 1 for datasets2, 3 respectively. Adaptive dual ascent step size parametersg, 1/r are set to 5, 0.1 respectively.

B. Performance accuracies

Fig. 3 shows performance accuracies on the SimulatedContact dataset. Fig. 3(a), (b) qualitatively show estimatedobject poses using ICS, iSAM respectively at four intermedi-ate time steps where the gripper was in contact with object.Incorporating contact as an equality constraint in ICS is ableto satisfy constraints more precisely (i.e. object in contactwith gripper) than incorporating it as an unconstrained factoras in the case of iSAM. This can also be seen quantitativelywhere lower mean constraint violations in Fig. 3(c) leadsto improved state estimates with lower RMSE absolutetrajectory errors (ATE) in Fig. 3(d).

Fig. 5 shows performance accuracies on the SimulatedNavigation dataset. Fig. 5(a) qualitatively show estimatedrobot trajectory using ICS, iSAM at the final time step.Incorporating corridor constraints as inequality constraintsis able to satisfy the constraint more precisely (i.e. collision-free trajectories) than incorporating it as an unconstrainedfactor. This can also be seen quantitatively where lower meanconstraint violations in Fig. 5(b) leads to improved stateestimates with lower RMSE ATE in Fig. 5(c).

Fig. 6 shows similar set of results as Fig. 5 but now on thereal-world MIT CSAIL 2D navigation dataset [21]. For ourevaluations, we treat the trajectory with loop closures as theground truth, and disable loop closures for all other runs. Ascan be seen qualitatively in Fig. 6(a) ICS is able to satisfythe constraint more precisely (i.e. collision-free trajectories).This is also seen quantitatively in Fig. 6(b),(c) where lowermean constraint violations leads to lower RMSE ATE values.

0 50 100Time step

0

1

2

3

4

5

Abso

lute

traj

ecto

ry e

rror (

cm) Odom

iSAMICS

(a) (b) (c) (d)

0 50 100Time step

0

0.5

1

1.5

2

Mea

n co

nstra

int v

iola

tion

(cm

)

OdomiSAMICS

Fig. 3. Dataset 1: Simulated Contact Performance Accuracies

OdomiSAMICS

OdomiSAMICS

(a) (b) (c)

0 20 40 60 80 100 120Time step

0

0.5

1

1.5

Abso

lute

traj

ecto

ry e

rror (

m) Odom

iSAMICS

0 20 40 60 80 100 120Time step

0

0.5

1

1.5

2

2.5

3

Mea

n co

nstra

int v

iola

tion

(m) Odom

iSAMICS

Fig. 5. Dataset 2: Simulated Navigation Performance Accuracies

OdomiSAMICS

(a) (b) (c)

OdomISAMICS

0 500 1000Time step

0

0.5

1

1.5

Abso

lute

traj

ecto

ry e

rror (

m) Odom

iSAMICS

0 500 1000Time step

0

0.5

1

1.5

Mea

n co

nstra

int v

iola

tion

(m) Odom

iSAMICS

Fig. 6. Dataset 3: Real-world Navigation Performance Accuracies

The jumps in constraint violations as seen with iSAM inFigs. 5(b), 6(b) happen as iSAM is correcting pure odometrytrajectory to minimize the overall unconstrained cost: acombination of trajectory and constraint violation costs.

C. Effect of varying cost weights

Since constraints within iSAM are incorporated as costs orsoft constraints, the degree to which they are satisfied woulddepend on their weights relative to other terms in the costfunction. Fig. 7 shows variation of mean constraint violationsand RMSE ATE values for iSAM, ICS with varying valuesof ρ2 for Dataset 2. ρ2 is the weighting coefficient ofthe augmentation term ρ2

2 ||Hδx + h||22 associated with theinequality constraint factor. It can be seen that accuracy of anunconstrained framework like iSAM is much more sensitiveto changes in this weighting. ICS ensures that constraint

0 100 200 3000

0.5

1

1.5

2

2.5

Mea

n co

nstra

int v

iola

tion

(m) iSAM

ICS

(a) (b)

0 100 200 3000.6

0.8

1

1.2

1.4

1.6

Abso

lute

traj

ecto

ry e

rror (

m) iSAM

ICS

⇢2

<latexit sha1_base64="MOpMNLT2MrBtSBxpcTaGCrpSQec=">AAAB7XicbVBNSwMxEJ34WetX1aOX0CJ4KruloseCF48V7Ae0S8mm2TY2myxJVihL/4MXD4p49f9489+YtnvQ1gcDj/dmmJkXJoIb63nfaGNza3tnt7BX3D84PDounZy2jUo1ZS2qhNLdkBgmuGQty61g3UQzEoeCdcLJ7dzvPDFtuJIPdpqwICYjySNOiXVSu6/HalAblCpe1VsArxM/JxXI0RyUvvpDRdOYSUsFMabne4kNMqItp4LNiv3UsITQCRmxnqOSxMwE2eLaGb5wyhBHSruSFi/U3xMZiY2ZxqHrjIkdm1VvLv7n9VIb3QQZl0lqmaTLRVEqsFV4/joecs2oFVNHCNXc3YrpmGhCrQuo6ELwV19eJ+1a1a9Xr+7rlUY5j6MA51CGS/DhGhpwB01oAYVHeIZXeEMKvaB39LFs3UD5zBn8Afr8AUH2jtQ=</latexit>

⇢2

<latexit sha1_base64="MOpMNLT2MrBtSBxpcTaGCrpSQec=">AAAB7XicbVBNSwMxEJ34WetX1aOX0CJ4KruloseCF48V7Ae0S8mm2TY2myxJVihL/4MXD4p49f9489+YtnvQ1gcDj/dmmJkXJoIb63nfaGNza3tnt7BX3D84PDounZy2jUo1ZS2qhNLdkBgmuGQty61g3UQzEoeCdcLJ7dzvPDFtuJIPdpqwICYjySNOiXVSu6/HalAblCpe1VsArxM/JxXI0RyUvvpDRdOYSUsFMabne4kNMqItp4LNiv3UsITQCRmxnqOSxMwE2eLaGb5wyhBHSruSFi/U3xMZiY2ZxqHrjIkdm1VvLv7n9VIb3QQZl0lqmaTLRVEqsFV4/joecs2oFVNHCNXc3YrpmGhCrQuo6ELwV19eJ+1a1a9Xr+7rlUY5j6MA51CGS/DhGhpwB01oAYVHeIZXeEMKvaB39LFs3UD5zBn8Afr8AUH2jtQ=</latexit>

Fig. 7. Performance accuracies with varying cost weights (Dataset 2)

(a) (b)

0 50 100Time step

0

0.005

0.01

0.015

0.02

Run

time

(s)

iSAMICS

0 50 100Time step

0

0.005

0.01

0.015

0.02

Run

time

(s)

iSAMICS

0 500 1000Time step

0

0.1

0.2

0.3

0.4

0.5

Run

time

(s)

iSAMICS

(c)

Fig. 8. Runtime every time step for (a) Dataset 1 (equality constraints)(b) Dataset 2 (inequality constraints) (c) Dataset 3 (inequality constraints)

violations lie under a set threshold irrespective of ρ2.

D. Runtime performanceFig. 8 shows runtime of an incremental step in ICS/iSAM

against number of time steps. ICS is more computationallyexpensive than iSAM, since iterative primal-dual iterationsneed to be done every incremental step. However, in compar-ison to a batch constrained solver (eg. active set method) thatsolves for the KKT conditions directly, incrementally solvingfor constraints will be more efficient for large matrices. Thisis since a direct solution to the KKT conditions involves theexpensive operation of factorizing a matrix with (n+ p+ q)rows/columns from scratch every time step, where n is thedimension of ATA, and p, q are number of equality, activeinequality constraints respectively.

Note that ICS runtimes for dataset 1 (Fig. 8a) increasesmore steadily compared to datasets 2, 3 (Figs. 8b,c). Thishappens since equality constraints are always active for anyoptimal point that is feasible, while inequality constraintsmay or may not be active at optimal points that are feasible.

VI. DISCUSSIONS

We presented a framework ICS for incremental con-strained smoothing. By combining a primal-dual method likeAugmented Lagrangian with an incremental Gauss Newtonreusing previously computed matrix factorizations, we wereable to satisfy hard constraints precisely within a thresholdwithout having to recompute solutions from scratch.

A current limitation is that of a fixed linearization pointfor older states (similar to iSAM) making it unsuitablefor highly nonlinear problems. Future work would be toexplore the proposed approach within iSAM2 [4] that allowsfor fluid relinearizations. Runtime performance of ICS canalso be further improved by exploiting sparsity in constraintjacobians, locality in dual updates, and better step sizeadaptation techniques.

REFERENCES[1] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira,

I. Reid, and J. J. Leonard, “Past, present, and future of simultaneouslocalization and mapping: Toward the robust-perception age,” IEEETrans. Robotics, vol. 32, no. 6, pp. 1309–1332, 2016.

[2] F. Dellaert and M. Kaess, “Factor graphs for robot perception,”Foundations and Trends in Robotics, vol. 6, no. 1-2, pp. 1–139, 2017.

[3] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incrementalsmoothing and mapping,” IEEE Trans. Robotics, vol. 24, no. 6,pp. 1365–1378, 2008.

[4] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Del-laert, “iSAM2: Incremental smoothing and mapping using the Bayestree,” Intl. J. of Robotics Research, vol. 31, pp. 216–235, Feb. 2012.

[5] K.-T. Yu and A. Rodriguez, “Realtime state estimation with tactile andvisual sensing. application to planar manipulation,” in IEEE Intl. Conf.on Robotics and Automation (ICRA), pp. 7778–7785, IEEE, 2018.

[6] S. Boyd and L. Vandenberghe, Convex optimization. Cambridgeuniversity press, 2004.

[7] S. Boyd, N. Parikh, E. Chu, B. Peleato, J. Eckstein, et al., “Dis-tributed optimization and statistical learning via the alternating di-rection method of multipliers,” Foundations and Trends R© in Machinelearning, vol. 3, no. 1, pp. 1–122, 2011.

[8] S. Thrun, “Robotic mapping: A survey,” in Exploring Artificial In-telligence in the New Millenium (G. Lakemeyer and B. Nebel, eds.),Morgan Kaufmann, 2002.

[9] M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, andM. Csorba, “A solution to the simultaneous localization and mapbuilding (SLAM) problem,” IEEE Trans. Robotics and Automation,vol. 17, no. 3, pp. 229–241, 2001.

[10] S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics. MIT press,2005.

[11] H. Durrant-Whyte and T. Bailey, “Simultaneous localization andmapping (SLAM): Part I,” IEEE Robotics & Automation Magazine,vol. 13, no. 2, pp. 99–110, 2006.

[12] R. M. Eustice, H. Singh, and J. J. Leonard, “Exactly sparse delayed-state filters for view-based SLAM,” IEEE Trans. Robotics, vol. 22,no. 6, pp. 1100–1114, 2006.

[13] F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous localiza-tion and mapping via square root information smoothing,” Intl. J. ofRobotics Research, vol. 25, no. 12, pp. 1181–1203, 2006.

[14] A. Cunningham, M. Paluri, and F. Dellaert, “DDF-SAM: Fully dis-tributed SLAM using constrained factor graphs,” in IEEE/RSJ Intl.Conf. on Intelligent Robots and Systems (IROS), pp. 3025–3030, IEEE,2010.

[15] D.-N. Ta, M. Kobilarov, and F. Dellaert, “A factor graph approach toestimation and model predictive control on unmanned aerial vehicles,”in Intl. Conf. on Unmanned Aircraft Systems (ICUAS), pp. 181–188,IEEE, 2014.

[16] S. Choudhary, L. Carlone, H. I. Christensen, and F. Dellaert, “Exactlysparse memory efficient SLAM using the multi-block alternatingdirection method of multipliers,” in IEEE/RSJ Intl. Conf. on IntelligentRobots and Systems (IROS), pp. 1349–1356, IEEE, 2015.

[17] I. D. D. Jimenez Rodriguez, “A factor graph approach to constrainedoptimization,” 2017.

[18] L. Carlone, G. Calafiore, C. Tommolillo, and F. Dellaert, “Planar posegraph optimization: Duality, optimal solutions, and verification,” IEEETrans. Robotics, vol. 32, no. 3, pp. 545–565, 2016.

[19] L. Carlone, D. Rosen, G. Calafiore, J. Leonard, and F. Dellaert,“Lagrangian duality in 3D SLAM: Verification techniques and optimalsolutions,” in IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems(IROS), pp. 125–132, 2015.

[20] F. Bai, T. Vidal-Calleja, and S. Huang, “Robust incremental slam underconstrained optimization formulation,” IEEE Robotics and AutomationLetters, no. 2, pp. 1207–1214, 2018.

[21] L. Carlone, R. Aragues, J. A. Castellanos, and B. Bona, “A fastand accurate approximation for planar pose graph optimization,” TheInternational Journal of Robotics Research, vol. 33, no. 7, pp. 965–987, 2014.