{'fraction_non_alphanumeric': 0.0434598958088891, 'fraction_numerical': 0.01586333129957291, 'mean_word_length': 3.850443888003642, 'pattern_counts': {'":': 0, '<': 2, '<?xml version=': 0, '>': 4, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 133, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Iterated Posterior Linearization PMB Filter for 5G SLAM

5G millimeter wave (mmWave) signals have inherent geometric connections to the propagation channel and the propagation environment. Thus, they can be used to jointly localize the receiver and map the propagation environment, which is termed as simultaneous localization and mapping (SLAM). One of the most important tasks in the 5G SLAM is to deal with the nonlinearity of the measurement model. To solve this problem, existing 5G SLAM approaches rely on sigma-point or extended Kalman filters, linearizing the measurement function with respect to the prior probability density function (PDF). In this paper, we study the linearization of the measurement function with respect to the posterior PDF, and implement the iterated posterior linearization filter into the Poisson multi-Bernoulli SLAM filter. Simulation results demonstrate the accuracy and precision improvements of the resulting SLAM filter.


I. INTRODUCTION
5G mmWave signals provide unique opportunities for simultaneous localization and mapping (SLAM), due to their inherent geometric connection to the propagation environment [1]. Signals from the base station (BS) reach the user equipment (UE) via multiple propagation paths. Each path is determined by the propagation environment and the locations of the BS and the UE. State-of-the-art channel estimators can provide accurate estimates for those paths by using received signals, in terms of groups of channel gain, time of arrival (TOA), angles of arrival (AOA), and angles of departure (AOD), which contain information needed for SLAM [2], [3].
Much work has been done in SLAM using 5G signals, also called 5G SLAM, including geometry-based methods [4], [5], which cannot provide uncertainty information; message passing methods [6]- [8], which provide uncertainty information, but are inherently sub-optimal; more powerful algorithms using random finite set (RFS) theory [9]- [13]. The latter class of algorithms can handle the data association (DA) problem between measurements and landmarks [14] and have certain optimality guarantees. In particular, probability hypothesis density (PHD) filters are used in [9], [10], Poisson multi-Bernoulli mixture (PMBM) filters are used in [11]- [13], and the low-computational version Poisson multi-Bernoulli (PMB) filters are also applied in [13]. Because PMB(M) filters enumerate all possible DAs explicitly, they provide  more accurate results than PHD filters in 5G SLAM, as shown in [13].
Common to all 5G SLAM studies, it is the requirement to account for the nonlinearity of the measurement function, which in the case of 5G mmWave signals relate TOA, AOA, and AOD, to positions and orientations of the UE and landmarks. To this end, [11], [12] follow a Rao-Blackwellized approach, and utilize the particle filter (for the UE state) [15,Ch. 7.1], and the cubature Kalman filter (CKF) (for the landmark states, conditioned on the UE state) which uses sigma-points drawn from the prior probability density function (PDF) [16] to propagate through the measurement nonlinearity [15,Ch. 5.2]. In [13], the extended Kalman filter (EKF) is implemented into the PMB(M) SLAM filter, and the approximation of the nonlinearity is formed by utilizing the first-order Taylor series [15,Ch. 5.2], which is equivalent to a linearization at the prior mean. However, these methods either have high computational burden, or perform poorly with nonlinear measurement functions, if the measurement noise is low enough [17]. A more accurate and efficient linearization method is proposed in [18], [19], which linearizes the measurement function with respect to the posterior PDF, rather than the prior PDF. To our best knowledge, the evaluation of such an approach in 5G SLAM has not yet been conducted.
In this paper, we show that the linearization of the measurement function can be done with respect to the posterior joint PDF of the UE state and the landmark, and extend our previous work in [13] to implement the posterior linearization into the PMB-based 5G SLAM filter. The main contributions of this paper are summarized as follows: (i) we develop the iterated posterior linearization filter (IPLF) integrated to the 5G PMB SLAM filter; (ii) we show that the proposed IPL-PMB SLAM filter can improve the mapping and positioning accuracy and precision, while guaranteeing near real-time operation, although a minor computational cost needs be paid.
Notations: Scalars (e.g., x) are denoted in italic, vectors (e.g., x) in bold lower-case letters, matrices (e.g., X) in bold capital letters, sets (e.g., X ) in calligraphic. Transpose is denoted by (⋅) T , the union of mutually disjoint sets is denoted by ⊎, a Gaussian density with mean u and covariance Σ, evaluated in value x, is denoted by N (x; u, Σ), and d x = dim(x).

II. SYSTEM MODEL
In this section, the UE model, the environment model, and the measurement model for a 5G downlink localization scenario, as shown in Fig. 1, are introduced.

A. User Model
A single-user scenario is considered, thus the cooperation among UEs is out of scope of this paper. We denote the dynamic state of the UE at time step k as s k , which at least contains the UE position x UE,k = [x k , y k , z k ] T , the heading k and the clock bias B k . If the process noise is zero-mean Gaussian, the transition density of s k can be expressed as where v(⋅) denotes a known transition function, and Q k−1 denotes a known covariance matrix.

B. Environment Model
We consider an environment with three different types of landmarks, which are the BS, reflecting surfaces, and small objects. In the environment, there is a BS with known position, which sends downlink signals to the UE, and a few unknown reflecting surfaces and small objects. They can reflect and scatter the downlink signals to the UE, respectively, and are modeled as virtual anchors (VAs) and scattering points (SPs) (see Fig. 1). We model the landmark state as x = [x T LM , m] T , where x LM ∈ R 3 represents the landmark location, and m ∈ {BS, VA, SP} represents the landmark type. Therefore, the map of the environment can be represented by a set of landmark X = {x 1 , . . . , x I }, with I representing the total number of landmarks.

C. Measurement Model
At time step k, the UE receives downlink signals from the BS. When considering OFDM transmissions, we can express the received signal at subcarrier κ at time step k as [20] where S κ is the (possibly pre-coded) pilot signal over subcarrier κ, Y κ,k is the received signal over subcarrier κ, N κ,k is white Gaussian noise, and C κ,k is the channel frequency response. As the transmitted signals can reach the UE directly, which is the line-of-sight (LOS) path, and/or reflected by reflecting surfaces or scattered by small objects, which are non-line-of-sight (NLOS) paths, C κ,k can be denoted as where W k represents a combining matrix, a R (⋅) and a T (⋅) denote the steering vectors of the receiver and transmitter antenna arrays, respectively, and ∆f denotes the subcarrier spacing. Moreover, I k is the number of all visible landmarks, and we assume that there is only one path from each landmark. The LOS path corresponds to i = 0, and the NLOS paths to i > 0. Each path i can be described by a complex gain g i k , a TOA τ i k , an AOA pair θ i k in azimuth and elevation, and an AOD pair φ i k in azimuth and elevation. Those channel parameters depend on the hidden geometric relation among the BS, UE and landmarks, which can be found, e.g., in [12, At the UE side, a channel estimator, such as [21]- [25], provides estimates of angles and delays of paths from Y κ,k . However, the channel estimation is out of the scope of this paper, and the UE directly utilizes output of the channel estimator that provides the angel and delay estimates. At time step k, a set of measurements Z k = {z 1 k , . . . , zÎ k k } is provided, where usuallyÎ k ≠ I k , as there may be some clutter measurements and misdetected landmarks. If the measurement noise is zero-mean Gaussian, the measurement originating from landmark x i follows where T represents the nonlinear function that transforms the geometric information to the TOA, AOA and AOD, and R i k is the measurement covariance.

III. PMB(M) SLAM FILTER
In this section, we approximate the map X conditioned on the UE state s k as a PMB density. In other words, it is a PMB RFS. We will now briefly introduce the basics of the PMB(M) density and the PMB(M) SLAM filter.

A. Basics of PMB(M) Density
The PMBM RFS X can be viewed as the union of two disjoint RFSs, X U and X D , which are the set of undetected objects that have been never detected, and the set of detected objects that have been detected at least once, respectively [26]. The RFS X U is usually modeled as a Poisson point process (PPP), with the density following where λ(⋅) is the intensity function. The RFS X D is usually modeled as a multi-Bernoulli mixture (MBM), with the density following where I is the index set of all global hypotheses and w j ≥ 0 is the weight for j-th global hypothesis, satisfying ∑ j∈I w j = 1 [27]; n is the number of potentially detected objects; f j,i B (⋅) is the Bernoulli density of the i-th landmark under the j-th global hypothesis. Each Bernoulli follows where r j,i ∈ [0, 1] is the existence probability, and f j,i (⋅) is the state density. More details of the PPP and MBM densities can be found in [26]- [28]. Then, the density of X can be computed using the convolution formula [29, eq. (4.17)] as which can also be parameterized by λ(x) and

B. PMB(M) SLAM Filter
The PMBM SLAM filter follows the prediction and update steps of the Bayesian filtering recursion with RFSs [30]. In practice, instead of tracking the joint posterior f (s 0∶k , X Z 1∶k ), we keep track of marginal posteriors f (X Z 1∶k ) and f (s k Z 1∶k ) to reduce complexity. To do this, the prediction of the UE state follows the Chapman-Kolmogorov equation, given by As all landmarks are static, there is no prediction for the map. By marginalizing out the map state in the joint posterior, the update step for the UE state becomes whereas by marginalizing out the UE state, the map state follows where (Z k+1 s k+1 , X ) is the RFS likelihood function, given by [26, eqs. (5)-(6)], and ∫ ψ(X )δX refers to the set integral [27, eq. (4)]. In practice, (9), (10), (12) are usually translated into prediction and update steps of the PMBM parameters λ(x) and {w j , {r j,i , f j,i (x)} i∈I j } j∈I . As the number of DAs increases very rapidly over time, the exact PMBM SLAM filter has high complexity. To mitigate this, the PMB SLAM filter is often used, which approximates the PMBM density to a PMB density at the end of each time step by marginalizing over DAs.

C. EK-PMB(M) SLAM Filter
The EK-PMB(M) SLAM filter from [13] computes (10)-(13) by determining the γ ≥ 1 most likely DA hypotheses with corresponding weights. For each DA, the joint posterior of the UE state and landmarks is computed by the EKF approximation around the prior mean. These posteriors are marginalized and finally fused according to their weights. This leads to an efficient implementation, amenable for near realtime implementation.
IV. POSTERIOR LINEARIZATION In the PMB(M) SLAM filter, we need to update the state of UE and the map jointly for each DA. To linearize the nonlinear measurement function of the joint state of the UE and landmarks is important. In this section, we will introduce the basics of linearization, argue that linearization should be done with respect to the posterior PDF instead of at the prior mean, and propose a method to realize the posterior linearization. We drop the time index and the DA index for simplicity. Hence, we can denote the joint state of the UE and landmarks given a certain DA asš, the corresponding measurement function asȟ(š), and the associated measurement vector asž. More details can be found in [13, Section. IV.D].

A. Linearization Principle
To linearize the nonlinear measurement functionȟ(š) is to approximate it by a linear function with a zero-mean Gaussian noise, asȟ where H ∈ R dž×dš denotes the linearized matrix, b ∈ R dž×1 denotes the bias vector, and e ∈ R dž×1 denotes a zeromean Gaussian distributed variable with covariance matrix Ω that is independent ofš and the measurement noise. In other words, to linearizeȟ is to select suitable (H, b, Ω) for (14) by minimizing a problem-specific objective function (see Section IV-B). Once the linearization in (14) is performed, measurementž follows N (ž; Hš + b, Ω +Ř), whereŘ is the overall measurement noise covariance. Moreover, if the prior follows N (š; m − , P − ), the posterior PDF becomes N (š; m + , P + ) with [18] where K = P − H T (HP − H T + Ω +Ř) −1 is the Kalman gain.

B. Prior and Posterior Linearization 1) Prior Linearization with the EKF:
In the EKF,ȟ(š) is approximated by the first-order Taylor series, and (14) is linearized at the prior mean with Ω = 0, and H representing the Jacobian ofȟ(⋅) with respect toš, evaluated at the prior mean ofš. However, this approach does not make use of the measurement, thus its performance may deteriorate in some cases. Comparatively, it provides worse approximation than methods that use all available information, especially for nonlinear measurement functions with relatively low measurement noise [18]. 2) Posterior Linearization: To obtain a better approximation in (14), the linearization should be done with respect to the posterior PDF rather than at the prior mean [18]. The main reason is that the posterior PDF is always narrower than the prior PDF, especially if the measurement noise is low. Therefore, it is possible that linearization of the nonlinear measurement function at the prior mean makes the approximation lying outside the support of the posterior PDF. Fig. 2 provides examples of linearization of a given measurement function at the prior mean and with respect to the posterior PDF. It is obvious that the linearization with respect to the posterior PDF provides better approximation and less uncertainty, as the linearization is more closer to the measurement function in the 95% confidence interval, and the 95% confidence interval is much shorter. The limitation is that the posterior is not yet available.

3) Iterative Posterior Linearization Filter:
A practical approach to implement posterior linearization is the IPLF [18], which iteratively approximates (H, b, Ω) by solving the optimization problem [18] arg min where E[⋅] represents the expectation with respect to the posterior PDF. From (17), we find the optimal (H, b) that can give the best linearization ofȟ in the sense of minimizing its mean square error (MSE), and the corresponding MSE matrix is then recovered as Ω in (18). To solve this optimization problem, we can perform (17) and (18) with respect to iterative approximations of the posterior PDF, starting from the prior PDF. After each iteration, we obtain an improved approximation to the posterior PDF, from which we can obtain an improved linearization. Given an approximation, the expectation in (17) can be computed using sigma point principle of the CKF [16]. The entire IPLF procedure is summarized in Algorithm 1. The integration of the IPLF is summarized in Fig. 3 < l a t e x i t s h a 1 _ b a s e 6 4 = " Z A I k v p B Y x d k l + V d 5 p 8 C x B t B c i F o = " > A A A C C H i c b V D L S g M x F M 3 U V 6 2 v U Z c u D B a h g p S Z U l B 3 B T c u K 9 g H t E P J Z N I 2 N M k M S U Y o w y z d + C t u X C j i 1 k 9 w 5 9 + Y a W d R W w + E n J x z L 7 n 3 + B G j S j v O j 1 V Y W 9 / Y 3 C p u l 3 Z 2 9 / Y P 7 M O j t g p j i U k L h y y U X R 8 p w q g g L U 0 1 I 9 1 I E s R 9 R j r + 5 D b z O 4 9 E K h q K B z 2 N i M f R S N A h x U g b a W C f V v p + y A I 1 5 e Z K e H q 5 + G y m F w O 7 7 F S d G e A q c X N S B j m a A / u 7 H 4 Q 4 5 k R o z J B S P d e J t J c g q S l m J C 3

