5G SLAM Using the Clustering and Assignment Approach with Diffuse Multipath

Abstract

5G communication systems operating above 24 GHz have promising properties for user localization and environment mapping. Existing studies have either relied on simplified abstract models of the signal propagation and the measurements, or are based on direct positioning approaches, which directly map the received waveform to a position. In this study, we consider an intermediate approach, which consists of four phases—downlink data transmission, multi-dimensional channel estimation, channel parameter clustering, and simultaneous localization and mapping (SLAM) based on a novel likelihood function. This approach can decompose the problem into simpler steps, thus leading to lower complexity. At the same time, by considering an end-to-end processing chain, we are accounting for a wide variety of practical impairments. Simulation results demonstrate the efficacy of the proposed approach.

1. Introduction

Progressive generations of cellular communication systems have a common property—each generation relies on a larger bandwidth and higher carrier frequency than the previous one. This is particularly noticeable in 5G, where in Frequency Range 2 (FR2), carriers above 24 GHz are utilized in combination with contiguous bandwidths of up to 400 MHz. Early commercial deployments in the US have demonstrated data rates beyond 1 Gbps [1]. The same increases in bandwidth and carrier frequency also have direct benefits for positioning [2]. This has led to intense research activities into 5G positioning as well as a dedicated study item in 3GPP. The final report of this study in 3GPP [3] revealed that by using both delay and angle measurements, it is possible to satisfy commercial positioning performance requirements. Beyond positioning, the sparse nature of the channel at higher carriers allows the receiver to resolve the multipath components, which transforms them from foe to friend [4,5]. In particular, the geometric nature of the channel at mmWave frequencies leads to a relation between the multipath parameters and the physical environment, as the parameters are related to the location of the user (UE) with respect to the base station (BS) and the propagation environment. The simultaneous localization and mapping problem is then invoked to invert multipath parameters into geometric information that determines the user’s position and the locations of objects, based on the signal from a single base station. Several papers have addressed this problem, exploiting both line-of-sight (LOS) and non-LOS (NLOS) paths for position estimation, synchronization, and mapping in mmWave multiple-input multiple-output (MIMO) [6,7,8] and mmWave multiple-input single-output (MISO) [9,10] contexts. However, this inversion is challenging due to a number of reasons: (i) estimation of the channel parameters involves solving a high-dimensional problem; (ii) the association between the objects in the environment and the measurements is not available; (iii) an object may give rise to multiple measurements, due to diffuse multipath; (iv) the measurements per object are not clustered. We will now treat each of these four challenges in turn.

Channel estimation for mmWave is a rich research area, which we cannot cover in detail here. Rather, we categorize these methods as search-based and search-free. Search-based methods, such as those using maximum likelihood (ML) [11] and compressed sensing (CS) techniques [12,13], require an exhaustive search in the high-dimensional space of channel parameters, which entails high complexity. On the other-hand, search-free methods [14], such as matrix or tensor decomposition based subspace methods, directly provide estimates of the channel parameters [15] or rely on low-dimensional search [16], thus avoiding the need for high-dimensional optimization. An important challenge of channel estimation for SLAM is that the different dimensions (angles of arrival and departure, delays and gains) should be correctly matched.

The unknown association between measurements and objects is a common problem in SLAM, and powerful methods to address it can be found in the literature [17,18]. SLAM with radio-based measurements has been considered in the context of ultra-wideband (UWB) communication [19,20] using only distance measurements, which is referred to multipath-assisted SLAM, or channel SLAM. Focusing on the application of SLAM in a 5G mmWave context (which we designate ’5G SLAM’), message passing-based estimators were introduced in References [7,21], based on the concept of non-parametric belief propagation, without the data association (DA). Extension of such methods to include the hidden DAs is possible, following the approaches from Reference [22]. In Reference [23], the probability hypothesis density (PHD) filter, which is a random-finite-set filter, was used to solve the 5G SLAM problem, considering only one measurement per object. In Reference [24], a more powerful random-finite-set filter, Poisson multi-Bernoulli mixture (PMBM) filter, was used, which enumerates all possible DAs.

Multiple measurements per object, while common in the SLAM and extended object tracking literature [25], have been generally ignored in the above 5G SLAM works. The multiple measurements per object are caused by diffuse multipath due to object roughness with respect to the wavelength, depicted in Figure 1. In Reference [5], diffuse multipath is seen as a perturbation, leading to false measurements. In Reference [26], exploitation of the diffuse multipath in radar is proposed by means of diffuse multipath statistics. In Reference [27], surface roughness was considered in radar applications, modeled as a number of sub-reflectors, in an environment with known wall geometry. A similar model with random sub-reflectors was evaluated in Reference [28], where the estimated diffuse paths were used for positioning and mapping, using a simple geometric approach.

Finally, when measurements from an object are not clustered, the unknown grouping can be considered within the SLAM filter [29,30,31], though this comes at a high computational cost. In Reference [28], a K-means clustering was utilized, but this requires a priori knowledge of the number of clusters. In Reference [32], K-power-means was proposed, as well as several criteria to decide the number of clusters. In Reference [24], the perfect clustering was assumed.

In this paper, we address the aforementioned challenges, building on the extensive literature in each of the above research areas in order to provide an end-to-end framework for SLAM harnessing diffuse multipath. The proposed end-to-end framework provides a general approach for user localization and environment mapping in 5G downlink transmissions from a single BS. Therefore, the purposed framework can be utilized in many application areas, including personal navigation [4], localization of cars and robots  [33], smart homes [34], indoor location analysis [35], immersive customer experiences [36], location-aided communication [37], personal radar [38], to name but a few. Moreover, the proposed framework can form a foundation for future Beyond 5G and 6G localization and sensing approaches [39]. Our framework is built on a layered approach, comprising three main parts (channel estimation, clustering, and SLAM), which are evaluated separately and end-to-end. The main contributions of this paper are as follows:

• The description of an end-to-end framework for SLAM harnessing diffuse multipath and its performance evaluation.
• The evaluation of clustering and assignment methods, which is suitable for estimated channel parameters under both specular and diffuse multipath, as well as a method to utilize the estimated channel gains for improving the clustering in the 5G SLAM problem.
• The extension of the 5G SLAM likelihood function, in order to harness both specular and diffuse multipath components and to classify different object types according to their roughness, while accounting for clustering errors.

The novelty of the proposed approach compared to the existing random finite set (RFS) based 5G SLAM work [23,40] is three-fold—first of all, References [23,40] did not use a real channel estimator, which makes the problem easier. Secondly, they assumed at most one measurement from an object, which is not the real case. Finally, the PHD filter is not optimal, which does not contain the enumeration of the different data associations. In Reference [24], although the measurements are from the ESPRIT channel estimator and the diffuse multipath is considered, the channel gain is still ignored and the channel estimation results are assumed to be well grouped based on the source. In the current paper, we study the whole framework, from downlink signals to SLAM filter. We also fully use the information given by the channel estimator, including diffuse multipath and channel gain. The PMBM filter is used, which is optimal and enumerates all possible data associations.

The remainder of this paper is structured as follows. The system model is described in Section 2, including the signal model, environment model, sensor, and measurement model. The end-to-end framework is then presented in Section 3, specifying the components that will be detailed in the subsequent sections, starting with channel estimation in Section 4, clustering in Section 5, and the novel likelihood in Section 6. Simulation results are presented in Section 7, followed by our conclusions in Section 8. The paper also contains several appendices describing the geometric expressions of the channel parameters, as well as the details of the SLAM method.

Notations

Scalars (e.g., x) are denoted in italic, vectors (e.g., $\mathit{x}$) in bold, matrices (e.g., $\mathit{X}$) in bold capital letters, sets (e.g., $\mathcal{X}$) in calligraphic, tensors (e.g., $\mathcal{X}$) in bold calligraphic. Transpose and Hermetian are denoted by ${·}^{\mathsf{T}}$ and ${·}^{\mathsf{H}}$, and $ȷ=\sqrt{-1}$. Furthermore, $\left|·\right|$ denotes the absolute value of a scalar, or the cardinality of a set; $∥·∥$ denotes the Euclidean norm of a vector. A Gaussian density with mean $\mathit{\mu }$ and covariance $\mathbf{\Sigma }$, evaluated in value $\mathit{x}$ is denoted by $\mathcal{N}(\mathit{x};\mathit{\mu },\mathbf{\Sigma })$. Finally, the notations of important variables are summarized in

Table 1. Notations of important variables.

Notation	Definition	Notation	Definition
$\mathit{s}$	state of the user	${\mathit{x}}_{\mathrm{LM}}$	landmark location
m	landmark type	$\mathit{\theta }$	angle of arrival (AOA) pair
$\mathit{\varphi }$	angle of departure (AOD) pair	$\tau$	time of arrival (TOA)
g	channel gain	$\mathcal{Z}$	measurement set
$\mathit{z}$	single measurement	c	speed of light
$p_{\mathrm{D}}$	detection probability	k	time index
i	surface index	$l,p$	path index
s	subcarrier index	r	dimension index

.

2. System Model

In this section, we describe the basic models of the user, the environment, the propagation channel, and the observed waveform. We use the 5G positioning reference signals (PRS) [41] as pilot signals. These signals are broadcast by the BS, and used for positioning according to the 3GPP standards [3,42]. Since the signals are broadcast, there is no multi-user issue or interference, so that we only focus on a single UE, where the proposed algorithms can be assumed to be executed per UE.

2.1. User Model

In this paper, we only consider the single-user scenario, so the cooperation between different users is beyond the scope of this paper. The dynamic state of the user at time step k is denoted by ${\mathit{s}}_k$, which contains the position of the user ${\mathit{x}}_{\mathrm{UE},k}={[x_k,y_k,z_k]}^{\mathsf{T}}$, heading ${\varpi }_k$, translation speed ${\zeta }_k$, turn rate ${\varrho }_k$ and clock bias $B_k$. The dynamic model of ${\mathit{s}}_k$ is

$${\mathit{s}}_k=\mathit{v}\left({\mathit{s}}_{k-1}\right)+{\mathit{q}}_k,$$

(1)

where $\mathit{v}(·)$ denotes a known transition function; ${\mathit{q}}_k$ is the process noise, modeled as a zero-mean Gaussian with known covariance ${\mathit{Q}}_k$. The above dynamic model can be expressed, equivalently, in terms of a transition density $f({\mathit{s}}_k|{\mathit{s}}_{k-1})=\mathcal{N}({\mathit{s}}_k;v\left({\mathit{s}}_{k-1}\right),{\mathit{Q}}_k)$.

2.2. Environment Model

We consider an environment with a few landmarks. There is a fixed and known BS, located at ${\mathit{x}}_{\mathrm{BS}}\in {\mathbb{R}}^3$. The other unknown landmarks are modeled as different types of surfaces (see Figure 2). A surface can be described with an extended state comprising:

• A point $\mathit{f}$ on a corner of the surface and a vector $\mathit{u}$ normal to the surface.
• The size of the surface in length l and height h. The width w is not relevant.
• The smoothness of the surface, denoted by ${\alpha }_R\ge 0$.
• The scattering attenuation $1\ge S\ge 0$ and reflection attenuation $1\ge R\ge 0$, with $R^2+S^2\le 1$, in which the remaining power is absorbed in the surface.

To avoid dealing with such a high-dimensional surface state description, we use a simplified state, containing a fixed virtual anchor (VA) with location ${\mathit{x}}_{\mathrm{VA}}\in {\mathbb{R}}^3$, which is the reflection of the BS with respect to the surface, that is,

