Robust Multipath ‐ Assisted SLAM with Unknown Process Noise and Clutter Intensity

: In multipath ‐ assisted simultaneous localization and mapping (SLAM), the geometric association of specular multipath components based on radio signals with environmental features is used to simultaneously localize user equipment and map the environment. We must contend with two notable model parameter uncertainties in multipath ‐ assisted SLAM: process noise and clutter intensity. Knowledge of these two parameters is critically important to multipath ‐ assisted SLAM, the uncertainty of which will seriously affect the SLAM accuracy. Conventional multipath ‐ assisted SLAM algorithms generally regard these model parameters as fixed and known, which cannot meet the challenges presented in complicated environments. We address this challenge by improving the belief propagation (BP) ‐ based SLAM algorithm and proposing a robust multipath ‐ assisted SLAM algorithm that can accommodate model mismatch in process noise and clutter intensity. Specifically, we describe the evolution of the process noise variance and clutter intensity via Markov chain models and integrate them into the factor graph representing the Bayesian model of the multipath ‐ assisted SLAM. Then, the BP message passing algorithm is leveraged to calculate the marginal posterior distributions of the user equipment, environmental features and unknown model parameters to achieve the goals of simultaneous localization and mapping, as well as adaptively learning the process noise variance and clutter intensity. Finally, the simulation results demonstrate that the proposed approach is robust against the uncertainty of the process noise and clutter intensity and shows excellent performances in challenging indoor environments.


Introduction
Simultaneous localization and mapping (SLAM) [1,2] aims to estimate the timevarying position of the mobile agent when it is moving in an unknown environment and build a map of the surroundings from measurements provided by sensors. SLAM is significant in many fields, such as robotics [3], autonomous driving [4], and situation awareness [5]. In multipath-assisted methods [6][7][8][9][10], the relation between multipath components (MPCs) of radio signals and local geometry is used to turn the multipath propagation from an error source to an advantage that can improve the positioning accuracy. Recently, multipath-assisted SLAM, a combination of the two, has attracted considerable attention from scholars and has been applied in the field of indoor positioning [11][12][13][14][15][16].
Instead of mapping the environment in conventional SLAMs, the map in multipathassisted SLAM is represented by an unknown number of features with unknown spatial positions. Features denote the physical transmitters (PTs) and virtual transmitters (VTs) in the environment. Radio signals are transmitted from base stations (PTs) to the user equipment (UE). Specular MPCs in the multipath radio channel are described by VTs, which are mirror images of the PTs. Due to the introduction of VTs, specular MPCs can algorithm based on the factor graph and the sum product algorithm [34] was proposed. The algorithm can be applied to complex scenes where the target dynamic model and detection probability change at the same time.
Most of the existing multipath-assisted SLAM algorithms regard the process noise and clutter intensity as known and fixed. However, in practical applications, the dynamic model of UEs cannot remain invariable all the time, and the process noise of UEs may change at any time. Clutter environments are often complicated due to the influence of electromagnetic interference, signal scattering and diffraction; hence, it is difficult to match the assumption of known process noise and clutter intensity with reality. Significant mismatches in the process and clutter model parameters seriously affect the SLAM accuracy, and even the estimation results diverge. Therefore, it is of critical importance to study the robust multipath-assisted SLAM algorithm when the process and clutter model parameters are unknown. Based on the framework of the BP-SLAM algorithm, in this paper, we propose a robust multipath-assisted SLAM algorithm, which can address the uncertainties of process and clutter model parameters in multipathassisted SLAM. First, the UE state is extended to an augmented state including the process noise, and the evolution of the augmented state is described by new prediction and update rules compared to the conventional BP-SLAM algorithm. Second, we add a new variable to represent the clutter intensity in the Bayesian model of BP-SLAM and describe the evolution of the clutter intensity via a Markov chain model. Finally, we integrate the augmented state of the UE and the new variable node into a factor graph representing the Bayesian model of BP-SLAM and use the BP message passing algorithm to calculate marginal posterior distributions of the process noise, the clutter intensity, the UE and other features to realize the online adjustment of the process noise and the clutter intensity in multipath-assisted SLAM. The simulation results demonstrate that the proposed algorithm can adaptively learn the process noise and clutter intensity for SLAM, improving the robustness of the multipath-assisted SLAM system.
The remainder of this paper is organized as follows: Section 2 provides the environment map and the signal model. Section 3 describes the system model with unknown process noise and clutter intensity. Moreover, our robust multipath-assisted SLAM algorithm is proposed in Section 4. Then, the simulation results are reported in Section 5. Finally, Section 6 contains the conclusion of the paper.

