Expectation–Maximization-Based Simultaneous Localization and Mapping for Millimeter-Wave Communication Systems

In this paper, we proposed a novel expectation–maximization-based simultaneous localization and mapping (SLAM) algorithm for millimeter-wave (mmW) communication systems. By fully exploiting the geometric relationship among the access point (AP) positions, the angle difference of arrival (ADOA) from the APs and the mobile terminal (MT) position, and regarding the MT positions as the latent variable of the AP positions, the proposed algorithm first reformulates the SLAM problem as the maximum likelihood joint estimation over both the AP positions and the MT positions in a latent variable model. Then, it employs a feasible stochastic approximation expectation–maximization (EM) method to estimate the AP positions. Specifically, the stochastic Monte Carlo approximation is employed to obtain the intractable expectation of the MT positions’ posterior probability in the E-step, and the gradient descent-based optimization is used as a viable substitute for estimating the high-dimensional AP positions in the M-step. Further, it estimates the MT positions and constructs the indoor map based on the estimated AP topology. Due to the efficient processing capability of the stochastic approximation EM method and taking full advantage of the abundant spatial information in the crowd-sourcing ADOA data, the proposed method can achieve a better positioning and mapping performance than the existing geometry-based mmW SLAM method, which usually has to compromise between the computation complexity and the estimation performance. The simulation results confirm the effectiveness of the proposed algorithm.