$$\begin{array}{c}\hfill {\mathit{x}}_{\mathrm{VA}}=(\mathit{I}-2\mathit{u}{\mathit{u}}^{\mathsf{T}}){\mathit{x}}_{\mathrm{BS}}+2{\mathit{f}}^{\mathsf{T}}\mathit{u}\mathit{u}.\end{array}$$

(2)

Note that the VA location is surface-specific. We only consider 3 different types of surfaces: smooth surface (SM, with $S=0$, $R=0.8$, ${\alpha }_{\mathrm{R}}=100$), medium rough surface (MR, with $S=0.4,R=0.6,{\alpha }_{\mathrm{R}}=4$) and very rough surface (VR, with $S=0.8,R=0,{\alpha }_{\mathrm{R}}=0$). Signals from the base station can only be reflected via SM to the receiver; while they can be reflected and diffused via MR to the receiver; whereas VR can only diffuse signals. This allows us to have a compact state representation, with the landmark state $\mathit{x}={[{\mathit{x}}_{\mathrm{LM}}^{\mathsf{T}},m]}^{\mathsf{T}}$, where $m\in \{\mathrm{BS},\mathrm{SM},\mathrm{MR},\mathrm{VR}\}$ and ${\mathit{x}}_{\mathrm{LM}}={\mathit{x}}_{\mathrm{BS}}$ for $m=\mathrm{BS}$, while ${\mathit{x}}_{\mathrm{LM}}={\mathit{x}}_{\mathrm{VA}}$, for $m\ne \mathrm{BS}$ [24].

2.3. Channel Model

At time step k, the channel from the BS to the user in the frequency domain is given by  [11]:

$$\begin{array}{c}\hfill H_k(f,\mathit{\theta },\mathit{\varphi })=\sum _{i=0}^{I_k-1}\sum _{l=0}^{L_k^i-1}{g}_k^{i,l}\delta (\mathit{\theta }-{\mathit{\theta }}_k^{i,l})\delta (\mathit{\varphi }-{\mathit{\varphi }}_k^{i,l})e^{-ȷ2\pi f{\tau }_k^{i,l}},\end{array}$$

(3)

where $\delta (·)$ is the delta Dirac function, f is the frequency, $\mathit{\theta }$ is the angle of arrival (AOA), $\mathit{\varphi }$ is the angle of departure (AOD), $I_k$ is the number of landmarks in the environment ($i=0$ represents the BS); $L_k^i$ is the number of paths from each landmark ($l=0$ represents the specular path). For each surface, there is at most one deterministic specular component among the paths, while all remaining paths are diffuse components and thus random [28]. We denote the total number of paths by $P_k={\sum }_{i=1}^{I_k}{L}_k^i$. Each path $i,l$ can be described by a complex gain $g_k^{i,l}$, a time of arrival (TOA) ${\tau }_k^{i,l}$, an AOA pair ${\mathit{\theta }}_k^{i,l}$ in both azimuth and elevation, and an AOD pair ${\mathit{\varphi }}_k^{i,l}$ in both azimuth and elevation. The generative model for each of these parameters is described as

• LOS path: When $i=0$, $L_k^i=1$, the gain has uniform phase and has power

  $$\begin{array}{c}\hfill {\left|g_k^{0,0}\right|}^2=\frac{{\lambda }^2}{{\left(4\pi \right)}^2{∥{\mathit{x}}_{\mathrm{UE}}-{\mathit{x}}_{\mathrm{BS}}∥}^2},\end{array}$$

  (4)

  where $\lambda$ is the wavelength, and the TOA, AOA, and AOD follow the geometric relations between the BS and the UE. They are given in Appendix A.
• Specular path from surface i: For $i>0$, $l=0$, the point of incidence on the i-th surface (denoted by ${\mathcal{L}}_i$ with virtual anchor ${\mathit{x}}_{\mathrm{VA},i}$) is the intersection of the surface ${\mathcal{L}}_i$ and the line between the i-th virtual anchor ${\mathit{x}}_{\mathrm{VA},i}$ and the UE ${\mathit{x}}_{\mathrm{UE}}$. The specular path gain has uniform phase and power

  $$\begin{array}{c}\hfill {\left|g_k^{i>0,0}\right|}^2=\frac{{\lambda }^2{R}^2}{{\left(4\pi \right)}^2{∥{\mathit{x}}_{\mathrm{UE}}-{\mathit{x}}_{\mathrm{VA},i}∥}^2}.\end{array}$$

  (5)
  The TOA, AOA, and AOD follow the relative position of the UE, BS, and the incidence point on the surface. They are given in Appendix A.
