Bayesian Filtering for Joint Multi-User Positioning, Synchronization and Anchor State Calibration

Millimeter (mmWave) positioning can go beyond classical localization, allowing to extract more complete situational awareness in terms of, e.g. clock offsets, antenna orientations or landmark locations. In this article, we formulate an extended Kalman filtering (EKF)-based framework called MU-PoSAC (Multi-User Positioning, Synchronization and Anchor State Calibration), that allows to jointly estimate and track the locations and clock offsets of multiple users together with the unknown locations and orientation offsets of the anchors, building on angle-of-arrival (AoA) and time-of-arrival (ToA) measurements. We provide an extensive set of numerical results in the context of mmWave 5G New Radio (NR) deployment in an industrial facility with moving robots and other industrial vehicles, incorporating full-scale ray-tracing for accurate propagation modeling as well as actual uplink reference signal based AoA and ToA estimators. Our numerical results show that estimating and tracking the overall system state is feasible, and that a single reference anchor can further enhance the estimation accuracy. In addition, more users are shown to lead to better performance, due to the beneficial coupling of the anchor state. Therefore, our study demonstrates that in order to maximize the estimation performance, it is desirable to have at least one anchor state precisely known, and to have multiple users in the system. Finally, the important practical aspect of Line-of-Sight (LoS) blockage is addressed. It is shown that in the considered industrial use case, the proposed MU-PoSAC framework can offer robustness against intermittent LoS blockage.


I. INTRODUCTION
I N ADDITION to the enhanced mobile broadband service, the 5G New Radio (NR) mobile network technology is designed Manuscript  to support various industrial and other mission-critical use cases and applications [1]. One important ingredient, especially in vehicular systems, is the ability to extract accurate and timely situational awareness [2]. In general, such situational awareness can be obtained with various sensors, such as cameras, radars, and lidars, but also through the 5G NR radio measurements [3], [4], [5]. Compared to location awareness [6], situational awareness subsumes not only the locations (i.e., the coordinates) of the involved vehicles and other devices but also other relevant knowledge such as the orientation of either transmitter or receiver, or both [7], [8], [9], prevailing clock offsets [10], [11], [12], as well as features of the propagation environment [3], [4], [5], [13], [14], such as landmark locations. In general, improved situational awareness can enable enhanced quality-of-service (QoS) [15], safety [16] and operational efficiency, e.g., in different industrial Internet-of-Things (IIoT) applications [17]. Localization is generally performed under the assumption of perfect knowledge of the location and orientation of the available infrastructure (anchors) [18], [19], [20]. In practice, however, the locations and orientations of the involved network anchors -referred to as the anchor state in the continuation -may only be imperfectly known [21], [22]. In this article, we focus on the challenging case of localizing users under unknown anchor state with specific focus on millimeter wave (mmWave) 5G NR systems with time-and angle-based radio measurements. The motivation for considering probabilistic or unknown anchor states stems from the fact that the base-station or transmission point (TRP) deployments in 5G and other future generation radio access networks are not necessarily permanent, but can also be dynamic or frequently regenerating. Particularly in IIoToriented private networks [23], [24], dynamic entities such as unmanned aerial vehicles (UAVs) that can be flexibly deployed in the areas of interest can serve as the anchors for positioning, communications and/or sensing [25], [26], [27]. In such cases, there clearly exist uncertainties in both the locations and the orientation offsets of the anchors that need to be estimated together with the users' locations and relative clock offsets. This is the main objective of this work. Furthermore, such uncertainties can also take place in more permanently deployed network anchors, e.g., in a distributed network structure where the coordinates and orientations of the involved remote radio units (RRUs) are not necessarily accurately known [24]. This is because the communication system and corresponding radio network planning related figures of merit (network capacity and Earlier works on positioning with anchor state uncertainty have mainly been conducted in the context of cooperative wireless sensor networks and, in particular, non-Bayesian localization. The anchor uncertainty can be either treated as a nuisance parameter or estimated jointly with the user locations. In the former category, [22] performed robust localization in the presence of anchor uncertainty via convex optimization, while [28] considered a spring mass method for the same purpose. Explicit estimation of anchor positions was considered from an algorithmic perspective in [29] and performance bound perspective in [30]. In [31], the anchor node location is considered as a nuisance parameter in an expectation-maximization algorithm, while [32] interprets agents as mobile anchors. Compared to non-Bayesian approaches developed in the aforementioned works, variational Bayesian inference was also applied to perform positioning of nodes in [33], [34], under line-of-sight (LoS) or mixed LoS / non-line-of-sight (NLoS) conditions. In terms of dynamic models, particle-based approaches have been considered to deal with anchor location uncertainties [35], while extended Kalman filter (EKF) formulation for estimating anchor orientation uncertainties was provided in [15]. Notably, except [15], all the existing works have considered only the anchor location uncertainty, thus ignoring anchor orientation which is of crucial importance with angle-based measurements.
Furthermore, multi-user positioning and synchronization have been addressed in [36], [37], [38] in the context of underwater networks and ultra wideband (UWB) networks. Specifically, in [36], UWB-based multi-user positioning was pursued while addressing the achievable performance under different synchronization accuracies and time-difference-of-arrival (TDoA) measurements. It was observed that the synchronization performance highly depends on the update frequency of synchronization packets, yielding a tradeoff between positioning accuracy and the synchronization update rate. The impact of varying user numbers on positioning performance was, however, not addressed. In [37], [38], multi-user underwater acoustic positioning systems were proposed and tested, in which the synchronization among the surface vessels serving as the system anchors was achieved through global positioning system (GPS). Such GPS-based approach can certainly provide an accurate time base and accurate anchor coordinates, however, GPS is commonly not available indoors or in other challenging radio environments while also does not resolve the anchor orientation challenge. Finally, as overviewed in [39], hybrid systems fusing, e.g., acoustic measurements and UWB-based radio measurements can offer very good positioning accuracy even in challenging environments such as underground mines, however, calling for a carefully designed dedicated positioning infrastructure.
In this article, we describe a network-centric Bayesian framework for multi-user positioning, synchronization, and anchor state calibration (MU-PoSAC), with primary application focus on 5G NR mmWave systems assuming no additional aiding infrastructure such as GPS. An illustration of the considered system scenario is shown in Fig. 1, where all the involved entities and the corresponding unknown or uncertain parameters are depicted and highlighted. As the orientation offsets and the locations of the network anchors are all estimated simultaneously with the locations and the relative clock offsets of the involved users, multiple users are assumed to be involved for enhanced measurement geometry and degrees of freedom. Compared to the existing literature, MU-PoSAC is a sequential estimation framework, which takes into consideration not only the user clock offsets, but also the uncertainty of both location and orientation offsets of the anchors. Thus, compared to the existing methods that treat the anchor state as a nuisance variable -leading, at best, to robust localization methods -we aim to infer also the anchor states, which is a different and more challenging problem formulation. The key contributions and novelty of this article can be thereon stated and summarized as follows: 1) We propose and describe an EKF based formulation for network-centric joint user positioning, synchronization, and anchor location and orientation estimation with multiple moving users; 2) Building on 5G NR beam management process [40] and the underlying uplink reference signals, we provide a cascaded angle-of-arrival (AoA) and time-of-arrival (ToA) estimation method with raw physical layer inphase/quadrature (IQ) samples, containing novel multibeam fusion for enhanced performance; 3) We address the initialization challenge, and describe efficient methods for the initialization of the unknown parameters, by using the TDoA measurements at the first time step to construct a measurement-induced prior density of the user positions; 4) We provide extensive ray-tracing based numerical evaluations in an industrial facility containing moving vehicles, while assess the estimation and tracking performance and its dependence on various involved system parameters; 5) We address the practical NLoS or blockage challenge and show that the proposed MU-PoSAC approach has built-in robustness against missing LoS measurements; The rest of the article is organized as follows. Section II provides the assumptions, physical signal models and the AoA and ToA estimation methods. In Section III, we present and formulate the proposed MU-PoSAC framework for joint multiuser positioning, synchronization and anchor state calibration. Section IV presents the ray-tracing based industrial evaluation environment, provides numerical results regarding the AoA and ToA estimation accuracy, and describes the EKF initialization. Section IV then present and analyzes the actual MU-PoSACbased performance results for joint estimation and tracking of both user and anchor states under different scenarios. Finally, conclusions are drawn in Section VI.
Notation: Throughout this paper, the identity matrix is denoted by I, while complex-conjugation, transpose and Hermitian trasponse are denoted by (·) * , (·) T , and (·) H , respectively. Furthermore, diag(·) and blkdiag(·) refer to diagonal and block-diagonal matrices, respectively, while and ⊗ denote the Hadamard and Kronecker products, correspondingly. Finally, the C and R are individually denoted as the complex and real number sets. The normal distribution is referred to as N (·) and, respectively, the complex normal distribution as CN (·).

