Millimeter-Wave Radio SLAM: End-to-End Processing Methods and Experimental Validation

In this article, we address the timely topic of cellular bistatic simultaneous localization and mapping (SLAM) with specific focus on end-to-end processing solutions, from raw I/Q samples, via channel parameter estimation to user equipment (UE) and landmark location information in millimeter-wave (mmWave) networks, with minimal prior knowledge. Firstly, we propose a new multipath channel parameter estimation solution that operates directly with beam reference signal received power (BRSRP) measurements, alleviating the need to know the true antenna beampatterns or the underlying beamforming weights. Additionally, the method has built-in robustness against unavoidable antenna sidelobes. Secondly, we propose new snapshot SLAM algorithms that have increased robustness and identifiability compared to prior art, in practical built environments with complex clutter and multi-bounce propagation scenarios, and do not rely on any a priori motion model. The performance of the proposed methods is assessed at the 60GHz mmWave band, via both realistic ray-tracing evaluations as well as true experimental measurements, in an indoor environment. A wide set of offered results demonstrate the improved performance, compared to the relevant prior art, in terms of the channel parameter estimation as well as the end-to-end SLAM performance. Finally, the article provides the measured 60GHz data openly available for the research community, facilitating results reproducibility as well as further algorithm development.


I. INTRODUCTION
W hile the primary purpose of mobile cellular networks is to provide efficient connectivity services, the ability to extract location information and situational awareness of the surrounding environment is also receiving increasing interest [1]- [4].The related notion of integrated sensing and communications (ISAC) refers to extending the situational awareness from ordinary user equipment (UE) positioning to the ability to sense also various passive objects in the environment, through cellular radio-based measurements and corresponding signal processing [1], [5].Such knowledge of the UE locations and surrounding environment can be harnessed in numerous ways, for example in different extended reality (XR) use cases [6], vehicular applications [7], or industrial systems [8].In general, the prospects for highaccuracy situational awareness are known to improve [9], [10]  when the networks are expanding towards the millimeter-wave (mmWave) frequency bands.The current fifth generation New Radio (5G NR) specifications support already operating bands up to 71 GHz [11], while further extensions towards the sub-THz regime are expected in the 6G era [12].
Bistatic cellular simultaneous localization and mapping (SLAM) is one of the prominent ISAC applications where the coordinates of both the UE as well as those of the environment scattering points -commonly referred to as the landmarks -are all jointly estimated, based on either uplink (UL) or downlink (DL) reference signals and known base-station (BS) locations [13]- [15].This is illustrated conceptually in Fig. 1.Complete end-to-end solutions for SLAM comprise estimators for spatiotemporal channel parameters, often in the form of time of arrival (ToA), angle-of-arrival (AoA), and/or angle-of-departure (AoD) of the involved propagation paths, combined with the actual SLAM filters or snapshot estimators that process the channel parameter estimates into the corresponding estimates of the UE and landmark locations [16].This is also the main scope of this article, in contrast to many other studies that focus either exclusively on channel parameter estimation in the context of wireless communication or on cellular SLAM, without considering the challenges of obtaining the channel parameters.We have a specific focus on implementationfeasible yet high-performing end-to-end processing solutions that can operate with minimal knowledge of the involved antenna system beampatterns and facilitate mmWave SLAM in practical complex built environments, particularly indoors, while operating on downlink positioning reference signal (PRS) standardized already for the existing 5G NR networks.Additionally, we emphasize research reproducibility and provide measured mmWave I/Q and channel data openly for the research community, while use the same measured data to evaluate and benchmark the proposed methods against the relevant prior art.

A. Prior Art
The available literature on channel parameter estimation is generally-speaking wide, however, a vast majority of works is carried out under the assumptions of known antenna steering vectors, beamforming weights, and thereon beampatterns.Such methods are described, e.g., in [17]- [25].To this end, [17] and [18] present Bayesian channel estimation algorithms based on different variants of Kalman filters, while [19], [21], [22] harness the mmWave channel sparsity through compressed sensing (CS) methods.The work in [20], in turn, proposes an AoA estimation method using virtual subarrays which, however, requires a special beamformer or antenna pattern design, similar to [23].Furthermore, [24], [25] propose joint AoD and AoA estimation methods with specific frequency-dependent codebooks in true-time-delay array context.Opposed to these previous works, an angle estimation algorithm building on the beam reference signal received power (BRSRP) measurements is described in the recent work in [15].The method builds on thresholding and successive cancellation principle, operating on AoA-AoD power map, however, is lacking, e.g., explicit treatment of antenna sidelobes whose impact can be substantial in mmWave systems.This is particularly so in SLAM context where the multipaths and their dynamic range are one of the key factors.For clarity, we also note that works like [26] exist that deploy machine-learning to the channel parameters estimation -however, such works are still commonly at their early phases.
In the context of the available SLAM methods, we first note that both snapshot approaches [16], [27]- [29] as well as sequential filtering based solutions [13]- [15], [30], [31] exist.Both research directions are relevant and the preferred choice depends on the overall system and application scenario.Snapshot SLAM is fundamentally important as it serves as a baseline for what can be done with radio signals alone, without any movement models, while a snapshot method can also be used as input to filtering [27].On the other hand, filtering based SLAM methods process the observations sequentially over time and are expected to remove false detections and improve the accuracy [14].Both SLAM methods also have their limitations.A major drawback of snapshot SLAM is that it is not always applicable, since the measurements may not be sufficient to solve the SLAM problem [16].For example, the UE cannot be localized with the line-of-sight (LoS) alone if the clocks of the UE and BS are not synchronized [27] and one must resort to filtering based techniques to solve the problem over time.On the other hand, filtering methods always require a snapshot algorithm for initialization when prior information is not available.Another disadvantage of filtering methods is that they require solving a complex data association problem which increases computational overhead of the algorithms [14], [31].It is to be noted that low complexity alternatives exist [13], [30], however, at the expense of reduced accuracy.Finally, different from mono-static sensing where transmitter (TX) and receiver (RX) entities are directly mutually synchronized [5], [32], [33], an important practical aspect and challenge in bi-static SLAM is the ability to estimate and track the time-varying clockbias between the UE and the network elements.Examples of existing works where clock parameters are incorporated as part of the overall state estimation or tracking problem include, e.g., [14], [27].