Environment Map and Signal Model
Multipath-assisted SLAM combines the geometric parameters (delays, angles of arrival (AOAs) and angles of departure (AODs)) of the LOS signal in received radio signals with the geometric parameters of specular MPCs to localize the UE and build the environment map. These parameters are obtained according to the relationship between the positions of the UE and the PTs or VTs. The VT positions are mirror images of the PT positions with respect to the reflection surface. From the UE side, the reflected and virtual propagation paths originating from the VTs have the same AOA and delay; hence, the reflected path can be described as a LOS path between the VT and the UE. Regardless of how the UE moves, the VTs will remain static as long as the PTs are static. As an example, Figure 1 depicts an indoor environment map containing two PTs and some of the corresponding VTs. The red solid circle and the blue solid box denote PT1 and PT2 at (1) 1 a and (2) 1 a , respectively. The gray lines represent the doors, windows and walls in the room. They all have smooth surfaces that can reflect the radio signals from the PTs. The red hollow circles and blue hollow boxes represent the corresponding VTs, which are mirror images of PT1 and PT2. The green dot is the estimated position of the UE at n u , while the magenta line denotes the real trajectory of the UE. We consider an environment map with one UE and J PTs. The UE position is unknown and time-varying, which is denoted by The first term on the right in Equation (1) denotes the PF (j, k) position. We also define the stacked vectors of ( ) , j k n y , i.e., At any time n, two kinds of PFs exist: one is the legacy PF, which was established in the past, and the other is the new PF, which is established for the first time at time n. The augmented states of legacy PFs and new PFs for PT j are expressed as From Equation (2), we see that the PF set updates as a dynamic increasing process, i.e., legacy PFs at time n − 1 and new PFs at time n constitute the legacy PFs at time n + 1. (Note that the number of PFs ( ) j n K at time n does not actually increase ( ) j n M compared with that at time n − 1, since some PFs with low existence probability are pruned, which will be explained in Section 4.1.) We also define the stacked vectors related to PF states as follows: for legacy PFs for PT j, may not originate from any PF (false alarms or clutter), or it may not be detected by the channel estimator (missed detections). We assume that each PF generates at most one measurement at any time n and that each measurement can be generated by at most one PF [36]. At time n, the association between the measurements ( ) , j m n z and PFs can be expressed as a feature-oriented . Furthermore, we consider a measurement-oriented The DA vectors h and q are random and completely equivalent since one of them can be determined by the other.

Markov Chain Modeling of the Process Noise and the Clutter Intensity
In addition to the essential information to be tracked, i.e., the UE state, PF states and DA vectors, model parameters such as the process noise and the clutter intensity are also unknown and time-varying. The process noise determines the difference of the UE dynamic models (assuming that the UE is in a nearly constant velocity model), and the clutter intensity describes the amount of clutter (false alarms) in the environment. Significant mismatches in the process and clutter model will seriously affect the SLAM accuracy. To obtain a more robust multipath-assisted SLAM algorithm, we propose tracking unknown model parameters, process noise and clutter intensity in the framework of the BP-SLAM algorithm. The process noise and the clutter intensity can be modeled in the same way: (  . We define the stacked vector of ( ) Based on the assumptions above, the prior PMF of  factorizes as

State Evolution with Unknown Process Noise
The probability density function (PDF) of the UE state transition is determined by the process noise variance if the UE is in a nearly constant velocity model. Using the interacting multiple model (IMM) algorithm [37], the UE can switch among nearly constant velocity models with different process noise variances at any time. To track the change of the UE dynamic model, the original state of the UE n  x is augmented by the UE process noise variance as Additionally, we define the stacked vectors The evolutions of the UE augmented state n x and the legacy PF augmented state where s P denotes the survival probability of the PF and denotes an arbitrary "dummy PDF" [38]. If PF (j, k) does not exist at time n−1, it cannot exist at time n. Thus, we have

Prior Distributions with Unknown Clutter Intensity
Clutter are false measurements in the environment, which are usually modeled as Poisson point processes; hence, all the characteristics of the clutter can be completely described by the intensity information. The clutter intensity is defined as the product of the average number of clutter at each time is equal at all times and only related to PT. The initial distribution of ( ) We also define the stacked vector . Similar to clutter, we model the number of newly detected features as a Poisson point process with mean value ( ) new , j n  . We also define the detection probability ( ) , which refers to the probability that the PF can be detected or the probability that the PF can generate the measurement ( ) , j m n z in the channel estimation process [39]. The detection probability is supposed known in this paper.
Then, given the UE augmented state n x , the legacy PF augmented state The exclude indicator function in (10) is (11) where when ( ) j j k n m n h q  is 1. The indicator function enforces the assumption in Section 3.1 that each PF generates at most one measurement at any time n and that each measurement can be generated by at most one PF. Note that this DA constraint function is redundant since it considers both the feature-oriented and measurement-oriented association vectors ( ) j n h and ( ) j n q . However, such a redundant representation is the key to obtaining an algorithm that has low computational complexity and excellent scalability [38,40]. Furthermore, (15) where the indicator function . In particular, when n = 1, since the legacy PF ( ) 1 j y is an empty vector, we have Assume that the spatial distribution PDF of newly detected features is where (10) and Equation (16)), and the last four factors are obtained from Equation (6), Equation (9), Equation (10) and Equation (16), respectively.