II. PHYSICAL SIGNALS AND AOA/TOA ESTIMATION
We consider a multi-user system, as illustrated in Fig. 1. The main purpose of our work is to estimate and track the locations of both the users and the anchors, while the orientation offsets of the anchors are also assumed unknown and hence estimated. In addition, the clocks of the users are assumed to be independently drifting, while anchors are mutually synchronized.
In terms of the available physical signals and corresponding measurements, we assume that all the users are periodically transmitting uplink pilot or reference signals. Specifically, in the context of 5G NR, the uplink sounding reference signal (SRS) transmissions [41] form one feasible option that are also considered as concrete signal structure in our numerical experiments in Sections IV and V. While the reference signal transmission can be omni-directional, we further assume that the anchors observe them in a beam-based manner, reflecting NR beam-sweeping procedure [40] at the RX side. The RX beam-sweeping procedure can take place, e.g., once per 100 ms, depending on the network configuration, and we refer to such time window as a measurement cycle in the following, indexed with i. Furthermore, as a concrete example, a uniform rectangle array (URA) is assumed as the antenna model in each anchor with a total number of N R = N el N az antenna elements. Finally, the existence of a LoS path is implicitly assumed in the algorithm derivations, while in the ray-tracing based numerical evaluations, true propagation geometry with all physical paths are considered. Furthermore, intermittent lack of LoS measurements is explicitly addressed in the actual positioning and state estimation stage along the numerical results in Section V.