I r G E p l / j c t s e l M z I Q F k = " > A A A B 8 n i c b V D L S g M x F M 3 U V 6 2 v q k s 3 w S K 4 K j O l o O 4 K b l x W s A + c D i W T Z t r Q T D I k d 4 Q y 9 D P c u F D E r V / j z r 8 x 0 8 5 C W w 8 E D u f c S 8 4 9 Y S K 4 A d f 9 d k o b m 1 v b O + X d y t 7 + w e F R 9 f i k a 1 S q K e t Q J Z T u h 8 Q w w S X r A A f B + o l m J A 4 F 6 4 X T 2 9 z v P T F t u J I P M E t Y E J O x 5 B G n B K z k D 2 I C E 0 p E 9 j g f V m t u 3 V 0 A r x O v I D V U o D 2 s f g 1 G i q Y x k 0 A F M c b 3 3 A S C j G j g V L B 5 Z Z A a l h A 6 J W P m W y p J z E y Q L S L P 8 Y V V R j h S 2 j 4 J e K H + 3 s h I b M w s D u 1 k H t G s e r n 4 n + e n E F 0 H G Z d J C k z S 5 U d R K j
A o n N + P R 1 w z C m J m C a G a 2 6 y Y T o g m F G x L F V u C t 3 r y O u k 2 6 l 6 z f n P f r L U a R R 1 l d I b O 0 S X y 0 B V q o T v U R h 1 E k U L P 6 B W 9 O e C 8 O O / O x 3 K 0 5 B Q 7 p + g P n M 8 f l V u R b A = = < / l a t e x i t > Fig. 3. The flowchart of the integration of the PMB SLAM filter from [13] with the IPLF.