B. Novelty and Contributions
Compared to the available methods and literature, this article provides the following contributions.Firstly, as opposed to the vast majority of existing literature in [17]- [25], we focus on enabling accurate multipath AoA/AoD estimation with BRSRP measurements only, without knowledge of the complex antenna patterns or the underlying steering vectors and beamforming weights.This is motivated by the fact that in real networks, only the beam indices and corresponding nominal beam directions are commonly available [34].Furthermore, various practical aspects such as errors in the antenna element spacings as well as mutual coupling create anyway varying levels of uncertainties in the true antenna patterns -in particular in mmWave networks [35] where analog/RF beamforming dominates and the design and implementation of antenna elements and beamforming units are tedious.Additionally, the proposed channel parameter estimator that builds on the singular value decomposition (SVD) of the AoA-AoD BRSRP map is shown to have builtin robustness against the antenna sidelobes which is a clear additional benefit compared to the prior art in [15].The proposed method is also compatible with 5G NR PRS signal structure and beam-sweaping procedures defined in [36].
Secondly, we focus on advancing the SLAM in three directions: first, through increased robustness and tolerance of snapshot SLAM, compared to the prior art in [16], [28], [29], against practical measurement imperfections or outliers; second, by improving the SLAM problem identifiability; and third, to perform SLAM without knowledge of the user motion model.These aspects have high importance in real mmWave deployment environments, where the amount of the available LoS/non-line-of-sight (NLoS) measurements can easily vary from measurement location to another [37].Additionally, despite the advances in channel parameter estimation, measurement outliers are commonly occurring, e.g., due to the clutter, multi-bounce phenomenon and antenna sidelobes [5].Specifically, we improve the snapshot SLAM identifiability via including an appropriate regularization term into the objective function that embeddes the prior information to the processing system.Additionally, a robust cost function is introduced to handle outliers originating, e.g., from false detections or clutter.
Finally, the openly available mmWave SLAM measurement data is scarce -or almost non-existing -hence, we bridge this important gap and provide 60 GHz indoor measurement data building on 5G NR standard-compliant beam-based PRS transmissions.We also utilize the measurement data to assess the achievable end-to-end performance of the methods proposed in this article, while benchmarking against the prior-art.
Thus, to summarize, the novelty and contributions of the article can be shortly stated as follows: • End-to-end robust snapshot SLAM approach: We develop and demonstrate an approach for end-to-end snapshot SLAM, under minimal knowledge of the array parameters and the user motion model.The end-to-end approach comprises a novel channel parameter estimation method and a novel snapshot SLAM method.The rest of this article is organized as follows: Section II describes the basic assumptions, the problem geometry and the corresponding fundamental system model.The proposed channel parameter estimation methods are described in Section III, while the proposed snapshot SLAM method is provided in Section IV.The considered indoor evaluation environment, raytracing assumptions and the actual 60 GHz measurement setup and data are all described in Section V.The complete end-toend performance results and corresponding comparisons against benchmark methods are reported and analyzed in Section VI.Finally, the conclusions are drawn in VII, while selected modeling details are provided in the Appendix.
Notations: Vectors are denoted by bold lowercase letters (i.e., a), bold uppercase letters are used for matrices (i.e., A) and scalars are denoted by normal font (i.e., a).The operators and ∥ • ∥ denote the transpose, conjugate, Hermitian transpose, pseudoinverse, expectation, absolute value and Euclidian norm, respectively.Finally, i denotes the imaginary unit for which i 2 = −1.

A. Basic Assumptions and System Geometry
In this work, we consider orthogonal frequency-division multiplexing (OFDM) based mmWave cellular systems where BSs are regularly broadcasting beamformed downlink reference signals, that allow UEs to estimate the multipath channel parameters for localization, sensing and mapping purposes.Concrete example in 5G NR context is the PRS [36], however, also e.g. the synchronization signal (SS) burst can in practice be utilized, though offering lower bandwidth compared to PRS.Additionally, since time-and angle-based measurements allow for a paradigm shift from classical multi-BS localization and SLAM approaches [7] towards single-BS solutions [38], we also focus on the single-BS scenario.However, the proposed channel Finally, as our main emphasis is on indoor mmWave systems, we eventually focus mostly on 2D (azimuth) estimation and SLAM algorithms.The corresponding 2D system geometry is illustrated in Fig. 2.However, the basic system and received signal models are provided in 3D for generality.

B. Received Signal Model
We assume that coarse timing information is established between TX and RX entities, through, e.g., correlation with known PRS sequences as discussed further in Section III.Now, under multipath radio propagation environment with N propagation paths, the received signal at k th subcarrier and m th OFDM symbol, using the i th TX beam and j th RX beam, can be represented as where w TX,i ∈ C NTX and w RX,j ∈ C NRX are the TX and RX beamformers, x k,m ∈ C with |x k,m | = 1 ∀k, m is the transmitted PRS sample, and n i,j k,m ∈ C N RX denotes the antenna element wise additive white Gaussian noise (AWGN) at the RX.Furthermore, H k,m ∈ C NRX×NTX is the effective spatial multipath channel matrix defined as [37, eq. ( 4)] where ∆f is the subcarrier spacing (SCS), T sym denotes the OFDM symbol duration, and ξ n , τ f,n and f D,n are the complex path coefficient, the propagation delay with respect to the beginning of the received OFDM symbol, and the Doppler frequency for the n th propagation path, respectively.The true physical propagation delay for path n is denoted by τ n = τ c + τ f,n , with d n = cτ n denoting the corresponding physical distance, where τ c refers to the delay between the received OFDM symbol and transmit time.Considering now a receiver with an unknown clock bias b UE , the corresponding biased propagation delay τ b n can be expressed as Estimation of τ c , subject to unknown bias b UE , can be considered part of regular OFDM symbol synchronization, for example, through time-based correlation, as discussed further in Section III-C.Additionally, a TX (ψ TX,n ) ∈ C NTX and a RX (ψ RX,n ) ∈ C NRX denote the TX and RX steering vectors, respectively.The exact way how the PRS sequences are mapped to the physical resources (OFDM symbols and the underlying subcarriers), in 5G NR context, is described in [36].Now, by combining (1) and ( 2), the received signal model can be re-expressed as where ñi,j k,m = w H RX,j n i,j k,m denotes beamformed noise, while Importantly, the expression in (4) applies to arbitrary antenna systems with G TX,i (ψ TX,n ) ∈ C and G RX,j (ψ RX,n ) ∈ C denoting the corresponding angular responses for the i th and j th beams at the TX and RX sides.
The fundamental technical problems considered in the article are (i) to estimate the involved path angles and delays, with received PRS samples, and (ii) to estimate the UE location and the locations of the landmarks, with angle and delay estimates as the inputs.These are addressed in Sections III and IV, respectively, where Section IV also details the relations between the locations and the involved path angles and delays.

III. CHANNEL PARAMETER ESTIMATION METHODS
In this section, we provide a detailed description of the proposed AoA/AoD estimation method, along with an explanation of the ToA estimation approach we have considered.
To ensure clarity and simplify the presentation, we focus on describing these methods in the 2D/azimuth domain.This approach is particularly relevant in indoor scenarios, where the floor and ceiling impose constraints on the extent of the vertical direction.We further assume that directional mmWave beams are deployed, such that the TX and RX beam indices i and j have physical correspondence to the TX and RX beamforming angles, respectively.Such directional beams are considered in large majority of the mmWave systems research, particularly when analog/RF beamforming is assumed.

A. BRSRP Measurements
Taking reference signal received power (RSRP) measurements is generally a standard procedure in 5G NR [39], e.g., for paging and beam alignment purposes.To this end, for each TX-RX beam pair (i, j), let us define the corresponding BRSRP as a beam-based RSRP measurement of the considered reference signal (RS).Specifically, this is defined as where M RS is a set of all RS symbols and subcarriers mapped to the OFDM resource grid with cardinality for any n 1 ̸ = n 2 .Then, the BRSRP measurement in (5) can be approximated as where σ 2 noise is the variance of ñi,j k,m .Proof.Please see the Appendix.