A. Signal and Channel Models
In the following, the set of considered users is denoted by K, while the corresponding set of anchors is M. To this end, the kth user with k ∈ K = {1, 2, . . . , K} is transmitting the uplink pilot signal utilizing orthogonal frequency division multiplexing (OFDM) waveform with the allocated set of subcarriers being denoted by P k . Furthermore, we assume that |P k | = P , independent of k, and that when multiple users are involved, they are allocated orthogonal sets of subcarriers such that P k 1 and P k 2 are disjoint for any k 1 = k 2 . For notational simplicity, omni-directional transmission is assumed while the extension to beamformed or other directional antenna systems is straightforward, see, e.g., [4], [13].
To this end, the received complex sample at anchor m ∈ M = {1, . . . , M} at measurement cycle i and subcarrier p ∈ P k , stemming from the user k, can be expressed as refers to the RX spatial noise vector at subcarrier p during the RX beamformer setting q within measurement cycle i.
Furthermore, by considering all the propagation paths in the environment, the single-input multiple-output (SIMO) spatial channel vector at sub-carrier p, denoted by Λ (k) m [p, i] ∈ C N R ×1 , can be expressed as where L is the overall number of propagation paths and f sc refers to the sub-carrier spacing. Each path is described by the path gain η l,m ) ∈ C N R ×1 refers to the URA response, which can be calculated as [42], [43] where ⊗ denotes the Kronecker product and β 0 N R , φ represents the antenna gain of the URA, which is a function of the overall number of antenna elements N R = N el N az as well as the AoA pair that is defined as where the azimuth AoA θ (k) l,m = (−π, π) together with the coelevation AoA ϕ (k) l,m = (−π/2, π/2) specify the received signal direction of the lth path. Finally, we express the corresponding uniform linear array (ULA) responses as

B. AoA and ToA Estimation
Based on the beamformed reference signal observations described above, the AoA and the ToA can be estimated as described in the following. These AoA and ToA estimates are then serving as inputs or measurements to the actual positioning, synchronization and anchor state calibration engine, described in Section III.
1) AoA Estimation: The overall estimation procedure starts by calculating the beam reference signal received power (B-RSRP) quantities. At anchor m, and considering the qth RX beamformer and kth user reference signal transmission, the B-RSRP can be expressed as Since the received samples are obtained via a specific beamformer that is a function of an AoA pair as shown in (4), a given B-RSRP is essentially parameterized with the same AoA pair as the utilized beamformer. Thus, by collecting the B-RSRP values over all the angle pairs within the utilized beamformer space or codebook, the LoS AoA of the considered transmitter-receiver (i.e., user -anchor) pair at time instant i, denoted for simplicity by φ whereq is the beam index that corresponds to the maximum B-RSRP at measurement cycle i while Ω refers to the overall set of angles that is covered by the set of beamformers. Intuitively speaking, the estimation performance of the method in (7) is inevitably subject to the angular resolution of the utilized set of beamformers, e.g., a discrete Fourier transform (DFT)-based codebook [44]. Typically, the angular resolution of the utilized codebook is proportional to the antenna array size, i.e., the larger the array size, the finer the angular resolution. Therefore, the AoA estimation accuracy of the method in (7) may be insufficient for high-efficiency positioning with array sizes of practical interest.
One way of improving the AoA estimation accuracy, and thus to address the above challenge, is to fuse the angular information from multiple beams -referred to as the multi-beam fusion method in the rest of the article. To this end, instead of one beam, a set of N B highest beams is selected and fused together to obtain the final LoS AoA estimate. In this case, the estimator in (7) can be re-defined aŝ where Ω selected [i] denotes the vector consisting of the N B selected angle pairs, withq 1 referring to the beam index that corresponds to the highest B-RSRP whileq N B is the beam index corresponding to the N B th highest B-RSRP at measurement cycle i. Additionally, the normalized weight vector Γ ∈ C N B ×1 can be expressed as The AoA estimation accuracy through the fusion of different numbers of beams will be assessed and discussed along the numerical results in Section IV-B. In general, stemming from the beam-sweeping like physical observation model, it can be noted that there is an inherent tradeoff between the estimation accuracy and the estimation latency. Specifically, a higher estimation accuracy relies on a finer beam resolution, which in turn implies a larger number of beamformers and thereon a longer reference signal measurement latency. This tradeoff can be resolved by narrowing down the possible angular range based on the previous estimates, such that only a sub-set of the overall angle pool Ω is applied in the beam sweeping process for the AoA estimation.
2) ToA Estimation: The ToA estimation is essentially dependent on the estimated AoA, given the fact that the delay and angle information of the same path are associated with each other. In other words, the received signal that contains the LoS AoA can also be utilized to find the LoS ToA, denoted for simplicity by τ in the continuation. Denoting again the beam index with the highest beam power byq 1 , the corresponding received signal samples in (1) can be stacked over all considered subcarriers into a vector r Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.
Next, according to the properties of the Fourier transform, the path delay experienced in the time-domain is reflected by the phase shifts in the frequency-domain. Therefore, the ToA can be efficiently estimated aŝ where corresponding to the beamformer q 1 , containing a phase response estimate of the channel, is denoted as where refers to the vector of the known reference signal samples, transmitted by user k at the considered subcarriers, during the beamformer setting q 1 .

C. Discussion
It is generally fair to state that there are many existing methods for AoA and ToA estimation, including methods based on MUSIC, ESPRIT, maximum likelihood, message passing, and sparse recovery. However, all these methods require perfect knowledge of the array steering vectors b URA (φ) as well as the beamformer weights w. This knowledge is hard to obtain in practice, for two reasons. First, real user devices and anchors (base-stations) only provide a beam codebook with beam index and nominal direction. This means that the beamforming weights w are not available. Second, the steering vector is generally impaired with array element spacing errors, unknown and spatially varying element responses, as well as mutual coupling. This means that b URA (φ) is only approximately known, at best. The combined lack of knowledge on b URA (φ) and w in real systems thus precludes the use of standard AoA estimation methods -a limitation that is often ignored in academic research. The proposed AoA estimation method, in turn, stems from the assumed analog array architecture together with the 5G NR beam sweeping procedure that is standardized by 3GPP for the mmWave 5G NR network. The AoA estimation method is thus fully realizable also in practical systems. Additionally, as illustrated in Section IV, its performance is close to the corresponding Cramér-Rao bound that is derived for beamformed power based observation model. Finally, the ToA estimation approach is essentially a maximum likelihood 1D search-based method and is thus asymptotically optimal, operating close to the corresponding Cramér-Rao bound.