Measurement Model and Likelihood Function
The relation between the measurement ( ) , an example of which will be given in Section 5.1. This PDF is the central element of the likelihood function is observed, i.e., that ( ) j n z is fixed, then, up to a constant factor, the likelihood function can be expressed as [25] ( ) and In particular, at the initial time when n = 1, we have Now, we consider the product of the likelihood function Equation (18), the prior PDF of the association vectors Equation (10) and the prior PDF of new PFs Equation (16), and we have   (14), Equation (12) and Equation (19), respectively, and k j j j j n m n n k n k n j k n j j j j j j n k n n n k n n mn and and

The Proposed Algorithm
In this section, the proposed BP-based robust multipath-assisted SLAM algorithm is described in detail.

Detection and Estimation
The ultimate goal of our algorithm is to estimate the UE original state n  x and PF To obtain ( j k n p e  z exceeds the predefined prune threshold pr P .

Joint Posterior Distribution and Factor Graph
The posterior PDFs independence hypotheses, using Equation (6), Equation (17), Equation (20) and Equation (21) and some simple derivations, the joint posterior PDF factorizes as ( , , , , , ) ( , , , ) ( , , , , , ) This factorization is represented by the factor graph [34] in Figure 2, in which the variable node labeled x denotes the UE augmented state , and the average number of clutter ( ) CL, j n  is denoted by the independent variable node labeled CL  .
Factor graph representing the factorization of the joint posterior probability density function (PDF) in (30). All factor nodes, variable nodes and messages related to the user equipment (UE) augmented state are represented in red color, those related to legacy potential features (PF) states are represented in purple color, those related to new PF states are represented in blue color, and the section of the clutter intensity and the iterative data association (DA) are represented in orange color and green color, respectively. The abbreviations are defined as

BP Message Passing Algorithm
Now, we calculate the marginal posterior PDFs Substituting (6) (33) where the belief of the legacy PF augmented state is also calculated at time n − 1. Substituting (7) and (8)  , we obtain Finally, the prediction also includes the prediction of the average number of clutter, which is calculated as  is calculated as Then, after the last iteration, the messages  In our proposed algorithm, clutter is modeled as Poisson distribution with mean value ( ) CL, j n  . We assume that the spatial PDF of clutter is uniform in the region of interest (ROI), which is a circular area with a radius of 30 m around the center of the environment map in Figure 1. Therefore, the clutter intensity can be uniquely determined by the mean value ( ) Table 1. Parameter setup.

First Scenario: Unknown Process Noise Only
In the first scenario, the UE switches between two nearly constant velocity models with different process noise variances,   Q Q . In this scenario, the clutter intensity is considered to be a fixed prior. We assume that the mean number of clutter is ( ) CL, =0.1 j n  . We compare our proposed algorithm with the conventional BP-SLAM algorithm [25] with unknown process noise variance and the ideal BP-SLAM algorithm with known process noise variance, which can obtain the prior information of the UE process noise variance at each time in advance. The ideal BP-SLAM algorithm is impossible in practice but can be used as a benchmark to evaluate the performance of our proposed algorithm. In the conventional BP-SLAM algorithm with unknown process noise variance, we considered two different parameter settings labeled BP-SLAM A and BP-SLAM B. The UE process noise variances in BP-SLAM A and BP-SLAM B both remain unchanged, but their settings are different, i.e.,  Figure 3 shows the performance of the proposed algorithm in the first scenario, which is obtained based on the average of 100 simulation runs. Figure 3a,b depict the mean optimal subpattern assignment (MOSPA) [45] errors for PT1 and PT2 and their associated VTs, respectively. The MOSPA error is based on Euclidean distance with a cutoff parameter of 5 m and order 1. The MOSPA error combines the estimation errors of correctly detected PFs and incorrectly detected PFs, which is often used as the standard to evaluate the mapping accuracy of multipath-assisted SLAM algorithms. In Figure 3a, period [0,720] n  , the MOSPA error of BP-SLAM B for PT1 and its associated VTs is always higher than the other three algorithms. Then, when the UE process noise variance suddenly changes, i.e., [720,730] n  , the MOSPA error of BP-SLAM A rises sharply with a maximum of more than 3 m. However, the MOSPA error of the proposed algorithm is always smaller than that of BP-SLAM A and BP-SLAM B in this period. This relationship still exists even after the UE returns to the normal motion state, i.e., [730,800] n  . The MOSPA error for PT2 and its associated VTs shown in Figure 3b also have similar performance. Figure 3c,d describe the average number of detected PFs associated with PT1 and PT2, respectively. From these two figures, we can see that the average number of detected PFs for the proposed algorithm is always close to the actual number of features in the environment, namely, (1) 6 n L  and (2) 5 n L  . However, the average number of detected PFs for BP-SLAM A deviates from the truth after 720 s, i.e., the UE process noise variance changes suddenly. The average number of detected PFs for BP-SLAM B deviates from the truth even when the UE process noise remains unchanged, i.e., [0,720] n  . Figure 3e depicts the root mean square errors (RMSEs) of the UE position for the four algorithms. Figure 3e shows that in period [720,730] n  , the UE position RMSE for BP-SLAM A rise sharply with a maximum of more than 1.2 m, while the UE position RMSE of the proposed algorithm is always less than 0.4 m in this period, which is close to the ideal BP-SLAM algorithm with known parameters. Although the UE position RMSE for BP-SLAM B has no sharp rise in this period, it is higher than that of the proposed algorithm all the time.  Table 2 summarizes the error statistics of different algorithms in the first scenario within 800 s, including the average UE position RMSE, the average MOSPA error and the average error for the estimated number of detected PFs. The latter two errors are based on the average of PT1 and PT2 statistics. From Table 2, we can see that the statistical error of the proposed algorithm is significantly smaller than that of BP-SLAM A and BP-SLAM B, and close to the ideal BP-SLAM algorithm with known parameters. Therefore, Figure 3 and Table 2 both illustrate that the proposed algorithm is robust to the uncertainty of the process noise.

Second Scenario: Unknown Clutter Intensity Only
In the second scenario, we assume that only the clutter intensity parameter is unknown, which means that the UE does not have a sudden change in the dynamic model, with the process noise variance keeping respectively. The ideal BP-SLAM algorithm with known parameters can obtain the prior information of the true clutter intensity in advance, which can also be used as the benchmark to evaluate the performance of our proposed algorithm in this scenario. Figure 4 illustrates the simulation results of the proposed algorithm in the second scenario, which are also obtained based on the average of 100 simulation runs. Figure 4a Figure 4e shows that the position accuracy of the proposed algorithm is significantly better than that of BP-SLAM A and BP-SLAM B. During the simulation run time of 800 s, the RMSE of the proposed algorithm is always lower than 0.15 m, which is close to the performance of the BP-SLAM algorithm with known parameters. However, the UE position RMSE of BP-SLAM A increases rapidly in 800 s, and the maximum is more than 0.65 m. Although the performance of BP-SLAM B is relatively good, its position accuracy is lower than that of the proposed algorithm all the time.   Table 3 summarizes the error statistics of different algorithms in the second scenario within 800 s. The MOSPA error and the error for the estimated number of detected PFs are also based on the average of PT1 and PT2 statistics. It can be seen from Table 3 that the statistical error of the proposed algorithm is significantly smaller than that of BP-SLAM A and BP-SLAM B, and close to the ideal BP-SLAM algorithm with known parameters. Therefore, Figure 4 and Table 3 both verify that the proposed algorithm is robust to the uncertainty of the clutter intensity.

Third Scenario: Unknown Process Noise and Clutter Intensity
In the third scenario, the uncertainty of the process noise and the clutter intensity are considered together, and the simulation time is 800 s. The variation in the UE process noise variance is the same as that in the first scenario. Most of the time, the UE process noise variance is   and 8 D , respectively. In the third scenario, the BP-SLAM algorithm with known parameters can obtain the prior parameters of the process noise and the clutter intensity at each time in advance, which can still be used as the benchmark to evaluate the performance of the proposed algorithm. Figure 5 depicts the performance of the proposed algorithm in the third scenario, which is also obtained based on the average of 100 simulation runs. Figure 5a,b describe the MOSPA errors for PT1 and PT2 and their associated VTs, respectively. Figure 5a shows that the MOSPA errors of the four algorithms all reach steady states within approximately 150 s, and BP-SLAM A and BP-SLAM B are stable at approximately 1.8 m and 0.5 m, respectively. However, the performance of the proposed algorithm and the BP-SLAM algorithm with known parameters is obviously better with both below 0.3 m. In particular, the MOSPA error of BP-SLAM A rises sharply during the sudden change of the UE process noise variance, i.e., [720,730] n  , and the maximum arrives near 3.2 m. The MOSPA errors of the proposed algorithm and the BP-SLAM algorithm with known parameters do not change significantly in this period. The MOSPA error for PT2 and the associated VTs described in Figure 5b has a similar performance to that in Figure 5a, i.e., regardless of whether the UE process noise changes, the performance of the proposed algorithm is better than that of BP-SLAM A and BP-SLAM B. Figure 5c,d show the average number of detected PFs associated with PT1 and PT2 every 60 s, respectively. These two figures show that the average numbers of detected PFs by the proposed algorithm are always near the true value, while the results of BP-SLAM A are not convergent. Although the results for BP-SLAM B are convergent, they converge on the wrong number. Therefore, in the third scenario, the estimation performance of the proposed algorithm for the average number of detected PFs is obviously better than that of BP-SLAM A and BP-SLAM B. Figure 5e  . Based on the above data, the UE position accuracy of the proposed algorithm is significantly better than that of the BP-SLAM algorithm with unknown parameters and close to that of the BP-SLAM algorithm with known parameters.  Table 4 summarizes the error statistics of different algorithms in the third scenario within 800 s. The MOSPA error and the error for the estimated number of detected PFs are also based on the average of PT1 and PT2 statistics. From Table 4, we can see that the statistical error of the proposed algorithm is significantly smaller than that of BP-SLAM A and BP-SLAM B, and close to the ideal BP-SLAM algorithm with known parameters. Therefore, Figure 5 and Table 4 show that the proposed algorithm is robust to both the clutter intensity and the process noise uncertainty.

Calculation Complexity Analysis
The computational complexity of the conventional BP-SLAM algorithm is proportional to par ( ) N  , where par N is the number of particles [25]. The proposed algorithm adds the evolutions of the process noise variance and the clutter intensity to the Bayesian model compared with the conventional BP-SLAM algorithm; hence, its computational complexity is proportional to  Table 5 shows the complexity comparison of different algorithms in terms of both the complexity orders and the CPU run times. From Table 5, we can see that the proposed algorithm has almost the same calculation complexity as the conventional BP-SLAM algorithm with unknown parameters, but the performance of positioning and mapping accuracy is much better, close to the ideal BP-SLAM algorithm with known parameters. The reason is that the proposed algorithm considers the influence of the process noise and the clutter intensity uncertainty.

Conclusions
In multipath-assisted SLAM using radio signals, the estimation of the process noise and the clutter intensity is difficult in practice. Therefore, it is highly significant to study the multipath-assisted SLAM algorithm with the ability to adaptively estimate these unknown parameters. A robust multipath-assisted SLAM algorithm is proposed in this paper to solve the problem that the accuracy of the multipath-assisted SLAM algorithm decreases when the process noise and the clutter intensity are statistically uncertain. The proposed algorithm can still work with unknown time-varying parameters by continuously inferring an unknown parameter model, the states of the UE and environmental features. Specifically, we integrate the process noise variance and the clutter intensity into the factor graph representing the statistical structure of the SLAM problem and use the Markov chain model to describe the evolution of these unknown parameters. Finally, the BP message passing algorithm is performed to calculate the marginal posterior PDFs of each parameter to achieve the goal of simultaneous localization and mapping and adaptively learning the process noise variance and the clutter intensity.
The simulation results demonstrate that the proposed algorithm is superior to the conventional BP-SLAM algorithm in positioning and mapping accuracy with the uncertainty of the process noise and the clutter intensity. In particular, when the process noise and the clutter intensity are both unknown, the UE position time average RMSE at all times of the proposed algorithm is significantly less than that of the conventional BP-SLAM algorithm (0.11 m versus 1.46 m), and the performance in the MOSPA error and the average number of detected PFs is also obviously better than that of the conventional BP-SLAM algorithm (0.77 m versus 2.24 m and 0.06 versus 8.5, respectively), taking BP-SLAM A as an example. Therefore, the proposed algorithm can effectively restrain the impact of the process noise and the clutter intensity uncertainty on SLAM estimation accuracy and improve the robustness of a multipath-assisted SLAM system using radio signals.
We assume time synchronization between the transmitters and the UE in this paper. A promising direction for future research is the extension of our robust multipath-assisted SLAM algorithm to an unsynchronized sensor network.

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