B. Proposed SVD-based AoD and AoA Extraction
Considering the PRS beam-sweeping procedure with L TX TX beams and L RX RX beams, comprising the corresponding TX and RX beamforming angles {Φ i } LTX i=1 and {Θ j } LRX j=1 , respectively, a total of L TX × L RX directional BRSRP measurements are available at the RX.The corresponding BRSRP matrix B ∈ R LTX×LRX is defined as The matrix B thus represents the spatial channel in the angular domain, in the form of an AoD-AoA power map.A concrete example is visualized in Fig. 3, building on measurement arrangements described in Section V.
1) Technical Premise and Intuition: Taking into account the fairly narrow beamwidths and the mmWave channel sparsity, the AoDs {ϕ n } N n=1 and AoAs {θ n } N n=1 for N propagation paths can be extracted by processing the power map B. For this purpose, based on ( 5) and the approximation in (7), the angular power map B can be first represented as where g TX (ψ TX,n ) and g RX (ψ RX,n ) denote the TX and RX beam gain vectors, with g TX (ψ TX,n , respectively.Furthermore, N is a noise matrix whose element at the i th row and j th column is defined as Inspired by the structure of B in (9), let us consider its singular value decomposition (SVD), expressed as B = UΛV ⊤ , where U ∈ R LTX×LTX , V ∈ R LRX×LRX are unitary matrices, and Λ ∈ R LTX×LRX is a rectangular diagonal matrix.In the main diagonal of Λ, singular values of B, [σ 1 , σ 2 , ..., , σ R ] > 0, are sorted in the descending order, where R = rank(B) ≤ min(L TX , L RX ).The SVD can also be expressed as where u r and v r are the r th columns of U and V, respectively.Importantly, based on (9), u r and v r belong to the corresponding spans of {g TX (ψ TX,n )} N n=1 and {g RX (ψ RX,n )} N n=1 .Thus, the r th singular vectors can be expressed as where q u r,n and q v r,n are the linear combination weights of the spanning set vectors g TX (ψ TX,n ) and g RX (ψ RX,n ), respectively.In addition, B (1) r in ( 10) is a rank-1 matrix associated with the r th singular value and the related basis vectors u r and v r .Furthermore, in the case of a sparse beamformed channel where all paths are well separated, combined with directional beamforming at the TX and RX, the following holds [37], [40] where δ n−n ′ is a unit impulse function.In such case, each SVD term in ( 10) corresponds essentially to one propagation path, and thus B (1) r is a rank-1 approximation of the power map stemming from the corresponding path.
Interestingly, based on ( 9)-( 11) and the corresponding fact that the singular vectors of the BRSRP matrix are in the span of the beam gain vectors, the SVD-based low-rank modeling approach can also inherently manage different unknown beam patterns, and is thus resilient to sidelobes and other beam pattern fluctuations.Specifically, the antenna sidelobes illustrated also in Fig. 3, can be decoupled from the peaks in B by using different low-rank approximations with increasing rank and analyzing the resulting power increment formed at each step.To this end, a pair of subsequent low-rank approximations of B, denoted as B K−1 and B K , with K < R, can be expressed as where the rank-1 matrix B (1) K represents a recovered BRSRP map increment between the two levels of approximation accuracy.
2) Overall Approach and Refinements: Due to the relation of singular vectors to antenna response in (11) and the relation of the singular vectors and singular matrix in (10), the AoD and AoA estimates can be extracted recursively by processing rank-1 matrices B (1) r for separate singular values, while assuming only known TX beam angles {Φ i } LTX i=1 and RX beam angles {Θ j } LRX j=1 .A suitable rank value for the low-rank approximation in (13) can be determined in various manner so that noise-dominated singular matrices are omitted from the approximation.In this article, we choose the approximation rank by ensuring that the low-rank approximation recovers a desired portion of the original total measured power, as described in Algorithm 1.Additionally, suitable rank selection can also help emphasizing LoS and single-bounce paths, over the corresponding multibounce paths [33] that are typically of low power.
The complete proposed procedure for obtaining AoD estimates { φn } N n=1 and AoA estimates { θn } N n=1 for N estimated paths, is stated and described in Algorithm 1.Besides the fundamental processing of rank-1 matrices for extracting angle information, three additional refinements are shown and applied, namely, power thresholding, clustering, and polynomial fitting.With power thresholding, we omit peaks which are close to noise level, and thus mitigate noise-related estimation errors.One feasible approach to assign a value for the related power threshold β th is to set it at or marginally higher than the prevailing noise floor.Concrete examples are provided along the numerical results, in Section VI.The clustering step, in turn, considers the fact that powerful peaks create large residuals and require more than one rank-1 singular matrix to be properly represented.Assuming spatially sparse channel, we expect such residual peaks to occur only in vicinity of powerful peaks that are spatially separated from each other, thus creating clusters in AoD-AoA domain.After the clustering step, where any existing clustering method [41] can be adopted, N clusters are obtained, whose cluster means represent the coarsely estimated AoD and Find the indices of the element with maximum value: ( îr, ĵr) = arg max i,j B (1) element at the i th row and j th column.

7:
Save the angles and power as a 3D set element: Compute power ratio: Set r ← r + 1 10: end while, K = r 11: Power thresholding -choose only the components with power exceeding a given threshold: Mest ← {Mest | elements with β îr , ĵr ≥ βth} 12: Clustering -Perform clustering of elements in Mest to obtain N ≤ K clusters M n est , n = 1, . . ., N .For each cluster, angle estimates are obtained by computing a power-weighted cluster mean as φn = ( s β îs, ĵs Φ îs )/ s β îs, ĵs and θn = ( s β îs, ĵs Θ îs )/ s β îs, ĵs , where the index s is used to select elements from the specific cluster.AoA angles.Lastly, to enhance the angle estimation precision, each peak associated with a coarsely estimated AoD and AoA is fitted with a parabolic surface using a weighted least squares method.Fitting is performed within a local window of size approximately representing the beamwidth.The final AoD estimates { φn } N n=1 and AoA estimates { θn } N n=1 can be then found in closed form as a vertex of the parabolic surface for each peak.
Finally, we note that if paths share the same AoD, but have different AoAs, or vice versa, multiple paths can be represented by one rank-1 singular matrix.Thus, one could argue that the AoD-AoA extraction algorithm would benefit from multipeak selection, however, with such measurement geometry, the weaker paths can be assumed as undesired multi-bounce paths in the considered SLAM setting.
3) Complexity Assessment: The fundamental complexity order of the proposed SVD-based angle estimation method is , excluding further refinements of thresholding, clustering and polynomial fitting.The corresponding complexities of the method in [15] as well as that of a classical cell-averaging constant false alarm rate (CFAR) detector [42], being used as benchmark methods along the numerical results, read respectively, where N peak is a maximum number of support squares [15] and N TB is a size of the CFAR training window.For good performance, N 2 TB is comparable to or larger than L (i.e., L TX or L RX ), and thus all three methods have similar complexity order of O(L 3 ).