A. Fundamentals and Notation
We next address how the estimated ToAs and AoAs, obtained at different measurement cycles i, can be efficiently utilized to estimate and track not only the locations of the involved users, but also the locations and orientations of the anchors as well as the time-varying clock offsets between each user and the network. The non-linear mapping procedure from the available ToA and AoA measurements to the locations, orientations and relative clock offsets is described and accomplished using an EKF, which is generally known to be a sequential minimum mean square error (MMSE) estimator [45].
For the purpose of later definitions and derivations, we next define the following quantities:  It is highlighted that the states of all K users are deliberately concatenated to be jointly estimated, since they are indirectly mutually coupled through the anchor states and the related measurements. With multiple users in the network, the anchor state can thus be more accurately calibrated due to a stricter geometric constraints formed by multiple users, with the achievable performance gains being demonstrated numerically in Section V.
For sequential Bayesian estimation [46], the following joint probability of the form is computed for each time instant in order to characterize the joint posterior density given the available controls and measurements from the first to the current time instant i, as well as the initial state [47]. Starting with an estimate for the distribution the joint posterior at time i is computed using Bayes theorem. This requires that the state transition model and observation model are defined describing the effect of the controls and measurements, respectively. Initialization of the filter is presented and discussed in Section IV-C.

B. User State Evolution and Measurement Models
As static anchors are considered, their states are modeled as time-invariant. The user states, in turn, are dynamic, and the transition density describing their evolution is expressed as where f (s[i − 1], u[i]) represents the nonlinear function that describes the user motion and Q is the covariance matrix. The nonlinear motion can be modeled using the kinematic bicycle model [48], and the clock behavior using a first order auto-regressive model [49]. Since a synchronous network is considered, the clocks of all the anchors are synchronized to a reference model, while each user has time-varying clock offset T , the transition model for a single user is now given by where is the steering angle, Δt denotes the time interval between two consecutive measurement instants and WB k is the distance between the front and rear axels of the user (which we simulate as industrial vehicle). The complete transition model is defined as . . .
For convenience, let us express the process noise covariance of the controls, Q u , and states, Q s , independently so that we can define them as In (17), σ 2 v and σ 2 γ are the variances of the speed and steering angle controls, and in (18), σ 2 z and σ 2 ζ are the variances of the z-coordinate and clock skew. Here, the noise related to the clock offset can be understood as the oscillator uncertainty of the applied clock, i.e., jitter.
The observation model is required to describe the measurements and their dependence on the state variables which we express as   s 1 [i], m) . . . where is the measurement vector of user k and the M anchors. The full covariance matrix is In general, the variances of measurements can be acquired empirically or through closedform equations as shown, e.g., in [50]. For our system model, we highlight this further along the numerical results in Section IV-B.

C. Extended Kalman Filter
As is generally well-known, the computation of the joint probability in (13) can be solved using the Bayes' rule [46] iteratively over time. Approximating the joint distribution using a Gaussian, expressed as (23) and using a first-order Taylor series based Gaussian approximation to the nonlinear models results in the well-known extended Kalman filter (EKF) recursion [51]. In (23) , (24) so that the time-update of the EKF can be expressed aŝ where are the Jacobians of f (·) evaluated with respect to s and u, respectively. Thereafter, once measurement y[i] becomes available at time i, the mean and covariance can be corrected using  (24) with the predicted values in (25). It is to be noted that the nonlinear filtering problem could be solved using any Gaussian assumed density approximation, such as the unscented Kalman filter (UKF) or cubature Kalman filter (CKF). However, since the underlying models are differentiable and the EKF provides good accuracy combined with low computational overhead, the EKF is the preferred choice in this article. The main advantage of the EKF over other nonlinear filtering methods, such as the UKF or the particle filter, is its relatively low computational overhead compared to its performance [51].

D. Performance Bound
The performance bound of all the parameters of interests that are estimated by the algorithm is referred to as posterior Cramér-Rao lower bound (PCRB). In essence, the bound infers that the mean square error (MSE) of an algorithm is in general larger than J −1 in the positive semi-definite sense [45], expressed as where J represents the Fisher information matrix (FIM) and x(y) is the estimate of the state x at the output of the algorithm whose inputs are the available measurements y. In general, the FIM can be computed from two additive parts [52], as where J prior = E[−Δ x x logP(x)] refers to the available prior information of the state and J meas = E[−Δ x x logP(y|x)] represents the information obtained from the measurements, with Δ x x referring to the second-order partial derivative. For the considered nonlinear filtering problem with additive noise, the FIM can be recursively computed by plugging [52, eq. (34-36)] into [52, eq. (21)]. Furthermore, using the matrix inversion lemma to simplify the resulting expression, the FIM can be calculated iteratively as where and We will utilize PCRB as one fundamental performance reference in our numerical studies in Section V.