Introduction
Recently, millimeter-wave (mmW) communications are considered key ingredients to achieve multiple Gbps link rates in 5G-and-beyond networks [1]. Benefiting from the wide bandwidth of the mmW communication system and the small antenna form given by the millimeter wavelength, mmW communication devices can achieve a high resolution in both the path-delay domain and the path-angle domain [2,3]. Moreover, the mmW propagation occurs in quasi-optical propagation pattern and multipath sparsity due to its low scattering effects [4,5]. Therefore, the mmW communication systems can provide great potential for achieving high positioning accuracy due to the multipath sparsity and the high temporal and spatial resolution of the mmW multipath components (MPCs) [3], which also synergizes in turn with mmW communications in fast beamforming to overcome the high attenuation [6,7].
However, the mmW MPCs-based positioning is severely subject to not only the unknown positions of the MPC sources, which are also the potential features characterizing the environment map, including the reflection mirrors of the physical access point (AP) and the scattering points, but also the uncertain data associations of the MPC measurements to the sources [8,9]. As a viable solution addressing the abovementioned challenges, the multipath-based SLAM (simultaneous localization and mapping) technique can detect and localize the scattering points and the reflective flat surfaces represented by virtual APs and jointly estimate the time-varying positions of the mobile agents. The existing mmW SLAM methods include geometry-based methods, the BP (Belief Propagation)-based methods and RFS (Random Finite Set)-based methods as follows.
The geometry-based methods usually assume that the data association of MPC measurements is realized by the full beam training procedure, which is commonly used in mmW communication systems. They gather the MPC measurements at a large number of random epochs, rather than successive epochs, then localize the agent positions as well as the AP (virtual or physical APs) positions by solving the over-determined geometric equations, corresponding to the geometric relationship between such MPC parameters and the positions of the APs and the MT (mobile terminal) [3,8,10]. Hence, these geometry-based methods can be simply applied to the crowd-sourcing data of the MPC measurements at random time slots, whereas they still have to compromise between low complexity and high information utilization because a large number of the MPC measurements are gathered to accumulate enough information. Based on the angle difference of arrival (ADOA), the JADE (Joint Anchor and Device location Estimation) algorithm in [3] decomposes the NP-hard joint estimation of the AP positions and random MT positions into iterations of two successive LS (least-squares) estimations on the AP positions and random MT positions, whereas such a decomposition requires the relaxation of the geometric constraint on the AP positions, random MT positions and the ADOA measurements; thus, it suffers from losing the partial information contained in the ADOA measurements. In addition, the CLAM (communication-driven localization and mapping) method in [11] estimates the locations of the APs (including physical and virtual APs) through solving a minimal number of equations between the ADOA measurements and the AP shape, which are found offline through automatic expression manipulation techniques, then employs an error-resilient version of the ADOA localization algorithm to estimate the MT position. However, this CLAM method does not make full use of the entire relationships among the ADOA measurements due to avoiding the prohibitively high computational complexity, thus also suffering from performance loss.
By regarding the multipath parameters in successive time slots as measurements of an interacting multiple model (IMM), the BP-based SLAM methods usually employ the factor-graph scheme, which decomposes the high-dimensional joint posterior estimation into recursive low-dimensional posterior estimations, to jointly perform a probabilistic data association and sequential Bayesian estimation of the states of an MT and the potential virtual APs characterizing the map [9,[12][13][14][15]. Hence, such BP-based SLAM methods can use not only the multipath information but the time evolution of the MPC parameters in an online manner, thus achieving a better SLAM performance with a relatively low complexity. For example, by modeling the mmW SLAM system as a Bayesian framework with the dynamics state and observation functions, the jointly tracking and mapping method in [13] sequentially estimates the positions of the moving MT and the time-invariant potential features by the factor-graph scheme and provides hard decisions regarding the associations of measurements to sources at each epoch. The proposed BP-based algorithm in [9,14] adapts to time-varying system models by jointly inferring the model parameters along with the MT and map-feature states, by representing the time evolution of the IMM parameters as a Markov chain and incorporating the parameters into the factor-graph problem. However, the strict requirements for the IMM statistical prior and the consecutive MPC parameters limit their application on the available crowd-sourcing MPC measurements.
Furthermore, several RFS (Random Finite Set)-based methods have been proposed for the mmW SLAM [8,[16][17][18]. These methods model the potential features corresponding to multipaths at successive epochs as a time-varying RFS with uncertain cardinality and state, then recursively update the posterior of the potential features set and the MT state by using the RFS theory, thus realizing the SLAM. However, such RFS-based methods suffer from an extremely high computation complexity due to the high-dimensional set and the rigorous assumption on the multipath statistical model.
In this paper, a novel expectation-maximization (EM)-based SLAM algorithm has been proposed for mmW systems. Similar to the geometry-based SLAM methods, the proposed algorithm assumes that the data association of the MPC measurements is realized by exploiting the characteristics of the MPCs' AOD and gains in the full beam training procedure and can be readily applied to the available crowd-sourcing MPCs' measurement data because it does not require the MPC measurements obtained at successive epochs. By regarding the MT positions as the latent variable of the AP positions, the proposed algorithm reformulates the SLAM problem as a maximum likelihood (ML) joint estimation problem of both the AP positions and the MT positions in a latent variable model; thus, it first employs an efficient stochastic approximation EM method to estimate both the AP positions and the MT positions and finally constructs the indoor map based on the estimated AP topology. Due to the efficient processing capability of the stochastic approximation EM method and taking full advantage of the abundant spatial information in the crowd-sourcing ADOA data, the proposed method can achieve a better positioning and mapping performance than the geometry-based mmW SLAM method, which usually has to compromise between the computation complexity and the estimation performance.
The main contributions of this paper are as follows: (1) By regarding the MT positions as the latent variable of the AP positions, the mmWave MPC ADOA-based SLAM problem is formulated as the ML joint estimation over both the AP positions and the MT positions in a latent variable model, so that the classical EM scheme can be employed to efficiently solve such an NP-hard estimation problem. (2) The stochastic approximation EM-based SLAM algorithm is developed to achieve both the better performance and the high computational efficiency. Due to the intractable analytical form of the MT position posterior probability, the stochastic Monte Carlo approximation, with a single sample drawn for each random MT position, is adopted in the E-step to approximately compute the expected statistics. In addition, the AP positions are then updated by the gradient descent-based optimization in the M-step, which monotonically increase the likelihood of the MT position samples with low complexity rather than maximizing it. (3) The simulation results demonstrate that the proposed method can achieve a better performance than the existing geometry-based mmW SLAM method due to the efficient processing capability of the stochastic approximation EM method and taking full advantage of the abundant spatial information in the crowd-sourcing ADOA data.
The rest of this paper is organized as follows. In Section 2, the related SLAM works are briefly investigated. In Section 3, the virtual AP-based system model is described and the multipath ADOA vector is defined. By exploiting the geometric ADOA relationship in the virtual AP-based system, a novel ADOA and EM-based SLAM method is developed to jointly estimate both the AP positions and the MT positions in Section 4. The simulation results are presented in Section 5 to evaluate the performance of the proposed method. Finally, conclusions are drawn in Section 6.

Visual SLAM Methods
Over the past decades, a great deal of research efforts has been devoted to visual SLAM, which is usually based on the information from ubiquitous optical sensors, commonly considered as cameras equipped by robots. The visual SLAM methods usually split the system into tracking tasks and mapping tasks [19][20][21][22] and fulfills them by aligning the extracted sparse features or the dense pixels against the probabilistic models [19,20] or the dense pixel models [21,22]. Although the existing visual SLAM methods have achieved a satisfactory performance and considerable complexity, such visual SLAM methods still suffer from some practical limitations: (1) Under some situations, such as nights or complex environments with blockages, the clear images of the surrounding environments are not easy to obtain; (2) Due to the privacy issue, the users may be unwilling to share the image data openly; (3) In the scenario with a massive number of mobile agents, the cost of extra optical equipment is unaffordable; thus, not all the mobile agents can be equipped with optical sensors.
Meanwhile, the mmW communication technology in 5G-and-beyond networks brings significant advantages to the multipath-assisted SLAM due to their large bandwidth and beamforming capability. This means a higher resolution in the delay and angular domains can be achieved; thus, efficiently resolving and identifying MPCs can provide great potential for achieving a better positioning and mapping accuracy [3]. Hence, the traditional visionbased SLAM methods are limited in multiple practical scenarios, which can be overcome with wireless mmW multipath-based SLAM solutions.

EM SLAM Methods
As a classic and popular ML method with a low computation complexity, the EM scheme has been usually revitalized in different SLAM scenarios, such as in [23][24][25]. Due to different measurements and different scenarios, the SLAM problem is formulated as different system models; thus, the proposed EM SLAM method and the existing EM-SLAM methods in [23][24][25] are still quite different from each other. The main differences between the proposed EMbased SLAM method and the existing EM-SLAM methods in [23][24][25] are listed as follows.
(a) Different system models In [23][24][25], the SLAM problem is similarly formulated as the hidden Markov models (HMMs) with a finite number of states and observations. Specifically, based on continuous observations of a moving trajectory, the MT positions are formulated as the hidden Markov states and the landmarks' positions are regarded as the parameters to be estimated, and the EM scheme is employed for such HMMs to estimate both the MT positions and the landmarks' positions.
However, the SLAM problem in our method is formulated as the hidden-variable models, instead of HMMs, based on crowd-sourcing observations at random positions, rather than continuous observations of a moving trajectory. By regarding the MT positions as the latent variable of the AP positions, the proposed algorithm reformulates the SLAM problem as the ML joint estimation over both the AP positions and the MT positions in a latent variable model.

(b) Different ways of implementing the E-step
Due to adopting the HMM, the EM-SLAM methods in [23,24] employ the sequential Monte Carlo approximation scheme to obtain the expectation of the probability function, while the EM-SLAM method in [25] uses the Extended Kalman Filtering (EKF) scheme to estimate such an expectation recursively.
On the other hand, our proposed method treats the random MT positions as the latent variable, not the hidden Markov variable, and employs the Monte Carlo approximation to obtain the expectation of the probability function in the E-step. Specifically, considering that a large number of random MT positions involve a prohibitively huge computational complexity in the E-step, we draw a single particle from the posterior of the latent variable for each random MT position, rather than a large number of particles for each latent variable in [23,24], then compute the expected sufficient statistics.
In addition, due to different measurements in different SLAM scenarios, the different observation functions are used in our method and the EM-SLAM methods in [23][24][25]. In order to further simplify the computation of the expectation in the E-step and/or the optimization in the M-step, the methods in [23,25] employ a first-order Taylor expansion to approximate the nonlinear observation model, while the proposed method and the method in [24] directly compute the expectation without any approximation on the nonlinear observation model.

(c) Different ways of implementing the M-step
According to the different ways of implementing the E-step, the EM-SLAM methods in [23,24] employ the explicit maximization scheme to estimate the parameters in the M-step, and the EM-SLAM methods in [25] employ the quasi-Newton minimization method.
Our proposed method uses the gradient descent-based optimization for the APs' positions as a viable substitute for estimating the APs' positions in the M-step, which is much more computationally efficient. Although the current optimal AP positions estimates are not exactly obtained in each M-step, such gradient descent-based optimization can monotonically increase the log likelihood of the APs' positions rather than maximizing it.

(d) Different requirements for initial values of the parameters
Because the EM-SLAM methods in [23,25] use a first-order Taylor expansion to approximate the nonlinear observation function, they need an initial value of the parameters to perform the EM iterations, which is also important for their performance. However, the proposed method and the EM-SLAM method in [25] do not have any requirements for the initial values of the parameters, which is more feasible in practical scenarios.
For clarity, the main differences between the proposed method and the existing EM-SLAM methods in [23][24][25] are listed in the following Table 1.

System Model
Consider a two-dimensional indoor scenario (shown in Figure 1) in which a single mmW AP, denoted as AP 1 , is deployed and its location a 1 is unknown. Due to the high attenuation and the quasi-optical propagation of mmW signals in the air [4], only the direct and first-order reflection signal are taken into account as in [3,11] because non-Lineof-Sight (NLOS) paths after higher order of bounces experience enormous attenuation and can be ignored. The first-order reflection NLOS paths can be viewed as the 'direct' path from the virtual APs, which are mirrors of first-order reflection of the physical AP through indoor surfaces (such as indoor walls). Denote such virtual APs as AP l (l = 2, 3, . . . , L) with unknown positions a l (l = 2, 3, . . . , L) and denote the set of all APs as A = {AP 1 , AP 2 , · · · , AP L }, where L is the number of APs.
At each position, the MT leverages beam training information, which is available at mmW communication MTs, to compute the angle difference of arrival (ADOA) between the multipath from every AP pair (including the physical AP or virtual APs). Denote the ADOA for the AP pair {AP l 1 , AP l 2 } at position p n as θ l 1 ,l 2 (p n ). According to the properties of analytic geometry, the ADOA θ l 1 ,l 2 (p n ) satisfies: where • denotes dot-product operation. From (1), the measured ADOA vector,θ(p n ) = [θ 1,2 (p n ),θ 2,3 (p n ), · · · ,θ L−1,L (p n )] T , at position p n for the AP pair {AP l 1 , AP l 2 } can be expressed as: with θ(p n ) = [θ 1,2 (p n ), θ 2,3 (p n ), · · · , θ L−1,L (p n )] T and W(n) =[W 1,2 (n), · · · , W 2,3 (n), · · · , W L−1,L (n)] T denoting an additive zero-mean Gaussian noise vector with covariance matrix σ = diag[σ 2 1,2 , · · · , σ 2 2,3 , · · · , σ 2 L−1,L ]. Further, we collect the ADOA observation vectors at a large number of random and unknown positions (p n ) N n=1 and set up a dataset of ADOA vectors as Q = {θ(p n )} N n=1 . From (1), such defined ADOA vector is inherently immune to orientation biases and completely dependent on positions of the MT and APs; thus, the ADOA vector corresponding to each position is unique and can be collected from different MTs at this position. In addition, the ADOA acquisition only requires the beam training information, which is available and necessary in mmW communication systems. Hence, the setup of such an ADOA observation vector dataset can be handily accomplished in an offline crowd-sourcing manner or even online in mmW communication systems. Specifically, the ADOA observation vectors can be collected or reported from different MTs at random positions whenever they are available in the indoor scenario, rather than from a single MT at only positions on its current trajectory in online manner, and then these ADOA observation vectors can be stored as elements of a dataset in offline manner.
In indoor environments, the main path number or the AP number L is usually larger than 3, which implies that the ADOA vector is redundant for estimating the 2-D MT position when the AP positions are known. Hence, both the AP positions and the MT positions can be estimated simultaneously based on collected ADOA vectors at a large number of random positions, because the ADOA vectors contain sufficient redundant information for an extra estimation of the AP positions.
Therefore, the problem of mmW simultaneous localization and mapping for mmW communication systems in an unknown indoor environment, i.e., jointly estimating the geo-metric topology of APs (including both physical APs and virtual APs) and the MT location, can be reformulated as the following maximum likelihood joint estimation problem: where p(·) denotes the probability function, and the semicolon separates the unknown parameters, including virtual sensors' positions a 3:L and the MT positions p 1:N to be estimated from the first two APs' coordinates a 1:2 , which are assumed fixed to resolve the estimation ambiguity of rotation, translation and scaling. Note that although two-dimension scenarios are assumed in the system model above and the proposed method in the following section, the system model and the proposed method can be extended directly to three-dimension scenarios.

Algorithm
It is obvious that the estimation problem in (3) is an NP-hard problem considering that both APs' positions (including both physical APs and virtual APs) and the MT positions are to be jointly estimated. In order to solve this problem, a novel EM (expectationmaximization)-based AP topology estimation method is first proposed in this section. The abovementioned probability maximization problem of the latent variable model in (4) can be efficiently solved by employing the classical EM method [26]; thus, the original ML joint estimation in (3) can be decomposed into 2 feasible steps: (1) estimating the APs' positions a 3:L through the EM method, (2) estimating the MT's positions based on the estimated physical and virtual APs' positions. The EM-based virtual sensors' positions estimation is first discussed hereinafter.

EM-Based AP Topology Estimation
Considering the EM-based virtual sensors' positions estimation is composed of T similar iterative steps, only the t-th iteration is exemplified as follows. E-Step (Estimation Step): Given the virtual sensors' positions estimateâ t−1 3:L in the (t − 1)-th iteration, the distribution of the MT's positions p 1:N at the t-th iteration, q t (p 1:N ), can be chosen as where E q t [·] denotes the expectation operation with respect to the distribution q t (p 1:N ).

M-Step (Maximization Step):
The APs' positions estimateâ t 3:L at the t-th iteration can be obtained by maximizing the expectation function Q a 3:L , q t (p 1:N in (6)

Stochastic Approximation EM
Because the relationship between {θ(p n )} N n=1 and a 1:L , p 1:N in (1) is complex and nonlinear, the analytical form of the expectation of the probability function Q a 3:L , q t (p 1:N ) in (6) is generally intractable, thus rendering the analytical approach of the abovementioned EM-based AP topology estimation infeasible. As an alternative way, the stochastic Monte Carlo approximation can be employed to obtain the expectation of the probability function p {θ(p n )} N n=1 , p 1:N |a 3:L ; a 1:2 in the E-step above [27]. Specifically, considering that a large number of random MT positions involve prohibitively huge computational complexity in the E-step, we draw a single sample from the posterior, q t (p n ), for each random MT position p n and then compute the expected sufficient statistics Q a 3:L , q t (p 1:N ) .
By adopting the stochastic Monte Carlo approximation above, the t-th iteration of EM-based AP positioning method can be modified as follows.

Modified E-Step
with θ l 1 ,l 2 (p m ) = arccos{ On the other hand, the chosen distribution of the MT's positions p 1:N at the t-th iteration, q t (p 1:N ), satisfies where the second proportion holds because the ADOA measurement errors for different AP pairs follow the independent normal distributions, i.e., θ l 1 ,l 2 (p n ) ∼ N θ l 1 ,l 2 (p n ), σ 2 Define the weighted Euclidean distance between the observed ADOA vectorθ(p n ) and the generated ADOA vector θ(p m ) as For each observed ADOA vector {θ(p n )}, the generated ADOA vector with the minimum weighted Euclidean distance can be obtained aŝ By integrating (10) where the third approximation is obtained by substituting (10) into the second approximation above. Because the APs' positions estimate in (13) is still not a closed-form solution and the exhaustive search method for high-dimensional parameters involves huge computational complexity, the gradient descent-based optimization for the APs' positions is used as a viable substitute for estimating the APs' positions in the M-step, which is much more computationally efficient. Although the current optimal AP positions estimates are not exactly obtained in each M-step, such gradient descent-based optimization can monotonically increase the log likelihood of the APs' positions rather than maximizing it.
Hence, the position of the l-th AP in the t-th iteration is updated aŝ where α denotes the learning rate, and the closed-form expression of the gradient, The pseudocode for the proposed stochastic approximation EM-based AP positioning method is given in Algorithm 1. Generate θ(â t−1 3:L ,p m ) based on Equation (8) 6: Obtain the position samples {p t n } N n=1 for observed ADOA vectors {θ(p n )} N n=1 according to Equation (12) M-step in t-th iteration: 7: updateâ t 3:L based on {p t n } N n=1 according to Equation (14) 8: until Output: the AP position estimatesâ t 3:L andâ 1:2

Localization of the MT And Construction of the Environment Map
Based on the estimated APs' positions above, the classical AOA-based positioning method, such as the constrained least-square approach in [10], can be employed to estimate the MT position for each ADOA vector. Further, given by the estimated MT positions {p n } N n=1 and the estimated AP positions {â l } L l=1 , the wall positions can be geometrically calculated as in [11]. Specifically, for each estimated MT position {p n } and each AP pair at {â 1 ,â l }, the corresponding reflection point on an indoor wall can be estimated as the intersection between the segmentp nâl and the line that bisects segmentâ 1âl . Thus, the indoor wall positions, i.e., the environment map, can be constructed.

Simulation Results
For evaluation purposes, we have conducted the computer simulations in a relatively simple 10 m × 8 m (length × width) two-dimension indoor WLAN environment, shown in Figure 2, in which there is only one physical AP located at (2, 2) and four reflective walls. Because the NLOS paths after the higher order of bounces experience enormous attenuation and can be ignored due to the high attenuation and the quasi-optical propagation pattern of the mmW signals in the air [4], only the indirect paths created by a single specular reflection off of a side wall and the direct path are taken into account in the simulations. The four virtual APs are located at the four mirroring positions of the physical AP through the corresponding reflecting walls.
In the simulations, the image-based ray-tracing method is adopted to generate the multipath AOAs at random indoor positions. To derive realistic estimation errors of the AOA measurements, we synthesize the beam shapes for a uniform linear antenna array with 32, 16 and 8 elements. Under typical signal-to-noise ratio conditions, these correspond to the zero-mean Gaussian distributed error of standard deviation {1 • , 2 • , 5 • } as in [3,7], respectively. Thus, the ADOA measurement noise is also assumed to follow the zero-mean Gaussian distribution. In addition, the learning rate of the gradient descentbased optimization, the predefined maximum iteration number and the convergence judgment threshold are, respectively, chosen as α = 0.02, ε = 0.0001 and MaxIT = 200, the number of the measured ADOA vectors at random indoor positions is set to 500 and the standard deviation of the AOA measurement noise is set to 2 • , which will be kept unless otherwise stated. For a fair comparison, we compare the localization and mapping performance of the proposed EM-based SLAM method with that of the JADE algorithm in [3] in our simulations, because the JADE algorithm in [3] also requires zero-initial information and can simultaneously estimate the AP positions and the MT positions based on the multipath ADOA measurements in indoor environments with only one AP deployed. In addition, the Cramer-Rao lower bound of localization is simulated as the ideal performance benchmark for the mmW positioning.

Complexity Analysis
In our proposed method, each iteration requires one E-step and one M-step. For the Estep, the main operation includes generating θ(p m ) for M = 500 position particles {p m } M m=1 based on (8) and computing the weighted Euclidean distance between the observed ADOA vectorθ(p n ) and the generated ADOA vector θ(p m ), according to (12), to obtain the single-position samplep n for the observed ADOA vectorθ(p n ). Hence, the complexity of our E-step is O(MN + M). For the M-step, the main complexity lies in the calculation of the Q-functions gradient with respect to the parameters, a 3:L , which is a sum of all individual gradients for each AP and expressed in (14)  On the other hand, the JADE algorithm mainly includes the initial estimation of the AP locations and iterative optimizations. The complexity of the initial estimation of the anchor locations is of O(L 2 G), where G = 2 16 is the number of grid search points for each AP in [3]. By relaxing the geometric constraint on the AP positions, the MT positions and the ADOA measurements, the JADE algorithm transforms the NP-hard joint estimation over the AP positions and random terminal positions into iterative optimization steps; each iteration has a complexity of O(L 2 N) [3]. As a result, the JADE algorithm has a complexity of O(L 2 (NT J ADE + G)), where T J ADE denotes the number of iterations and T J ADE 200 suffices for JADE to converge.
Obviously, both the proposed EM-based SLAM method and the JADE method have the complexity of the same order, which is proportional to the number of measurements. Therefore, the proposed EM-based SLAM method does not require relaxing the geometric constraints of the AP positions and achieves the comparable computation complexity with the JADE method. Figure 3 shows the cumulative probability-distribution curves of the positioning error of the proposed algorithm, the JADE algorithm and the Cramer-Rao lower bound. By relaxing the geometric constraint on the AP positions, the MT positions and the ADOA measurements, the JADE algorithm transforms the NP-hard joint estimation over the AP positions and random terminal positions into iteratively solving two successive LS (leastsquares) estimation problems; thus, it suffers from losing the partial information contained in the ADOA measurements. On the other hand, the proposed algorithm formulates the complex SLAM problem as the parameter estimation in a latent variable model and employs the stochastic approximation EM scheme to estimate the terminal position and the APs' positions without requiring any relaxation of the geometric constraint; thus, it can extract more information from the measured ADOA data than the JADE algorithm. This explains that the proposed algorithm achieves a better positioning performance than the JADE algorithm in Figure 3. Moreover, Figure 3 shows that there is a considerable gap between the cumulative probability-distribution curves of our algorithm and the Cramer-Rao lower bound due to insufficient ADOA vector measurements.  Figures 4 and 5, it is obvious that both the proposed algorithm and the JADE algorithm perform better with the decrease in the standard deviation of the ADOA measurement noise and the increase in the number of samples due to the averaging effects over the more accurate or more multipath ADOA measurements. It also shows that the localization error of the proposed algorithm is lower than that of the JADE algorithm, because the proposed algorithm not only achieves a higher utilization of the information contained in the ADOA measurement data but also works reliably against the measurement noise due to the robustness of the EM algorithm, while the JADE algorithm involves the LS estimation, which is sensitive to the noise.

Comparison of Mapping Performance
In order to evaluate the room boundary estimation capabilities, i.e., the mapping performance of the proposed method, Figures 6 and 7 demonstrate the mapping performance versus the AOA measurement noise and the number of ADOA vector measurements, respectively. Figures 6 and 7 show that the reconstruction of the room boundary in both the proposed algorithm and the JADE algorithm improves if the standard deviation of the ADOA measurement noise decreases or the number of samples increases, as the amount of available information in the observed ADOA measurements is larger. Moreover, the proposed algorithm reconstructs the environment more accurately than the JADE algorithm in all cases in Figures 6 and 7, because the proposed method makes full use of the geometric relationship among the ADOA measurement, the MT position and the APs' positions without any relaxation. This further confirms the merit of the proposed EM-based SLAM algorithm.

Conclusions
In this paper, we proposed a novel expectation-maximization-based simultaneous localization and mapping (SLAM) algorithm for mmW communication systems. The proposed SLAM algorithm can be readily applied to the available crowd-sourcing MPCs' measurement data because it does not require the MPC measurements obtained at successive epochs. By fully exploiting the geometric relationship of the multipath ADOA measurements and regarding the MT positions as the latent variable of the AP positions, it employs an efficient stochastic approximation EM method to estimate both the AP positions and the MT positions and further constructs the indoor map based on the estimated AP topology. Due to the efficient processing capability of the stochastic approximation EM method and taking full advantage of the abundant spatial information in the crowdsourcing ADOA data, the proposed method can achieve a better positioning and mapping performance than the existing geometry-based mmW SLAM method. The simulation results confirm the effectiveness of the proposed algorithm.
Author Contributions: L.C. proposed the EM-based SLAM algorithm, designed the simulation scheme and wrote the paper. Z.C. gave suggestions on the proposed algorithm and helped to improve the quality of the final manuscript. Z.J. helped to perform the simulation tests and analyzed the results. All authors have read and agreed to the published version of the manuscript.

Conflicts of Interest:
The authors declare no conflict of interest.