C. ToA Estimation
In general, the SS burst [36], [43] allows to establish the basic frame and OFDM symbol synchronization, however, PRS allows for larger bandwidth and thus facilitates more accurate symbol time estimation and particularly the fine ToA estimation.We thus next shortly address the ToA estimation, building on PRS and the corresponding PRS IDs [36], [43] -both known at the UE.The coarse ToA estimation is carried out using time-domain I/Q signals, already before the angle estimation phase, while the fine ToA estimation or refinement is carried out in frequency-domain only for the identified path angles.These together provide the estimates for the pathwise ToAs.
1) Coarse ToA Estimation in Time Domain: First, samplewise time delay estimation and beam ID search is performed by maximising the cross-correlation between the received waveform and potential reference waveforms in time domain.For sampling rate of F s , we denote the q th time domain signal sample for the received signal and the PRS with PRS ID ν as Y i,j (q) and X i,j ν (q), respectively.Now, the estimated PRS ID νi,j and sample-wise delay ∆q i,j can be obtained as Furthermore, the coarse delay estimate reads then which defines the delay between the received OFDM symbol and the transmit time with respect to the receiver clock -and is thus subject to bias.
2) Fine ToA Estimation in Frequency Domain: The fine time delay estimation is performed per estimated path, using the frequency domain samples y i,j k,m with (k, m) ∈ M RS , corresponding to the beam pair (i, j) whose beam angles are closest to the estimated AoD and AoA.Such fine ToA estimate for path n, determined with respect to the beginning of the OFDM symbol, can be obtained as [44,Ch. 3.2] τf,n = arg max where în = arg min i (|Φ i − φn |) and ĵn = arg min j (|Θ j − θn |) are the TX and RX beam indices associated with the n th estimated path, respectively.In practice, ( 16) can be solved using an optimization algorithm, interpolated IFFT, or performing a brute force search over suitable propagation delays.Finally, the complete (biased) ToA estimates are obtained as for path indices n = 1, . . ., N , which correspond to (biased) path distance estimates of cτ b n .As noted in [43], [45], the coarse ToA estimate can also be determined wrt. the frame start time.In such case, the possible excess time between the beginning of the frame and the actual RS transmission time can be taken into account in the coarse ToA estimate, while the path-wise fine ToA estimates remains intact.With unsynchronized TX and RX clocks, the underlying bias of ToA estimate in (17) can be estimated and dealt with in the considered SLAM scenario, as shown in Section IV.

IV. PROPOSED SNAPSHOT SLAM METHOD
Next, we describe the proposed snapshot SLAM method, building on the previously described AoA, AoD and ToA estimates.The fundamental problem geometry is illustrated in Fig. 2, while in the following, for clarity, we explicitly refer to BS and UE as the TX and RX entities, respectively.

A. Problem Formulation
It is assumed that the BS position and orientation, denoted with [p ⊤ BS , α BS ] ⊤ , are known while the unknown UE state is represented using the 2D position, heading and clock bias (cast in meters, Furthermore, the nth single bounce propagation path is represented using the 2D interaction point or landmark m n = [x n , y n ] ⊤ , n = 2, 3, ..., N , where N denotes the number of the available AoA, AoD and ToA estimates in a given measurement location.For notational convenience, the LoS path index -if existingis n = 1.
In addition, let ⊤ denote the joint state of the UE and the nth landmark, m = [m ⊤ 2 , . . ., m ⊤ N ] ⊤ denotes the map which is the joint state of the N − 1 landmarks, and the unknown joint state of the UE and map is x = [s ⊤ , m ⊤ ] ⊤ .Now, the estimation problem can be defined as where p(x) = N (x | µ, Σ) is the prior for x obtained for example using external sensors or a previous estimate and p(z n | x n ) is the likelihood of the nth measurement.It is to be noted that typically snapshot SLAM implies that no prior information exists since the primary interest is analyzing what can be done with radio signals alone [16], [28] whereas in this article, we evaluate the system performance both with and without the UE prior.No prior knowledge on the map is required, but for a comprehensive treatment of the problem, the solution is presented considering both the UE and map priors.The measurements are defined as z n = [cτ b n , φn , θn ] ⊤ , in which the delay estimates are converted to meters for convenience.An estimate for x can be obtained by maximizing (18), mathematically given by Since the UE does not known whether the LoS exists or not, (19) is solved under NLoS only and under LoS+NLoS, separately.Propagation paths with distance to within one meter of the shortest path and power within 3 dB to the path with maximum power are considered as candidate LoS signals and N LoS denotes the number of LoS candidates.Furthermore, for each candidate, ( 19) is solved with and without a prior (see Section IV-D).This will give 2N LoS + 1 solutions to (19) with different costs and the solution with lowest cost measured in terms of (23) can be selected as the estimate.

B. Measurement Models
Assuming that the measurement noise is zero-mean Gaussian, which is a common assumption is bistatic SLAM [14], [31], the likelihood function is Gaussian with mean h n (x) and covariance R n .Building on the geometry in Fig. 2, the mean is given by For the LoS path (n = 1), the parameters are defined as: Respectively for the nth NLoS path, the parameters are defined as:

C. Regularized Robust Least Squares Estimator
Maximizing the posterior as given in (19), is equivalent to in which L(x) is the objective function we wish to minimize.
In this article, we utilize the following objective function where the first term is a regularization term that encodes the prior information and the second term encodes the evidence provided by the measurements.In the second term, f (•) is a robust cost function which we will define later and defines a quadratic error.
The Gauss-Newton algorithm can be utilized to iteratively solve (23) and the method is based on approximating h n (x) using a first order Taylor series expansion [46], given by where x(j) is the estimate of x at the jth iteration and is the Jacobian.The parameter update of the Gauss-Newton algorithm can be derived by plugging (25) into (23), setting the gradient of L(x) to zero and solving for x, expressed as In order to minimize L(x), we then set the next estimate x(j+1) to be equal to the minimum, which gives In general, there are many possible robust cost functions that reduce the weight of components with large errors so that they have a smaller influence to the solution due to a reduced gradient [47].In this article, we utilize the Cauchy cost function, f (q n (x)) = log(1+q n (x)), such that the second term of the objective function in (23) becomes and the gradient is given by ( 26) in which Thus, the new covariance matrix Rn is just an inflated version of the original covariance matrix R n , given by ) and it gets bigger as the quadratic error increases.As a consequence, cost terms that are very large and potential outliers are given less trust to diminish their impact.
In practice, taking a full step according to (28) might be too large with respect to the neighborhood for which the Taylor series approximation in (25) is valid.To avoid this, a scaled Gauss-Newton step can be done instead, proportional to the direction given by the local approximation.In this article, we use backtracking line search [48] to compute the step length.The resulting algorithm is summarized in Algorithm 2 and initialization of x(0) is presented in the following section.