IV. EVALUATION SCENARIO, MEASUREMENT ACCURACY, AND INITIALIZATION
In this section, the ray-tracing based industrial evaluation scenario is first presented. Then, the estimation accuracy of the positioning-related parameters (i.e., AoA and ToA) based on the proposed beam sweeping approach is evaluated under different antenna and beamforming codebook configurations. Finally, the applied initialization method is introduced, offering the prior information for the initial state of the EKF-based MU-PoSAC.

A. Evaluation Scenario
In this work, we focus on the indoor industrial scenario illustrated in Fig. 2, where the warehouse or factory site consists of several industrial vehicles and devices together with several fixed or dynamically deployed but static anchors. In general, the anchor density plays an important role from the achievable performance point of view because more anchors leads to more measurements and increased LoS probability. However, as discussed in [53], one important design criteria is to minimize the total number of required anchors while achieving as good accuracy as possible. Thus, given the fact that both angle and delay measurements are utilized, we consider overall three anchors being deployed in the locations of green squares illustrated in Fig. 2. These anchor locations stem from the basic radio network planning such that they provide essentially full radio coverage for the considered warehouse whose physical dimensions are approximately 70 m × 26 m × 18 m.
Very importantly, for realistic and accurate propagation modeling, full ray tracing calculations are carried out with Wireless InSite [54], that incorporates an accurate digital map of the industrial facility and the different objects therein. This ensures that all the multipath components are also correctly modeled from the received signals perspective, in addition to the basic LoS path. The assumed 5G NR numerology and basic evaluation assumptions are summarized in Table I. Furthermore, in terms of the antenna configurations at the anchors, the anchor 3 contains two URA panels to cover a 360 • spatial area, while the anchors 1 and 2 contain one URA panel each. Such antenna system assumptions together with the anchor locations ensure seamless radio coverage for the considered industrial facility, while being also inline with 3GPP 5G NR standardization and evaluation assumptions [55]. Finally, the user trajectories are generated using the random waypoint (RWP) model [56], while the movement along the waypoints is driven by the kinematic bicycle model [48]. This way, realistic movement patterns of industrial vehicles can be modeled and generated.

B. AoA and ToA Measurement Accuracy
The accuracy of the estimated AoAs and ToAs that serve as the observations and inputs of the MU-PoSAC framework plays an important role in the achievable state estimation performance. Operating at 28 GHz with 100 MHz channel bandwidth, we compare three implementation-feasible antenna configurations each with different angular resolution or separation, namely 8 × 8 URA with 4 • separation, 16 × 16 URA with 2.2 • separation, 32 × 32 URA with 1.1 • separation. The angular resolutions are the same for both elevation and azimuth domain, and are deliberately selected to be smaller than the corresponding half-power beam width (HPBW) 1 [57] to ensure sufficient 3D spatial coverage in AoA estimation. The considered reference signal structure builds on 5G NR uplink SRS with comb-4 configuration [41]. For each beam during a beam-sweeping procedure, SRS is transmitted using a single OFDM symbol with full band allocation and the above noted comb-structure.
We will first focus on the estimation performance of the elevation and azimuth AoA when fusing different numbers of beams, through the approach proposed in Section II-B. In Fig. 3(a) and (b), the angular root mean square error (RMSE) results are shown for random UE trajectories along the open area within the warehouse. From the estimation accuracy perspective, the array configurations with more antenna elements results in enhanced performance. Furthermore, because of the finer angular resolution, the RMSE performance obtained by using 16×16 URA and 32×32 URA are observed to be closer to the corresponding Cramér-Rao lower bound (CRLB), calculated in this work according to [50], [58] for the considered beamformed power based observation model, compared to the 8×8 URA case.
Additionally, an interesting phenomenon is observed when it comes to the impact of varying the number of fused beams, N B . Specifically, for both elevation and azimuth AoA, the RMSE is clearly decreasing when N B increases from 1 to around 4 or 5. Such improved performance is mainly because of the weighted average calculation based on the B-RSRP, such that the potential beam misalignment using 1 beam can be corrected by proper fusion of angular information from other nearby beams. However, the RMSE starts to then increase when N B is further increased. Such degradation in performance mainly originates from the fact that after a certain N B value, the angular information from multipath scattering components are being integrated in the fused beam pool, resulting in a drift away from the LoS angle. Hence, we conclude that the multi-beam fusion can provide clear performance benefits while the exact number of fused beams is to be optimized in given deployment scenario.
Finally, we plot the cumulative density function (CDF) of the estimation errors of elevation AoA and azimuth AoA in Fig. 3(c), while include also the corresponding error CDF of the ToA estimation. In this illustration, we focus on the example case of 16×16 URA while fusing beams with the five highest B-RSRP values (i.e., N B = 5). We can clearly observe that very accurate ToA estimation can be obtained, with approximately 95% of the error samples being less than 1 m. The AoA estimation accuracy in both domains is also very high, though still somewhat higher in the elevation domain. This is mostly due to the fact that the users move across a larger horizontal span than the vertical span. Moreover, the angular spread is in general lower in elevation domain than in azimuth domain [55].

