Globally Optimal Data-Association-Free Landmark-Based Localization Using Semidefinite Relaxations Vassili Korotkine, Mitchell Cohen, James Richard Forbes Abstract This paper proposes a semidefinite relaxation for landmark-based localization with unknown data associations in planar environments. The proposed method simultaneously solves for the optimal robot states and data associations in a globally optimal fashion. Relative position measurements to a fixed set of known landmarks are used, but the data association is unknown in that the robot does not know which landmark each measurement is generated from. The relaxation is shown to be tight in a majority of cases for moderate noise levels. The proposed algorithm is compared to local Gauss-Newton baselines initialized at the dead-reckoned trajectory, and is shown to significantly improve convergence to the problem’s global optimum in simulation and experiment. Accompanying software and supplementary material can be found at https://github.com/decargroup/certifiable_uda_loc . I Introduction and Related Work Fig. 1: Algorithm comparison on a subsequence of the Lost in the Woods dataset. The proposed SDP method converges to a solution with the correct data associations, while the local method initialized using dead-reckoning is unable to find the globally optimal data associations, leading to an incorrect robot trajectory estimate. The poses are labeled \text{T}_{i} , where i corresponds to the time index. Adjacent poses are 20 seconds apart. Estimating the state of a robot from noisy and incomplete sensor data is a central task associated with autonomy. In the landmark-based localization task, the robot infers its position and orientation from measurements from landmarks with known positions. State estimation methods for localization can be split into filtering methods and batch optimization methods [ 1 ] . Filtering methods include Monte Carlo based localization [ 2 ] , where probabilistic sampling is used to represent the posterior belief on the robot state. This work focuses on batch optimization methods, where all data over a given time window is used to estimate all the robot states within the window. An example is [ 3 ] , where a joint probability data association algorithm is used to perform localization based on a high-definition map. An important component of landmark-based localization is determining the data associations between measurements and landmarks, where a correspondence must be drawn between the measurements received and the landmark they are assumed to be received from. The data associations can either be solved for before, or simultaneously with, the robot states. Solving for the data associations before the robot states can be done with a front-end sensor dependent system, such as by tracking specific camera pixel coordinates between frames [ 4 ] , or it can be done in the optimization back-end by using the provided data. In practice, the need to explicitly estimate data associations in the backend can arise from landmark detections that do not provide rich information, as is common in semantic object-level SLAM [ 5 ] , or failures due to incorrect data association in the frontend [ 6 ] . Algorithms such as joint compatibility branch and bound [ 7 ] , combined constraint data association [ 8 ] , and the CLEAR alignment and rectification algorithm [ 9 ] fall into this category. In all of the above algorithms [ 7 , 8 , 9 ] , the data associations are solved for before solving for the robot states with the data associations fixed, which can result in a suboptimal solution if the initial data associations are incorrect. Another approach consists of solving for the data associations simultaneously with the robot states, a problem that is NP-hard and combinatorial in nature [ 6 ] . When applied to this problem formulation, the Max-Mixture approach of [ 10 ] iterates through optimization steps selecting the most likely data association at each step. The data-association-free landmark-based simultaneous localization and mapping (SLAM) algorithm of [ 6 ] simultaneously solves for robot states, landmark positions, and the data associations in an inner loop using the Max-Mixture algorithm, while solving for the number of landmarks in an outer solver loop. Probabilistic data association in a Max-Mixture framework is also presented in [ 5 ] for the application of semantic SLAM. DC-SAM [ 11 ] is a general purpose method for problems involving continuous and discrete variables that involves alternation of solving for the continuous variables and then solving for the discrete variables. The algorithms described above that simultaneously solve for the data associations and robot state are inherently local in nature, as they require an initial guess that is refined to obtain the optimal solution. The quality of the solution is impacted by the initial guess. Furthermore, global optimality of the algorithm is not guaranteed. The incorrect minimum may be reached without any indication that it is the local minimum. The present work focuses on globally optimal solutions obtained from semidefinite relaxations of the problem. Globally optimal methods based on semidefinite relaxations are applicable to problems that may be formulated as polynomial optimization problems, which are written as quadratically constrained quadratic programs (QCQPs) using a suitable change of variables. The QCQP admits a semidefinite program (SDP) relaxation, termed Shor’s relaxation [ 12 , Chap. 3] . The SDP relaxation may be solved to global optimality, and if the result is feasible for the non-relaxed problem, it is guaranteed to be the global optimum of the non-relaxed problem. SDP relaxations have been used to treat outlier rejection [ 13 , 14 ] , where the algorithm must decide if a given measurement is an outlier or not, a choice that is encoded using discrete boolean states that are solved for alongside the continuous robot states. The contribution of this paper may thus be stated as a relaxation-based globally optimal method, in an optimization cost sense, for the data-association-free planar landmark-based localization problem where the number of landmarks is known . To the best of the authors’ knowledge, this is the first such method that scales in polynomial time. An example result is presented in Fig. 1 , where for an example with five poses and three landmarks, the proposed method obtains the globally optimal solution with the correct data associations, unlike the local method initialized using dead-reckoning. The method is applicable to ground vehicle localization, in situations where relative pose measurements as well as measurements to known landmarks are available. The implementation is open-sourced at https://anonymous.4open.science/r/certifiable_uda_loc-9BCF/README.md . Supplementary material at the same URL contains further details on the cost and constraint matrix setup. The rest of this paper is organized as follows. Sec. II describes the problem setup of landmark-based localization with unknown data associations. Sec. III describes the proposed SDP relaxation. Sec. IV presents results on simulated and experimental data, while Sec. V concludes and provides directions for future work. TABLE I: Notation Table Indices i \triangleq Timestep, sometimes generic index {N} \triangleq Number of timesteps j \triangleq Landmark index, sometimes generic index {n_{\ell}} \triangleq Number of landmarks {k} \triangleq Index of measurement received at given timestep, sometimes generic index {K} \triangleq Number measurements received at given timestep {n_{\theta}} \triangleq Number of discrete problem variables n_{x} \triangleq Number of columns in optimization variable \mbf{X} Quantities \mbf{1} \triangleq Identity matrix with unspecified dimension \mbf{1}^{n\times n} \triangleq Identity matrix of dimension n\times n \mbf{T}_{i}\in SE(3) \triangleq Robot pose at time index i \mathcal{X}\in{SE(3)}^{N} \triangleq Tuple of all robot poses {\theta}\in\{0,1\} \triangleq Single discrete problem variable \boldsymbol{{\theta}}\in\mathbb{R}^{n_{\theta}} \triangleq All discrete problem variables stacked into column matrix {\boldsymbol{\ell}}_{j}\in\mathbb{R}^{2} \triangleq j ’th landmark position \mbf{H}\in\mathbb{R}^{2\times 2} \triangleq Homogenization variable \mbf{X}\in\mathbb{R}^{2\times{n_{x}}} \triangleq Matrix variable with variables of interest concatenated {\boldsymbol{\Xi}}\in\mathbb{R}^{2\times(2+3{N})} \triangleq Concatenation of continuous problem variables \mbf{Z}\in\mathbb{R}^{{n_{x}}\times{n_{x}}} \triangleq Optimization matrix variable in Shor’s relaxation Operators \otimes \triangleq Kronecker product {\left\langle\mbf{A},\mbf{B}\right\rangle} \triangleq Matrix inner product. For \mbf{A} , \mbf{B}\in\mathbb{R}^{m\times n} , {\left\langle\mbf{A},\mbf{B}\right\rangle}={\mathrm{tr}}(\mbf{A}\mbf{B}^{\mathsf{T}}) \mathcal{N}(\mbf{x};\bar{\mbf{x}},\mbf{P}) \triangleq A multivariate Gaussian distribution in \mbf{x} , with mean \bar{\mbf{x}} and covariance \mbf{{P}} II Problem Setup For convenience, commonly used notation is summarized in Tab. I . The state of interest {\mathcal{X}} is given by \mathcal{X}=\left\{\mbf{T}_{1},\dots,\mbf{T}_{i},\dots,\mbf{T}_{{N}}\right\}\in SE(2)^{N} where the timestep index is denoted i and {N} is the number of timesteps. The robot poses are denoted \mbf{T}_{i}=(\mbf{C}_{i},\mbf{r}_{i})\in SE(2) . \mbf{C}_{i}\in SO(2) denotes the direction cosine matrix at timestep i describing the change of basis from the robot frame to the inertial frame, and \mbf{r}_{i} denotes the robot position at timestep i resolved in the inertial frame. At the i ’th timestep, the robot receives {K}_{i} relative landmark position measurements {\mbf{y}}_{{k}_{i}} , to landmarks with known positions, {\boldsymbol{\ell}}_{j} for j=1,\dots,{n_{\ell}} . The number of measurements received at the i ’th timestep {K}_{i} is typically less than the number of landmarks {n_{\ell}} . The generative isotropic noise model for the relative landmark position measurements is given by \displaystyle{\mbf{y}}_{{k}_{i}} \displaystyle=\mbf{C}_{i}^{\mathsf{T}}({\boldsymbol{\ell}}_{j}-\mbf{r}_{i})+\mbf{v},\hskip 11.49994pt\mbf{v}\sim\mathcal{N}\left(\mbf{v};\mbf{0},\mbf{1}{\sigma}_{\text{lndmrk}}^{2}\right), (1) where the covariance \mbf{1}{\sigma}_{\text{lndmrk}}^{2} is a multiple of identity scaled by variance \mbf{1}{\sigma}_{\text{lndmrk}}^{2} , since isotropic noise is considered. At timestep i , the data association variables are encoded as \theta_{i{k}_{i}j}\in\{0,1\} . A value of \theta_{i{k}_{i}j}=1 encodes that, at timestep i , the {k}_{i} ’th measurement received at that timestep corresponds to landmark {\boldsymbol{\ell}}_{j} . The data association variables are further constrained by \displaystyle\sum_{j=1}^{{n_{\ell}}}{\theta}_{i{k}j}=1,\quad\forall{i,{k}}, (2) meaning that every measurement comes from a single landmark. All the {n_{\theta}}=\sum_{i=1}^{N}\sum_{{k}_{i}=1}^{{K}_{i}}{n_{\ell}} data association variables may be stacked into a column vector \boldsymbol{{\theta}} . The problem formulation may thus be stated as \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mathcal{X},\boldsymbol{{\theta}}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mathcal{X},\boldsymbol{{\theta}}}{\mathrm{minimize}}}\quad J_{\text{odom}}(\mathcal{X})+J_{\text{prior}}(\mathcal{X})+J_{\text{uda, lndmrk}}(\mathcal{X},\boldsymbol{{\theta}}), ( Localization ) with \mathcal{X}\in SE(2)^{N} and \boldsymbol{{\theta}}\in\mathbb{R}^{{n_{\theta}}} . The first term is the odometric cost [ 15 ] given by \displaystyle J_{\text{odom}}(\mathcal{X}) \displaystyle=\sum_{i}^{{N}}\kappa_{\text{odom}}\left\|\mbf{C}_{i}-\mbf{C}_{i}\Delta\mbf{C}_{i}\right\|_{{\text{F}}}^{2}+ \displaystyle\quad\frac{1}{{\sigma}_{r,\text{odom}}^{2}}\left\|\mbf{r}_{i+1}-\mbf{r}_{i}-\mbf{C}_{k}\Delta\mbf{r}_{i}\right\|_{2}^{2}, (3) where \Delta\mbf{C}_{i} is the relative direction cosine measurement and \Delta\mbf{r}_{i} is the relative position measurement. The prior cost inserts a prior on the first pose and is given by \displaystyle J_{\text{prior}}(\mathcal{X}) \displaystyle=\kappa_{\text{prior}}\left\|\mbf{C}_{1}-\check{\mbf{C}}_{1}\right\|_{{\text{F}}}^{2}+\frac{1}{{\sigma}_{r,\text{prior}}^{2}}\left\|\mbf{r}_{1}-\check{\mbf{r}}_{1}\right\|_{2}^{2}, (4) where \check{\mbf{C}}_{1} and \check{\mbf{r}}_{1} are the prior orientation and position, respectively. The log-likelihood residual arising from ( 1 ) can be written as [ 15 ] , \displaystyle r(\mbf{C}_{i},\mbf{r}_{i};{\boldsymbol{\ell}}_{j},{\mbf{y}}) \displaystyle=\frac{1}{{\sigma}_{\text{lndmrk}}^{2}}\left\|({\boldsymbol{\ell}}_{j}-\mbf{r}_{i})-\mbf{C}_{i}{\mbf{y}}\right\|_{2}^{2}, (5) Such that the unknown data association landmark measurement cost J_{\text{uda, lndmrk}}(\mathcal{X},\boldsymbol{{\theta}}) becomes \displaystyle J_{\text{uda, lndmrk}}(\mathcal{X},\boldsymbol{{\theta}}) \displaystyle=\sum_{i=1}^{{N}}\sum_{{k}_{i}=1}^{{K}_{i}}\sum_{j=1}^{{n_{\ell}}}{\theta}_{i{k}j}r(\mbf{C}_{i},\mbf{r}_{i};{\boldsymbol{\ell}}_{j},{\mbf{y}}_{{k}_{i}}). (6) The outer loop is over the time indices i . The middle loop is over the {k} ’th received measurement at timestep i , {\mbf{y}}_{{k}_{i}} . The inner loop is over the possible data associations. The SDP formulation used for treating unknown data associations is very similar to the certifiable outlier rejection case [ 13 , 14 ] , However, the present work uses the assumption that a measurement came from one of a set of landmarks, as opposed to [ 13 , 14 ] where the measurements are assumed to follow a given probability distribution. The outlier problem solutions [ 13 , 14 ] add cost terms to the optimization, while the data-association-free assumption of the present work leads to additional SDP constraints arising from the sum constraint ( 2 ). Outlier rejection using SDPs has been considered by encoding the outlier status of measurements as boolean variables that are solved for alongside the continuous states of interest [ 13 , 14 ] . Cost terms that are dependent on the boolean variables that depend on the assumed outlier distribution are included in the optimization. Furthermore, the number of landmarks is assumed known in this problem setup. Thus, the problem setup essentially corresponds to the inner loop of the local algorithm proposed in [ 6 ] . A nuance of the proposed approach is in the assumption that a measurement comes from one of a set of landmarks. This assumption does not prevent many measurements being assigned to a single landmark in a “many-to-one” fashion, leaving open the possibility of using a null hypothesis landmark in future work, similarly to [ 16 ] . III Proposed Approach The proposed approach consists of a semidefinite relaxation in the spirit of the outlier rejection approach in [ 13 ] . III-A Mathematical Formulation A homogenization variable \mbf{H}\in\{-\mbf{1}_{2\times 2},\mbf{1}_{2\times 2}\} , which can be enforced quadratically, is introduced to be able to write cost and constraint terms that are linear in \mathcal{X} and \boldsymbol{{\theta}} . The impact of homogenization is discussed in [ 17 ] . To fix ideas, especially related to cost functions and constraints, \mbf{H}=\mbf{1}_{2\times 2} can be assumed. The direction cosine matrix variables may be concatenated to yield {\mbf{C}}=\begin{bmatrix}\mbf{C}_{1}&\cdots&\mbf{C}_{i}&\cdots&\mbf{C}_{{N}}\end{bmatrix} , while the position variables may be concatenated as {\mbf{r}}=\begin{bmatrix}\mbf{r}_{1}&\cdots&\mbf{r}_{i}&\cdots&\mbf{r}_{{N}}\end{bmatrix} . The continuous problem variable may be written \displaystyle{\boldsymbol{\Xi}} \displaystyle=\begin{bmatrix}\mbf{H}&{\mbf{C}}&{\mbf{r}}\end{bmatrix}\in\mathbb{R}^{2\times(2+3{N})}. (7) The columns of {\boldsymbol{\Xi}} are denoted as {\boldsymbol{\xi}}_{k} such that {\boldsymbol{\Xi}}=\begin{bmatrix}{\boldsymbol{\xi}}_{1}&\dots&{\boldsymbol{\xi}}_{k}&\dots&{\boldsymbol{\xi}}_{K}\end{bmatrix} . The odometric cost ( 3 ) may be written as {\left\langle{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}},\mbf{Q}_{\text{odom}}\right\rangle} where the matrix inner product {\left\langle\cdot,\cdot\right\rangle} is defined for matrices \mbf{A},\mbf{B}\in\mathbb{R}^{m\times n} as {\left\langle\mbf{A},\mbf{B}\right\rangle}={\mathrm{tr}}(\mbf{A}\mbf{B}^{\mathsf{T}})=\sum_{i=1}^{M}\sum_{j=1}^{N}a_{ij}b_{ij} , and \mbf{Q}_{\text{odom}} is a data matrix that depends on the measurements \Delta\mbf{C}_{i},\Delta\mbf{r}_{i} . Similarly, the prior cost ( 4 ) can be written as {\left\langle{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}},\mbf{Q}_{\text{prior}}\right\rangle} with \mbf{Q}_{\text{prior}} a data matrix that depends on the prior values. The known data association loss term corresponding to a given timestep and landmark measurement ( 5 ) can also be written as {\left\langle{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}},\mbf{Q}_{\text{lndmrk},i,{k},j}\right\rangle} where \mbf{Q}_{\text{lndmrk},i,{k},j} depends on the j ’th landmark position {\boldsymbol{\ell}}_{j} as well as on the measurement received, \mbf{y}_{{k}_{i}} . To write the SDP formulation for the unknown data association, let the optimization variable \mbf{X} be defined as \displaystyle\mbf{X} \displaystyle=\begin{bmatrix}\mbf{H}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\boldsymbol{\Xi}&\boldsymbol{\Xi}\end{bmatrix}, (8) where \mbf{X} has {n_{x}}=(2+2{n_{\theta}}+3{N}({n_{\theta}}+1)) columns such that \mbf{X}\in\mathbb{R}^{2\times{n_{x}}} . The unknown data association loss ( 6 ) may then be written as {\left\langle\mbf{X}^{\mathsf{T}}\mbf{X},\mbf{Q}_{\text{uda}}\right\rangle} , which contains the individual known data association cost matrices \mbf{Q}_{\text{lndmrk},i,{k},j} to achieve equality between {\left\langle\mbf{X}^{\mathsf{T}}\mbf{X},\mbf{Q}_{\text{uda}}\right\rangle} and ( 6 ). The overall problem cost matrix may be written \mbf{Q}=\mbf{Q}_{\text{uda}}+\mbf{Q}_{\text{odom}}+\mbf{Q}_{\text{prior}} , and Problem ( Localization ) may be formulated as \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{X}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{X}}{\mathrm{minimize}}}\quad\sum_{i=1}^{{n_{\theta}}}{\left\langle\mbf{X}^{\mathsf{T}}\mbf{X},\mbf{Q}\right\rangle} ( Reform. Prob ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{X}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle\mbf{X}\in\{-\mbf{1},\mbf{1}\}\times\{\mbf{0},\mbf{1}\}^{{n_{\theta}}}\times \displaystyle\quad\left(SO(2)^{N}\times(\mathbb{R}^{2})^{N}\right)^{(1+{n_{\theta}})}. The convention of [ 13 ] for overloading the \times operator is used for defining blockwise matrix membership constraints. Relaxing the \mbf{C}_{i}\in SO(2) orthonormality constraint to the orthogonal constraint \mbf{C}_{i}\in O(2) yields the constraint \mbf{X}\in\{-\mbf{1},\mbf{1}\}\times\{\mbf{0},\mbf{1}\}^{{n_{\theta}}}\times\left(O(2)^{N}\times(\mathbb{R}^{2})^{N}\right)^{(1+{n_{\theta}})} , which can be written fully in terms of a homogenization constraint {\left\langle\mbf{A}_{\text{hom}},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle}=1 as well as quadratic constraints of the form {\left\langle\mbf{A}_{i,\text{init}},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle}=0,\quad i=1,\dots,n_{\text{constr, init}} . These constraints, with constraint matrices \mbf{A}_{i,\text{init}} , are termed the initial constraints, as additional redundant constraints have to then be added to tighten the relaxation. The orthogonality relaxation can thus be written \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{X}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{X}}{\mathrm{minimize}}}\quad{\left\langle\mbf{X}^{\mathsf{T}}\mbf{X},\mbf{Q}\right\rangle} ( Orth. Relax. ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{X}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle{\left\langle\mbf{A}_{\text{hom}},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle} \displaystyle=1 \displaystyle{\left\langle\mbf{A}_{i,\text{init}},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle} \displaystyle=0,\quad i=1,\dots,n_{\text{constr, init}}. Introducing a matrix variable \mbf{Z} that corresponds to \mbf{X}^{\mathsf{T}}\mbf{X} an equivalent problem to Problem ( Orth. Relax. ) as \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{Z}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{Z}}{\mathrm{minimize}}}\quad{\left\langle\mbf{Z},\mbf{Q}\right\rangle} ( Orth. Relax. Z. ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{Z}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle{\left\langle\mbf{A}_{\text{hom}},\mbf{Z}\right\rangle} \displaystyle=1 \displaystyle{\left\langle\mbf{A}_{i,\text{init}},\mbf{Z}\right\rangle} \displaystyle=0,\quad i=1,\dots,n_{\text{constr, init}} \displaystyle{\mathrm{rank}}(\mbf{Z}) \displaystyle=2. Relaxing the {\mathrm{rank}}(\mbf{Z})=2 constraint yields the semidefinite program, \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{Z}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{Z}}{\mathrm{minimize}}}\quad{\left\langle\mbf{Q},\mbf{Z}\right\rangle} ( Rank Relax. ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{Z}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle{\left\langle\mbf{A}_{\text{hom}},\mbf{Z}\right\rangle} \displaystyle=1 \displaystyle{\left\langle\mbf{A}_{i,\text{init}},\mbf{Z}\right\rangle} \displaystyle=0,\quad i=1,\dots,n_{\text{constr, init}} \displaystyle\mbf{Z} \displaystyle{\ \succcurlyeq\ }0. ( Rank Relax. ) is convex and admits a globally optimal solution in polynomial time. If the solution to ( Rank Relax. ) has rank two, then it is feasible for ( Orth. Relax. Z. ), and is thus the global optimum for ( Orth. Relax. Z. ). The relaxation is then considered tight , specifically in the rank sense as the solution to ( Rank Relax. ) would satisfy the rank two constraint of ( Orth. Relax. Z. ). The variable \mbf{X} may then be extracted from \mbf{Z} and is the global optimum for ( Orth. Relax. ). In turn, if this \mbf{X} is also feasible for ( Reform. Prob ), such that the direction cosine matrix blocks of \mbf{X} have determinant equal to one, the globally optimal solution to ( Reform. Prob ), and thus the localization problem ( Localization ), is obtained. However, tightness is not guaranteed since the explicit rank constraint is dropped from ( Orth. Relax. Z. ). To encourage tightness, redundant constraints of the form {\left\langle\mbf{A}_{\text{red},i},\mbf{Z}\right\rangle}=0 are added to the relaxation to yield \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{Z}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{Z}}{\mathrm{minimize}}}\quad{\left\langle\mbf{Q},\mbf{Z}\right\rangle}, ( Tightened Relax. ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{Z}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle{\left\langle\mbf{A}_{\text{hom}},\mbf{Z}\right\rangle} \displaystyle=1 \displaystyle{\left\langle\mbf{A}_{i,\text{init}},\mbf{Z}\right\rangle} \displaystyle=0,\quad i=1,\dots,n_{\text{constr, init}} \displaystyle{\left\langle\mbf{A}_{j,\text{red}},\mbf{Z}\right\rangle} \displaystyle=0,\quad j=1,\dots,n_{\text{constr, red}} \displaystyle\mbf{Z} \displaystyle{\ \succcurlyeq\ }0. III-B Redundant Constraints Finding the redundant constraints is problem-specific and can be difficult. A general purpose method is proposed in [ 18 ] that exploits the fact that any constraint matrix \mbf{A} lies in the nullspace, in a matrix inner product sense, of all feasible points for the SDP. By generating many such feasible points and examining the nullspace of the resulting data matrix, redundant constraints may be obtained. The work of [ 18 ] proposes both a method for finding constraints for a given problem instance, AutoTight , as well as a method of generalizing them, AutoTemplate . The present work used AutoTight on small problem instances, which yielded interpretable constraints that were then applied to larger problem instances. These analytical constraints are presented in the hope that they illuminate the problem structure and can be useful to potential analytical future work. Herein, given a matrix \mbf{A}=\begin{bmatrix}\mbf{a}_{1}&\dots&\mbf{a}_{i}&\dots&\mbf{a}_{N}\end{bmatrix} , the integer i={\textrm{slice}_{\mbf{A}}(\mbf{a}_{i})} is used to denote the column index corresponding to \mbf{a}_{i} in \mbf{A} . For instance, denoting the first column of the homogenization variable \mbf{H} as \mbf{h}_{1} , then {\mbf{Z}[{\textrm{slice}_{\mbf{X}}(\mbf{h}_{1})},{\textrm{slice}_{\mbf{X}}(\mbf{h}_{1})}]} is used to refer to the entry of \mbf{Z} that has both the row and column index equal to the position of \mbf{h}_{1} in the optimization variable \mbf{X} in ( 8 ). III-B 1 Discrete Variable Constraints Let {\tilde{\boldsymbol{{\theta}}}}^{\mathsf{T}}=\begin{bmatrix}1&\boldsymbol{{\theta}}^{\mathsf{T}}\end{bmatrix} . These constraints arise from algebraic observations of the form {\tilde{\boldsymbol{{\theta}}}}^{\mathsf{T}}\mbf{A}_{\theta}{\tilde{\boldsymbol{{\theta}}}}=0 III-B 2 Continuous Variable Constraints The continuous variable constraints are of the form \displaystyle{\left\langle\mbf{A}_{\Xi},\begin{bmatrix}\mbf{1}\\ {\boldsymbol{\Xi}}^{\mathsf{T}}\end{bmatrix}\begin{bmatrix}\mbf{1}&{\boldsymbol{\Xi}}\end{bmatrix}\right\rangle} \displaystyle=0. (9) For the robot navigation problem considered, these constraints correspond to the direction cosine matrix parts being constrained as orthogonal. III-B 3 Combining Discrete and Continuous Variable Constraints Constraints acting on rows and columns corresponding to combinations of discrete and continuous variables can be formed as follows. First, considering two columns of {\boldsymbol{\Xi}} , denoted {\boldsymbol{\xi}}_{k},{\boldsymbol{\xi}}_{\ell} , as well as two discrete variables in the problem {\theta}_{i},{\theta}_{j} , a set of redundant constraints comes from the algebraic observation given by \displaystyle{\boldsymbol{\xi}}_{k}^{\mathsf{T}}{\boldsymbol{\xi}}_{\ell}{\tilde{\boldsymbol{{\theta}}}}^{\mathsf{T}}\mbf{A}_{\theta}{\tilde{\boldsymbol{{\theta}}}} \displaystyle=\sum_{i,j}a_{ij}{\tilde{{\theta}}}_{i}{\tilde{{\theta}}}_{j}{\boldsymbol{\xi}}_{k}^{\mathsf{T}}{\boldsymbol{\xi}}_{\ell} (10) \displaystyle=\sum_{i,j}a_{ij}\left({\tilde{{\theta}}}_{i}{\boldsymbol{\xi}}_{k}^{\mathsf{T}}\right)\left({\tilde{{\theta}}}_{j}{\boldsymbol{\xi}}_{\ell}\right), (11) which corresponds to constraint matrix \mbf{A} with elements \displaystyle\mbf{A}[{\textrm{slice}_{\mbf{X}}({\tilde{{\theta}}}_{i}{\boldsymbol{\xi}}_{k})},{\textrm{slice}_{\mbf{X}}({\tilde{{\theta}}}_{j}{\boldsymbol{\xi}}_{\ell})}] \displaystyle=\mbf{A}_{\tilde{{\theta}}}[{\textrm{slice}_{\boldsymbol{{\tilde{\boldsymbol{{\theta}}}}^{\mathsf{T}}}}({\tilde{{\theta}}}_{i})},{\textrm{slice}_{{\tilde{\boldsymbol{{\theta}}}}^{\mathsf{T}}}({\tilde{{\theta}}}_{j})}]. (12) Second, given a constraint of the form \displaystyle g({\boldsymbol{\Xi}}) \displaystyle={\left\langle\mbf{A}_{{\boldsymbol{\Xi}}},\begin{bmatrix}\mbf{H}&{\boldsymbol{\Xi}}\end{bmatrix}^{\mathsf{T}}\begin{bmatrix}\mbf{H}\\ {\boldsymbol{\Xi}}\end{bmatrix}\right\rangle}=\mbf{0}, (13) it can be premultiplied by {\theta}_{i} to yield \displaystyle{\theta}_{i}g({\boldsymbol{\Xi}}) \displaystyle={\left\langle\mbf{A}_{{\boldsymbol{\Xi}}},\begin{bmatrix}{\boldsymbol{\Theta}}_{i}&\theta_{i}{\boldsymbol{\Xi}}\\ \theta_{i}{\boldsymbol{\Xi}}^{\mathsf{T}}&(\theta_{i}{\boldsymbol{\Xi}})^{\mathsf{T}}(\theta_{i}{\boldsymbol{\Xi}})\end{bmatrix}\right\rangle}, (14) where the boolean constraint \theta_{i}^{2}=\theta_{i} was used. In addition, moment constraints as well as constraints due to the particular matrix structure of ( 8 ) have to be added to the relaxation. These constraints are described in the supplementary material. III-C Sparsity A sparse basis is used to reduce the computational footprint of the algorithm. The \boldsymbol{\theta}^{\mathsf{T}}\otimes\boldsymbol{\Xi} portion of the optimization variable definition ( 8 ) is reduced solely to the terms necessary to express the unknown data association cost ( 6 ). Furthermore, redundant constraints that involve subsets of the discrete variables {\theta}_{i} and continuous variable columns {\boldsymbol{\xi}}_{k} are reduced to those where {\theta}_{i} and {\boldsymbol{\xi}}_{k} correspond to the same timestep i . This is found necessary to keep the problem size and computational footprint tractable, while still yielding an acceptably tight relaxation. III-D Extraction of Robot States From Optimization Variable The extraction of the robot states from the solution to ( Tightened Relax. ) is done in two steps. First, the optimization variable \mbf{X} of ( Reform. Prob ) is extracted from \mbf{Z} . This is done by recognizing that \mbf{Z}=\mbf{X}^{\mathsf{T}}\mbf{X} , and thus that the first two rows of \mbf{Z} are \mbf{Z}[:2,:]=\mbf{H}^{\mathsf{T}}\mbf{X} , and thus directly correspond to \mbf{X} . The robot states then correspond to the continuous variable {\boldsymbol{\Xi}} , extracted from the last block matrix of \mbf{X} in ( 8 ). The data association variables are then extracted from the \boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2} block of \mbf{X} in ( 8 ). IV Simulation and Experiments The experiments aim to demonstrate that the global minimum of the objective in Problem ( Localization ) is attained by the proposed approach, while the local method baseline gets trapped in local minima with incorrect data associations. The proposed algorithm is evaluated on simulated examples as well as real-world data. The parameters of interest are given by relative pose measurement noise levels, the relative landmark position measurement noise, the prior on the first pose, as well as the number of poses and landmarks in the setup. The proposed method consists of solving the SDP ( Rank Relax. ), and is referred to as the “SDP” method in text and figures. The local method consists of using the Max-Mixture method [ 10 ] , applied to the objective function of Problem ( Reform. Prob ), with the association variables analytically eliminated [ 19 , Chap. 4] . The data associations are solved for implicitly and recovered from the continuous robot states upon convergence. The solution considered the “true” solution is obtained by initializing the Max-Mixture baseline at the ground-truth robot states. The Gauss-Newton method is used, with a right perturbation Lie group formulation used to handle the orientations. The same loss function is used as for the relaxation ( Reform. Prob ), with the Frobenius norm handled using the approach outlined in [ 20 ] . The baseline algorithm, termed Max-Mix DR, consists of the Max-Mixture baseline initialized at the dead-reckoned state estimates. The proposed SDP relaxation only yields a certifiably optimal result when it is tight and the obtained solution is feasible for the non-relaxed problem, with {\mathrm{rank}}(\mbf{Z})=2 . This is measured using the eigenvalue ratio of the second and third-largest eigenvalues of \mbf{Z} . In an ideal scenario, this eigenvalue ratio is infinite, and the matrix rank is exactly two. In practice, a numerical cutoff threshold is used due to the numerical nature of the solver. An eigenvalue ratio threshold of 10^{6} is used in this work, following [ 21 ] . The experiments were run on an OptiPlex 7000 workstation with an Intel Core i9-12900K processor and 32 GB of memory. IV-A Metrics Position and data association errors are used to quantify algorithm performance. The position error for a given trial is computed as the absolute trajectory error (ATE) quantified using the root-mean-square error (RMSE) [ 22 ] , \text{ATE}_{\text{pos}}=\frac{1}{{N}}\sum_{i}^{N}\left\|\hat{\mbf{r}}_{i}-\bar{\mbf{r}}_{i}\right\|_{2} . where \hat{\mbf{r}}_{i} and \bar{\mbf{r}}_{i} are the estimated and true robot positions at timestep i . When computing \text{ATE}_{\text{pos}} for many trials, such as for trials corresponding to given noise values, the median across the trials is used. The data association error is computed as a binary quantity, corresponding to whether all the data associations are computed correctly in the problem. When computing the data association error for many trials, the fraction of trials where the data associations are computed correctly is used. IV-B Simulated Example IV-B 1 Setup Ground truth data for simulated examples was generated by first generating {N} random poses on SE(2) as \mbf{T}_{i}=\exp({\boldsymbol{\xi}}^{\wedge}) , with {\boldsymbol{\xi}}^{\mathsf{T}}=\begin{bmatrix}\xi^{\phi}&{\boldsymbol{\xi}}^{\text{r}^{\mathsf{T}}}\end{bmatrix} , with \xi^{\phi} generated using the uniform distribution \xi^{\phi}\sim\mathcal{U}(0,2\pi) and {\boldsymbol{\xi}}^{\text{r}} generated from the Gaussian distribution {\boldsymbol{\xi}}^{\text{r}}\sim\mathcal{N}\left({\boldsymbol{\xi}}^{\text{r}};\mbf{0},\mbf{1}\right) . The exponential map \exp({\boldsymbol{\xi}}^{\wedge}) is associated with the SE(2) matrix Lie group [ 23 ] . Landmarks are generated on a grid with {\boldsymbol{\ell}}_{j}\sim\mathcal{U}([0,10]\times[0,10]) . Relative pose and landmark measurements are generated from the ground truth and corrupted by noise to be used in the estimator. The same approach for parametrizing relative pose measurement noise is used as in [ 15 ] . Baseline noise levels are set separately for the orientation and position components, then scaled by a multiplier m_{\text{noise, rel}} . The multiplier is used as a measure of the noise. Relative orientation noise is parametrized by 1/\kappa of ( 3 ). The value of 1/\kappa increases with higher noise. The relative pose position component has covariance {\sigma}_{r}^{2}\mbf{1} . The base values of 1/\kappa and {\sigma}_{r}^{2} are set to 0.01\frac{1}{\text{rad.}^{2}} and 0.745~\text{m}^{2} . The parameters used for the Monte Carlo simulation runs are given by ({n_{\text{poses}}},{n_{\text{lndmrk}}},m_{\text{noise}},{\sigma}_{p}^{2})\in(3,5)\times(2,3)\times(0.1,1,10,20,30,40,50,60)\times(0.5,1,2,3,4,5) , with 10 Monte Carlo trials generated for each parameter configuration, for a total of 1920 trials. The relative landmark position measurement is directly parametrized by the scalar variance multiplied by identity covariance, {\sigma}_{p}^{2}\mbf{1} . Priors on the first pose were set to the ground truth with \kappa_{\text{prior, rot}}=100 and 1/{\sigma}_{\text{prior, pos}}^{2}=0.01 . To evaluate optimality of the proposed approach, when state errors are computed, unless explicitly stated otherwise, they are computed with respect to a local method result initialized at the ground truth , since the ground truth may not correspond to the global objective minimum. IV-B 2 Discussion The proposed method is tight for reasonable noise levels, as demonstrated in Figure 2 . The method starts breaking down to around 60% tightness rate for a landmark covariance of 4 m 2 and a relative pose noise multiplier of 40. The expected behavior is for tightness fraction to go down with noise. The trend is not perfect, as particularly for Rel. Pos Noise Scale=1 , the tightness fraction decreases to 0.7 before going up again and resuming the expected trend, which is attributed to experimental spread. However, the general trend is as expected, with tightness rates going down with increasing landmark measurement noise as well as increasing relative pose measurement noise. The data association correctness for tight cases is presented in Figure 3 , where the heatmaps plot the fraction of cases where at least one computed data association did not match the original data associations versus the relative pose and relative landmark position measurement noise levels. For the tight cases, it is expected that the SDP method attains the correct data associations, while the Max-Mix DR performs worse, especially with increasing noise levels. Furthermore, it is expected that the Max-Mix GT attains the correct data associations in all cases. While it is observed that the Max-Mix DR does perform worse than SDP, neither SDP nor Max-Mix GT attain the ground truth data associations in all cases. This is because the minimum of the objective function with noisy measurements of Problem ( Localization ) does not necessarily have the same data associations as the ground truth noiseless setup. Similarly, the SDP solution does not always match the Max-Mix GT solution because the ground truth initialization might not attain the global minimum of the problem. This was supported by finding that, in tight cases, the cost attained by SDP was always lower than that of Max-Mix GT. Furthermore, the improved data association obtained by SDP compared to Max-Mix DR is shown to improve position error in Figure 4 . The drawback of the proposed implementation is scalability. A runtime comparison is shown in Table II , where the proposed method has a median runtime of 32.81 seconds for the case of 5 poses and 3 landmarks. Generic SDP solvers such as MOSEK do not scale well past medium-size problems, a threshold that is reached quickly since the problem size scales linearly with the amount of discrete variables added. Fig. 2: Fraction of tight cases in simulated example, evaluated using a threshold of \lambda_{2}/\lambda_{3}\geq 10^{6} . The breakdown occurs at a relative pose measurement noise scale of around 30 and a landmark position noise scale of 3. Fig. 3: Evaluation of data association error rate for tight cases in simulated example. For each noise level combination, the corresponding heatmap value denotes the fraction of trials in which at least one data association is incorrect. Fig. 4: Evaluation of position error for tight cases in simulated example. Each entry in the heatmap corresponds to the median position error for trials for the corresponding noise values. TABLE II: Median Solver Time for Simulation Experiment (seconds) Landmarks 3 Poses 5 Poses SDP MM DR INIT SDP MM DR INIT 2 0.64 \mbf{0.03} 2.19 \mbf{0.04} 3 6.14 \mbf{0.03} 32.81 \mbf{0.06} IV-C Lost in the Woods Dataset IV-C 1 Setup The Lost in the Woods dataset [ 24 ] is used to validate the proposed approach on real data. It consists of a wheeled robot driving in a forest of plastic tubes that are treated as landmarks. The robot receives wheel odometry measurements that provide forward and angular velocity measurements. A laser rangefinder provides range-bearing measurements to the landmarks, whose positions are provided in the dataset. The wheel odometry measurements are integrated to provide relative pose measurements using the body frame velocity process model [ 25 ] , while the range-bearing measurements are converted to relative position measurements. The orientation component of the relative pose cost ( 3 ) arises as a log-likelihood of an isotropic Langevin noise distribution on SO(2) [ 26 ] . Given a set of pairs of true relative orientations and corresponding measured relative orientations computed from odometry, the parameter \kappa in ( 3 ) is computed by reverse-engineering the isotropic Langevin distribution sampling procedure in [ 26 ] . Priors on the first pose were set to the ground truth with \kappa_{\text{prior, rot}}=100 and {\sigma}_{\text{prior, pos}}^{2}=0.01 . The relative pose position components and the relative landmark position measurement noise covariances are also computed using the measurements compared to the ground-truth data. The dataset is used to extract subsequences, parametrized by the number of poses {n_{\text{poses}}} , the number of landmarks used {n_{\text{lndmrk}}} , as well as the spacing between poses {\Delta t} , measured in seconds. The higher the spacing between the poses, the more error accumulates for the relative pose measurements obtained by integrating the odometry. The subsequences extracted for each dataset parameter setting ({n_{\text{poses}}},{n_{\text{lndmrk}}},{\Delta t}) are non-overlapping. While the dataset provides 17 landmarks, only a subset is used for each sequence. For a set amount of landmarks, {n_{\text{lndmrk}}} , the landmarks used are chosen that are visible at the largest amount of the selected subsequence timesteps. The Monte Carlo trials are run for parameter configurations given by ({n_{\text{poses}}},{n_{\text{lndmrk}}},{\Delta t})\in(3,5)\times(2,3)\times(20,40,60) . The overall dataset is 20 minutes long. For each parameter set ({n_{\text{poses}}},{n_{\text{lndmrk}}},{\Delta t}) , the first subsequence starts at t=0 , with each following subsequence starting at the end of the previous one. For each combination of {n_{\text{poses}}},{\Delta t} , the number of subsequences is chosen to be the maximum allowed for by the dataset length. IV-C 2 Discussion The parameters used for the x and y axes are different for the experiment section compared to the simulated examples. Since the landmark measurement noise is fixed, the varied parameters consist of {n_{\text{poses}}} and {\Delta t} . The {\Delta t} essentially corresponds to the odometry measurement noise, while {n_{\text{poses}}} corresponds to increasing the problem size and trajectory length. Thus, increasing either {\Delta t} or {n_{\text{poses}}} is expected to worsen the quality of the dead-reckoned initialization. Furthermore, increasing {\Delta t} is expected to degrade the tightness of the relaxation. The fraction of tight cases for each parameter configuration is presented in Fig. 5 . While the method is tight in the majority of cases, it is hard to extract a clear pattern of worsening tightness with noise in this setup. This can be ascribed to the fewer amount of subsequences used in this setup compared to the simulation. In Fig. 2 , each square corresponds to 40 Monte Carlo trials, while in Fig. 5 the number of subsequences used ranges from 21 for {\Delta t}=20,{n_{\text{poses}}}=3 to only four for the {\Delta t}=60,{n_{\text{poses}}}=5 case. The data association results are presented in Fig. 6 , where both tight and nontight results are plotted together. This is unlike the simulated case where only the tight results were plotted. In this experiment, both tight and nontight solutions were shown to yield good results. In both tight and nontight cases, SDP matches Max-Mix GT in all cases, while Max-Mix DR has a very high failure rate for {n_{\text{poses}}}=5 . This is expected, since with a higher number of poses, the quality of the dead-reckoned initialization worsens leading to degradation of performance of the local method that is dependent on the initialization. The position error in Table III follows the same trend as the data association plot in Fig. 6 , with position errors increasing for {n_{\text{poses}}}=5 , showing that the incorrect data association of the local method also leads to a degradation of the estimated robot state. The position error is computed with respect to the trajectory computed by the Max-Mix GT method. A scalar position error is computed for each trajectory, and the median for each parameter configuration is shown in the table. Fig. 5: Fraction of tight cases in Lost in the Woods, evaluated using a threshold of \lambda_{2}/\lambda_{3}\geq 10^{6} . While the method is tight in the majority of cases, there is no clear relationship demonstrated between an increase in pose spacing, essentially corresponding to odometry noise, and the tightness of the relaxation. Fig. 6: Evaluation of data association correctness for the Lost in the Woods example. This includes all results, both tight and nontight. TABLE III: Median Position Error For Lost in The Woods Dataset Pose Spacing (s) 3 Poses 5 Poses SDP MM DR INIT SDP MM DR INIT 20 1.8\!\times\!10^{-5} \mbf{2.4\!\times\!10^{-11}} 5.3\!\times\!10^{-5} \mbf{3.2\!\times\!10^{-11}} 40 7.8\!\times\!10^{-6} \mbf{2.0\!\times\!10^{-11}} \mbf{5.5\!\times\!10^{-5}} 1.2\!\times\!10^{-1} 60 8.9\!\times\!10^{-6} \mbf{7.5\!\times\!10^{-11}} \mbf{1.8\!\times\!10^{-5}} 1.7\!\times\!10^{-1} V Conclusion This paper proposes a relaxation-based globally optimal method for robot localization. The relaxation is tight in the majority of cases in simulation, and the majority of cases in real-world experiment. While offering global optimality in cases where tightness is attained, the scalability of the algorithm is limited by the difficulty in solving large-scale SDPs. While the scaling of semidefinite program solvers is generally still prohibitive, advances have been made for specific problem instances, particularly for SDPs where the solution is of low rank [ 26 , 27 , 28 , 14 ] , and future SDP solvers may allow scaling of the proposed method beyond the small problem instances analyzed in this paper. Future work also includes development of certification schemes based on the relaxation. The use of a null hypothesis to account for misidentified landmarks is also a promising direction of future research, where, similarly to [ 16 ] , outlier measurements are assigned to a dummy landmark. The null hypothesis would make use of the possibility of “many-to-one” assignment of measurements to landmarks. Furthermore, while this paper specifically considers the setting of robot localization, the overall approach and redundant constraints used are applicable to other settings with unknown data association, investigating which is another topic of future work. The appendix provides details on the construction of the cost and constraint matrices. In turn, the following subproblems are considered: The continuous subproblem, the discrete subproblem, and the combination of the two. -A Continuous Problem A homogenization variable \mbf{H}\in\{-\mbf{1}_{2\times 2},\mbf{1}_{2\times 2}\} , which can be enforced quadratically, is introduced to be able to write cost and constraint terms that are linear in \mathcal{X} and \boldsymbol{{\theta}} . The impact of homogenization is discussed in [ 17 ] . To fix ideas, especially related to cost functions and constraints, \mbf{H}=\mbf{1}_{2\times 2} can be assumed. The direction cosine matrix variables may be concatenated to yield {\mbf{C}}=\begin{bmatrix}\mbf{C}_{1}&\cdots&\mbf{C}_{i}&\cdots&\mbf{C}_{{N}}\end{bmatrix} , while the position variables may be concatenated as {\mbf{r}}=\begin{bmatrix}\mbf{r}_{1}&\cdots&\mbf{r}_{i}&\cdots&\mbf{r}_{{N}}\end{bmatrix} . The continuous problem variable is denoted {\boldsymbol{\Xi}} and is obtained by horizontal concatenation of the robot states as well as of the homogenization variable \displaystyle{\boldsymbol{\Xi}} \displaystyle=\begin{bmatrix}\mbf{H}&{\mbf{C}}&{\mbf{r}}\end{bmatrix}\in\mathbb{R}^{2\times(2+3{N})}. (15) A cost term that depends on the continuous variable may be expressed as J_{{\boldsymbol{\Xi}}}({\boldsymbol{\Xi}}) \displaystyle J_{{\boldsymbol{\Xi}}}({\boldsymbol{\Xi}})={\left\langle\mbf{Q}_{{\boldsymbol{\Xi}}},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle}, (16) while a constraint may be expressed as \displaystyle{\left\langle\mbf{A}_{\Xi},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle}=b, (17) where \mbf{A}_{\Xi} is a matrix and b a scalar. While there are many cost terms and constraints in the problem, the cost term and constraint index is dropped for brevity of notation. The matrix inner product {\left\langle\cdot,\cdot\right\rangle} is defined for matrices \mbf{A},\mbf{B}\in\mathbb{R}^{m\times n} as {\left\langle\mbf{A},\mbf{B}\right\rangle}={\mathrm{tr}}(\mbf{A}\mbf{B}^{\mathsf{T}})=\sum_{i=1}^{M}\sum_{j=1}^{N}a_{ij}b_{ij} . Importantly, the costs and constraints can only be expressed using sums of dot products of the columns of {\boldsymbol{\Xi}} since, for a generic matrix \mbf{A} , \displaystyle{\left\langle\mbf{A},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle} \displaystyle=\sum_{i=1}^{M}\sum_{j=1}^{N}a_{ij}({\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}})_{ij} (18) \displaystyle=\sum_{i=1}^{M}\sum_{j=1}^{N}a_{ij}{\boldsymbol{\xi}}_{i}^{\mathsf{T}}{\boldsymbol{\xi}}_{j}. (19) It is worth discussing the main alternative to this approach, which consists of flattening the variable into a single column matrix {\boldsymbol{\xi}}\in\mathbb{R}^{1+6N} , containing all of the robot states and a scalar homogenization variable. This alternative leads to cost and constraint terms expressed using the quadratic expressions of the form {\boldsymbol{\xi}}^{\mathsf{T}}\mbf{A}{\boldsymbol{\xi}} . This alternative leads to a larger problem dimension and can require a larger amount of redundant constraints on the relaxation. However, it is more flexible, since constraints and costs can be expressed using any scalar in the continuous variable {\boldsymbol{\Xi}} , unlike the approach used in this work where only dot products of the columns of {\boldsymbol{\Xi}} may be used. A downstream consequence of only being able to use column dot products to express the cost is that the noise models used are less flexible. -A 1 Cost Matrix Construction Cost terms in the continuous variable are expressed using the quadratic form \displaystyle J_{{\boldsymbol{\Xi}}}({\boldsymbol{\Xi}})={\left\langle\mbf{Q}_{{\boldsymbol{\Xi}}},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle}. (20) For the prior and odometry terms, the entries of \mbf{Q}_{{\boldsymbol{\Xi}}} are inserted directly into corresponding entries of the overall cost matrix. For the unknown data associations, they are inserted into entries of the overall cost matrix that involve corresponding discrete variables. -A 2 Lifting a Pose to Known Landmark Measurement The error is the same as for the unknown landmark position measurement, \displaystyle e(\mbf{C}_{ab},\mbf{r};{\boldsymbol{\ell}},\mbf{y}) \displaystyle={\boldsymbol{\ell}}^{\mathsf{T}}{\boldsymbol{\ell}}+\mbf{r}^{\mathsf{T}}\mbf{r}+\mbf{y}^{\mathsf{T}}\cancel{\mbf{C}^{\mathsf{T}}\mbf{C}}\mbf{y}+2\mbf{r}^{\mathsf{T}}\mbf{C}\mbf{y}-2\mbf{r}^{\mathsf{T}}{\boldsymbol{\ell}}-2{\boldsymbol{\ell}}^{\mathsf{T}}\mbf{C}\mbf{y} (21) \displaystyle={\boldsymbol{\ell}}^{\mathsf{T}}{\boldsymbol{\ell}}+\mbf{r}^{\mathsf{T}}\mbf{r}+\mbf{y}^{\mathsf{T}}\mbf{y}+2\mbf{r}^{\mathsf{T}}\mbf{C}\mbf{y}-2\mbf{r}^{\mathsf{T}}{\boldsymbol{\ell}}-2{\boldsymbol{\ell}}^{\mathsf{T}}\mbf{C}\mbf{y}, (22) where subscripts are dropped for the sake of brevity. The last term bears a bit more consideration to include it in the matrix inner product formulation such that \displaystyle{\boldsymbol{\ell}}^{\mathsf{T}}\mbf{C}\mbf{y} \displaystyle={\mathrm{tr}}(\mbf{C}\mbf{y}{\boldsymbol{\ell}}^{\mathsf{T}}) (23) \displaystyle={\mathrm{tr}}(\mbf{C}({\boldsymbol{\ell}}\mbf{y}^{\mathsf{T}})^{\mathsf{T}}) (24) \displaystyle={\left\langle\mbf{C},{\boldsymbol{\ell}}\mbf{y}^{\mathsf{T}}\right\rangle}. (25) However, now the landmark {\boldsymbol{\ell}} is known, such that the error is a function of \mbf{C}_{ab},\mbf{r} and parametrized using {\boldsymbol{\ell}},\mbf{y} , where \mbf{y} is the measured relative position of the landmark. Using the state \begin{bmatrix}\mbf{1}&\mbf{C}&\mbf{r}\end{bmatrix} , the error may be written \displaystyle e(\mbf{C}_{ab},\mbf{r};{\boldsymbol{\ell}},\mbf{y}) \displaystyle={\boldsymbol{\ell}}^{\mathsf{T}}{\boldsymbol{\ell}}+\mbf{r}^{\mathsf{T}}\mbf{r}+\mbf{y}^{\mathsf{T}}\mbf{y}+2\mbf{r}^{\mathsf{T}}\mbf{C}\mbf{y}-2\mbf{r}^{\mathsf{T}}{\boldsymbol{\ell}}-2{\boldsymbol{\ell}}^{\mathsf{T}}\mbf{C}\mbf{y} (26) \displaystyle={\left\langle\mbf{Q},\mbf{X^{\mathsf{T}}\mbf{X}}\right\rangle} (27) \displaystyle={\left\langle\mbf{Q},\begin{bmatrix}\mbf{1}&\mbf{C}&\mbf{r}\\ &\mbf{1}&\mbf{C}^{\mathsf{T}}\mbf{r}\\ &&\mbf{r}^{\mathsf{T}}\mbf{r}\\ \end{bmatrix}\right\rangle} (28) \displaystyle={\left\langle\begin{bmatrix}{\mathrm{diag}}([({\boldsymbol{\ell}}^{\mathsf{T}}{\boldsymbol{\ell}}+\mbf{y}^{\mathsf{T}}\mbf{y})\mbf{e}_{1}])&-2{\boldsymbol{\ell}}\mbf{y}^{\mathsf{T}}&-2{\boldsymbol{\ell}}\\ &&2\mbf{y}\\ &&1\\ \end{bmatrix},\begin{bmatrix}\mbf{1}&\mbf{C}&\mbf{r}\\ &\mbf{1}&\mbf{C}^{\mathsf{T}}\mbf{r}\\ &&\mbf{r}^{\mathsf{T}}\mbf{r}\\ \end{bmatrix}\right\rangle} (29) -A 3 Lifting a Relative Pose Measurement Given a relative pose measurement such that \displaystyle\mbf{T}_{k+1} \displaystyle=\mbf{T}_{k}\Delta\mbf{T} (30) \displaystyle\begin{bmatrix}\mbf{C}_{k+1}&\mbf{r}_{k+1}\\ \mbf{0}&1\end{bmatrix} \displaystyle=\begin{bmatrix}\mbf{C}_{k}\Delta\mbf{C}&\mbf{r}_{k}+\mbf{C}_{k}\Delta\mbf{r}\\ \mbf{0}&1\end{bmatrix}, (31) the first error corresponds to the translation error and is given by \displaystyle J_{r} \displaystyle=\left\|\mbf{r}_{k+1}-\mbf{r}_{k}-\mbf{C}_{k}\Delta\mbf{r}\right\|_{2}^{2} (32) \displaystyle=(\mbf{r}_{k+1}-\mbf{r}_{k}-\mbf{C}_{k}\Delta\mbf{r})^{\mathsf{T}}(\mbf{r}_{k+1}-\mbf{r}_{k}-\mbf{C}_{k}\Delta\mbf{r}) (33) \displaystyle=\mbf{r}_{k+1}^{\mathsf{T}}\mbf{r}_{k+1}+\mbf{r}_{k}^{\mathsf{T}}\mbf{r}_{k}+\Delta\mbf{r}^{\mathsf{T}}\Delta\mbf{r}+2\mbf{r}_{k}^{\mathsf{T}}\mbf{C}_{k}\Delta\mbf{r}-2\mbf{r}_{k+1}^{\mathsf{T}}\mbf{C}_{k}\Delta\mbf{r}-2\mbf{r}_{k+1}\mbf{r}_{k}. (34) For \mbf{X}=\begin{bmatrix}\mbf{C}_{k}&\mbf{r}_{k}&\mbf{r}_{k+1}\end{bmatrix} , this yields the following cost, \displaystyle J_{r} \displaystyle={\left\langle\mbf{Q},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle} (35) \displaystyle={\left\langle\mbf{Q},\begin{bmatrix}\mbf{C}_{k}^{\mathsf{T}}\\ \mbf{r}_{k}^{\mathsf{T}}\\ \mbf{r}_{k+1}\end{bmatrix}\begin{bmatrix}\mbf{C}_{k}&\mbf{r}_{k}&\mbf{r}_{k+1}\end{bmatrix}\right\rangle} (36) \displaystyle={\left\langle\mbf{Q},\begin{bmatrix}\mbf{C}_{k}^{\mathsf{T}}\mbf{C}_{k}&\mbf{C}_{k}^{\mathsf{T}}\mbf{r}_{k}&\mbf{C}_{k}^{\mathsf{T}}\mbf{r}_{k+1}\\ \mbf{r}_{k}^{\mathsf{T}}\mbf{C}_{k}&\mbf{r}_{k}^{\mathsf{T}}\mbf{r}_{k}&\mbf{r}_{k}^{\mathsf{T}}\mbf{r}_{k+1}\\ \mbf{r}_{k+1}^{\mathsf{T}}\mbf{C}_{k}&\mbf{r}_{k+1}^{\mathsf{T}}\mbf{r}_{k}&\mbf{r}_{k+1}^{\mathsf{T}}\mbf{r}_{k+1}\end{bmatrix}\right\rangle} (37) \displaystyle={\left\langle\begin{bmatrix}\mbf{0}&2\Delta\mbf{r}&-2\Delta\mbf{r}\\ &1&-2\\ &&1\end{bmatrix},\begin{bmatrix}\mbf{C}_{k}&\mbf{r}_{k}&\mbf{r}_{k+1}\\ \mbf{r}_{k}^{\mathsf{T}}\mbf{C}_{k}&\mbf{r}_{k}^{\mathsf{T}}\mbf{r}_{k}&\mbf{r}_{k}^{\mathsf{T}}\mbf{r}_{k+1}\\ \mbf{r}_{k+1}^{\mathsf{T}}\mbf{C}_{k}&\mbf{r}_{k+1}^{\mathsf{T}}\mbf{r}_{k}&\mbf{r}_{k+1}^{\mathsf{T}}\mbf{r}_{k+1}\end{bmatrix}\right\rangle}. (38) The second error corresponds to the rotation error and is given by \displaystyle J_{c} \displaystyle=\left\|\mbf{C}_{k+1}-\mbf{C}_{k}\Delta\mbf{C}\right\|_{F}^{2} (39) \displaystyle={\mathrm{tr}}\left(\left(\mbf{C}_{k+1}-\mbf{C}_{k}\Delta\mbf{C}\right)\left(\mbf{C}_{k+1}-\mbf{C}_{k}\Delta\mbf{C}\right)^{\mathsf{T}}\right) (40) \displaystyle=2{\mathrm{tr}}(\mbf{1})-2{\mathrm{tr}}(\mbf{C}_{k+1}^{\mathsf{T}}\mbf{C}_{k}\Delta\mbf{C}) (41) \displaystyle=2{\mathrm{tr}}(\mbf{1}-\Delta\mbf{C}\mbf{C}_{k+1}^{\mathsf{T}}\mbf{C}_{k}) (42) Making use of {\mathrm{tr}}(\mbf{A}\mbf{B}^{\mathsf{T}})=\sum_{i}\sum_{j}\mbf{A}_{ij}\mbf{B}_{ij}={\left\langle\mbf{A},\mbf{B}\right\rangle} allows to write \displaystyle J_{c} \displaystyle=2{\mathrm{tr}}(\mbf{1}-\Delta\mbf{C}\mbf{C}_{k+1}^{\mathsf{T}}\mbf{C}_{k}) (45) \displaystyle=2\left({\mathrm{tr}}\mbf{1}-{\left\langle\Delta\mbf{C},\mbf{C}_{k}^{\mathsf{T}}\mbf{C}_{k+1}\right\rangle}\right) (46) \displaystyle={\left\langle\mbf{Q},\begin{bmatrix}\mbf{1}\\ \mbf{C}_{k}^{\mathsf{T}}\\ \mbf{C}_{k+1}^{\mathsf{T}}\end{bmatrix}\begin{bmatrix}\mbf{1}&\mbf{C}_{k}&\mbf{C}_{k+1}\end{bmatrix}\right\rangle} (47) \displaystyle={\left\langle\begin{bmatrix}2\mbf{1}&&\\ &\mbf{0}&-2\Delta\mbf{C}\\ &&\mbf{0}\end{bmatrix},\begin{bmatrix}\mbf{1}&\mbf{C}_{k}&\mbf{C}_{k+1}\\ \mbf{C}_{k}^{\mathsf{T}}&\mbf{C}_{k}^{\mathsf{T}}\mbf{C}_{k}&\mbf{C}_{k}^{\mathsf{T}}\mbf{C}_{k+1}\\ \mbf{C}_{k+1}^{\mathsf{T}}&\mbf{C}_{k+1}^{\mathsf{T}}\mbf{C}_{k}&\mbf{C}_{k+1}^{\mathsf{T}}\mbf{C}_{k+1}\end{bmatrix}\right\rangle}. (48) -A 4 Lifting a Prior Given a prior such that the error is given by \displaystyle J_{p} \displaystyle=\left\|\mbf{x}-\check{\mbf{x}}\right\|_{2}^{2} (49) \displaystyle=(\mbf{x}-\check{\mbf{x}})^{\mathsf{T}}(\mbf{x}-\check{\mbf{x}}) (50) \displaystyle=\mbf{x}^{\mathsf{T}}\mbf{x}-2\check{\mbf{x}}^{\mathsf{T}}\mbf{x}+\check{\mbf{x}}^{\mathsf{T}}\check{\mbf{x}} (51) for \mbf{X}=\begin{bmatrix}\mbf{1}&\mbf{x}\end{bmatrix} , this yields the following cost \displaystyle J_{p} \displaystyle=\mbf{x}^{\mathsf{T}}\mbf{x}-2\check{\mbf{x}}^{\mathsf{T}}\mbf{x}+\check{\mbf{x}}^{\mathsf{T}}\check{\mbf{x}} (52) \displaystyle={\left\langle\mbf{Q},\mbf{X}^{\mathsf{T}}\mbf{X}\right\rangle} (53) \displaystyle={\left\langle\begin{bmatrix}\check{\mbf{x}}^{\mathsf{T}}\check{\mbf{x}}&-2\check{\mbf{x}}^{\mathsf{T}}\\ &1\end{bmatrix},\begin{bmatrix}\mbf{1}&\mbf{x}\\ \mbf{x}&\mbf{x}^{\mathsf{T}}\mbf{x}\end{bmatrix}\right\rangle}. (54) -A 5 Constraint Matrix Definition The continuous variable constraints enforce orthonormality of the direction cosine matrix (DCMs) in the relaxation. For brevity, the constraints are given for the specific subvariables considered. In the overall problem, they are then inserted directly into the entries of the overall constraint matrix corresponding the subvariables. Symmetric constraint matrices are used in SDPs. Any non-symmetric constraint matrix may be symmetrized as \mbf{A}_{\text{sym}}=(\mbf{A}+\mbf{A}^{\mathsf{T}})/2 . Given a DCM \mbf{C} , the continuous variable constraints are of the form \displaystyle{\left\langle\mbf{A},\begin{bmatrix}\mbf{H}&\mbf{C}\\ \mbf{C}^{\mathsf{T}}&\mbf{C}^{\mathsf{T}}\mbf{C}\end{bmatrix}\right\rangle}=0, (55) where the homogenization variable \mbf{H} can be thought of as identity. The matrix \mbf{A} is used as a generic constraint matrix in this section. The continuous constraint matrices then correspond to 1. Orthonormality. For each column index combination i,j\in(1,2)\times(1,2) , a constraint matrix is added with the following structure. • If i\neq j , constraint matrix \mbf{A} has {\mbf{A}[{\textrm{slice}(\mbf{c}_{1})},{\textrm{slice}(\mbf{c}_{2})}]}=1 corresponding to orthogonality of different columns of \mbf{C} . • If i=j , constraint matrix \mbf{A} has {\mbf{A}[{\textrm{slice}(\mbf{c}_{1})},{\textrm{slice}(\mbf{c}_{2})}]}=1 , and {\mbf{A}[{\textrm{slice}(\mbf{h}_{1})},{\textrm{slice}(\mbf{h}_{1})}]}=-1 , corresponding to unit norm for each column of \mbf{C} . 2. Structure of two-dimensional DCM. For the planar case, the DCM has the structure \mbf{C}=\begin{bmatrix}c&-s\\ s&c\end{bmatrix} . Two additional constraints may then be added, • Constraint matrix \mbf{A} with {\mbf{A}[{\textrm{slice}(\mbf{c}_{1})},{\textrm{slice}(\mbf{h}_{1})}]}=1 , {\mbf{A}[{\textrm{slice}(\mbf{c}_{2})},{\textrm{slice}(\mbf{h}_{2})}]}=-1 , • Constraint matrix \mbf{A} with {\mbf{A}[{\textrm{slice}(\mbf{c}_{1})},{\textrm{slice}(\mbf{h}_{2})}]}=1 , {\mbf{A}[{\textrm{slice}(\mbf{c}_{2})},{\textrm{slice}(\mbf{h}_{1})}]}=1 . -B Discrete Problem The discrete variables arise from the unknown data association cost given by \displaystyle J_{\text{uda, lndmrk}}(\mathcal{X}) \displaystyle=\sum_{i=1}^{{N}}\sum_{{k}_{i}=1}^{{K}_{i}}\sum_{j=1}^{{n_{\ell}}}{\theta}_{i{k}j}r(\mbf{C}_{i},\mbf{r}_{i};{\boldsymbol{\ell}}_{j},{\mbf{y}}_{{k}_{i}}). (56) The outer loop is over the time indices i . The middle loop is over the {k} ’th received measurement at timestep i , {\mbf{y}}_{{k}_{i}} . The inner loop is over the possible data associations. The discrete variables are constrained to be boolean, \theta_{i{k}_{i}j}\in\{0,1\} . Furthermore, they are constrained by \displaystyle\sum_{j=1}^{{n_{\ell}}}{\theta}_{i{k}j}=1,\quad\forall{i,{k}}. (57) meaning that every measurement comes from a single landmark. The overall problem variable involves both discrete and continuous variables, and requires redundant constraints. These constraints in turn require quadratic constraints on the discrete variables of the form \displaystyle\begin{bmatrix}1&\boldsymbol{{\theta}}^{\mathsf{T}}\end{bmatrix}\mbf{A}_{\theta}\begin{bmatrix}1\\ \boldsymbol{{\theta}}\end{bmatrix}=0. (58) Constraint matrices \mbf{A}_{\theta} may be constructed from the following observations. Since they are used to construct redundant constraints, it is insufficient to only use the boolean and sum constraints, and a few more are required. • \sum_{j=1}^{{n_{\ell}}}{\theta}_{i,{k},j}=1 . This corresponds to \mbf{A}_{\theta} with {\mbf{A}[{\textrm{slice}(1)},{\textrm{slice}({\theta}_{i,{k},j})}]}=1 for every j , as well as {\mbf{A}[{\textrm{slice}(1)},{\textrm{slice}(1)}]}=-1 . • {\theta}_{i,{k},j}^{2}-{\theta}_{i,{k},j}=0 . This corresponds to \mbf{A}_{\theta} with {\mbf{A}[{\textrm{slice}({\theta}_{i,{k},j})},{\textrm{slice}({\theta}_{i,{k},j})}]}=1 , {\mbf{A}[{\textrm{slice}(1)},{\textrm{slice}({\theta}_{i,{k},j})}]}=-1 . • {\theta}_{i,{k},j_{1}}{\theta}_{i,{k},j_{2}}=0 , for j_{1}\neq j_{2} . This corresponds to \mbf{A}_{\theta} with {\mbf{A}[{\textrm{slice}({\theta}_{i,{k},j_{1}})},{\textrm{slice}({\theta}_{i,{k},j_{2}})}]}=1 . • {\theta}_{i,{k}_{2},j_{2}}\sum_{j=1}^{{n_{\ell}}}{\theta}_{i,{k}_{1},j}-{\theta}_{i,{k}_{2},j_{2}}=0 , for {k}_{1}\neq{k}_{2} , one constraint for each applicable j_{2} . This is a premultiplication of the sum constraint for one unknown data association measurement with discrete variables corresponding to other unknown data association measurements, from the same timestep. This corresponds to \mbf{A}_{\theta} constraints, one for each {k}_{1}\neq{k}_{2} and applicable j_{2} . For given {k}_{1},{k}_{2},j_{2} , the constraint matrix \mbf{A}_{\theta} has {\mbf{A}[{\textrm{slice}({\theta}_{i,{k}_{2},j_{2}})},{\textrm{slice}({\theta}_{i,{k}_{1},j})}]}=1 and {\mbf{A}[{\textrm{slice}({\theta}_{i,{k}_{2},j_{2}})},{\textrm{slice}(1)}]}=-1 . -C Combination of Discrete and Continuous Problems The overall problem contains both discrete and continuous variables with corresponding costs and constraints. The optimization variable of interest is given by \displaystyle\mbf{X} \displaystyle=\begin{bmatrix}\mbf{H}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\boldsymbol{\Xi}&\boldsymbol{\Xi}\end{bmatrix}. (59) The localization problem is expressed as \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\mbf{X}}{\mathrm{subject~to}}$}][l]{\underset{\displaystyle\mbf{X}}{\mathrm{minimize}}}\quad\sum_{i=1}^{{n_{\theta}}}{\left\langle\mbf{X}^{\mathsf{T}}\mbf{X},\mbf{Q}\right\rangle} ( Reform. Prob ) \displaystyle\mathmakebox[\widthof{$\underset{\displaystyle\phantom{\mbf{X}}}{\mathrm{subject~to}}$}][c]{{\mathrm{subject~to}}}\quad \displaystyle\mbf{X}\in\{-\mbf{1},\mbf{1}\}\times\{\mbf{0},\mbf{1}\}^{{n_{\theta}}}\times \displaystyle\quad\left(SO(2)^{N}\times(\mathbb{R}^{2})^{N}\right)^{(1+{n_{\theta}})}. The cost matrix \mbf{Q} is the overall cost matrix for the problem. The loss may be split into two parts, one solely depending on the continuous robot states \mathcal{X} , and one depending on both \mathcal{X} and the discrete variables \boldsymbol{{\theta}} . \displaystyle J(\mathcal{X},\boldsymbol{{\theta}}) \displaystyle=J_{1}(\mathcal{X})+J_{2}(\mathcal{X},\boldsymbol{{\theta}}). (60) The part of the loss J_{1}(\mathcal{X}) depending solely on \mathcal{X} may be written as \displaystyle J_{1}(\mathcal{X}) \displaystyle=\sum_{i=1}^{n_{\text{f, cont}}}{\left\langle\mbf{Q}_{{\boldsymbol{\Xi}},i},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle}, (61) where n_{\text{f, cont}} is the number of error terms that depend solely on the continuous variable. The continuous variable cost matrices \mbf{Q}_{{\boldsymbol{\Xi}},i} are inserted directly into the corresponding entries of the overall cost matrix \mbf{Q} . The discrete-variable dependent loss J_{2}(\mathcal{X},\boldsymbol{{\theta}}) takes the form \displaystyle J_{2}(\mathcal{X},\boldsymbol{{\theta}})=\sum_{i=1}^{{n_{\theta}}}{\theta}_{i}{\left\langle\mbf{Q}_{{\boldsymbol{\Xi}},i},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle}, (62) where i now loops over the discrete variables, each of which has an associated relative landmark measurement error expressed using {\left\langle\mbf{Q}_{{\boldsymbol{\Xi}},i},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle} . Each summand may be written as \displaystyle{\theta}_{i}{\left\langle\mbf{Q}_{{\boldsymbol{\Xi}},i},{\boldsymbol{\Xi}}^{\mathsf{T}}{\boldsymbol{\Xi}}\right\rangle} \displaystyle={\left\langle\mbf{Q}_{{\boldsymbol{\Xi}},i},{\boldsymbol{\Xi}}^{\mathsf{T}}\left({\theta}_{i}{\boldsymbol{\Xi}}\right)\right\rangle}. (63) Therefore, for each pair of columns {\boldsymbol{\xi}}_{j},{\boldsymbol{\xi}}_{k} of {\boldsymbol{\Xi}} , the corresponding parts of the overall cost matrix \mbf{Q} may be set as {\mbf{Q}[{\textrm{slice}({\boldsymbol{\xi}}_{j})},{\textrm{slice}(\theta_{i}{\boldsymbol{\xi}}_{k})}]}={\mbf{Q}_{{\boldsymbol{\Xi}},i}[{\textrm{slice}({\boldsymbol{\xi}}_{j})},{\textrm{slice}({\boldsymbol{\xi}}_{k})}]} . The discrete variable constraints of Sec. -B may be imposed on the overall optimization variable \mbf{X} by creating a constraint matrix \mbf{A} with {\mbf{A}[{\textrm{slice}({\boldsymbol{\Theta}}_{i,1})},{\textrm{slice}({\boldsymbol{\Theta}}_{j,1})}]}={\mbf{A}_{\boldsymbol{\Theta}}[{\textrm{slice}({\theta}_{i})},{\textrm{slice}({\theta}_{j})}]} . The column {\boldsymbol{\Theta}}_{i,1} corresponds to the first column of the \theta_{i}\mbf{1} matrix present in the \boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2} section of the optimization variable ( 64 ). Similarly, the column {\boldsymbol{\Theta}}_{j,1} corresponds to the first column of the \theta_{j}\mbf{1} matrix present in the \boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2} section of the optimization variable ( 64 ). Furthermore, redundant constraints are created by combining the discrete and continuous variable constraints, the details for which are present in the paper itself. -D Redundant Constraints Corresponding to Problem Structure -D 1 Moment Constraints Moment constraints are added that reflect the particular matrix structure. For the moment constraint presentation only, {\theta}_{i,1} and {\theta}_{i,2} are used to refer to the first and second columns of the matrix subblock of \mbf{X} that corresponds to {\theta}_{i} . • Moment constraint form 1 – {\mbf{A}[{\textrm{slice}_{\mbf{X}}(\mbf{H}_{i})},{\textrm{slice}_{\mbf{X}}({\theta}_{i}{\boldsymbol{\xi}}_{k})}]}=1 , – {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i})},{\textrm{slice}_{\mbf{X}}({\boldsymbol{\xi}}_{k})}]}=-1 . • Moment constraint form 2 – {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,1}{\boldsymbol{\xi}}_{k})},{\textrm{slice}_{\mbf{X}}({\theta}_{i,2})}]}=1 , – {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,2}{\boldsymbol{\xi}}_{k})},{\textrm{slice}_{\mbf{X}}({\theta}_{i,1})}]}=-1 . • Moment constraint form 3 – {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,1}{\boldsymbol{\xi}}_{{k},1})},{\textrm{slice}_{\mbf{X}}({\theta}_{i,2}{\boldsymbol{\xi}}_{{k},2})}]}=1 , – {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,1}{\boldsymbol{\xi}}_{{k},2})},{\textrm{slice}_{\mbf{X}}({\theta}_{i,2}{\boldsymbol{\xi}}_{{k},1})}]}=-1 . -D 2 Column Structure of Optimization Variable Due to the matrix structure of the optimization variable, \displaystyle\mbf{X} \displaystyle=\begin{bmatrix}\mbf{H}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\mbf{1}^{2\times 2}&\boldsymbol{\theta}^{\mathsf{T}}\otimes\boldsymbol{\Xi}&\boldsymbol{\Xi}\end{bmatrix}, (64) where diagonal matrices are used to encode the discrete variables, extra redundant constraints need to be imposed on the relaxation. Let {\theta}_{i,d_{1}} denote the matrix column name corresponding to the d_{1} ’th column of the diagonal matrix corresponding to {\theta}_{i} . Similarly, let {\theta}_{j,d_{2}} denote the matrix column name corresponding to the d_{2} ’th column of the diagonal matrix corresponding to the discrete variable {\theta}_{j} , with d_{1}\neq d_{2} . Then, a redundant constraint is given by {\mbf{Z}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,d_{1}})},{\textrm{slice}_{\mbf{X}}({\theta}_{j,d_{2}})}]}=0 . This redundant constraint may be encoded using a quadratic constraint as {\mbf{A}[{\textrm{slice}_{\mbf{X}}({\theta}_{i,d_{1}})},{\textrm{slice}_{\mbf{X}}({\theta}_{j,d_{2}})}]}=1 . Furthermore, additional constraints are used to impose the diagonal structure of the blocks of the continuous variable ( 8 ) that are diagonal, in the same fashion as in [ 13 ] .