D. Initialization
A major disadvantage of the Gauss-Newton approach is that the linearization of the measurement model is local.In highly nonlinear problems, this means that the solution can converge to a local minima and therefore, initialization of the algorithm is very important.To this end, let us define the prior mean and inverse of the covariance matrix as where µ s , Σ ss , µ mn and Σ mnmn denote the mean and covariance of the UE and nth map entry, respectively.Algorithm Calculate the update direction as given in (28): Compute step length γ using Algorithm 9.2 in [48] 5: Update parameter estimate: x(j+1) = x(j) + γ∆x (j+1) 6: Set j ← j + 1 7: until Converged 8: x = x(j) 2 is initialized using the prior mean, that is, x(0) = µ.Snapshot SLAM typically implies that no prior information exists (Σ −1 = 0) [16], [28], whereas in this article, we evaluate the system performance both with and without the prior.If prior information is not available, the LoS must exist so that we can first compute a prior for the UE state as described in Section IV-D1 which can then be used to initialize the landmarks as presented in IV-D2.If a prior for the UE exists, the landmarks can be directly initialized using the prior as described in Section IV-D2.If a prior for the UE and map exists, Algorithm 2 can be directly initialized using the prior but in this article it is always assumed that no prior knowledge of the map is available, that is, Σ −1 mnmn = 0, n = 2, . . ., N .), in which σ 2 BUE is variance of the bias which is set higher than variance of the delay estimates.Then, the mean and covariance of the UE prior are given by µ s = ȟ1 (ž) and Σ ss = J1 (ž) Ř1 J1 (ž) ⊤ where J1 (ž) denotes the Jacobian of ȟ1 (ž) evaluated with respect to ž and the mean is defined as ) and ŷ1 = (cτ b 1 − BUE ) sin(α BS + φ1 ).After computing the moments using (34), the landmark locations can be computed as presented in Section IV-D2.Then the trial with lowest cost according to (23), which also involves the landmarks, is selected as the UE prior.Due to computational reasons, we solve the described problem using constrained nonlinear optimization [48] for which the problem can be defined as with cost L(x) as given in (23).
2) Landmark Initialization: The first order Taylor series based linear approximation of the measurement likelihood in (20) around the UE prior reads (36) where h n (x) is evaluated at x = [µ ⊤ s , m ⊤ n ] ⊤ , and J n (µ s ) = ∇ s h n (x)| s=µ s is the Jacobian of h n (x) with respect to state s computed at µ s .Following the derivations in [49, Eqs.(5.6) -(5.13)], the likelihood can be approximated as The nth map element is initialized by solving a nonlinear optimization problem, defined as where The optimization problem is initialized in a similar way as described in [14] and solved using the Gauss-Newton algorithm, with the exact details here omitted, as the algorithm is very similar to the one presented in Algorithm 2.

V. INDOOR ENVIRONMENT, TOOLS AND DATA
We consider a modern indoor environment at the Hervanta Campus of Tampere University, Finland, located in the socalled Campus Arena building.The environment is illustrated in terms of a floor plan and an actual photograph in Fig. 4, consisting of a fairly large partially open space containing a number of different landmarks such as columns, short walls, booths, and so forth.The BS is located in a narrow annex 3m wide while the UE moves in the area with a trajectory shown in Fig. 4. Furthermore, 4×16 planar antenna arrays are considered at both the transmitting and receiving ends, with azimuth 3 dB beamwidth of around 10 • .Such antenna system assumption is certainly implementation feasible at BS end while for UEs the current mmWave implementations consider somewhat reduced antenna counts and thus broader beams.The considered PRS bandwidth is 400 MHz following [36].

A. Ray Tracing Tool and Assumptions
To carry out evaluations with well-defined and known groundtruth, ray tracing was performed with Wireless InSite ® [50].The true physical environment is reproduced using the indoor floor plan editor fitted to the building blueprint, while noting also accurately different movable entities and objects such as an indoor phone booth.The ITU 60 GHz material models [51], namely, layered drywall, wood, glass, floor and ceiling board were used.The BS position and UE trajectory consisting of 45 points 0.5m apart are accurately matched to those used in the actual measurements.Omnidirectional antennas were used for the ray casting, and the reception with path elevation was limited to 80 • − 100 • , reflecting essentially the azimuth plane.Furthermore, ray casting was limited to 25 rays per UE position, and the number of material interactions to four reflections and one diffraction.The ray-tracing model considers specular reflection (SR) and diffraction (D), such that an unambiguous ground truth for the performance assessment of the proposed and benchmark angle estimation methods can be obtained.
The ray-tracing model is further combined with I/Q waveform processing in Matlab, such that realistic received I/Q samples can be generated, per UE location.Here, the TX and RX beampatterns are modelled through classical matched filter type responses where the beamforming weights are matched to the corresponding steering vector of the beamforming angle.Additionally, we allow for similar mechanical rotation patterns as in the actual measurements in order to cover 180 • field-ofview (FoV) in TX and 360 • FoV in RX (for further details, see the following subsection).These together facilitate a maximum of L TX = 126 and L RX = 252 beams at TX and RX, respectively.Moreover, we model accurately the signal-to-noise ratio (SNR) characteristics of the environment such that the prevailing SNR at each UE/RX point is adjusted according to the actual SNR observed in the corresponding physical RF measurements.

B. Measurement Setup and Data
In the actual mmWave measurements, Sivers Semiconductors Evaluation Kits EVK06002 [52] were used as the TX and RX entities.The overall operational band of EVK06002 is 57 − 71 GHz, and it consists of strip antenna elements arranged in 4 × 16 arrays for each polarization, integrated with the electrical phase and amplitude control of the individual elements.The TX EVK was connected to a PC-controlled Arbitrary Waveform Generator M8195A, whereas the RX EVK was connected to the Keysight DSOS804A oscilloscope, which serve as data conversion interfaces towards digital signal processing.Similar to the ray-tracing case, the transmitted PRS-carrying I/Q waveforms were created using the Matlab 5G Toolbox including also embedding of different PRS IDs.
The beamforming capabilities of the Sivers EVK allows for synthesizing electrically controlled beams, building on embedded proprietary codebook.The exact beampatterns are unknown but based on elementary antenna measurements resemble those of the ray tracing model complemented with additional tapering.Since the electrical beamforming FoV of the EVK is limited to [−45 • , 45 • ], both TX and RX EVK were complemented with FLIR Pan Tilt PTU-46 for additional mechanical rotation capabilities, in order to cover 180 • and 360 • FoVs with maximum of L TX = 126 and L RX = 252 TX and RX beams, respectively.Both receiver and transmitter were controlled by the same PC and synchronized to the same clock via a coaxial cable to provide a reference for ToA measurements.Additionally, the true UE locations were recorded accurately for ground-truth purposes.The measurement setup and a glimpse of the physical environment are shown along the Fig. 4.
The complete 60 GHz I/Q measurement data set as well as the corresponding processed channel parameter data set, together with supportive scripts are all openly shared along this article -refer to the first page footnote for further details.

VI. RESULTS
In this section, the angle estimation results as well as the complete end-to-end SLAM results are presented and analyzed.The angle estimator assessment builds on the ray tracing data, as it contains the ground-truth information also for the landmarks.The end-to-end SLAM assessment, in turn, builds on the true measurement data.