C. Filter Initialization
All sequential filtering solutions, such as the proposed EKFbased framework, need to be initialized at the beginning of the filtering process. From the anchor state perspective, the initial location uncertainty of the anchors Σ mm defined in (24) can be decomposed as Σ mm = I M ×M ⊗ Σ (j) mm for each individual anchor, the jth element being where the location covariance matrix is defined as Σ loc = σ 2 A I 3×3 in which σ A refers to the uncertainty in any of the individual directions in 3D. Similarly, an equal initial uncertainty in the orientation offsets in both elevation and azimuth domains is assumed and denoted as Σ α = σ 2 α I 2×2 . For simplicity, we also assume that all the anchors share the same initial uncertainty in their states, and the prior distribution of the jth anchor state is a Gaussian withm j ∼ N (m j , Σ (j) mm ). In terms of the user initialization, the parameters that need to be initialized are locations, headings and relative clock offsets. For the location, we apply a TDoA-based quasi-Newton method to find the initial user location estimates based on the erroneous initial anchor location estimates. The principle of the applied method is illustrated in Fig. 4(a), with the corresponding distributions under three numerical examples values of σ A being calculated over 800 trials and shown in Fig. 4(b). It can be noted that the resulting initial location errors are larger than the corresponding initial anchor location uncertainty, the 68th percentiles being 0.9 m, 2.2 m and 4.1 m for σ A = 0.2 m, 1 m and 2 m, respectively.
As the proposed Bayesian framework utilizes also the user heading, the corresponding initialization can build on local motion sensor(s) being then transmitted towards the network. The corresponding uncertainty is modeled as a Gaussian error with a standard deviation of 4 • , modeling realistic performance of state-of-the-art sensors [59]. Finally, the initial relative clock offsets ρ and skewsρ for all the users are randomly picked from Gaussian distributions of N (0, (10 us) 2 ) and N (20 ppm, (200 ppm) 2 ), where ppm stands for parts per million [49]. Furthermore, the clock skew driving noise is set to σ 2 ζ = 10 −14 s 2 [60], and the process noise variance in the z-coordinate is set to σ 2 z = 10 −10 m 2 since we primarily focus on industrial vehicles moving on ground. Finally, the control noise defined in (18) is set as σ v = 0.3 m/s and σ γ = 3 • [48] throughout the evaluations.

V. FILTERING RESULTS AND ANALYSIS
In this section, we evaluate and demonstrate the actual MU-PoSAC EKF performance for positioning, synchronization and orientation offset estimation. We use the AoA and ToA estimates from the previous section, with 8 × 8 antenna configuration, building on the ray-tracing environment and the described estimator solutions. The performance is also compared with multiuser positioning and anchor state calibration (MU-PoAC), i.e., an alternative approach that deploys only AoA estimates, which can be obtained as a special case from MU-PoSAC. Also the PCRB expressions presented in Section III are evaluated and shown as reference. In case only a single user is considered, the methods are called simply PoSAC and PoAC, respectively. Finally, in the following numerical results, we apply the closedform approach as in [26], [50] for setting the measurement covariance entries. Supplementary multimedia and other materials are available at https://research.tuni.fi/wireless/research/ positioning/muposac/

A. Estimation Performance Vs. PCRB
First, we focus on the estimation performance in the basic single-user case and corresponding comparisons against the PCRB. In Fig. 5, the achieved RMSE and the PCRB of the four key parameters are shown. In this evaluation, the initial uncertainty of anchor location is set to σ A = 1 m while the orientation uncertainty is set as σ α = 6 • . The red solid, blue dashed-dot and black dashed curves represent the performance of PoSAC (utilizing both AoA and ToA measurements), PoAC (utilizing only AoA) and PCRB, respectively, where the PCRB is calculated for the case of both AoA and ToA measurements. From the anchor state perspective, we see that the EKF solutions progressively tend towards the PCRB for both the locations and the orientation offsets. Similarly, it is seen that the estimated and tracked user state becomes gradually better, getting closer to the PCRB as the time goes on. Furthermore, we can observe that PoSAC outperforms PoAC in all the estimated parameters mainly due to the additional measurements in delay domain. More importantly, the very minor difference or gap between the PoSAC and PCRB after initial convergence shows that the proposed EKF solution is efficient. It is also important to note that the estimation performance does not significantly improve even if the iteration steps are continued beyond the shown 1000 steps. This motivates towards the actual multi-user study and the potential positive impacts of multiple users, addressed next.

B. Impact of Multiple Users
Next, we address and analyze the estimation performance when multiple users are present and considered in the system. Two σ A values (0.2 m and 2 m) are considered to assess the performance under low and high anchor location uncertainties, while the uncertainty of orientation offsets, σ α , is again set to 6 • . The achieved positioning error performance in 3D, covering both the anchors and the users, is presented in Fig. 6(a) for 1, 3 and 5 users in terms of the error CDFs after initial convergence referring to the first 500 iteration steps. It is seen that the positioning error is in general higher when σ A is larger. Importantly, the existence of multiple users helps to improve the overall estimation performance, especially at larger values of σ A . As a concrete numerical example, the average positioning error is 1.5 m lower with 5 users compared to the single-user case. Similar behavior can be observed for the performance of orientation offsets that are presented in Fig. 6(b). We can also observe that for a given σ α value, larger initial anchor location uncertainty leads to a worse performance in orientation offset estimation. This stems from the joint estimation approach and nature of MU-PoSAC.
Overall, the obtained results show that having more users in the system clearly helps in achieving more accurate estimation results. Finally, the performance of user heading and clock offset estimation is not explicitly shown since very similar performance trends were observed.