A. Simulation Scenario
Simulations are performed for a 5G application scenario at 28 GHz with a single known BS and an unknown vehicle, which does a counterclockwise constant turn-rate movement around the BS. The transmitter at the BS side and the receiver at the vehicle side are both equipped with a uniform rectangular array (URA) with 8 × 8 antennas. Every time step, the transmitter downlinks OFDM signals to the vehicle, with 16 symbols, 64 subcarriers, and 200 MHz bandwidths. Apart from the BS and the vehicle, there are 4 VAs, and 4 SPs in the scenario. We implemented the IPLF into the PMB SLAM filter as in Fig. 3, which we denote as the IPL-PMB SLAM filter. We compared the proposed IPL-PMB SLAM filter with the EK-PMB SLAM filter [13], which both consider the 10-best data associations every time step. We evaluated the mapping performance by the generalized optimal subpattern assignment (GOSPA) distance [31] for both VAs and SPs, and positioning performance by the root mean squared error (RMSE) and standard deviation over time. We also measured the execution time of the two SLAM filters. More details and parameter settings can be found in [13]. The results were averaged over 100 Monte Carlo simulations. All codes were written in MATLAB, and simulations were run on a MacBook Pro with a 2.6 GHz 6-Core Intel Core i7 processor and 16 Gb memory. Factorize the covariance P by