A. Angle Extraction Results with Ray Tracing Data
The performance of the proposed SVD-based method is assessed and compared to the available successive cancellationbased reference method described in [15].Additionally, the classical CFAR detector [42] is also implemented and considered as an additional benchmark solution, as such is widely used, e.g., in radar context for target detection subject to clutter.
The method in [15] is parameterised with the so-called support power increment, ϵ, and the maximum number of searched peaks, N peaks .We additionally apply power thresholding to reduce the amount of false detections, and thereon to have as fair comparison as possible.The CFAR technique, in turn, uses training cells of size N TB around the target cell to estimate the local noise level to reach the given false alarm probability P FA .The Matlab 2D CFAR detector implementation from phased array system toolbox was used, while further clustering was also applied to the results as CFAR method easily multiple AoD-AoA estimates per one peak.Finally, the proposed SVDbased method is parametrized as follows.For the power ratio parameter p, we consider the values of p = 99%, 99.9% and 99.99%.The power thresholding related parameter β th is, in turn, set at 10% above the prevailing noise floor.This value is designated with notation β * th while additional complementary results where the value of β th is varied are also provided.
1) Qualitative Comparison: The capabilities of the different methods are first visually illustrated in Fig. 5, covering both NLoS and LoS UE locations, and with the parametrizations as shown in the caption.As can be observed, the method from [15] has notable challenges in dealing with antenna sidelobes, while also several true propagation paths are missed especially in LoS case.The CFAR has also clear limitations with sidelobes while missing a large number of true landmarks in NLoS.The proposed method, in turn, is able to offer enhanced performance in both NLoS and LoS, while processing efficiently also the antenna sidelobes.
2) Quantitative Comparison: As illustrated in Fig. 5, the amounts of the detected paths differ from method to another, while the ray-tracing model limits the amount of ground-truth paths to the earlier noted number of 25.Such data of different cardinality can be reliably compared and quantitatively assessed using the generalized optimal subpattern assignment (GOSPA) metric [53], which takes into account, in addition to root-mean-squared errors (RMSEs) of the quantities of interest, the numbers of false detection, N FD , and missed detection, N MD .We thus utilize the GOSPA metric for quantitative assessment, and express it in degrees as γ GOSPA = (γ RMSE + (N FD + N MD )d P c /α) (1/P) , where the parameters d c = 10 • , P = 2 and α = 2 are cutoff distance, exponent power and cardinality penalty factor, respectively.
Furthermore, a subset of all false detections are due to the sidelobes.Such sidelobe effects, unlike those of a noise, cannot be reduced by increasing the number of observations.Thus, we consider sidelobe false detection (SLFD) a systematic error that is especially detrimental to the performance of the whole endto-end system.To this end, we introduce an additional metric that quantifies the number of SLFDs, N SLFD .We specifically express the SLFD through the following conditions of (i) the ToA estimates are within a threshold of |τ n1 − τn2 | ≤ τ th , and (ii) the corresponding TX or RX beam indices differ at most by one, i.e., (| în1 − în2 | ≤ 1) ∨ (| ĵn1 − ĵn2 | ≤ 1).A tight delay threshold of τ th = 0.3 ns is used in the numerical assessment.Finally, to present the sidelobe detection metric comparable to GOSPA, the same penalty factor is used, and thus the final metric is expressed as The quantitative comparison between the three methods in terms of GOSPA and sidelobe detection metric are presented in Fig. 6, covering the whole UE/RX trajectory shown in Fig. 4, while also noting the options for the different parametrizations.In terms of the GOSPA metric, the proposed SVD-based method clearly outperforms the reference methods.Additionally, in terms of the SLFD metric, the proposed method outperforms that in [15] by a large margin.The CFAR with P FA = 0.002 is closer in performance, however, having clear challenges to detect the actual multipaths as shown in Fig. 5 already.It can also be observed that for the proposed method, the GOSPA metric decreases with the increase of p due to the increased number of recovered multipath components.At the same time, the sidelobe detection rate slightly increases with the number of detected components, however, the sidelobe detection metric stays generally low for all considered values of p compared to the reference methods.
We next provide further complementary assessment of the SVD-based method in terms of the impact of the power thresholding parameter β th .The results are shown in Fig. 7), with β * th denoting the baseline value used otherwise in the article.We can observe that the exact value of the power thresholding is impacting the performance only when working under a vary large value of the power ratio parameter p (particularly p = 99.99%).This is because in such case, the baseline SVD processing passes through a large number of components, some of which being most likely noise already.Overall, the results in Fig. 7 show the robust behavior and performance of the proposed scheme.
In summary, we conclude that the proposed SVD approach outperforms the prior-art benchmark methods.As demonstrated by the ray-tracing results, it offers robust performance in both LoS and NLoS scenarios, while is also having built-in mechanism to suppress the impacts of the unavoidable antenna sidelobes.