C. Impact of Initial Anchor State Uncertainty
We next address the performance of PoSAC and PoAC under a wider range of initial uncertainty of the anchor state, i.e., σ α and σ A . The corresponding estimation accuracy of the anchor locations is presented in Fig. 7, in terms of the steady-state RMSE, as a function of the orientation offset's uncertainty σ α while considering 3 different values of the anchor location uncertainty σ A . In this case, 2 users are considered in the system.
Generally speaking, based on the results in Fig. 7, MU-PoSAC again systematically outperforms the MU-PoAC. Additionally, for the whole considered value ranges of the uncertainties, MU-PoSAC provides gain compared to the prior, though at largest orientation offsets the gains are already fairly small. These results and findings thus indicate that when there is no accurately known reference anchor available, achieving very high estimation performance is difficult. This is because the posterior distribution calculated by the EKF based on the measurements is not maximized at the true state of the users or the anchors. Such finding motivates towards the further numerical studies presented in Section V-F, where the performance is evaluated when there exists at least one precisely known reference anchor.

D. Impact of User Velocity and Measurement Time Interval
Next, the impacts of user speed and measurement time interval are investigated. In these simulations, only one user is considered for simplicity, the anchors are initialized using σ A = 1 m and σ α = 6 • , and the durations of the experiments with different speeds are adjusted such that the trajectories have equal length. The anchor and user localization RMSEs are illustrated in Fig. 8, and at a first glance, the results may seem counter-intuitive since the performance improves with higher user speeds. In general, it is expected that localization accuracy of the user degrades when maneuverability of the user grows. However, in our scenario, it is important to note that the user and anchor estimates are tightly coupled and as long as the anchors can be estimated accurately, good user tracking performance will follow. Interestingly, RMSEs of the static anchors converge approximately to the same value when the measurement time intervals are the same as shown in Fig. 8(a). Since the anchors can be estimated with comparative performance, also the localization accuracy of the users converge to the same value as illustrated in Fig. 8(b). On the other hand, locating the anchors relies on accurate user estimates and in general, lower/higher measurement time interval Δt enhances/degrades tracking accuracy of the user which in turn increases/decreases the localization accuracy of the anchors as illustrated in Fig. 8. Thus, we can conclude that the measurement time interval and the path of the user and the underlying geometric constraints it imposes on the anchor estimates, have in general a much larger impact on accuracy than the exact speed.

E. Impact of Intermittent LoS Blockage
In practical deployments, it is possible that the LoS path does not always exist between the user and a network anchor, the impact of which we will analyze next. We model the existence of LoS as a two-state Markov process such that the existence of the LoS path at the next time instant depends only on the present state. The state transition matrix of the model is given by in which P 1,1 and P 2,2 denote the transition probabilities of the LoS and NLoS states, respectively. Furthermore, we consider two scenarios for the existence of LoS. In scenario 1, the Markov process is independent for the different anchors and for each time instant, mixed LoS / NLoS conditions can thus exist. In scenario 2, the Markov process is shared among the anchors and at time step i, either the LoS exists to all anchors, or to none of them.
At time instances when the LoS is blocked for one or more anchors, the update step of the filter can still be carried out using a reduced set of LoS measurements. In this case, the elements of the Jacobian matrix corresponding to the blocked anchors are set to zero and the filter update is carried out normally. It is important to note that even though an anchor is blocked, the estimates of the anchor are updated using measurements from the other anchors that are not blocked. This might seem counter-intuitive, but is possible due to the fact that the covariance matrix is full, meaning that the anchors' states have cross-correlations among each other. Hence, the measurements of an anchor with LoS can be used to update the state of an anchor that is in NLoS. In case that all the anchors are in NLoS, the update step has no effect on the estimated mean and covariance since the Jacobian is an all zeros matrix.
The results in the two scenarios and for various transition probabilities are visualized in Fig. 9. As illustrated by the results, PoSAC can tolerate intermittent LoS blockages well: if P 1,1 ≥ 0.9 and P 2,2 ≤ 0.5, the filter nearly achieves the same performance as without LoS blockages (P 1,1 = 1.0). The most notable differences are in the convergence properties of the filter and since there a fewer measurements, convergence of the filter takes longer. The results also imply that the developed estimator can tolerate quite severe NLoS conditions. For example, if P 1,1 = 0.5 and P 2,2 = 0.9, under 20% of the measurements are received, with the longest realized time period without a LoS path being 96 samples and the corresponding average duration being 9.8 samples. Nonetheless, accuracy of the system degrades approximately by 5% for the anchor localization and by 10% for user localization. This implies that the system is robust to measurement outages, since the filter can rely on the open loop transition model when the LoS path does not exist. However, relying solely on the transition model accumulates errors of the estimator, but as long as the time period without measurements is short enough, the developed filter is able to compensate for the accumulated errors until the LoS is present again.
In general, performance of the system degrades as the process noise increases and it solely defines the accuracy of the system during LoS blockages. To demonstrate the system performance with higher process noise, the control noises are next doubled to σ v = 0.6 m/s and σ γ = 6 • in the most difficult LoS blockage scenario. The results are illustrated in Fig. 9 using the dashed cyan lines and in both scenarios, the accuracy degrades slightly. It is expected that under yet more severe LoS blockage scenarios the performance decreases more, whereas in milder LoS blockage scenarios the accuracy is not affected as much since the filter can correct the inaccurate transition model estimates using the measurements.