• Diffuse paths from surface i: For $i>0$, $l>0$, the number of paths per surface and their spread in angle and delay, as well as the channel gains, depend on the roughness of that surface. These paths can be interpreted as coming from random points ${\mathit{x}}_k^{i,l}$ on the surface, with a spatial distribution $p_i({\mathit{x}}_{\mathrm{sc}}|{\mathit{x}}_{\mathrm{UE}},{\mathit{x}}_{\mathrm{BS}})$ that depends on the roughness, where ${\mathit{x}}_{\mathrm{sc}}$ is the random variable that describes the position of the diffuse point. The diffuse points are generated from the distribution ${\mathit{x}}_k^{i,l}\sim p_i({\mathit{x}}_{\mathrm{sc}}|{\mathit{x}}_{\mathrm{UE}},{\mathit{x}}_{\mathrm{BS}})$ ([43],Chapter 3)

  $$\begin{array}{cc}\hfill p_i({\mathit{x}}_{\mathrm{sc}}|{\mathit{x}}_{\mathrm{UE}},{\mathit{x}}_{\mathrm{BS}})& \propto \left\{\begin{array}{cc}\frac{{(1+cos{\nu }_1\left({\mathit{x}}_{\mathrm{sc}}\right))}^{{\alpha }_R}}{2}\frac{|cos{\nu }_2\left({\mathit{x}}_{\mathrm{sc}}\right)||cos{\nu }_3\left({\mathit{x}}_{\mathrm{sc}}\right)|}{\parallel {\mathit{x}}_{\mathrm{sc}}-{\mathit{x}}_{\mathrm{BS}}{\parallel }^2{\parallel {\mathit{x}}_{\mathrm{sc}}-{\mathit{x}}_{\mathrm{UE}}\parallel }^2}\hfill & {\mathit{x}}_{\mathrm{sc}}\in {\mathcal{L}}_i\hfill \\ 0\hfill & \mathrm{else},\hfill \end{array}\right.\hfill \end{array}$$

  (6)

  where ${\mathcal{L}}_i\subset {\mathbb{R}}^3$ denotes the set of points that make the i-th surface, ${\nu }_1\left({\mathit{x}}_{\mathrm{sc}}\right)$ is the deviation of the scattering angle with respect to the angle of the specular path (i.e,. ${\nu }_1\left({\mathit{x}}_{\mathrm{sc}}\right)=0$ when ${\mathit{x}}_{\mathrm{sc}}$ is the incidence point of the specular path), ${\nu }_2\left({\mathit{x}}_{\mathrm{sc}}\right)$ is the angle between the impinging ray (i.e., from the transmitter to ${\mathit{x}}_{\mathrm{sc}}$) and the surface normal, and ${\nu }_3\left({\mathit{x}}_{\mathrm{sc}}\right)$ is the angle between the departing ray and the surface normal. The diffuse paths have uniform and independent phases and equal power

  $$\begin{array}{c}\hfill {\left|g_k^{i>0,l>0}\right|}^2=\frac{{\lambda }^2{\mathrm{S}}^2({\alpha }_R+1)}{{\left(4\pi \right)}^2(L_k^i-1)(1-2^{-({\alpha }_R+1)})({\alpha }_R+2){∥{\mathit{x}}_{\mathrm{UE}}-{\mathit{x}}_{\mathrm{VA},i}∥}^2}.\end{array}$$

  (7)
  The locations of the diffuse points ${\mathit{x}}_k^{i,l}$ fully determine their corresponding TOA, AOA, and AOD, provided in Appendix A.

2.4. Signal Model

We will assume a transmitter and receiver with uniform rectangular arrays (URAs) with $M_T=M_1\times M_2$ (i.e., $M_1$ columns and $M_2$ rows) and $M_R=M_3\times M_4$ elements, respectively. The corresponding steering vectors are

$$\begin{array}{cc}\hfill {\mathit{a}}_{\mathrm{T}}(\mathit{\varphi }=[{\varphi }_{az},{\varphi }_{\mathrm{el}}])& ={\mathit{a}}_c\left(\mathit{\varphi }\right)\otimes {\mathit{a}}_r\left(\mathit{\varphi }\right)\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill [{\mathit{a}}_c{\left(\mathit{\varphi }\right)]}_{m_c}& =exp(ȷ\pi m_{c}sin\left({\varphi }_{\mathrm{el}}\right)),m_c\in \{0,\dots ,M_2-1\}\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill [{\mathit{a}}_r{\left(\mathit{\varphi }\right)]}_{m_r}& =exp(ȷ\pi m_{r}cos\left({\varphi }_{\mathrm{el}}\right)sin\left({\varphi }_{\mathrm{az}}\right)),m_r\in \{0,\dots ,M_1-1\}\hfill \end{array}$$

(10)

and

$$\begin{array}{cc}\hfill {\mathit{a}}_{\mathrm{R}}(\mathit{\theta }=[{\theta }_{\mathrm{az}},{\theta }_{\mathrm{el}}])& ={\mathit{a}}_c\left(\mathit{\theta }\right)\otimes {\mathit{a}}_r\left(\mathit{\theta }\right)\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill [{\mathit{a}}_c{\left(\mathit{\theta }\right)]}_{m_c}& =exp(ȷ\pi m_{c}sin\left({\theta }_{\mathrm{el}}\right)),m_c\in \{0,\dots ,M_4-1\}\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill [{\mathit{a}}_r{\left(\mathit{\theta }\right)]}_{m_r}& =exp(ȷ\pi m_{r}cos\left({\theta }_{\mathrm{el}}\right)sin\left({\theta }_{\mathrm{az}}\right)),m_r\in \{0,\dots ,M_3-1\},\hfill \end{array}$$

(13)

in which ⊗ is the Kronecker product.

In addition, we consider OFDM transmission with $M_S=M_5$ subcarriers and subcarrier spacing $\Delta f$. The received signal at subcarrier s at time step k is then [44]

$$\begin{array}{c}\hfill {\mathit{Y}}_{s,k}=\underset{{\mathit{H}}_s}{\underbrace{\sum _{i=0}^{I_k-1}\sum _{l=0}^{L_k^i-1}{g}_k^{i,l}{\mathit{a}}_{\mathrm{R}}\left({\mathit{\theta }}_k^{i,l}\right){\mathit{a}}_{\mathrm{T}}^{\mathsf{H}}\left({\mathit{\varphi }}_k^{i,l}\right)e^{-ȷ2\pi s\Delta f{\tau }_k^{i,l}}}}{\mathit{S}}_s+{\mathit{N}}_{s,k},\end{array}$$

(14)

where ${\mathit{S}}_s\in {\mathbb{C}}^{M_T\times T}$ is the pilot signal over subcarrier s (spanning T OFDM symbols), ${\mathit{N}}_{s,k}$ is white Gaussian noise with $\mathrm{vec}\left({\mathit{N}}_{s,k}\right)\sim \mathcal{CN}(\mathbf{0},{\sigma }^2{\mathit{I}}_{M_{R}T})$, and ${\mathbf{H}}_s$ is the channel frequency response,

3. Methodology and End-to-End Framework

Using the raw measurements ${\mathit{Y}}_s$ directly in SLAM is challenging, due to the high dimensionality of the measurement, the complex nonlinear relation to the user and landmark states, and the fact that not all paths may be resolvable, due to a limited number of transmitter and receiver antennas and bandwidth. While such direct localization has performance benefits [45,46,47], we instead consider a layered approach, visualized in Figure 3, comprising the following steps after downlink transmission of the signals ${\mathit{Y}}_s$.

• First of all, channel estimation is performed to recover the channel parameters (angles, delays, gains). Due to the finite resolution at the receiver side, not all paths are resolvable. Hence, the number of estimated paths (denoted by ${\widehat{P}}_k$) will be much smaller than $I_k\times L_k^i$. The channel estimator thus provides a set of channel parameter estimates ${\mathcal{Z}}_k$ at time k, ${\mathcal{Z}}_k=\{{\mathit{z}}_k^0,{\mathit{z}}_k^1,\cdots ,{\mathit{z}}_k^{{\widehat{P}}_k-1}\}$. Each element ${\mathit{z}}_k^p\in {\mathcal{Z}}_k$ is either a clutter, which is caused by noise peaks that are detected as paths during channel estimation, with clutter intensity $c\left(\mathit{z}\right)$ or follows

  $$\begin{array}{cc}\hfill {\mathit{z}}_k^p& ={[{\tau }_k^p,{\left({\mathit{\theta }}_k^p\right)}^{\mathsf{T}},{\left({\mathit{\varphi }}_k^p\right)}^{\mathsf{T}},g_k^p]}^{\mathsf{T}}+{\mathit{w}}_k^p\hfill \end{array}$$

  (15)

  $$\begin{array}{cc}\hfill & =\mathit{h}({\mathit{x}}_k^p,{\mathit{s}}_k)+{\mathit{w}}_k^p,\hfill \end{array}$$

  (16)

  where ${\mathit{w}}_k^p$ denotes the measurement noise; ${\mathit{x}}_k^p$ is ${\mathit{x}}_{\mathrm{BS}}$ for LOS, the incidence point of the deterministic specular components, or a (random) point on the surface for a NLOS component. We recall that the underlying geometric relation $\mathit{h}({\mathit{x}}_k^p,{\mathit{s}}_k)$ can be found in Appendix A. We describe the channel estimator in Section 4.
• After channel estimation, we group the unordered elements in ${\mathcal{Z}}_k$ in clusters ${\mathcal{Z}}_k^i$, where each cluster should correspond to one landmark. This removes the need to consider all possible partitions of the measurements in the SLAM method, drastically reducing overall complexity. Clustering is challenging as measurement clusters may be non-convex. In addition, diffuse paths may be far away from the specular paths, leading to possible miss-classifications. The proposed clustering method is described in Section 5.
• Finally, after clustering, the SLAM method requires a likelihood function that expresses the statistical relation between the state and the clustered measurements, $\ell ({\mathcal{Z}}_k^i|\mathit{x},{\mathit{s}}_k)$. The SLAM method is deferred to Appendix B, while in the main text we focus on the proposed likelihood function in Section 6. The SLAM filter follows a Rao-Blackwellized approach, where we use a set of particles (indexed by n) to represent the user state, and use PMBM densities conditioned on each particle to represent the map. Clustering and likelihood computation are conditioned in the user state and are thus performed per particle.

4. Channel Estimation

In this section, the ESPRIT channel estimator is introduced.

4.1. Background

The standard formulation of the channel estimation problem is an ML problem, where

$$\begin{array}{c}\hfill {\widehat{\mathbf{\Theta }}}_k=arg\underset{\mathbf{\Theta }}{min}-\sum _{s}logp({\mathit{Y}}_{s,k}|\mathbf{\Theta },{\mathit{S}}_s),\end{array}$$

(17)

in which $\mathbf{\Theta }$ contains the delays, gains, and angles of all the paths, as well as the number of paths. Since the number of paths is unknown and paths are not all resolvable, model order selection techniques can be applied, for example, by adding a regularizer to (17) ([11], Section 5.2), for example, following the minimum description length (MDL) or Akaike information criterion (AIC). The dense multipath can be treated as a stochastic process, by writing (14) as

$$\begin{array}{c}\hfill {\mathit{Y}}_{s,k}=\sum _{i=0}^{I_k-1}{g}_k^{i,0}{\mathit{a}}_{\mathrm{R}}\left({\mathit{\theta }}_k^{i,0}\right){\mathit{a}}_{\mathrm{T}}^{\mathsf{H}}\left({\mathit{\varphi }}_k^{i,0}\right)e^{-ȷ2\pi s\Delta f{\tau }_k^{i,0}}{\mathit{S}}_s+{\mathit{W}}_{s,k}^{\mathrm{dm}}+{\mathit{N}}_{s,k},\end{array}$$

(18)

where ${\mathit{W}}_{s,k}^{\mathrm{dm}}$ is the dense multipath containing all the contributions from the diffuse paths. The dense multipath is then modeled as complex Gaussian process with zero mean and covariance ${\mathit{R}}_k^{\mathrm{dm}}$, possibly with unknown elements. Then the entire noise process has covariance ${\mathit{R}}_k^{\mathrm{dm}}+{\sigma }^2{\mathit{I}}_{M_{R}T}$. Alternatively, we can estimate the geometric parameters of the dominant diffuse multipath components from MIMO channel measurements, which leads to a multidimensional harmonic retrieval (HR) problem [48]. Numerous HR techniques have been developed, ranging from multidimensional searching or optimization based, such as multiple signal classification (MUSIC), ML and CS techniques [49], and polynomial-rooting or matrix-shifting based search-free methods, such as root-MUSIC [14] and estimation of parameters by rotational invariant techniques (ESPRIT) and their multidimensional extensions [50]. Due to its simplicity and high-resolution capability, ESPRIT-type algorithms have become one of the popular HR techniques [51].

4.2. ESPRIT Channel Estimator

For notational convenience, we drop the time index k. From (14), the received signal on subcarrier s is of the form

$$\begin{array}{c}\hfill {\mathbf{Y}}_s={\mathbf{H}}_s{\mathbf{S}}_s+{\mathbf{N}}_s,\end{array}$$

(19)

where ${\mathbf{H}}_s$ is the channel frequency response, ${\mathbf{S}}_s$ is a known pilot signal with orthogonality property (${\mathbf{S}}_s{\mathbf{S}}_s^{\mathsf{H}}=\beta {\mathbf{I}}_{M_T\times M_T}$), and ${\mathbf{N}}_s$ is i.i.d. Gaussian noise. Then we have

$$\begin{array}{c}\hfill {\mathbf{X}}_s=\frac{1}{\beta }{\mathbf{Y}}_s{\mathbf{S}}_s^{\mathsf{H}}={\mathbf{H}}_s+\frac{1}{\beta }{\mathbf{N}}_s{\mathbf{S}}_s^{\mathsf{H}}={\mathbf{H}}_s+{\mathbf{W}}_s,\end{array}$$

(20)

where ${\mathbf{W}}_s$ is also i.i.d. Gaussian noise with a scaled covariance matrix.

4.2.1. Observations in Tensor Form

We utilize a Tensor framework to exploit the R-D grid structure inherent in the data, as well as the Vandermonde structure in angle and delay domains. To this end, we map from geometric channel parameters to spatial frequencies by

$$\begin{array}{cc}\hfill {\mu }_{\left(1\right)}^p& =\pi sin\left({\theta }_{\mathrm{az}}^p\right)cos\left({\theta }_{\mathrm{el}}^p\right),{\mu }_{\left(2\right)}^p=\pi sin\left({\theta }_{\mathrm{el}}^p\right),\hfill \\ \hfill {\mu }_{\left(3\right)}^p& =\pi sin\left({\varphi }_{\mathrm{az}}^p\right)cos\left({\varphi }_{\mathrm{el}}^p\right),{\mu }_{\left(4\right)}^p=\pi sin\left({\varphi }_{\mathrm{el}}^p\right),\hfill \\ \hfill {\mu }_{\left(5\right)}^p& =2\pi {\Delta }_f{\tau }^p.\hfill \end{array}$$

(21)

For subcarrier i, ${\mathbf{X}}_i$ and ${\mathbf{W}}_i$ are $M_3{M}_4\times M_1{M}_2$ matrices. We convert these $M_5$ matrices (one per subcarrier) to a 5D tensor of suitable dimension, $\mathcal{X}$, $\mathcal{H}$ and $\mathcal{W}\in {\mathbb{C}}^{M_1\times M_2\times M_3\times M_4\times M_5}$. For the p-th path, the equivalent 5D array steering tensor can be written as

$${\mathcal{A}}^p=\mathbf{a}\left({\mu }_{\left(1\right)}^p\right)\circ \mathbf{a}\left({\mu }_{\left(2\right)}^p\right)\circ \cdots \circ \mathbf{a}\left({\mu }_{\left(5\right)}^p\right),$$

(22)

where ∘ represents the outer product (Note that $\mathit{A}={\mathit{a}}_1\circ {\mathit{a}}_2={\mathit{a}}_1{\mathit{a}}_{\mathbf{2}}^{\mathsf{T}}$, and $\mathcal{A}={\mathit{a}}_1\circ {\mathit{a}}_2\circ {\mathit{a}}_3$ with $A_{ijk}=a_{1i}{a}_{2j}{a}_{3k}$.), ${\mu }_{\left(r\right)}^p$ is the spatial frequency of the p-th source in the r-th dimension, $\mathbf{a}\left({\mu }_{\left(r\right)}^p\right)\in {\mathbb{C}}^{M_r\times 1}$ is equivalent to the uniform linear array steering vector composed of $M_r$ sensors, $p=1,2,\cdots ,P$, where P is the total number of paths, and $r=1,2,\cdots ,5$. This allows us to express the observation as

$$\mathcal{X}=\sum _{p=1}^P{g}^p{\mathcal{A}}^p+\mathcal{W}\in {\mathbb{C}}^{M_1\times M_2\times \cdots \times M_5},$$

(23)

where $g^p$ denotes the complex path gain of the p-th path.

4.2.2. Shift Invariance

We now introduce the multidimensional shift invariant structure in each dimension r, in order to apply Tensor-ESPRIT. We first introduce so-called selection matrices ${\mathit{J}}_{1,\left(r\right)},{\mathit{J}}_{2,\left(r\right)}\in {\mathbb{R}}^{M_r^{\left(\mathrm{sel}\right)}\times M_r}$, which select $M_r^{\left(\mathrm{sel}\right)}$ out of $M_r$ elements belonging to the first and the second subarray in the r-th mode, respectively. For example, when $M_r^{\left(\mathrm{sel}\right)}=M_r-1$, we have

$${\mathit{J}}_{1,\left(r\right)}=\left[\begin{array}{cc}{\mathit{I}}_{M_r-1}& {\mathbf{0}}_{\left(M_r-1\right)\times 1}\end{array}\right]\mathrm{and}{\mathit{J}}_{2,\left(r\right)}=\left[\begin{array}{cc}{\mathbf{0}}_{\left(M_r-1\right)\times 1}& {\mathit{I}}_{M_r-1}\end{array}\right],$$

(24)

where $\mathit{I}$ and $\mathbf{0}$ denote the identity matrix and zero vector, respectively, and the sizes are defined in subscripts. Secondly, we introduce ${\mathbf{A}}_{\left(r\right)}=\left[\begin{array}{cccc}\mathbf{a}\left({\mu }_{\left(r\right)}^1\right)& \mathbf{a}\left({\mu }_{\left(r\right)}^2\right)& \cdots & \mathbf{a}\left({\mu }_{\left(r\right)}^P\right)\end{array}\right]$, which has the shift invariance property

$${\mathit{J}}_{1,\left(r\right)}{\mathbf{A}}_{\left(r\right)}{\mathsf{\Phi }}_{\left(r\right)}={\mathit{J}}_{2,\left(r\right)}{\mathbf{A}}_{\left(r\right)},$$

(25)

where ${\mathbf{\Phi }}_{\left(r\right)}=diag\left({\mathrm{e}}^{ȷ·{\mu }_{\left(r\right)}^1},{\mathrm{e}}^{ȷ·{\mu }_{\left(r\right)}^2},\dots ,{\mathrm{e}}^{ȷ·{\mu }_{\left(r\right)}^L}\right)$ is a diagonal matrix that contains the unknown parameters ${\mu }_l^{\left(r\right)},l=1,2,\cdots ,L$, in dimension r.

4.2.3. Tensor-ESPRIT

In order to obtain the subspace spanned by ${\mathbf{A}}_{\left(r\right)}$, we take CANDECOMP/PARAFAC (CP) decomposition on $\mathcal{X}$ [52]. Because the total number of paths P is unknown, model order selection techniques [53] can be utilized to estimate $\widehat{P}$. In general, the estimated $\widehat{P}\ll P$ for rough surfaces with hundreds of closely located diffuse paths.

$$\mathcal{X}=\sum _{p=1}^{\widehat{P}}{\widehat{g}}^p{\mathbf{u}}_{\left(1\right)}^p\circ {\mathbf{u}}_{\left(2\right)}^p\circ \cdots \circ {\mathbf{u}}_{\left(5\right)}^p\equiv \left[\begin{array}{c}\widehat{\mathbf{g}};{\mathbf{U}}_{\left(1\right)},{\mathbf{U}}_{\left(2\right)},\cdots ,{\mathbf{U}}_{\left(5\right)}\end{array}\right],$$

(26)

where $\widehat{\mathbf{g}}=\left[{\widehat{g}}^1{\widehat{g}}^2\cdots {\widehat{g}}^{\widehat{P}}\right]$ are the estimated path gains, and ${\mathbf{U}}_{\left(r\right)}=\left[{\mathbf{u}}_{\left(r\right)}^1{\mathbf{u}}_{\left(r\right)}^2\cdots {\mathbf{u}}_{\left(r\right)}^{\widehat{P}}\right]$ is the estimated signal subspace with normalized column vectors ${\mathbf{u}}_{\left(r\right)}^p$. Note that after taking CP decomposition, the path gains $g^p$ and channel parameter associations are achieved synchronously. In other words, $g^p$ corresponds to a unique ${\mathbf{u}}_{\left(r\right)}^p$ in each dimension r, so that the output of (26) is $\left\{{\widehat{g}}^p,{\mathbf{u}}_{\left(1\right)}^p,{\mathbf{u}}_{\left(2\right)}^p,\cdots ,{\mathbf{u}}_{\left(5\right)}^p\right\},p=1,2,\cdots ,\widehat{P}$.

Since ${\mathbf{A}}_{\left(r\right)}$ and ${\mathbf{U}}_{\left(r\right)}$ span the same subspace, ${\mathbf{A}}_{\left(r\right)}={\mathbf{U}}_{\left(r\right)}{\mathbf{T}}_{\left(r\right)}$, where ${\mathbf{T}}_{\left(r\right)}\in {\mathbb{C}}^{\widehat{P}\times \widehat{P}}$ is a non-singular transform matrix. Entering this relation in (25), we obtain

$$J_{1,\left(r\right)}{\mathbf{U}}_{\left(r\right)}{\mathbf{\Psi }}_{\left(r\right)}=J_{2,\left(r\right)}{\mathbf{U}}_{\left(r\right)},$$

(27)

where ${\mathbf{\Psi }}_{\left(r\right)}={\left({\mathbf{T}}_{\left(r\right)}\right)}^{-1}{\mathbf{\Phi }}_{\left(r\right)}{\mathbf{T}}_{\left(r\right)}$. In (27), only ${\mathbf{\Psi }}_{\left(r\right)}$ is unknown, but can be estimated via the least squares method:

$$\begin{array}{c}\hfill {\widehat{\mathbf{\Psi }}}_{\left(r\right)}={\left(J_{1,\left(r\right)}{\mathbf{U}}_{\left(r\right)}\right)}^{†}{J}_{2,\left(r\right)}{\mathbf{U}}_{\left(r\right)},\end{array}$$

(28)

where ${·}^{†}$ denotes the pseudo-inverse. Since the matrix ${\mathbf{\Psi }}_{\left(r\right)}$ is similar to the diagonal matrix ${\mathbf{\Phi }}_{\left(r\right)}$, they share the same eigenvalues. Hence, the spatial frequency can be recovered as ${\widehat{\mu }}_{\left(r\right)}^p=arg\left({\lambda }_{\left(r\right)}^p\right)$, where ${\mathit{\lambda }}_{\left(r\right)}=\left[{\lambda }_{\left(r\right)}^1,{\lambda }_{\left(r\right)}^2,\cdots ,{\lambda }_{\left(r\right)}^{\widehat{P}}\right]$ are the eigenvalues of ${\mathbf{\Psi }}_{\left(r\right)}$. The $\left\{{\widehat{\mu }}_{\left(r\right)}^p\right\},r=1,2,\cdots ,5$, map to delay ${\widehat{\tau }}^p$, AOA pair ${\widehat{\mathit{\theta }}}^p={\left[{\widehat{\theta }}_{\mathrm{az}}^p,{\widehat{\theta }}_{\mathrm{el}}^p\right]}^{\mathsf{T}}$, and AOD pair ${\widehat{\mathit{\varphi }}}^p={\left[{\widehat{\varphi }}_{\mathrm{az}}^p,{\widehat{\varphi }}_{\mathrm{el}}^p\right]}^{\mathsf{T}}$ via (21).

Finally, each tuple $\left[{\widehat{\tau }}^p,{\widehat{\theta }}_{\mathrm{az}}^p,{\widehat{\theta }}_{\mathrm{el}}^p,{\widehat{\varphi }}_{\mathrm{az}}^p,{\widehat{\varphi }}_{\mathrm{el}}^p,{\widehat{g}}^p\right]$, for $p\in \{1,\dots ,\widehat{P}\}$ is returned as the output of the channel estimator. We denote this combined output as $\mathcal{Z}$.

The most computationally demanding part of channel parameter estimation is the CP decomposition. In (Reference [54], Table 1), the complexities of major computations in popular CP decomposition algorithms is summarized. For example, the alternating least squares (ALS) algorithm with line search has a complexity of order $\mathcal{O}\left(2^R\widehat{P}J+R{\widehat{P}}^3\right)$, where $J={\prod }_{r=1}^R{M}_r$ and $R=5$ and $\widehat{P}$ denotes the total number of paths.

5. Channel Parameter Clustering

At time step k, the channel estimator provides a set of channel parameter estimates ${\mathcal{Z}}_k$, ${\mathcal{Z}}_k=\{{\mathit{z}}_k^0,{\mathit{z}}_k^1,\cdots ,{\mathit{z}}_k^{{\widehat{P}}_k-1}\}$, and we need to cluster ${\mathcal{Z}}_k$ based on different landmarks. However, clustering is challenging, since ${\mathcal{Z}}_k$ is in a 6D space, and the delay, angle, and gain are in different scales and spaces, so that they should be properly weighted. We also do not have any advance knowledge of the number of landmarks in the environment. In this section, we try to address these challenges. For brevity, we will omit the time index k.

5.1. Background

As a general term, a cluster is defined as a collection of objects that are similar to each other in some agreed-upon sense [55]. In radio channel analysis, a cluster is usually described as a group of multipath components (MPCs) with the same parameter distribution. The MPCs that have similar values in delay and angular domain are jointly classified as a single cluster, which reflects the physical environment, as similar MPCs are usually spatially close to each other, for example, reach the receiver via the same landmark.

Clustering methodology in radio channel analysis has been expanded from visual clustering to automatic clustering, where the visual clustering is usually valid for data with limited dimensions [55,56], while the automatic clustering can handle data with more than three dimensions [57]. Automatic clustering algorithms, focusing on the parameter space of MPCs, such as Hierarchical, K-means, and Gaussian mixture, have been widely used in radio channel characterizations [32,58,59,60,61]. Hierarchical clustering algorithms cluster the data based on a binary cluster tree, which is limited by the data size, and therefore limited to small data sets. K-means [62] assumes a known number of clusters and iteratively assigns data to each cluster and computes cluster centroids until the cluster centroids are converged. While K-means is a widely used clustering method, it suffers from several drawbacks: (i) the number of clusters is predefined, which limits the capabilities to reflect the reality of the environment; (ii) it cannot cope with outliers, which implies all the observed data will eventually be part of some clusters, even observations that are far away in 6D space and should be considered as outliers; (iii) it usually gives spherical-like shape clusters. MPCs that reflect other physical properties can be assigned as the edge of a spherical cluster, which has notable effects on the cluster properties, that is, cluster centroids, and loses the ability to link to physical reality. The more sophisticated affinity propagation [63] partially avoids these problems, but still leads to circular clusters and has very high complexity. Gaussian mixture clustering [57] gives more variation to the shapes of the clusters extracted with K-means, and follows the similar structure of K-means. It can give clusters in different shape if the original data is not distributed circularly. However, the method is complex and converges slowly.

Density-based spatial clustering of applications with noise (DBSCAN) [64] is one of the most widely applied clustering methods for data sorting. Without a predefined number of clusters, DBSCAN utilizes two critical parameters—the minimum number of points clustered together for a region to be considered dense, and a distance measure to locate the points in the neighborhood of any point. The density-based clustering method has no limitations on the number of clusters, uses a dense distance, and gives more freedom regarding the shape of clusters. While we have evaluated different methods in Section 7, here we only present the best performing method, which is an extension of the DBSCAN algorithm.

5.2. Modified DBSCAN

In this section, we will present the DBSCAN clustering method with the obtained channel estimates. The method involves three phases—(i) the mapping of the geometric channel parameters (delay and angles) to a 3D point; (ii) clustering with DBSCAN; (iii) refinement by using the channel gain.

5.2.1. Phase 1: 5D to 3D Mapping

Distinct from existing work where the channel parameters are used as features for clustering [28,61], we transform the 5D channel parameter into a 3D position. In the absence of noise, this point should be on the surface of a landmark for NLOS paths, while in the presence of noise, the most likely estimation of the point can be founded close to the surface. The transformation reduces the dimensionality, and avoids different scales and spaces problems. Hence, the 5D parameters, that is, the delay ${\widehat{\tau }}^p$, the AOA pair ${\widehat{\mathit{\theta }}}^p={[{\widehat{\theta }}_{\mathrm{az}}^p,{\widehat{\theta }}_{\mathrm{el}}^p]}^{\mathsf{T}}$ and the AOD pair ${\widehat{\mathit{\varphi }}}^p={[{\widehat{\varphi }}_{\mathrm{az}}^p,{\widehat{\varphi }}_{\mathrm{el}}^p]}^{\mathsf{T}}$ can be converted to a point ${\widehat{\mathit{x}}}^p$.

The general idea to transform the 5D parameters into a 3D point is illustrated in Figure 4. Specifically, for the p-th diffuse multipath, a virtual anchor position can be estimated as

$$\begin{array}{c}\hfill {\widehat{\mathit{x}}}_{\mathrm{LM}}^p={\mathit{x}}_{\mathrm{UE}}+c({\widehat{\tau }}^p-\widehat{B}){\mathit{f}}_{\mathrm{R}}^p,\end{array}$$

(29)

where c is the speed of light, $\widehat{B}$ is the estimated clock bias, ${\mathbf{f}}_{\mathrm{R}}^p$ is the unit vector pointing along with the direction of arrival, given by

$$\begin{array}{c}\hfill {\mathit{f}}_{\mathrm{R}}^p=\left[\begin{array}{c}cos{\widehat{\theta }}_{\mathrm{az}}^{p}cos{\widehat{\theta }}_{\mathrm{el}}^p\\ sin{\widehat{\theta }}_{\mathrm{az}}^{p}cos{\widehat{\theta }}_{\mathrm{el}}^p\\ sin{\widehat{\theta }}_{\mathrm{el}}^p\end{array}\right].\end{array}$$

(30)

Given ${\widehat{\mathit{x}}}_{\mathrm{LM}}^p$ and ${\mathit{x}}_{\mathrm{BS}}$, a hypothesized surface can be defined via a point on the surface ${\mathit{x}}_e^p=\left({\mathit{x}}_{\mathrm{BS}}+{\widehat{\mathit{x}}}_{\mathrm{LM}}^p\right)/2$ and the normal vector to the surface ${\mathit{e}}^p=\left({\mathit{x}}_{\mathrm{BS}}-{\widehat{\mathit{x}}}_{\mathrm{LM}}^p\right)/∥{\mathit{x}}_{\mathrm{BS}}-{\widehat{\mathit{x}}}_{\mathrm{LM}}^p∥$. With knowledge of ${\widehat{\mathit{x}}}_{\mathrm{LM}}^p$ and the surface, the incidence point location can be computed from from Snell’s law of reflection as

$$\begin{array}{c}\hfill {\widehat{\mathit{x}}}^p={\widehat{\mathit{x}}}_{\mathrm{LM}}^p+\frac{{({\mathit{x}}_e^p-{\widehat{\mathit{x}}}_{\mathrm{LM}}^p)}^{\mathsf{T}}{\mathit{e}}^p}{{({\mathit{x}}_{\mathrm{UE}}-{\widehat{\mathit{x}}}_{\mathrm{LM}}^p)}^{\mathsf{T}}{\mathit{e}}^p}({\mathit{x}}_{\mathrm{UE}}-{\widehat{\mathit{x}}}_{\mathrm{LM}}^p).\end{array}$$

(31)

Note that this method also works for the LOS path, and it will return a point near ${\widehat{\mathit{x}}}_{\mathrm{BS}}$, since ${\widehat{\mathit{x}}}_{\mathrm{LM}}^p$ and ${\mathit{x}}_e^p$ are both approximate to ${\widehat{\mathit{x}}}_{\mathrm{BS}}$. From $\mathcal{Z}$, we obtain the corresponding $\widehat{P}$ 3D points $\mathcal{D}=\{{\widehat{\mathit{x}}}^0,{\widehat{\mathit{x}}}^1,\cdots ,{\widehat{\mathit{x}}}^{{\widehat{P}}_k-1}\}$. Next, we will perform the clustering methods with the 3D positions of points.

5.2.2. Phase 2: Clustering with DBSCAN

In DBSCAN [64], all points are classified into either clusters or identified as noise. In a specific cluster $\mathcal{C}$, the core points have an $ϵ$-neighbourhood with $N_{min}\in \mathbb{N}$ points. We define a distance measure $d:{\mathbb{R}}^3\times {\mathbb{R}}^3\to \mathbb{R}$ from which the $ϵ$–neighbourhood of the p-th point is defined as

$$\begin{array}{c}\hfill {\mathcal{C}}_p\stackrel{\Delta }{=}\left\{{\widehat{\mathit{x}}}^q\in \mathcal{D}:d({\widehat{\mathit{x}}}^p,{\widehat{\mathit{x}}}^q)<ϵ\right\}.\end{array}$$

(32)

In this paper, we use $d({\widehat{\mathit{x}}}^p,{\widehat{\mathit{x}}}^q)=∥{\widehat{\mathit{x}}}^p-{\widehat{\mathit{x}}}^q∥$. The main idea of DBSCAN is that clusters are not characterized by their variance (as in K-means), but by density-reachability. In particular, ${\widehat{\mathit{x}}}^q$ is directly density-reachable from ${\widehat{\mathit{x}}}^p$, if and only if $|{\mathcal{C}}_p|\ge N_{min}$ and ${\widehat{\mathit{x}}}^q\in {\mathcal{C}}_p$. In addition, ${\widehat{\mathit{x}}}^p$ is density-reachable from ${\widehat{\mathit{x}}}^q$, if there exists an ordered sequence of points ${\widehat{\mathit{x}}}^{p_0}={\widehat{\mathit{x}}}^q,{\widehat{\mathit{x}}}^{p_1},\dots ,{\widehat{\mathit{x}}}^{p_n}={\widehat{\mathit{x}}}^p$ in $\mathcal{D}$, where ${\widehat{\mathit{x}}}^{p_{i+1}}$ is directly density-reachable from ${\widehat{\mathit{x}}}^{p_i}$. If the sphere of ${\widehat{\mathit{x}}}^p$ contains at least $N_{min}$ points, that is, $\left|{\mathcal{C}}_p\right|\ge N_{min}$, the point ${\widehat{\mathit{x}}}^p$ will be a core point of a cluster. The border points can have a smaller size $ϵ$-neighbourhood, but can be density-reachable from the core points.

Given the parameters $ϵ$ and $N_{min}$, we can discover a cluster in a two-step approach: (i) choose an arbitrary point from the database satisfying the core point conditions as a seed; (ii) find all points that are density-reachable from the seeds [64]. We follow these two steps, that is, to identify a core point, to find a cluster with the core point, and to find all clusters. The algorithms are provided in Algorithm 1. Note that Algorithm  2 is used in Algorithm  1 to find all points in each cluster. The output of the clustering is a partitioning of $\mathcal{Z}$ (or $\mathcal{D}$) into clusters ${\mathcal{P}}_l$ and a set of noise points ${\mathcal{P}}_{\mathrm{N}}$.

Algorithm 1: DBSCAN for Clustering

Input: Points $\{{\widehat{\mathit{x}}}^0,{\widehat{\mathit{x}}}^1,\cdots ,{\widehat{\mathit{x}}}^{{\widehat{P}}_k-1}\}$, threshold $ϵ$, and $N_{min}$;

Output: All clusters and the associated points.

1: Compute pair-wise Euclidean distance for all points, store them in $\mathbf{D}$, and label all points unvisited; 2: $l\leftarrow 0$ and $p\leftarrow 0$; // cluster index l and point index p 3: while$p<P$do 4:     if Point p is unvisited then 5:         Label point p as visited; 6:         Find all neighbours within $ϵ$ distance of p; 7:         if $ϵ$-neighbourhood is less than $N_{min}$ then 8:            Classify the point p as noise; 9:         else 10:            $l\leftarrow l+1$; // new cluster 11:            Discover all points in cluster l by using Algorithm 2. 12:         end if 13:     end if 14: end while

Algorithm 2: Find All Points in Cluster l

Input: Cluster index l, point index p and its $ϵ$-neighbourhood ${\mathcal{C}}_p$;

Output: The associated points in cluster l.

1: Classify p into cluster l; 2: Store all indexes of the points in ${\mathcal{C}}_p$ in vector $\mathbf{c}$; 3: $k\leftarrow 1$; 4: while$k\le \left|\mathbf{c}\right|$do 5:     $j\leftarrow {\mathbf{c}}_k$; 6:     if Point ${\widehat{\mathit{x}}}^j$ is unvisited then 7:         Label point ${\widehat{\mathit{x}}}^j$ as visited; 8:         Find all neighbours within $ϵ$ distance of ${\widehat{\mathit{x}}}^j$, that is, ${\mathcal{C}}_j$; 9:         if $\left|{\mathcal{C}}_j\right|\ge N_{min}$ then 10:            Add all indexes in ${\mathcal{C}}_j$ to $\mathbf{c}$; 11:         end if 12:     end if 13:     if ${\widehat{\mathit{x}}}^j$ is not classified to any cluster then 14:         Classify ${\widehat{\mathit{x}}}^j$ in cluster l; 15:     end if 16:     $k\leftarrow k+1$; 17: end while

5.2.3. Phase 3: Extract Isolated Specular Paths and Outliers Using Channel Gain

The DBSCAN clustering has two drawbacks in our application:

• The tensor ESPRIT channel estimator from Section 4 can generate estimates, whose 3D points (as obtained in Section 5.2.1) are still on or near the corresponding surfaces, but are far away from the cluster centers. Hence, they are informative for the SLAM algorithm, but are part of ${\mathcal{P}}_{\mathrm{N}}$, so they are not clustered correctly. We have observed that the channel gains of these paths are very small.
• The LOS path and specular paths from smooth surfaces are not part of any cluster, as such landmarks have one or few associated paths. We have observed that the channel gains of these paths are very large (approximately following the path loss models from Section 2.3).

To solve these problems, we use the estimated channel gains. We partition ${\mathcal{P}}_{\mathrm{N}}$ into two sets according to the channel gains.

$$\begin{array}{cc}\hfill {\mathcal{P}}_{\mathrm{N},L}& =\{q:c({\widehat{\tau }}^p-\widehat{B})|{\widehat{g}}^p|\frac{4\pi }{\lambda }\le \delta \}\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {\mathcal{P}}_{\mathrm{N},H}& =\{q:c({\widehat{\tau }}^p-\widehat{B})|{\widehat{g}}^p|\frac{4\pi }{\lambda }>\delta \},\hfill \end{array}$$

(34)

where $\delta$ is a predefined threshold, $\widehat{B}$ is the estimated clock bias, and c is the speed of light. The scaling with $c({\widehat{\tau }}^p-\widehat{B})\frac{4\pi }{\lambda }$ is added to compensate for the path loss. For each of the high-gain paths in ${\mathcal{P}}_{\mathrm{N},H}$, we create a new cluster. For each of the low-gain paths in $p\in {\mathcal{P}}_{\mathrm{N},L}$, we find the nearest cluster $l^{*}$ as

$$\begin{array}{c}\hfill l^{*}=arg\underset{l}{min}\underset{q\in {\mathcal{P}}_l}{min}d({\widehat{\mathit{x}}}^p,{\widehat{\mathit{x}}}^q).\end{array}$$

(35)

and add p to cluster ${\mathcal{P}}_{l^{*}}$.

The overall computations in channel parameter clustering consist of three parts, that is, the computations in each phase. Given the number of paths $\widehat{P}$, the transition from 5D to 3D mapping requires $\mathcal{O}\left(\widehat{P}\right)$ computations. According to Reference [64], the complexity of the DBSCAN algorithm without the use of index structure for acceleration is $\mathcal{O}\left({\widehat{P}}^2\right)$. The last phase requires $\mathcal{O}\left(\widehat{P}\right)$ computations. Therefore, the overall complexity is $\mathcal{O}\left({\widehat{P}}^2\right)$.

6. Likelihood Function for SLAM

After clustering, the measurements ${\mathcal{Z}}_k$ are grouped into clusters ${\mathcal{Z}}_k=\{{\mathcal{Z}}_k^0,{\mathcal{Z}}_k^1,\dots ,{\mathcal{Z}}_k^{{\widehat{I}}_k-1}\}$, using Algorithm 1, where ${\widehat{I}}_k$ is the number of estimated clusters. The clustered measurements will be the input into the SLAM filter during the update process, via an appropriate likelihood function. For brevity, we omit the time index k again. We work with the compact representation of the landmark state from Section 2.2. Hence, our goal is to determine $\ell ({\mathcal{Z}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)$, where ${\mathit{x}}_{\mathrm{LM}}$ is the landmark position (i.e., the BS location or a virtual anchor location) and $m\in \{\mathrm{BS},\mathrm{SM},\mathrm{MR},\mathrm{VR}\}$ is the landmark state. This likelihood function will influence the probabilities of different associations between landmarks and clusters, and it will also influence the particle weight and the distribution of the landmark given a state $\mathit{s}$ and the association.

6.1. Background

In previous 5G SLAM works, Reference [21] assumed there is at most one measurement from each source; the channel gain is not used, and the inter-path interference is not considered. The measurements are only generated by known multivariate Gaussian distributions, then the likelihood function falls into the likelihood of the signal measurements, which follows the Gaussian format. According to the PMBM formalism, multiple measurements per source should follow a Poisson or Bernoulli distribution. Under the standard point model, the likelihood function corresponds to a Bernoulli distribution, with [65]

$$\begin{array}{c}\hfill \ell ({\mathcal{Z}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)=\left\{\begin{array}{cc}1-p_{\mathrm{D}}\hfill & {\mathcal{Z}}^i=\varnothing \hfill \\ p_{\mathrm{D}}p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)\hfill & {\mathcal{Z}}^i=\left\{{\mathit{z}}^{i,0}\right\}\hfill \\ 0\hfill & \mathrm{else},\hfill \end{array}\right.\end{array}$$

(36)

where $p_{\mathrm{D}}$ is the detection probability. Under the extended target model, the likelihood function corresponds to a Poisson point process (PPP) ([25], Equation (5)):

$$\begin{array}{c}\hfill \ell ({\mathcal{Z}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)=e^{-{\gamma }_m}{\gamma }_m^{|{\mathcal{Z}}^i|}\prod _{l=0}^{|{\mathcal{Z}}^i|-1}p({\mathit{z}}^{i,l}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m),\end{array}$$

(37)

where ${\gamma }_m\ge 0$ is the Poisson rate for surfaces of type m. For both (36) and (37), it has been proven that the PMBM density is a conjugate prior.

6.2. Likelihood Function

In this section, we describe the proposed likelihood function. We here assume that the measurements within each cluster ${\mathcal{Z}}^i$ are independent, as diffuse points are generated independently. We also assume the number of paths $|{\mathcal{Z}}^i|$ only depends on the source type m. It is important to note that for measurements from diffuse paths, a measurement is a function of a random incidence point on the surface. In order to express the measurements as a function of the compact state, we propose the following approach.

We first separate ${\mathcal{Z}}^i$ into two parts, the path with the shortest delay $\left\{{\mathit{z}}^{i,0}\right\}$, which is the closest path to the specular component, and any remaining paths ${\tilde{\mathcal{Z}}}^i=\{{\mathit{z}}^{i,1},{\mathit{z}}^{i,2},\cdots ,{\mathit{z}}^{i,|{\mathcal{Z}}^i|-1}\}$, which we view as diffuse multipath. We associate the deterministic incidence point ${\mathit{x}}^{i,0}$ with the assumed specular component, which can be derived by (31) using ${\mathit{x}}_{\mathrm{BS}}$ and ${\mathit{x}}_{\mathrm{LM}}$. Therefore, we can write $p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)=p({\mathit{z}}^{i,0}|{\mathit{x}}^{i,0},\mathit{s},m)$, for ${\mathit{z}}^{i,0}$, which is in the desired form (16). Hence

$$\begin{array}{c}\hfill \ell ({\mathcal{Z}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)=\left\{\begin{array}{cc}1-p_{\mathrm{D}}\hfill & {\mathcal{Z}}^i=\varnothing \hfill \\ p_{\mathrm{D}}p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)\ell ({\tilde{\mathcal{Z}}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)\hfill & \mathrm{else}.\hfill \end{array}\right.\end{array}$$

(38)

In other words, the object may be miss-detected with probability $1-p_{\mathrm{D}}$. If the object is detected, then the likelihood contains a contribution from the first arriving path (the specular path, which is directly related to the VA location or BS location) and from the set of all remaining paths.

Note that as a special case for $m=\mathrm{BS}$ there can be at most one associated measurement, so that

$$\begin{array}{c}\hfill \ell ({\mathcal{Z}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m=\mathrm{BS})=\left\{\begin{array}{cc}1-p_{\mathrm{D}}\hfill & {\mathcal{Z}}^i=\varnothing \hfill \\ p_{\mathrm{D}}p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)\hfill & {\mathcal{Z}}^i=\left\{{\mathit{z}}^{i,0}\right\}\hfill \\ ,0\hfill & \mathrm{else}.\hfill \end{array}\right.\end{array}$$

(39)

which is in the desired form. Hence, we can focus on the diffuse paths. As the measurements contain both a specular component and diffuse paths, the likelihood function does not follow the standard models (36)–(37). Nevertheless, we use the likelihood in the PMBM filter without any proof of conjugacy or optimality.

6.2.1. Likelihood for Diffuse Paths

We recall from Section 2.3 that diffuse multipath originates from random points ${\mathit{x}}^{i,l}$ on the surface and from Section 4 that the estimates points will differ from those from Section 2.3, due to the finite resolution of the receiver. Hence, it is fundamentally impossible to estimate the original random incidence points ${\mathit{x}}^{i,l}$ from the observations ${\tilde{\mathcal{Z}}}^i$. To avoid estimating ${\mathit{x}}^{i,l}$, we propose estimating an artificial incidence point${\tilde{\mathit{x}}}^{i,l}$ from ${\mathit{z}}^{i,l>0}$ as the projection of ${\widehat{\mathit{x}}}^{i,l}$ (obtained using the method in Section 5.2.1) onto the surface. The projection point ${\tilde{\mathit{x}}}^{i,l}$ can be derived by

$$\begin{array}{c}\hfill {\tilde{\mathit{x}}}^{i,l}={\widehat{\mathit{x}}}^{i,l}+{({\mathit{x}}_e-{\widehat{\mathit{x}}}^{i,l})}^{\mathsf{T}}\mathit{e}\mathit{e},\end{array}$$

(40)

where ${\mathit{x}}_e=\left({\mathit{x}}_{\mathrm{BS}}+{\mathit{x}}_{\mathrm{LM}}\right)/2$ is a point on the surface, and $\mathit{e}=\left({\mathit{x}}_{\mathrm{BS}}-{\mathit{x}}_{\mathrm{LM}}\right)/∥{\mathit{x}}_{\mathrm{BS}}-{\mathit{x}}_{\mathrm{LM}}∥$ is a normal to the surface. Then, $p({\mathit{z}}^{i,l}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)$ can be expressed in the desired form $p({\mathit{z}}^{i,l}|{\tilde{\mathit{x}}}^{i,l},\mathit{s},m)$, where ${\tilde{\mathit{x}}}^{i,l}$ is the artificial incidence point that gave rise to measurement ${\mathit{z}}^{i,l}$. The likelihood can be further simplified by noticing the error component of ${\tilde{\mathit{x}}}^{i,l}$, which is only due to the the projection distance of ${\widehat{\mathit{x}}}^{i,l}$ to the surface. We therefore use this distance directly as a compressed measurement to replace the delay and angles

$$\begin{array}{c}\hfill {\widehat{d}}^{i,l}\left({\mathit{z}}^{i,l}\right)={\mathit{e}}^{\mathsf{T}}({\mathit{x}}_e-{\widehat{\mathit{x}}}^{i,l}).\end{array}$$

(41)

Figure 5 shows the principle of calculating ${\widehat{d}}^{i,l}\left({\mathit{z}}^{i,l}\right)$.

We thus have a general model for the likelihood function associated with the diffuse paths, where the cardinality distribution is arbitrary and (cf. (Reference [18], Equation (1))

$$\begin{array}{cc}\hfill & \ell ({\tilde{\mathcal{Z}}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)=p(|{\tilde{\mathcal{Z}}}^i||m)|{\tilde{\mathcal{Z}}}^i|!\prod _{l=1}^{|{\tilde{\mathcal{Z}}}^i|}p([{\widehat{d}}^{i,l}\left({\mathit{z}}^{i,l}\right),{\widehat{g}}^{i,l}]|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m),\hfill \end{array}$$

(42)

in which all constituent distributions can be obtained from the simulation of a channel estimator or provided directly in closed-form by a channel estimator. In (42), the first term $p(|{\tilde{\mathcal{Z}}}^i||m)$ represents the cardinality distribution, $|{\tilde{\mathcal{Z}}}^i|!$ is a normalization constant to ensure that $\ell ({\tilde{\mathcal{Z}}}^i|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)$ integrates to one over ${\tilde{\mathcal{Z}}}^i$. The element in ${\tilde{\mathcal{Z}}}^i$ are represented in compressed format through $[{\widehat{d}}^{i,l}\left({\mathit{z}}^{i,l}\right),{\widehat{g}}^{i,l}]$ and are modeled as independent and identically distributed.

6.2.2. Clustering Errors and Marginal Likelihood Function

Due to clustering errors and channel estimator errors, it is possible that the specular path is not associated with the set of the specular paths. Then the shortest path in ${\mathcal{Z}}^i$ may be a diffuse path, leading to low likelihood $p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)$, so that the SLAM method is likely to view the source of this measurement cluster as a new landmark, which is not desirable. In order to solve this problem, we introduce $p_{\mathrm{m}}\in (0,1]$, which is the probability that the first path in a cluster is in fact the specular path. This probability can be determined by observation of the overall clustering performance. By marginalizing over the event of missing the specular path in a cluster, the likelihood function can be written as

$$\begin{array}{cc}\hfill p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)& =p_{\mathrm{m}}p({\mathit{z}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)\hfill \\ \hfill & +(1-p_{\mathrm{m}})\frac{p(|{\tilde{\mathcal{Z}}}^i|+1|m\left)\right(|{\tilde{\mathcal{Z}}}^i|+1)}{p(|{\tilde{\mathcal{Z}}}^i||m)}p([{\widehat{d}}^{i,0}\left({\mathit{z}}^{i,0}\right),{\widehat{g}}_k^{i,0}]|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m),\hfill \end{array}$$

(43)

where the factor $p(|{\tilde{\mathcal{Z}}}^i|+1|m\left)\right(|{\tilde{\mathcal{Z}}}^i|+1)/p(|{\tilde{\mathcal{Z}}}^i||m)$ is due to there being an additional specular path, which was not present in the cluster. The complexity of the likelihood function is $\mathcal{O}\left(|{\tilde{\mathcal{Z}}}^i|\right)$, as we consider $(|{\tilde{\mathcal{Z}}}^i|+1)$ paths.

7. Results

In the implementation, all the codes are written in MATLAB, and the simulations and experiments are run on a MacBook Pro (15-inch, 2019) with a 2.6 GHz 6-Core Intel Core i7 processor and 16 Gb memory.

7.1. Simulation Parameters

We consider a scenario with a single vehicle with the initial state ${[70.7285\mathrm{m},0\mathrm{m},0\mathrm{m},\pi /2\mathrm{rad},22.22\mathrm{m}/\mathrm{s},\pi /10\mathrm{rad}/\mathrm{s},1\mu \mathrm{s}]}^{\mathrm{T}}$, a BS located at ${[0\mathrm{m},0\mathrm{m},40\mathrm{m}]}^{\mathrm{T}}$, a SM, with the VA at ${[200\mathrm{m},0\mathrm{m},40\mathrm{m}]}^{\mathrm{T}}$, two MRs, with VAs at ${[0\mathrm{m},\pm 200\mathrm{m},40\mathrm{m}]}^{\mathrm{T}}$ and a VR, with the VA at ${[0\mathrm{m},-200\mathrm{m},40\mathrm{m}]}^{\mathrm{T}}$. The BS and the receiver are both equipped with a URA with $8\times 8$ antennas. The vehicle moves around the BS with a known constant turn rate. Every time step, the BS sends $10\times 64$ OFDM symbols to the vehicle with 200 subcarriers using the transmit power of 5.05 W at carrier frequency of 28 GHz. The subcarrier spacing is 0.5 MHz. The transmitted signals are disturbed by the noise with power spectral density of $4.0049\times {10}^{-9}$ mW/Hz. The transition function of the movement, the initial vehicle state, the landmark locations, the process noise, the initial prior, the survival probability, the detection probability, the birth rate, the clutter intensity, and pruning thresholds are the same as in Reference [23].

7.2. Channel Estimation Results

In this section, we show the results of the tensor ESPRIT channel estimator for a MR surface, which generated one specular path and 100 diffuse paths. To visualize the channel estimator output, we transform the estimated TOAs, AOAs, AODs into 3D points. The result is shown in Figure 6, where the original diffuse points are marked with blue dots and the estimated diffuse points with red discs. We observe that the channel estimator returns only 5 paths, much less than the original 101 paths, due to the finite resolution of the receiver. All projected points are close to the surface and there is a projected point very close to the deterministic reflection point. This is because the specular path of the MR has larger power than the other diffuse paths, so it is less affected by inter-path interference.

7.3. Clustering Performance Evaluation

To evaluate the clustering performance, we generate 40 snapshots of the channel with known landmarks. We then obtain the channel estimates with 5D geometric parameters, and transform them into 3D point positions. The DBSCAN clustering algorithm is applied to each snapshot, with $ϵ=30$ m and $N_{\min }=2$. Then we use the estimated channel gain to modify the results, with $\delta =0.25$. As a comparison, we also compare with other clustering methods, including the well-known K-means, gap statistics (‘GS’) [66], and affinity propagation (’AP’) [63] on the same data set. For the K-means, we assume that the number of clusters, K, is known, while for the remaining methods this priori knowledge is not needed.

To evaluate the clustering performance, we consider the following performance metrics, which use as ground-truth hand-labeled data: the clustering accuracy (CA), which is the percentage of correctly clustered points, and the impurity (IMP), which is the percentage of points clustered into a wrong cluster

$$\begin{array}{cc}\hfill \mathrm{CA}(\mathcal{G},\mathcal{C})& =\frac{{max}_{\gamma \in \mathsf{\Gamma }}\left(\widehat{P}+{\sum }_{(i,j)\in \gamma }(|{\mathcal{G}}_i\cap {\mathcal{C}}_j|-|{\mathcal{C}}_j|)\right)}{\widehat{P}}\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill \mathrm{IMP}(\mathcal{G},\mathcal{C})& =\frac{{\sum }_{k\in \{1,\cdots ,\widehat{N}\}}(|{\mathcal{C}}_k|-{max}_{m\in \{1,\cdots ,N\}}|{\mathcal{G}}_m\cap {\mathcal{C}}_k|)}{\widehat{P}},\hfill \end{array}$$

(45)

where $\mathsf{\Gamma }$ is the set of all possible assignments; $\mathcal{G}=\{{\mathcal{G}}_1,\cdots ,{\mathcal{G}}_N\}$ is the ground truth clusters; $\mathcal{C}=\{{\mathcal{C}}_1,\cdots ,{\mathcal{C}}_{\widehat{N}}\}$ is the cluster results provided by the clustering algorithm; $\widehat{P}$ is the number of all target points. The performance is presented in

Table 2. Performance comparison of different clustering methods.

Clustering Method	Clustering Accuracy	Impurity
Modified DBSCAN	99.61%	0
DBSCAN	99.07%	0
K-means	94.63%	5.37%
Gap Statistics (GS)	68.64%	27.19%
Affinity Propagation (AP)	84.35%	12.54%

. The proposed clustering method based on DBSCAN provides the best clustering performance, with an average clustering accuracy over 99%. The modified DBSCAN goes through all noise points using channel gain and refines the results, so it performs slightly better. K-means achieves an average accuracy of 94.63%. GS-based clustering method has an average accuracy of only 68.64% since the estimated value of the number of clusters can be erroneous. The AP clustering method has an average accuracy of 84.35% since it still based on space partition and the estimated value of the number of clusters can be erroneous either. In terms of impurity, the last three algorithms cluster measurements from different sources together when errors occur, but for DBSCANs this never happens, so that all points in each cluster correspond to one landmark. By observing the clustering results of modified DBSCAN, losing the shortest path from the same landmark happens rarely, and we have estimated $p_m\approx 1/200$.

Though the K-means algorithm shows good average clustering performance, it suffers from a few limitations as indicated in Section 5.1. This can be demonstrated in Figure 7, where we present the clustering results with DBSCAN and K-means with the data from the 33rd snapshot. We use different colors to indicate the different clusters obtained from the corresponding clustering methods. The proposed DBSCAN can correctly cluster the measurements into the right clusters shown in Figure 7a; however, K-means shows unstable clustering performance indicated in Figure 7b–d. This is because the clustering performance of K-means is highly dependent on the initialization of the partitions. In addition, we observe from Figure 7b,d that in K-means, the data points from the same cluster can be divided into separated clusters, and the points from different clusters can be clustered in a single cluster. All these mismatches will cause performance degradation in SLAM, which will be further demonstrated in Section 7.5.

7.4. Estimated Likelihoods

Given the channel estimation and the clustering performance, we now describe the obtained likelihood functions. For simplicity, we consider all dimensions independent and report the observed distributions of the number of paths and error of the specular path gain, delay and 4 angles. For diffuse paths, we report the distribution of the distances ${\widehat{d}}^{i,l}$ and the channel gains. All these distributions are based on gathered channel estimation and clustering results in various vehicle locations. We use the setting in Section 7.1, and run the channel estimator many times, collect all channel estimation results and study statistics of collected data. Since the channel gains are strongly affected by path loss, (4)–(7) reduce the impact of the distance dependence by considering

$$\begin{array}{c}\hfill {\check{g}}^{i,l}\left[\mathrm{dB}\right]=20{log}_{10}\left(|{\widehat{g}}^{i,l}|∥{\mathit{x}}_{\mathrm{UE}}-{\mathit{x}}_{\mathrm{LM}}∥\frac{4\pi }{\lambda }\right).\end{array}$$

(46)

We first focus on the case $m=\mathrm{MR}$ as an example, and study $p(|{\tilde{\mathcal{Z}}}^i||\mathrm{MR})$, $p(c{\tau }^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},\mathrm{MR})$, $p({\varphi }_{\mathrm{el}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},\mathrm{MR})$, $p({\check{g}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},\mathrm{MR})$, $p({\widehat{d}}^{i,l}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},\mathrm{MR})$ and $p({\check{g}}^{i,l>0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},\mathrm{MR})$ in Figure 8. Figure 8a shows the histogram of the number of paths, as well as a Poisson approximation. We observe that there are always a (assumed) specular path and 1 to 5 diffuse paths, and the existing of more than one diffuse path follows a Poisson distribution. Figure 8b shows the histogram of the delay error of the first estimated path ${\tau }^{i,0}$ (i.e., subtracted with the delay of the specular path and multiple by the speed of light c) as well as a Gaussian fit. We observe that there is a shift of delay of 0.17 m, which is caused by inter-path interference. Figure 8c shows the histogram of the elevation AOD error of first estimated path ${\varphi }_{\mathrm{el}}^{i,0}$ (i.e., subtracted with the angle of the specular path) as well as a Gaussian fit. We observe that inter-path interference does not influence the angle so much. Figure 8d shows the histogram and Gaussian fit of the distances ${\widehat{d}}^{i,l},l>0$. As the inter-path interference leads to a positive delay shift, the estimated diffuse points are more likely to be behind the surface. Finally, Figure 8e,f shows the histogram of ${\check{g}}^{i,0}$ and ${\check{g}}^{i,l>0}$ as well as Gaussian fits. We observe that the specular path is stronger than diffuse paths, as the mean of ${\check{g}}^{i,0}$ is larger, and diffuse paths are influenced by inter-path interference more seriously, as the distribution of ${\check{g}}^{i,l>0}$ is more spread.

Table 3. Likelihood function for 5G SLAM for different types of landmarks.

Type m	$\mathit{p}(|{\tilde{\mathcal{Z}}}^{\mathit{i}}||\mathit{m})$	$\mathit{p}([{\widehat{\mathit{d}}}^{\mathit{i},\mathit{l}};{\check{\mathit{g}}}^{\mathit{i},\mathit{l}}]|{\mathit{x}}_{\mathbf{LM}},\mathit{s},\mathit{m})$
BS	$(|{\tilde{\mathcal{Z}}}^i{|-1)}^{*}\sim 0.9{\Delta }_0+0.1\mathrm{Geo}\left(0.9\right)$	N/A
SM	$|{\tilde{\mathcal{Z}}}^i|\sim 0.9{\Delta }_0+0.1\mathrm{Geo}\left(0.9\right)$	N/A
MR	$(|{\tilde{\mathcal{Z}}}^i|-1)\sim 0.1{\Delta }_{-1}+0.9\mathrm{Poi}\left(1.5\right)$	$\mathcal{N}([{\widehat{d}}^{i,l};{\check{g}}^{i,l}];[0.48;-14.1],\mathrm{diag}\left({[0.35,3.8]}^2\right)$
VR	$(|{\tilde{\mathcal{Z}}}^i|-3)\sim 0.1{\sum }_{n=1}^3{0.5}^{n-1}{\Delta }_{-n}+0.825\mathrm{Geo}\left(0.27\right)$	$\mathcal{N}([{\widehat{d}}^{i,l};{\check{g}}^{i,l}];[0.48;-9.4],\mathrm{diag}\left({[0.35,3.8]}^2\right)$
	$p({\check{\mathit{z}}}^{i,0}|{\mathit{x}}_{\mathrm{LM}},\mathit{s},m)$
BS	$\mathcal{N}({\check{\mathit{z}}}^{i,0};[{\mathit{h}}_{\mathrm{VA}};0],\mathrm{diag}\left({[0.003,0.0001\times {\mathbf{1}}_4,0.005]}^2\right))$
SM	$\mathcal{N}({\check{\mathit{z}}}^{i,0};[{\mathit{h}}_{\mathrm{VA}};-1.93],\mathrm{diag}\left({[0.01,0.002\times {\mathbf{1}}_4,0.02]}^2\right))$
MR	$\mathcal{N}({\check{\mathit{z}}}^{i,0};[{\mathit{h}}_{\mathrm{VA}};-3.1]+{[0.17,{\mathbf{0}}_{1\times 5}]}^{\mathrm{T}},\mathrm{diag}\left({[0.19,0.012\times {\mathbf{1}}_4,2.3]}^2\right))$
VR	$\mathcal{N}({\check{\mathit{z}}}^{i,0};[{\mathit{h}}_{\mathrm{VA}};-6.3]+{[0.8,{\mathbf{0}}_{1\times 5}]}^{\mathrm{T}},\mathrm{diag}\left({[0.5,0.05\times {\mathbf{1}}_4,3.8]}^2\right))$

* $|Z|-M\sim p{\Delta }_n+(1-p)\mathrm{Geo}\left(\gamma \right)$ means that $|Z|$ is equal to M plus a random variable drawn from a discrete mixture distribution with two components: one with mass p at value n and one that is a scaled Geometric distribution (with scaling $1-p$). $\mathrm{Poi}\left(\gamma \right)$ represents a Poisson distribution. ${\check{\mathit{z}}}^{i,0}$ denotes $[c{\tau }^{i,0};{\mathit{\theta }}^{i,0};{\mathit{\varphi }}^{i,0};{\check{g}}^{i,0}\left[\mathrm{dB}\right]]$. ${\mathit{h}}_{\mathrm{BS}}$, ${\mathit{h}}_{\mathrm{VA}}$ are the geometric relations $\mathit{h}({\mathit{x}}_{\mathrm{BS}},\mathit{s})$ and $\mathit{h}({\mathit{x}}_{\mathrm{VA}},\mathit{s})$, which can be found in Appendix A.

provides a complete overview of the likelihood function for all landmark types. Based on all components of the likelihood functions for all landmarks, we have the following observations: for both cases $m=\mathrm{BS}$ and $m=\mathrm{SM}$, there is always one path presents and all the angles, delays and ${\check{g}}^{i,0}$ follow Gaussian distributions. For case $m=\mathrm{MR}$, apart from the assumed specular path, there are 3 to 12 more paths present, and angles, delays, ${\check{g}}^{i,l}$ and ${\widehat{d}}^{i,l}$ also follow Gaussian distributions. We also find that the smoother surface has a higher gain for the specular path and lower gain for the diffuse path.

However, since the clustering algorithm is not perfect, it is possible that the cluster associated with a landmark may have fewer or more paths than the ground truth cluster. For example, the cluster associated with the MR may only have a specular path, due to the clustering error. We therefore set those possibility none zero in $p(|{\mathcal{Z}}^i||m)$ manually, in order to increase the tolerance of the wrong clustering, as shown in Table 3.

7.5. SLAM Performance Evaluation

7.5.1. Mapping Performance

Firstly, we study the performance of the proposed 5G SLAM scheme in mapping, conditioned on the true vehicle state. We use the generalized optimal subpattern assignment (GOSPA) distance [67] as the metric to evaluate the mapping result. The GOSPA metric considers the estimation error, which is the error between the estimated positions with the real positions of landmarks, the miss-detection error, which is the error of miss-detecting the existing landmarks, and the false alarm error, which is the error of detecting non-existing landmarks. The GOSPA between the estimate landmark position set $\widehat{\mathcal{X}}={\left\{{\widehat{\mathit{x}}}_i\right\}}_{i=1}^{\widehat{N}}$ and the real landmark position set $\mathcal{X}={\left\{{\mathit{x}}_j\right\}}_{j=1}^N$ is defined as

$$\begin{array}{cc}\hfill & d_{\mathrm{GOSPA}}(\widehat{\mathcal{X}},\mathcal{X})=\underset{\gamma \in \mathsf{\Gamma }}{min}{\left(\sum _{(i,j)\in \gamma }{d}^{q_c}({\widehat{\mathit{x}}}_i,{\mathit{x}}_j)+\frac{q_c^{q_p}}{q_a}(N_{\mathrm{miss}}^{\gamma }+N_{\mathrm{false}}^{\gamma })\right)}^{\frac{1}{q_p}}\hfill \end{array}$$

(47)

where $\mathsf{\Gamma }$ is the set of all possible assignments; $N_{\mathrm{miss}}^{\gamma }$ is the number of miss detection of assignment $\gamma$; $N_{\mathrm{false}}^{\gamma }$ is the number of false alarm of assignment $\gamma$. The cardinality penalty factor $q_a$, the cut-off distance $q_c$, and the exponent factor $q_p$ are set as 20, 2, and 2, respectively.

We make two comparisons: (i) we compare different clustering methods (SLAM filter using modified DBSCAN which is the proposed method, DBSCAN without channel gains, and K-means); (ii) we compare SLAM filters with different numbers of paths (using all paths in every signal cluster based on the proposed likelihood function, which is the proposed method; using all paths but without using channel gains [24]; using the single (specular) path in every cluster based on the proposed likelihood function; using the single (specular) path in every cluster but without using channel gains, which is the PMBM extension of Reference [23]).

Figure 9 shows the outcome of the first comparison. We observe that the SLAM filter with K-means clustering performs poorly, even though the number of clusters is already known. Although the K-means performance in Table 2 seems good, grouping measurements from different sources leads to significant errors. The reason is that K-means only divides the space linearly and cannot handle complicated shapes. It often groups projected points from different sources into the same clusters; for example, at time step 17, it groups the measurements from BS and SM together. Modified DBSCAN and simple DBSCAN perform much better, as the blue and red lines are much lower, since both of them never group measurements from different sources together. The tensor ESPRIT channel estimator may create measurements that are far away from the specular path, and have low gains but still contain location information. Treating those outliers individually as single-element clusters in DBSCAN loses some information, and also forces the SLAM filter to create a new landmark, when similar outliers appear in successive time steps. This is apparent in the peak at time step 37–39 in Figure 9a, since there are some clutters pointing to a similar region between time step 36–39. With the help of the channel gains, the modified DBSCAN groups those measurements into the right clusters, which helps in mapping. That is the reason why the blue line is lower than the red line, for example at time step 2 in Figure 9c and time step 3 in Figure 9d.

Figure 10 shows the comparison of the GOSPA results among 4 different settings (with or without using diffuse multipath, with or without using channel gain). We observe that SLAM filter has better performance when using diffuse multipath, as the solid lines are lower than the dashed lines, and it can also help the system to identify the landmark types, as there are no sharp peaks in solid lines, which is because more paths provide more information. We also observe the gain can improve the performance, especially in helping the SLAM filter to recognize the surface type, as the blue lines are lower than the red lines. Without information from channel gains, the SLAM filter has trouble to distinguish SM and MR when only using the specular path, as indicated by the sharp peaks of red dashed lines in Figure 10b,c and the red solid line dropping more slowly than the blue solid line in Figure 10b. This is because the distributions of delays and angles of specular paths from SM and MR are very close to each other, and some disturbance may cause a wrong identification. The information from channel gains will alleviate this problem. However, the channel gain is usually disturbed by inter-path interference, which increases with the roughness of the surface. Therefore, the gain is more informative for paths from SM and MR, but less informative for paths from VR, so the blue dashed lines are much lower than the red dashed lines in Figure 10b,c, but only slightly lower than the red dashed line in Figure 10d.

7.5.2. Localization Performance

Next, we study the performance of the proposed 5G SLAM scheme in vehicle state estimation and compare the estimation results among the SLAM filter with different settings (with or without using diffuse multipath, with or without using channel gain). The landmarks are unknown and are mapped with tracking the vehicle state. We add ${[0.9\mathrm{m},0.9\mathrm{m},0\mathrm{m},4.5\mathrm{deg},0\mathrm{m}/\mathrm{s},0\mathrm{rad}/\mathrm{s},3\mathrm{ns}]}^{\mathrm{T}}$ bias to the initial state, and the initial covariance is set as $\mathrm{diag}\left({[0.3\mathrm{m},0.3\mathrm{m},0\mathrm{m},0.1rad,0\mathrm{m}/\mathrm{s},0\mathrm{rad}/\mathrm{s},1\mathrm{ns}]}^2\right)$. We use 2000 particles to represent the vehicle state, and obtain the mean absolute error (MAE) between the real vehicle state and the estimated vehicle state after time step 5 until the simulation ends at time step 40

$$\begin{array}{c}\hfill \mathrm{MAE}=\frac{{\sum }_{k=6}^{40}|{\mathit{s}}_k-{\widehat{\mathit{s}}}_k|}{35},\end{array}$$

(48)

as shown in Figure 11. The clock bias is multiplied with the speed of light c for evaluation. Overall, when using diffuse multipath, the SLAM filter has better performance in positioning, as MAEs are lower. The reason is that diffuse multipath provides more information than the single path. Using channel gain also improves the positioning performance, especially very helpful to estimate the clock bias, since gain contains information about propagation distance directly without being affected by the clock bias.

7.5.3. Complexity Evaluation

We also evaluate the complexity of the proposed framework by measuring the execution time for each phase—channel estimation, clustering, mapping, SLAM (per particle), as shown in Figure 12. We measured a runtime per time step of 8.12 s for the channel estimation. The reason for taking a long time is that the ESPRIT channel estimator is based on a high dimensional tensor decomposition with high complexity. We measured the modified DBSCAN takes 0.21 ms per snapshot. For mapping, it takes 110 ms per time step. Although the proposed SLAM filter considers all possible data associations, as there are not too many landmarks under the simulation scenario, the number of all possible data associations is not large. However, the runtime will increase with more data associations. For SLAM, the proposed SLAM filter takes 149 ms per each particle (298.52 s in total) per time step. As 2000 particles are used and we need to fuse maps of all particles, the complexity is much higher than mapping, where the true vehicle state is given.

8. Conclusions

In this paper, we have treated the 5G SLAM problem from an end-to-end perspective, including downlink data transmission, channel estimation, clustering, and the SLAM filter. In the 5G SLAM problem, we aim to localize and synchronize a user while mapping the propagation environment, with the help of downlink signals from a single base station. We have proposed a novel method to cluster the MPCs by projecting the high-dimensional data into 3D points and then cluster the points based on the DBSCAN algorithm, which we augmented to account for the channel gains. We have also proposed a novel likelihood function in the 5G SLAM filter, which accounts for both the specular path as well as the diffuse multipath components.

Our results show that the ESPRIT channel estimator can estimate the channel parameters of both specular and diffuse multipath, and that the proposed system can directly use the raw un-clustered channel estimation results by applying the proposed clustering algorithms. With the help of the novel likelihood function, the proposed scheme can accurately estimate the number of landmarks, their types (i.e., roughness), and positions, and the channel gain is helpful in clustering and mapping and positing. The results also confirm that the proposed method can handle mapping and vehicle state estimation simultaneously, and highlight the benefit of considering both specular and diffuse multipath. In addition, the channel gains turn out the be highly informative for synchronizing the user to the base station.

The proposed framework has two computational bottleneck—the ESPRIT channel estimator and the particle filter used in the PMBM SLAM. In order to enable real-time execution, there is a need for faster solutions for both the channel estimation and the SLAM filter. The solutions could be either in the form of new algorithms, or by offloading the computation to more powerful edge computing systems, where edge servers can provide high-performance computing capability closer to end users [68,69,70].