B. SLAM Results with Measurement Data
Next, the actual end-to-end SLAM results are provided and analyzed, utilizing the true measurement data.We use the proposed SVD method for angle estimation, and assume p = 99%.The ToA estimation is carried out as described in ( 14)- (17), while using brute force search to solve (16).
The proposed SLAM method is benchmarked with respect to two other SLAM approaches [28], [31].The first benchmark (a) BM1 [28] (b) BM2 [31] (c) Proposed SLAM approach Fig. 8. Visual illustration of the performance of the different SLAM algorithms with measurement data including UE clock bias.In the figures, location of the BS illustrated using ( ), the ground truth UE trajectory with ( ), the UE position estimates using ( ) and the estimated landmark locations with ( ).
(a) BM1 [28] (b) BM2 [31] (c) Proposed SLAM approach Fig. 9. Visual illustration of the SLAM performance in the reference case where the UE and BS are mutually synchronized.For other details, refer to the caption of Fig. 8).
is a geometry based snapshot SLAM algorithm [28] referred to as BM1 and the second is a recursive probability hypothesis density (PHD)-SLAM filter [31] referred to as BM2.In the following, the time delay and clock bias are converted to distance for convenience, and the clock bias is emulated to evolve according to a random walk model with variance Recall that no prior knowledge of the map is assumed as discussed in Section IV-D.The mean and covariance of the UE prior for the proposed method are µ s = µ s,prev and Σ ss = I 4 , in which µ s,prev denotes the UE state estimate at the previous measurement position.In the first measurement position, which is located at [0.55, −2.75] ⊤ , there is no prior and estimation is possible because the LoS signal exists.The UE initialization algorithm presented in Section IV-D1 uses d min = 1 m and d max = 20 m in the constrained nonlinear optimization problem defined in Equation (35).The benchmark PHD-SLAM filter considers three types of landmarks [14]: i) BS for the LoS path; ii) virtual anchors (VAs) for large reflecting surfaces; and iii) scattering points (SPs) for small objects.However, the VAs are converted to SPs using [14,Eq. (42)] in the following illustrations so that the map of the PHD-SLAM filter is directly comparable to the maps of the snapshot SLAM algorithms.The benchmark PHD-SLAM filter uses similar parameter values as in [31], [54], while the values were slightly tuned to maximize the performance in the considered experiment.The PHD-SLAM filter is implemented using 1000 particles, the UE state is modeled to evolve according to a random walk model and the process noise is set to Q = I 4 which is the same as covariance of the UE prior.The PHD-SLAM filter is initialized using the proposed snapshot SLAM algorithm.Since BM1 is sensitive to outliers, measurements labeled as outliers 2 are removed from the data for BM1 unless otherwise stated.
1) Qualitative Comparison: The example mapping and localization performance of the algorithms are visualized in Fig. 8. Qualitative analysis indicates that the map and UE position estimates are more accurate with BM2 and the proposed SLAM algorithm than with BM1.For BM1, the estimate is close to the ground truth and the estimated landmark locations are inline with the map in many measurement positions.In several locations however, the estimates are very 2 Under the Gaussian assumption, the quadratic error in (24) follows a χ 2 distribution with three degrees of freedom and if qn(x) > T h , the measurement is labeled an outlier.The quadratic error is evaluated using the ground truth UE state and T h ≈ 16.3 is computed by choosing tail probability 0.001 followed by evaluating the inverse cumulative distribution of the χ 2 distribution at 0.999.inaccurate and there are two primary reasons for this.First of all, the method requires four NLoS signals so that the system is identifiable and this criterion is not satisfied at every measurement position.For example, when the UE is located at p UE = [4.06,−5.70] ⊤ , there are only three propagation paths meaning that the system is underdetermined resulting in an inaccurate estimates as illustrated in Fig. 8.The second reason is that in several measurement positions, the cost function of BM1 is multimodal and the global minima is not the one closest to the ground truth.The proposed method and BM2 can operate in mixed LoS/NLoS conditions and the prior or posterior from the previous time step can be viewed as a regularization term which constrains the posterior update so that the system state is identifiable at every measurement position.Moreover, the resulting estimates for BM2 and the proposed method can be viewed as a weighted average of the evidence provided by the data and the regularization term.Thus, the estimate is expected to be close to the ground truth as long as the prior is not biased and the covariance captures the underlying uncertainties correctly.Lastly, both BM2 and the proposed approach result in sufficient SLAM performance despite the measurement data being corrupted by outliers as shown in Fig. 8. Since both the TX and RX were controlled by the same PC, with an opportunity to utilize also a dedicated synchronization cable allowing to synchronize to a common clock, we can also analyze the reference performance of the algorithms with known clock bias B UE .Example performance is illustrated in Fig. 9 while the results are further elaborated and discussed in Section VI-B3.
2) Quantitative Comparison: The algorithms are next evaluated quantitatively while since the ground truth landmark locations are unknown, the mapping accuracy is excluded which a common practice in SLAM when using experimental data.The performance metrics are tabulated in Table I in which the position, heading and bias errors are computed using the RMSE and standard deviation (STD).Even without outliers, BM1 results in unsatisfactory performance due to the reason discussed above.The other two methods outperform BM1 and have comparative performance among each other.However, the computational complexity of BM2 is two orders of magnitude higher than with the proposed method.One could decrease the computational complexity by using less particles (see BM2 * in Table I) but at the same time, the accuracy of the filter degrades notably.Thus, a major advantage of the proposed method is that it combines high accuracy together with low computational overhead.
Next, we evaluate the different algorithms with and without outliers, and in LoS and NLoS conditions.The results are summarized in Fig. 10.The performance of BM1 degrades significantly if the data contains outliers, whereas the performance of BM2 and the proposed method only degrade slightly.This indicates that both methods are capable of handling noisy measurements that are not inline with the models.The mechanisms how the two methods deal with outliers are quite different.The PHD-SLAM filter associates the outliers to clutter so that they do not affect the UE estimate, whereas the proposed method inflates the covariance according to (31) such that outliers are given less trust thus diminishing their impact.Interestingly, the proposed method outperforms BM2 in LoS conditions and vice versa in NLoS conditions.For the proposed method and when the LoS signal exists, the hypothesis that minimizes the cost function is typically the one for which the prior is computed using the LoS and the estimate is computed only from the evidence provided by the data.In this particular scenario, in which the prior uncertainty and the process noise of the dynamic model used by BM2 are high, relying solely on the measurements is beneficial.In NLoS conditions and when the proposed method cannot be initialized using the measurements, filtering is beneficial and BM2 outperforms the proposed method -however, at the expense of substantially higher complexity.
Lastly, we decompose the objective function of the proposed method and analyze the performance impact of the prior and the robust cost function.The results are illustrated in Fig. 10 from which we can conclude the following: (i) Without prior information and using a quadratic cost function (OFv0), the SLAM solution is useless in NLoS conditions and/or when the data contains outliers.The method outperforms BM1 only if the outliers are removed from the data and if the LoS signal exists.(ii) Without prior information and using a robust cost function (OFv1), the estimator yields comparative performance as the proposed method in LoS conditions, whereas in NLoS conditions the estimates are very innaccurate.(iii) With prior information and using a quadratic cost function (OFv2), the results are comparative to BM1 in both LoS and NLoS conditions and the performance improves significantly if the outliers are removed from the data.Thus to conclude, the robust cost function is a strict requirement of snapshot SLAM algorithms that are utilized in realistic scenarios in which the measurements are noisy and contain outliers.Moreover, prior information is required to improve identifiability of the system and enable estimation in mixed LoS/NLoS conditions.
3) SLAM Performance with Known Clock Bias: The estimated angles determine the problem geometry up to a scaling which is defined by the unknown clock bias.Thus, the SLAM problem is notably easier to solve if the clock bias is known.The experimental arrangements allowed for an additional dedicated synchronization cable between the TX and RX entities, such that we could also evaluate the reference system performance with perfect synchronization.Example performance with known clock bias was already illustrated in Fig. 9 while the corresponding quantitative performance metrics are tabulated in Table I.As can be observed, the performance improves significantly with all methods when comparing to the practical case with unknown clock bias.Perfect synchronization improves the performance of BM1 the most.Since B UE is known, only three propagation paths are required to solve the problem and this criterion is satisfied at every measurement position.The cost function is also unimodal and the minima are close to the corresponding ground truth in every measurement position.In addition, the computational complexity of the algorithm is much lower since the problem is solved by finding the minimum cost over α UE , whereas with unknown clock bias, the minimum cost is found by jointly optimizing α UE and B UE .With unknown clock bias, the underlying likelihood function that is used to update the particle weights of BM2 is commonly multimodal and the filter can converge to the wrong solution.On the other hand, the likelihood function is generally unimodal if B UE is known so that the filter more frequently converges to the correct solution.The computational complexity of BM2 only reduces slightly since the only notable difference is that the UE state dimension reduces from four to three.Also with the proposed algorithm the performance improves significantly and the underlying cause is the same as with BM1 and BM2, that is, the likelihood function is typically unimodal resulting in an objective function that has a minimum close to the ground truth.Furthermore, the initialization of the proposed algorithm simplifies if the TX and RX entities are synchronized since mean and covariance of the UE prior are directly given by (34) and optimization over different trial values of BUE is not required.Thanks to this straightforward initialization procedure and faster convergence of the Gauss-Newton algorithm, the computational complexity of the algorithm is reduced by an order of magnitude.
4) Impact of Power Ratio to SLAM Accuracy: Finally, we evaluate and show the SLAM performance using channel estimates that are obtained with three different values for the power ratio parameter p in the proposed SVD method.As already discussed in Section VI-A2, increasing p grows the rank of the SVD-based approximation and as an outcome, the number of estimated paths increases.However, the number of resolvable propagation paths cannot be grown indefinitely since a limited number of physical propagation paths exist and power threshold β th truncates channel estimates for which the power is close to the noise floor.Higher p values also increase the number of measurements that do not follow the model in (21),  such as multi-bounce paths.Such measurements must be treated as unwanted outliers that cannot be utilized by the considered SLAM algorithms.The number of inliers and outliers in each of the 45 measurement positions is illustrated in Fig. 11.As shown, increasing the power ratio from 99% to 99.9% increases both the number of inliers and outliers.Increasing p even more, the channel estimates of p = 99.9% and p = 99.99%only differ slightly in two measurement positions since already with p = 99.9%, the SVD-based channel estimator finds all resolvable propagation paths.The power ratio value has the biggest impact on BM1.Recall that four or more propagation paths are required to solve the SLAM using BM1 and if p = 99%, this criterion is not satisfied at measurement position number 14 and 35 as illustrated in Fig. 11.Now increasing p also increases the number of inliers such that the system is identifiable at every measurement position which improves the accuracy as tabulated in Table II.The system is always identifiable with the proposed and BM2 methods since the prior constrains the posterior update and the two methods yield similar performance with the tested power ratio values.In general, it is expected that SLAM performance improves as the number of propagation paths increases [27].At the same time, however, the number of outliers increases which can have a negative impact on SLAM performance and therefore, the accuracy with BM2 and the proposed approach only improves very little (BM2) or not at all.