F. Impact of Known Reference Anchor(s)
We next continue studying the estimation performance when certain amount of anchors in the system are actually precisely known. From hereon, we also focus on (MU-)PoSAC only for presentation simplicity. To this end, in Fig. 10, the steady-state RMSEs of all five parameters of interests are presented when there are 0, 1, 2 or 3 reference anchors with perfectly known states (locations and orientation offsets). The states of uncertain anchors are initialized with σ A = 1 m and σ α = 6 • , and only a single user is considered in the system. Overall, two major observations clearly stand out. Firstly, the existence of known reference anchor(s) brings clear benefits to the estimation performance of all the involved parameters. Secondly, the performance gain brought by having more than one known reference anchor is already relatively smaller. Considering the user location as a concrete example, having one known reference anchor improves the RMSE from 1.1 m (all anchors uncertain) to 0.5 m, while the corresponding accuracy with three known reference anchors is 0.28 m.
Based on the observed insights in Fig. 10, we next further assess the estimation performance of anchor locations, orientation offsets and user locations when only one anchor is perfectly known. To this end, Fig. 11 shows the relevant RMSE behavior over time while also contains and shows the corresponding performance when all anchors are uncertain for reference and comparison purposes. The uncertain anchors are again initialized with σ A = 1 m and σ α = 6 • , while the impact of the number of users is also addressed.
Specifically, the anchor location estimation performance given in Fig. 11(a) shows that when all anchors are uncertain, the converged RMSE for anchor location is around 1.1 m when there is one user, while improves to 0.7 m when there are 5 users. Alternatively, with one certain reference anchor in the system, the performance achieved with only one user is about 0.6 m which is already better than that by having 5 users and no known reference anchors. Furthermore, the achieved RMSE with a known reference anchor reaches to around 0.3 m in the 5-user case. Similarly, in Fig. 11(b), we see that the highly accurate orientation offset estimation of around 0.3 • RMSE can be achieved when there are 5 users and one certain anchor. Additionally, a faster convergence is observed when more users are considered in the system. Meanwhile, the estimation performance of user localization is provided in Fig. 11(c). In general, the obtained performance is rather close to that of the anchor locations shown in Fig. 11(a). Similar conclusions can be drawn, namely that multi-user approach helps improving the performance and convergence time, and that the achieved accuracy with one user and one known reference anchor is already better than the case with 5 users and no known anchors.
Finally, we further address the performance of user location estimation that is anyway one of the most important parameters in any application, and specifically focus on the impact of σ A and σ α . The results are shown in Fig. 12, covering the cases of zero or one known reference anchor, and focusing on a two-user system in this case. It can be observed that the obtained improvement through the known reference anchor is significant, Fig. 12. Steady-state RMSE performance of user localization under different σ α and σ A values with two users in the system, and when either all anchors are uncertain or when one anchor state is perfectly known. especially at larger values of σ A . Importantly, through the known reference anchor, the user localization performance becomes considerably more robust against both the initial anchor location uncertainty and the anchor orientation uncertainty. Additionally, the results show that clearly sub-meter 3D user positioning accuracy can be reached in the system. Considering the 5G NR requirements for horizontal and vertical positioning accuracy in [61], the achieved sub-meter 3D positioning accuracy can be shown to satisfy requirements for all positioning service levels regarding absolute positioning. Similarly, as part of studying positioning use cases of 5G NR in [62], 3GPP has evaluated the desirable horizontal and vertical positioning accuracy of trolleys in factories as 0.5 m and 1-3 m, respectively. When combining the given horizontal and vertical accuracies into a 3D accuracy, it can be seen that the achieved 3D sub-meter accuracy is adequate also for the trolley use case. However, in certain use cases the importance of horizontal and vertical accuracies can be fundamentally different, and thus generalizing them to the 3D accuracy is not necessarily always suitable.
Overall, based on the provided numerical results and their analysis, especially from Fig. 10 to Fig. 12, we clearly see that the benefit of having one known reference anchor in the system is significant. The estimation performance can be further enhanced by having more certain anchors, however, the additional performance improvements are already relatively smaller. Therefore, for any industrial or other applications where the anchors may suffer from some uncertainty in their states, it is highly recommended to have at least a single anchor whose state can be precisely measured and known. In all cases, the impact of multiple users is also beneficial.

VI. CONCLUSION
In this article, we formulated and presented an EKF-based Bayesian framework -called MU-PoSAC -for jointly estimating and tracking the 3D locations and time-varying clock offsets of users, together with the unknown anchor locations and orientation offsets. With specific focus on applications in mmWave 5G NR networks, also AoA and ToA estimators were provided building on beamformed uplink reference signal measurements and novel multi-beam fusion processing. A vast collection of numerical results was provided and analyzed, in the context of an indoor industrial warehouse with moving robots and other industrial vehicles as users, incorporating full ray-tracing for accurate propagation modeling at the considered 28 GHz center frequency. On the physical measurement accuracy side, the obtained results show that fusing multiple beams is beneficial and that highly accurate AoA and ToA measurements can be obtained with practical antenna array models and reference signal bandwidths. In terms of the actual positioning, synchronization and anchor state calibration, our results show that estimating and tracking the overall system state is technically feasible. Through the concatenated overall state definition, our results also clearly show that having multiple users in the system is beneficial for the state estimation performance. Additionally, the results also show that having one known reference anchor is further benefiting the estimation accuracy. With one known anchor in the considered industrial deployment scenario, estimation and tracking of the user positions and anchor locations were shown to be feasible with RMSE levels clearly below 1 m. Finally, the proposed EKFbased joint estimation framework was shown to offer robustness against intermittent blockage of LoS measurements. Our future work will focus on extending the described Bayesian estimation framework to cover also localization of the scatterers and other landmarks in the environment, simultaneous to the positioning of the users and anchors.