B. Results and Discussion
3: for c ∈ {1, ⋯, 2dš} do 4: Compute cubature poinť Compute the propagated cubature poinť Compute approximations of innovationz, innovation covariance Sžž and cross-covariance Sšž bỹ 8: Update m and P using (15)   that solid lines are below dashed lines in both figures, which shows the proposed IPL-PMB SLAM filter has better mapping performance. The reason is that the linearization is done at the prior mean in the EK implementation, which does not use the information provided by the measurement and provides worse approximation. Unlike the EK implementation ignores the measurement in linearization, the proposed IPL- PMB filter makes use of the measurement, and linearizes the measurement function with respect to the posterior PDF, which provides more accurate and precise approximation to the measurement function, as we discussed in Section IV-B. This is also the reason why the IPL-PMB filter has slightly better accuracy in positioning performance, as its RMSEs are lower in Fig. 6. Although the accuracy is not improved significantly, the IPL implementation provides much more precise results, as standard deviations of the UE state decrease to half of what EK-PMB SLAM filter provides approximately, shown in Table I. Table II displays the execution time of the two SLAM filters. Since two algorithms have the exactly same prediction step, the prediction time is nearly identical. The proposed IPL-PMB SLAM filter takes longer time than the EK-PMB SLAM filter in the update step, with 28.1 ms and 13.6 ms per time step, respectively. The reason is that the linearization is done iteratively, and the sigma point principle is used to approximate innovation, innovation covariance, and crosscovariance between state and innovation in Algorithm 1, while the EKF directly linearizes the measurement function by using the first-order Taylor series at the prior mean. This leads to (2dš × A)-fold complexity in updating the joint state under each DA, where A represents the number of iterations and is 5.3 on average over time and DAs in our implementation. Although the IPL-PMB takes longer time, online and realtime operation of the filter could still be guaranteed with more accurate and precise performance. based SLAM filter in a 5G downlink scenario, which utilizes the measurement to linearize the measurement model with respect to the posterior PDF, and proposed the IPL-PMB SLAM filter. Via simulation results, we demonstrate that the proposed IPL-PMB SLAM filter is the same as the EK-PMB SLAM filter that can map the environment and estimate the UE simultaneously. Our results also indicate that the implementation of the posterior linearization helps the PMB SLAM filter acquire more accurate and precise estimates. Although additional computational cost is needed to obtain such performance gain, online and near real-time operation of the filter could still be guaranteed.