C. Limitations and Future Directions
One of the limitations of the proposed BRSRP-based AoA/AoD extraction algorithm is its inability to decouple two multipath components that have the same AoA or AoD, which may lead to a missed detection.This type of misdetections, however, do not largely impact the actual SLAM performance, as in majority of cases such undetected components correspond to multi-bounce paths due to the involved geometry.Nevertheless, one future work direction is extending the SVDbased AoA/AoD extraction to support multi-peak detection and selection.One potential way to pursue such can be performing the peak search directly in the singular vector domain, aided by further assumptions about the shape of the beam responses and ToA-based sidelobe rejection.Alternatively, the overall problem may be addressed through a different technical approach -for example, as a pattern recognition problem solved either through classical methods or potentially through machine learning.On the SLAM and related concepts side, extending the methods to extract information also about the material characteristics of the environment interaction points, on top of their locations, is an interesting future research avenue.

VII. CONCLUSIONS
In this article, we addressed the timely notion of mmWave radio SLAM from end-to-end processing perspective, under minimal knowledge regarding the array parameters and user mobility.We first proposed a novel SVD-based estimation approach for acquiring the AoAs and AoDs of the involved propagation paths.The method operates on BRSRP measurements and does not need information on the complex antenna patterns or the underlying steering vectors and beamforming weights, while offering built-in robustness against antenna sidelobes.The method relies only on known physical correspondence between the beam indices and the beamforming angles, and the sparsity of the beamformed mmWave channel.Secondly, a new snapshot SLAM method was also proposed, to jointly estimate the locations of the landmarks and the UE, offering improved robustness and identifiability compared to prior-art.The performance of the proposed methods was comprehensively assessed through ray tracing and true measurement data at 60 GHz.The results show that the methods outperform the relevant prior-art, with the end-to-end performance being comparable or even better compared to sequential filtering solutions while offering substantially reduced complexity.Finally, we provide the measured 60 GHz data openly available for the research community.Our future work considers extending the proposed angle estimation methods to include multi-bounce detection and estimation with rank-1 singular matrices, thus facilitating further evolved SLAM solutions beyond the common singlebounce approaches. Gi,j which establishes the result (7) in Lemma 1.

Fig. 1 .
Fig. 1.Illustration of the bistatic cellular SLAM paradigm where UE is jointly estimating its own state as well as those of the environment landmarks.
the frequencydomain and time-domain steering vectors of the n th path, respectively, where K is the number of active RS subcarriers, and M is the number of RS OFDM symbols.Moreover, the k th element of b n is defined as b n [k] = e −i2πk∆f τ f,n , and the m th element of c n as c n [m] = e i2πmTsymfD,n .Suppose that the paths in (2) are non-overlapping in either delay or Doppler, and that K and/or M are/is sufficiently large, i.e., b H n1

Fig. 3 .
Fig. 3. Example visualization of measured 60 GHz BRSRP data, with the LoS path and other landmarks being clearly visible.Antenna sidelobes are creating notable power spreading along the two axes.

13 :
Polynomial fitting -a subset of the power matrix B around each cluster mean is fitted with a 2 nd degree polynomial surface.For the n th cluster, the final AoD and AoA estimates are found in closed form as a vertex of the local polynomial surface as φn = (c5c3 − 2c6c2)/(4c6c4 − c 2 5 ), θn = (c5c2 − 2c4c3)/(4c6c4 − c 2 5 ), where c l , l = 1...6 are the coefficients of the polynomial surface.

Algorithm 2
Proposed Gauss-Newton algorithm Input: Initial parameter guess x(0) and measurements z Output: Parameter estimate x and covariance P 1: Set j ← 0 2: repeat 3:

1 )
UE Initialization Without Prior Information: The challenge in initializing the UE state using the LoS measurement is the unknown clock bias B UE .One can consider different trial values of B UE over a range of [B UE,min , B UE,max ] 1 and for each trial value BUE , we form an augmented measurement ž = [τ b 1 , φ1 , θ1 , BUE ] ⊤ and covariance Ř1 = blkdiag(R 1 , σ 2 BUE

Fig. 4 .
Fig. 4. Illustrations of the considered indoor environment.(a) Ray-tracing model visualization showing the TX/BS position and the RX/UE trajectory; (b) Photograph from the physical premises with the TX and RX trolleys also shown during the measurement campaign.

Fig. 10 .
Fig. 10.Position RMSEs of different algorithms in LoS and NLoS conditions with UE clock bias, and with () and without ( ) outliers.The heading and bias RMSE have a similar trend but are omitted from the figure for brevity.The impact of the different objective function variants in (23) are also shown, including i) OFv0: no prior and quadratic cost function; ii) OFv1: no prior and robust cost function; and iii) OFv2: prior and quadratic cost function.

Fig. 11 .
Fig. 11.The numbers of inliers and outliers in each measurement position and with different power ratio p values.Minimum number of inliers required by BM1 to solve SLAM is illustrated with the black dashed line.
where λ = c/f c denotes the wavelength at carrier frequency f c , and c is the speed of light.The total number of antenna elements in TX and RX are N TX = N v TX × N h BM2 implemented with 10 particles.† Known clock bias BUE. *

TABLE II POSITION
RMSE IN METERS WITH DIFFERENT POWER RATIO p VALUES