mmWave

    millimeter-wave
MIMO

    multiple-input multiple-output
OFDM

    orthogonal frequency-division multiplexing
MIMO-OFDM

    multiple-input multiple-output orthogonal frequency-division multiplexing
AoA

    angle-of-arrival
AoD

    angle-of-departure
ToA

    time-of-arrival
TDoA

    time-difference-of-arrival
CRB

    Cramér-Rao bound
CCRB

    constrained Cramér-Rao bound
FIM

    Fisher information matrix
EFIM

    equivalent Fisher information matrix
RMSE

    root mean square error
ML

    maximum likelihood
GNSS

    global navigation satellite system
IMU

    inertial measurement unit
PRS

    positioning reference signal
QOS

    quality of service
AWGN

    additive white Gaussian noise
PSD

    power spectral density
RF

    radio frequency
RIS

    reconfigurable intelligent surface
RTT

    round-trip-time
SLAM

    simultaneous localization and mapping
SNR

    signal-to-noise ratio
BS

    base station
UE

    user equipment
IP

    incidence point
LoS

    line-of-sight
NLoS

    non-line-of-sight
TX/RX

    transmitter/receiver
TX

    transmitter
RX

    receiver
PEB

    position error bound
MEB

    mapping error bound
OEB

    orientation error bound
IPEB

    IPs error bound
SEB

    synchronization error bound
CDF

    cumulative distribution function
PDF

    probability distribution function
UPA

    uniform planar array
RBL

    rigid body localization
VLP

    visible light positioning
RSS

    received signal strength
GLRT

    generalized likelihood ratio test

# MmWave 6D Radio Localization with a Snapshot Observation from a Single BS

Mohammad A. Nazari  Gonzalo Seco-Granados  Pontus Johannisson  and Henk
Wymeersch  This work was supported under the Wallenberg AI, Autonomous Systems
and Software Program (WASP), the Swedish Research Council under grant
2018-03701, the European Commission through the H2020 project Hexa-X (grant
agreement no. 101015956), the Spanish Research Agency project
PID2020-118984GB-I00, and by the ICREA Academia Programme.
Mohammad A. Nazari and Henk Wymeersch are with the Department of Electrical
Engineering, Chalmers University of Technology, 41258 Göteborg, Sweden
(e-mails: {mohammad.nazari,henkw}@chalmers.se).Gonzalo Seco-Granados is with
the Department of Telecommunications and Systems Engineering, Universitat
Autonoma de Barcelona, 08193 Bellaterra, Barcelona, Spain (e-mail:
gonzalo.seco@uab.cat).Pontus Johannisson was with Chalmers University of
Technology, and is now with Saab AB, 41276 Göteborg, Sweden (e-mail:
pontus.johannisson@saabgroup.com).

###### Abstract

Accurate and ubiquitous localization is crucial for a variety of applications
such as logistics, navigation, intelligent transport, monitoring, control, and
also for the benefit of communications. Exploiting millimeter-wave (mmWave)
signals in 5G and Beyond 5G systems can provide accurate localization with
limited infrastructure. We consider the single base station (BS) localization
problem and extend it to 3D position and 3D orientation estimation of an
unsynchronized multi-antenna user equipment (UE), using downlink multiple-
input multiple-output orthogonal frequency-division multiplexing (MIMO-OFDM)
signals. Through a Fisher information analysis, we show that the problem is
often identifiable, provided that there is at least one multipath component in
addition to the line-of-sight (LoS), even if the position of corresponding
incidence point (IP) is a priori unknown. Subsequently, we pose a maximum
likelihood (ML) estimation problem, to jointly estimate the 3D position and 3D
orientation of the UE as well as several nuisance parameters (the UE clock
offset and the positions of IPs corresponding to the multipath). The ML
problem is a high-dimensional non-convex optimization problem over a product
of Euclidean and non-Euclidean manifolds. To avoid complex exhaustive search
procedures, we propose a geometric initial estimate of all parameters, which
reduces the problem to a 1-dimensional search over a finite interval.
Numerical results show the efficiency of the proposed ad-hoc estimation, whose
gap to the Cramér-Rao bound (CRB) is tightened using the ML estimation.

###### Index Terms:

Localization, Orientation estimation, Mapping, Synchronization, Single anchor
localization.

##  I Introduction

mmWave is the key enabling component of the fifth generation (5G) and beyond
5G (B5G) communication systems, which empowers the implementation of large
antenna arrays for spatial multiplexing and provides massive bandwidths for
high data rates [1]. Despite the favorable properties of mmWave, undesired
effects, such as severe path loss and limited channel rank, challenge the
technology to come up with advanced beamforming and resource allocation
schemes [2]. To achieve this, information on the 3D location of mobile users
can provide important side information, so that the BS can adjust its
precoders to beam towards the UE [3]. Similarly, the UE can adjust its
combiners, based on its 3D orientation, to maximize the received signal-to-
noise ratio (SNR) [4]. While UE position estimation has been the main driver
in 5G mmWave [5], there are many applications that need 6D information (3D
position and 3D orientation, also known as the pose in robotics [6]): the
position and heading of vehicles is needed in intelligent transport systems
for driving assistance applications and platooning [7]; in assisted living
facilities, the pose of residents is informative about their health status
[8]; search-and-rescue operations involving UAVs require accurate and timely
pose information for control, self-localization, and victim recovery [9].
Moreover, 6D localization is expected to be of importance in 6G, with
applications such as augmented reality, robot interaction, and digital twins
[10].

The source of this 6D information, whose estimation is referred to as _6D
localization_ in this paper, can be either external or internal to the
communication systems. The external 6D localization systems can build on a
mixed-technology solution, such as the combination of the global navigation
satellite system (GNSS) (for 3D position) and inertial measurement unit (IMU)
(for 3D orientation) [11]. However, such solutions can be inefficient in cost,
complexity, or coverage. For example, GNSS might fail in indoor environments
or urban canyons, while IMUs suffer from drifts and accumulative errors [12].
The alternative is to exploit the already deployed cellular communication
infrastructure for 6D localization, and feed the 6D information to the
communication system internally.

Prior to 5G, the majority of localization schemes in cellular networks relied
on multiple synchronized base stations and time-difference-of-arrival (TDoA)
measurements [13]. With the introduction of new dedicated positioning
reference signals and measurements in 3GPP release 16[14], a combination of
angle and delay measurements has become possible [15]. Because of the high
resolution in both temporal and angular domains, thanks to $400$ MHz bandwidth
at mmWave and large antenna arrays, respectively, multipath components can be
better resolved [16], leading to new positioning architectures. A great deal
of research effort has been devoted to _multi- BS111Recent developments
relying on reconfigurable intelligent surfaces are considered as multi-BS
solutions, since a RIS acts as an additional multi-antenna BS in localization
[17, 18]. For similar reasons, approaches with a single moving BS are also
equivalent to multi-BS localization [9]. localization_ approaches, which
exploit these novel features [19, 20, 21, 22]. Simultaneously, there has been
a paradigm shift towards _single- BS localization_ solutions [23], which are
attractive because they require only minimal infrastructure and remove the
need for inter-BS synchronization. The enabler of single-BS localization is
the ability to turn multipath from foe to friend [8]: in contrast to previous
beliefs that non-line-of-sight (NLoS) components have unfavorable effects on
positioning, they contribute to the identifiability and accuracy of
localization in mmWave multiple-input multiple-output (MIMO) systems, provided
there is sufficient temporal and spatial resolution. This concept has been
exploited in recent advances in localization. In particular, [24] derived both
performance bounds and a method for 2D position and 1D orientation estimation,
with synchronized UE and BS, based on a compressed sensing algorithm. The
estimation method was refined via an atomic norm minimization approach in
[25], where the performance is not limited by quantization error and grid
resolution. For the same scenario of 2D position and 1D orientation, [26]
showed that each NLoS path gives rise to a rank-1 Fisher information matrix
(FIM) so that the UE can be localized with the LoS and a single NLoS path, or
with 3 NLoS paths when the LoS is obstructed. The case of obstructed LoS was
also treated in [27], without the requirement for synchronization, but still
with 2D position and accordingly a single orientation angle. This concept was
further extended to 3D position and 2D orientation estimation under perfect
synchronization in [16], where the asymptotic case with orthogonal multipath
components was studied. A further generalization was considered in [28],
focusing on a direct localization approach for the massive array regime,
considering a 3D position and 2D orientation estimation and a synchronized
user. The more practical case considering the synchronization error as well as
the Doppler shift-when the transmitter or receiver is moving-was addressed in
[29], where the authors performed the FIM analysis for 2D position and 1D
orientation estimation of a mobile user. In [30], the LoS 3D positioning
problem using the 5G uplink channel sounding reference signals is considered,
but the UE is synchronized and single-antenna, for which no orientation is
defined. The closest work we identified is [31], where a hybrid model/data-
driven approach for the 3D position estimation of a UE is proposed. The model-
based approach is based on the underlying geometry and the angle between each
two arrival directions being independent of UE orientation. However, no
algorithm is proposed for orientation estimation or estimating the IP
locations.

The extension of already existing methods to 3D position and 3D orientation
case, however, is not trivial, since the positions (both UE and IPs) are not
constrained to lie in a plane. The orientation also introduces more degrees of
freedom in the geometrical equations of channel parameters, and hence,
increases the complexity of search-based estimation algorithms. In addition,
the analysis of fundamental lower bounds for such a general case has not yet
been conducted. The 6D localization problem was introduced in the mmWave
context in [32], but has not yet been further developed. However, it has been
studied in other settings, e.g., pose estimation in robotics [6] and visible
light positioning [33]. In [33], a simultaneous 3D position and 3D orientation
estimation using the received signal strength (RSS) for a visible light system
containing multiple light emitting diodes and photodiodes is considered, and
an approximate solution using direct linear transformation method is proposed.
This solution is further refined using iterative algorithms for ML estimation.
On a parallel track, the problem is addressed under the label of rigid body
localization in [34, 35, 36], where the approach is to mount sensors with a
known topology on the body. The positions of the sensors in the global
coordinate frame are related to the position of the rigid body and its
orientation. The sensors then form a wireless sensor network, and the
position, as well as the orientation of the body, is estimated using time
and/or angle measurements from sensors.

In this paper, we consider a single-BS localization scenario, where the
downlink mmWave signal from a multi-antenna mmWave base station is used to
estimate the 3D-position and 3D-orientation of a UE in LoS to the BS. We
evaluate the lower bound on estimation error variance of position and
orientation of the UE, positions of the incidence points, and the clock
offset, by deriving the constrained Cramér-Rao bound (CRB) of all unknowns.
This reveals that the problem is generally identifiable under a single NLoS
path. To solve the corresponding high-dimensional ML problem, we propose an
efficient solution, combining a geometric ad-hoc estimator to initialize a
gradient descent over a product of manifolds. This solution is shown to attain
the corresponding CRBs. The proposed approach is related to the literature in
Table I. The main contributions of this work are the following:

  * •

6D localization algorithm: We pose a high-dimensional ML estimation problem
over a product of Euclidean and non-Euclidean manifolds, given the conditional
probability distributions of angle-of-arrival (AoA), angle-of-departure (AoD),
and time-of-arrival (ToA) measurements. The parameters of such distributions
are obtained, and the root mean square error (RMSE) of the ML estimation is
shown to attain the lower bounds. The ML problem is solved by gradient
descent, iterating between the various manifolds, starting from a good initial
solution.

  * •

A low-complexity estimator We propose and evaluate a low-complexity ad-hoc
estimation algorithm to initialize the solution of the ML problem, which
reduces the high-dimensional problem of estimating all unknowns, to a
1-dimensional search over a finite interval combined with closed-form
expressions. This recovers not only the 6D UE state, but also the UE clock
bias and IP locations.

  * •

Fisher information and numerical analysis: We obtain the lower bound on the
estimation of the 6D user state and its clock bias, as well as the map of the
environment, i.e., the positions of incidence points, and then we evaluate the
impact of bandwidth, number of antennas, and number of incidence points. The
analysis of the bounds indicates that in most cases a single incidence point
with a priori unknown location is sufficient to render the problem
identifiable, though certain configurations require several incidence points.
We also evaluate a low-complexity ad-hoc and the ML 6D localization algorithms
and demonstrate that it can attain the corresponding performance bounds.

TABLE I: Overview of the related work.

Ref. | Pos. | Ori. | Clock | Bound | Method | without LoS
---|---|---|---|---|---|---
[24] | 2D | 1D | ✗ | ✓ | ✓ | ✓
[25] | 2D | 1D | ✗ | ✓ | ✓ | ✗
[26] | 2D | 1D | ✗ | ✓ | ✗ | ✓
[27] | 2D | 1D | ✓ | ✗ | ✓ | ✓
[16] | 3D | 2D | ✗ | ✓ | ✗ | ✗
[28] | 3D | 2D | ✗ | ✓ | ✗ | ✗
[29] | 2D | 1D | ✓ | ✓ | ✓ | ✗
[30] | 3D | ✗ | ✗ | ✗ | ✓ | ✗
[31] | 3D | 3D | ✓ | ✗ | only pos. | ✗
this work | 3D | 3D | ✓ | ✓ | ✓ | ✗

The rest of the paper is organized as follows. Section II describes the system
model and provides the definitions. In section III, we state the ML estimation
problem, followed by a low-complexity estimation algorithm for obtaining
initial solutions for iterative routines in Section IV. Subsequently, in
Section V, the Fisher information analysis is done, and error bounds are
derived. Section VI presents numerical results, and finally, conclusion
remarks are given in Section VII.

#### Notations

We denote scalars, vectors, and matrices by italic, bold lowercase, and bold
uppercase letters, e.g., $x$, $\mathbf{x}$ and $\mathbf{X}$, respectively. The
element of matrix $\mathbf{X}$ in the $i$-th row and $j$-th column is
indicated by $[\mathbf{X}]_{i,j}$. We also use $[\mathbf{X}]_{i:k,j:l}$ to
refer to the sub-matrix lying between rows $i$ to $k$ and columns $j$ to $l$
of $\mathbf{X}$. The identity matrix of size $N$ is shown by $\mathbf{I}_{N}$,
whereas $\mathbf{1}_{N}$ and $\mathbf{0}_{N}$ indicate all-ones and all-zeros
vectors of size $N$. While $\mathrm{diag}(\mathbf{x})$ is a diagonal matrix
whose non-zero elements are given by $\mathbf{x}$, $\mathrm{diag}(\mathbf{X})$
is a vector composed of the diagonal elements of $\mathbf{X}$. Similarly,
$\mathrm{blkdiag}(\mathbf{X},\mathbf{Y})$ is a block-diagonal matrix made of
$\mathbf{X},\mathbf{Y}$. In order to show the expectation, trace, and
vectorization operators, we use $\mathbb{E}[\cdot]$, $\mathrm{tr}[\cdot]$, and
$\mathrm{vec}[\cdot]$, respectively. The transpose, and hermitian operators
are symbolized using $[\cdot]^{\mathsf{T}}$ and $[\cdot]^{\mathsf{H}}$; and we
consider $\odot$ and $\oslash$ in conjunction with pointwise product and
division, all in the given order. The Euclidean and Frobenius norms are
denoted by $\|\cdot\|$ and $\|\cdot\|_{\text{F}}$, respectively.

##  II System Model

We consider a downlink mmWave MIMO scenario consisting of a BS equipped with
an arbitrary array of $N_{\mathrm{BS}}$ antennas, and a UE equipped with an
arbitrary array of $N_{\mathrm{UE}}$ antennas, as shown in Fig. 1. Without
loss of generality, both BS and UE are considered to have a single radio
frequency (RF) chain.

###  II-A Geometric Model

The BS antenna array is centered at the known position
$\mathbf{p}_{\mathrm{BS}}=[p_{\mathrm{BS},x},p_{\mathrm{BS},y},p_{\mathrm{BS},z}]^{\mathsf{T}}\in\mathbb{R}^{3}$
with a known orientation, while UE antenna array is centered at the _unknown_
position
$\mathbf{p}_{\mathrm{UE}}=[p_{\mathrm{UE},x},p_{\mathrm{UE},y},p_{\mathrm{UE},z}]^{\mathsf{T}}\in\mathbb{R}^{3}$
with an _unknown_ orientation. The paths between BS and UE include the LoS, as
well as $M\geq 1$ _resolvable single-bounce_ NLoS paths,each corresponding to
a scatterer or a reflecting point, represented by an IP at _unknown_ position
$\mathbf{p}_{m}=[p_{m,x},p_{m,y},p_{m,z}]^{\mathsf{T}}\in\mathbb{R}^{3}$,
$m=1,\ldots,M$. All positions are given in a global coordinate frame as the
reference, whose axes are labeled as $x$, $y$, and $z$ (see Fig. 2(a)).

######  Remark 1 (On the NLoS model).

While we assume single-bounce NLoS paths, in practice multi-bounce paths may
also exist. Those paths can be identified and excluded from the localization
scheme, e.g., using methods such as [37] (where an iterative method based on
the generalized likelihood ratio test (GLRT) and change-point detection is
proposed to distinguish single-bounce and multi-bounce paths) or [38] (which
progressively identifies the LoS path, single bounce, double bounce, and
higher bounce paths). We also assume that all paths (including the LoS and
NLoS paths) are resolvable. This is generally correct in the mmWave scenarios,
due to the large bandwidth and antenna apertures.

The orientation of BS and UE describe how their antenna arrays with respect to
the reference orientation are arranged. We consider rotation matrices for
describing the orientations222Orientations in 3D can be represented by
_quaternions_ , _Euler angles_ , and _rotation matrices_ [39]. We adopt
rotation matrices., so that BS and UE orientations determine local coordinate
frames, respectively described by $3\times 3$ rotation matrices
$\mathbf{R}_{\mathrm{BS}}$ and $\mathbf{R}_{\mathrm{UE}}$, in the special
orthogonal group $\mathrm{SO}(3)$. In particular, we define a reference
orientation where the axes are in the same direction as those of the global
coordinate frame, as shown in Fig. 2(a). The local coordinate frames are thus
obtained by rotating the arrays in reference orientation through
$\mathbf{R}_{\mathrm{BS}}$ and $\mathbf{R}_{\mathrm{UE}}$ (see Fig. 2(b)).
Therefore, the given vector $\mathbf{y}$ in the global coordinate system is
corresponding to
$\mathbf{y}_{\mathrm{BS}}=\mathbf{R}_{\mathrm{BS}}^{\mathsf{T}}\mathbf{y}$ and
$\mathbf{y}_{\mathrm{UE}}=\mathbf{R}_{\mathrm{UE}}^{\mathsf{T}}\mathbf{y}$ in
BS and UE coordinate frames, respectively. We note that
$\mathbf{R}_{\mathrm{BS}}$ is known, while $\mathbf{R}_{\mathrm{UE}}$ is
unknown. The 6D localization problem refers to estimation of
$\mathbf{p}_{\mathrm{UE}}$ and $\mathbf{R}_{\mathrm{UE}}$.

$\mathbf{p}_{m}$$\mathbf{p}_{n}$BS$\mathbf{p}_{\mathrm{BS}},\mathbf{R}_{\mathrm{BS}}$UE$\mathbf{p}_{\mathrm{UE}},\mathbf{R}_{\mathrm{UE}}$
Figure 1: Schematic of system model with a UE at unknown 3D position and
unknown 3D orientation, where signals are received from LoS and NLoS paths.
The UE and BS are not synchronized.

###  II-B Signal and Channel Model

We consider downlink pilot transmission of $K$ orthogonal frequency-division
multiplexing (OFDM) symbols using $N_{f}$ subcarriers over a MIMO channel, for
the purpose of snapshot position and orientation estimation333We assume a slow
fading channel, which does not vary over the duration of the pilot
transmission.. Considering $M+1$ (resolvable) paths indexed by
$m=0,1,\ldots,M$ ($m=0$ is the LoS path between the BS and the UE, which is
assumed to be present, and $m\neq 0$ correspond to NLoS paths), the $k$-th
received OFDM symbol, $k=1,\ldots,K$, over subcarrier $n=1,\ldots,N_{f}$, is
given by [40]

$\displaystyle
y_{k,n}=\bm{w}_{k}^{\mathsf{H}}\sum_{m=0}^{M}\mathbf{H}_{m}\bm{f}_{k}x_{k,n}\mathrm{e}^{-j2\pi(n-1)\Delta_{f}\tau_{m}}+\bm{w}_{k}^{\mathsf{H}}\bm{n}_{k,n},$
(1)

in which $x_{k,n}$ is a unit-modulus pilot symbol with energy
$E_{\mathrm{s}}=P_{\mathrm{TX}}/B$, where $P_{\mathrm{TX}}$ is the transmit
power and $B=N_{f}\Delta_{f}$ is the total bandwidth for the subcarrier
spacing $\Delta_{f}$; $\bm{f}_{k}$ is the precoding vector at BS, $\bm{w}_{k}$
is the combining vector at UE, with $\|\bm{f}_{k}\|=\|\bm{w}_{k}\|=1$,
$\forall k$; and $\bm{n}_{k,n}\in\mathbb{C}^{N_{\mathrm{UE}}}$ is the complex
zero-mean additive white Gaussian noise (AWGN) with covariance matrix
$n_{0}N_{0}\mathbf{I}_{N_{\mathrm{UE}}}$, where $N_{0}$ is the noise power
spectral density (PSD), and $n_{0}$ is the UE noise figure. Furthermore, each
path $m$ is characterized by a ToA $\tau_{m}$ and a channel matrix
$\mathbf{H}_{m}\triangleq
h_{m}~{}\mathbf{a}_{\mathrm{UE}}(\bm{\theta}_{\mathrm{A},m})\mathbf{a}_{\mathrm{BS}}^{\mathsf{T}}(\bm{\theta}_{\mathrm{D},m})$,
where $h_{m}=h_{\mathrm{R},m}+jh_{\mathrm{I},m}$ is the complex channel gain;
$\mathbf{a}_{\mathrm{UE}}(\bm{\theta}_{\mathrm{A},m})$ and
$\mathbf{a}_{\mathrm{BS}}(\bm{\theta}_{\mathrm{D},m})$ are the array response
vectors, where
$\bm{\theta}_{\mathrm{A},m}=[\theta_{\mathrm{A},m}^{(\mathrm{az})},\theta_{\mathrm{A},m}^{(\mathrm{el})}]^{\mathsf{T}}$
and
$\bm{\theta}_{\mathrm{D},m}=[\theta_{\mathrm{D},m}^{(\mathrm{az})},\theta_{\mathrm{D},m}^{(\mathrm{el})}]^{\mathsf{T}}$
show the AoA and AoD in azimuth and elevation (as defined in Fig. 2(b) and
Section II-C), respectively. The array response vectors are given by

$\displaystyle[\mathbf{a}_{\mathrm{UE}}(\bm{\theta}_{\mathrm{A},m})]_{n}$
$\displaystyle=\exp\Big{(}j\frac{2\pi}{\lambda}[\bm{\Delta}_{\mathrm{UE}}]_{1:3,n}^{\mathsf{T}}\mathbf{d}(\bm{\theta}_{\mathrm{A},m})\Big{)},$
(2a) $\displaystyle[\mathbf{a}_{\mathrm{BS}}(\bm{\theta}_{\mathrm{D},m})]_{n}$
$\displaystyle=\exp\Big{(}j\frac{2\pi}{\lambda}[\bm{\Delta}_{\mathrm{BS}}]_{1:3,n}^{\mathsf{T}}\mathbf{d}(\bm{\theta}_{\mathrm{D},m})\Big{)},$
(2b)

in which $\lambda$ is the wavelength at the carrier frequency $f_{c}$; and
$\bm{\Delta}_{\mathrm{UE}}\triangleq[\mathbf{x}_{\mathrm{UE},1},\ldots,\mathbf{x}_{\mathrm{UE},N_{\mathrm{UE}}}]\in\mathbb{R}^{3\times
N_{\mathrm{UE}}}$ and
$\bm{\Delta}_{\mathrm{BS}}\triangleq[\mathbf{x}_{\mathrm{BS},1},\ldots,\mathbf{x}_{\mathrm{BS},N_{\mathrm{BS}}}]\in\mathbb{R}^{3\times
N_{\mathrm{BS}}}$ contain coordinates of antenna elements in reference
orientation, with respect to the UE and the BS coordinate frames (i.e., 3D
displacements from the phase center), respectively. In (2), we have introduced

$\displaystyle\mathbf{d}(\bm{\phi})=[\sin\phi^{(\mathrm{el})}\cos\phi^{(\mathrm{az})},\sin\phi^{(\mathrm{el})}\sin\phi^{(\mathrm{az})},\cos\phi^{(\mathrm{el})}]^{\mathsf{T}},$
(3)

which describes the unit-norm direction of arrival (for
$\bm{\phi}=\bm{\theta}_{\mathrm{A},m}$) and unit-norm direction of departure
(for $\bm{\phi}=\bm{\theta}_{\mathrm{D},m}$) for path $m$, in UE and BS
coordinate frames, respectively.

###  II-C Relation Between Geometric Model and Channel Model

Corresponding to the AoAs, the unit-norm arrival directions
$\mathbf{d}_{\mathrm{A},m}=\mathbf{d}(\bm{\theta}_{\mathrm{A},m})$ are given,
in the UE coordinate frame, by

$\displaystyle\mathbf{d}_{\mathrm{A},m}=\begin{cases}\mathbf{R}_{\mathrm{UE}}^{\mathsf{T}}(\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}})/\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}\|&m=0\\\
\mathbf{R}_{\mathrm{UE}}^{\mathsf{T}}(\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}})/\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}\|&m\neq
0,\end{cases}$ (4)

which define the AoAs as
$\theta_{\mathrm{A},m}^{(\mathrm{az})}=\mathrm{atan}2([\mathbf{d}_{\mathrm{A},m}]_{2},[\mathbf{d}_{\mathrm{A},m}]_{1})$
and
$\theta_{\mathrm{A},m}^{(\mathrm{el})}=\mathrm{acos}([\mathbf{d}_{\mathrm{A},m}]_{3})$,
where $\mathrm{acos}$ is the inverse cosine and $\mathrm{atan}2$ is the four-
quadrant inverse tangent. Similarly, using the unit-norm departure directions
$\mathbf{d}_{\mathrm{D},m}=\mathbf{d}(\bm{\theta}_{\mathrm{D},m})$ given by

$\displaystyle\mathbf{d}_{\mathrm{D},m}=\begin{cases}\mathbf{R}_{\mathrm{BS}}^{\mathsf{T}}(\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}})/\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|&m=0\\\
\mathbf{R}_{\mathrm{BS}}^{\mathsf{T}}(\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}})/\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}\|&m\neq
0,\end{cases}$ (5)

in BS coordinate frame, the AoDs are determined as
$\theta_{\mathrm{D},m}^{(\mathrm{az})}=\mathrm{atan}2([\mathbf{d}_{\mathrm{D},m}]_{2},[\mathbf{d}_{\mathrm{D},m}]_{1})$
and
$\theta_{\mathrm{D},m}^{(\mathrm{el})}=\mathrm{acos}([\mathbf{d}_{\mathrm{D},m}]_{3})$.
Fig. 2(b) shows how AoDs and AoAs and their corresponding directions are
defined. Note that the arrows corresponding to the arrival directions point
towards the BS/IPs, and not the UE.

Finally, denoting the propagation speed by $c$ and the _unknown_ clock bias
between the UE and the BS by $b$, one can express ToAs as

$\displaystyle\tau_{m}=\begin{cases}\hfil\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|/c+b&m=0\\\
(\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}\|+\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{m}\|)/c+b&m\neq
0.\end{cases}$ (6)

The aggregated vectors
$\bm{\theta}_{\mathrm{A}}=[\bm{\theta}_{\mathrm{A},0}^{\mathsf{T}},\bm{\theta}_{\mathrm{A},1}^{\mathsf{T}},\ldots,\bm{\theta}_{\mathrm{A},M}^{\mathsf{T}}]^{\mathsf{T}}$,
$\bm{\theta}_{\mathrm{D}}=[\bm{\theta}_{\mathrm{D},0}^{\mathsf{T}},\bm{\theta}_{\mathrm{D},1}^{\mathsf{T}},\ldots,\bm{\theta}_{\mathrm{D},M}^{\mathsf{T}}]^{\mathsf{T}}$,
$\bm{\tau}=[\tau_{0},\ldots,\tau_{M}]^{\mathsf{T}}$, and
$\bm{\eta}=[\bm{\theta}_{\mathrm{A}}^{\mathsf{T}},\bm{\theta}_{\mathrm{D}}^{\mathsf{T}},\bm{\tau}^{\mathsf{T}}]^{\mathsf{T}}$
are defined to be used later.

$x$$y$$z$$\theta^{(\mathrm{az})}$$\theta^{(\mathrm{el})}$$\mathbf{d}(\theta)$
(a)

$x^{\prime}$$y^{\prime}$$z^{\prime}$$\mathbf{d}(\theta)$$\theta^{(\mathrm{el})}$$\theta^{(\mathrm{az})}$
(b)

Figure 2: (a) The global coordinate system ($x\\!-\\!y\\!-\\!z$) and the
reference orientation $\mathbf{R}=\mathbf{I}$, together with the definition of
angles and their corresponding direction at orientation
$\mathbf{R}=\mathbf{I}$. (b) The local coordinate system
($x^{\prime}\\!-\\!y^{\prime}\\!-\\!z^{\prime}$) at a given orientation,
together with the definition of angles and their corresponding direction at
that orientation. Note that $\theta^{(\mathrm{az})}\in[0,2\pi]$ and
$\theta^{(\mathrm{el})}\in[0,\pi]$.

##  III 6D Localization Methodology

###  III-A Two-stage Localization and Problem Decomposition

We consider a two-stage localization scheme and decompose the problem into a
channel parameters estimation routine, followed by a localization routine. The
channel estimation routine determines the marginal posterior densities of the
channel parameters (in the form of estimates and the associated
uncertainties), based on the observations (1). The localization routine uses
the output of the channel parameters estimator to determine the 6D state of
the UE. We consider the contribution of this paper to provide a solution to
the second sub-problem, while considering a generic channel parameter
estimator.

####  III-A1 Channel Parameters Estimation

There exists a variety of channel parameter estimators in the literature,
including ESPRIT [41], generalized approximate message passing [40, 42],
orthogonal matching pursuit [24], sparse Bayesian learning [43], tensor
decomposition [44], and RIMAX/SAGE [45]. We assume an arbitrary estimator is
applied to obtain an estimate of $\bm{\eta}$.  We denote by
$\hat{\bm{\eta}}=[\hat{\bm{\theta}}_{\mathrm{A}}^{\mathsf{T}},\hat{\bm{\theta}}_{\mathrm{D}}^{\mathsf{T}},\hat{\bm{\tau}}^{\mathsf{T}}]^{\mathsf{T}}$,
the vector of estimated channel parameters, also referred to as the vector of
_measurements_. For each parameter, there is also an associated uncertainty,
which leads to the following likelihood functions. For the ToA measurements,
we assume a multivariate Gaussian distribution [46, 47]

$\displaystyle p(\hat{\bm{\tau}}|\bm{\tau})$
$\displaystyle=\frac{1}{\sqrt{(2\pi)^{M+1}|\Sigma_{\bm{\tau}}|}}e^{-\frac{1}{2}(\hat{\bm{\tau}}-\bm{\tau})^{\mathsf{T}}\Sigma_{\bm{\tau}}^{-1}(\hat{\bm{\tau}}-\bm{\tau})},$
(7)

where
$\Sigma_{\bm{\tau}}=\mathrm{diag}(\sigma_{0}^{2},\ldots,\sigma_{M}^{2})$. Note
that both $\hat{\tau}_{m}$ and $\sigma^{2}_{m}$ are provided by the channel
parameter estimator. For the AoAs and AoDs, we follow [48] and use a Von Mises
distribution, which can be interpreted as a Gaussian distribution over the 1D
manifold of angles [49, Chapter 3]. Correspondingly,444While it is shown in
[16] that azimuth and elevation angles are correlated in general, we neglect
the correlation and assume factorized likelihoods, for the sake of
tractability. We will later evaluate the impact of this independence
assumption (see Section VI-B).

$\displaystyle
p(\hat{\bm{\theta}}_{\mathrm{A},m}|\bm{\theta}_{\mathrm{A},m})=$ (8a)
$\displaystyle\exp\left(\kappa_{\mathrm{A},m}^{(\mathrm{az})}\cos(\hat{\theta}^{(\mathrm{az})}_{\mathrm{A},m}-\theta^{(\mathrm{az})}_{\mathrm{A},m})\right)/\big{(}2\pi
I_{0}(\kappa_{\mathrm{A},m}^{(\mathrm{az})})\big{)}$ $\displaystyle\times$
$\displaystyle\exp\left(\kappa_{\mathrm{A},m}^{(\mathrm{el})}\cos(\hat{\theta}^{(\mathrm{el})}_{\mathrm{A},m}-\theta^{(\mathrm{el})}_{\mathrm{A},m})\right)/\big{(}2\pi
I_{0}(\kappa_{\mathrm{A},m}^{(\mathrm{el})})\big{)},$ $\displaystyle
p(\hat{\bm{\theta}}_{\mathrm{D},m}|\bm{\theta}_{\mathrm{D},m})=$ (8b)
$\displaystyle\exp\left(\kappa_{\mathrm{D},m}^{(\mathrm{az})}\cos(\hat{\theta}^{(\mathrm{az})}_{\mathrm{D},m}-\theta^{(\mathrm{az})}_{\mathrm{D},m})\right)/\big{(}2\pi
I_{0}(\kappa_{\mathrm{D},m}^{(\mathrm{az})})\big{)}$ $\displaystyle\times$
$\displaystyle\exp\left(\kappa_{\mathrm{D},m}^{(\mathrm{el})}\cos(\hat{\theta}^{(\mathrm{el})}_{\mathrm{D},m}-\theta^{(\mathrm{el})}_{\mathrm{D},m})\right)/\big{(}2\pi
I_{0}(\kappa_{\mathrm{D},m}^{(\mathrm{el})})\big{)},$

where $\kappa^{(\mathrm{az})}_{\mathrm{A},m}$,
$\kappa^{(\mathrm{el})}_{\mathrm{A},m}$,
$\kappa^{(\mathrm{az})}_{\mathrm{D},m}$, and
$\kappa^{(\mathrm{el})}_{\mathrm{D},m}$, are the non-negative concentration
parameters of $m$-th AoA and $m$-th AoD, in azimuth and elevation,
respectively, and $I_{0}(\cdot)$ is the modified Bessel function of order $0$.
Again, both the estimated angles and the corresponding concentration
parameters are assumed to be provided by the channel parameter estimator.

####  III-A2 Location Parameters Estimation

The localization problem is formulated as the maximum likelihood estimation

$\displaystyle\hat{\mathbf{R}}_{\mathrm{UE}},\hat{\bm{\zeta}}=\arg\max_{\mathbf{R}_{\mathrm{UE}},\bm{\zeta}}\ln
p(\hat{\bm{\eta}}|\bm{\eta}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})),~{}\mathbf{R}_{\mathrm{UE}}\in\mathrm{SO}(3),$
(9)

where
$\bm{\zeta}=[\mathbf{p}_{\mathrm{UE}}^{\mathsf{T}},\mathbf{p}_{1}^{\mathsf{T}},\ldots,\mathbf{p}_{M}^{\mathsf{T}},b]^{\mathsf{T}}\in\mathbb{R}^{3(M+1)+1}$
and the likelihood
$p(\hat{\bm{\eta}}|\bm{\eta}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta}))$ is
expressed of the underlying geometry. The likelihood is of the form

$\displaystyle
p\big{(}\hat{\bm{\eta}}|\bm{\eta}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})\big{)}=\
$ $\displaystyle
p\big{(}\hat{\bm{\theta}}_{\mathrm{A}}|\bm{\theta}_{\mathrm{A}}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})\big{)}$
(10) $\displaystyle\times~{}$ $\displaystyle
p\big{(}\hat{\bm{\theta}}_{\mathrm{D}}|\bm{\theta}_{\mathrm{D}}(\bm{\zeta})\big{)}\times
p\big{(}\hat{\bm{\tau}}|\bm{\tau}(\bm{\zeta})\big{)},$

in which, by overloading the notation for cosines, we have

$\displaystyle
p\big{(}\hat{\bm{\theta}}_{\mathrm{A}}|\bm{\theta}_{\mathrm{A}}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})\big{)}$
$\displaystyle\propto\exp\Big{(}\bm{\kappa}_{\mathrm{A}}^{\mathsf{T}}\cos\big{(}\hat{\bm{\theta}}_{\mathrm{A}}-\bm{\theta}_{\mathrm{A}}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})\big{)}\Big{)},$
(11a) $\displaystyle
p\big{(}\hat{\bm{\theta}}_{\mathrm{D}}|\bm{\theta}_{\mathrm{D}}(\bm{\zeta})\big{)}$
$\displaystyle\propto\exp\Big{(}\bm{\kappa}_{\mathrm{D}}^{\mathsf{T}}\cos\big{(}\hat{\bm{\theta}}_{\mathrm{D}}-\bm{\theta}_{\mathrm{D}}(\bm{\zeta})\big{)}\Big{)},$
(11b)

where $\bm{\kappa}_{\mathrm{A}}$ and $\bm{\kappa}_{\mathrm{D}}$ are
$2(M+1)\times 1$ vectors aggregating the concentration parameters of Von Mises
distributions corresponding to the angle estimates in
$\hat{\bm{\theta}}_{\mathrm{A}}$ and $\hat{\bm{\theta}}_{\mathrm{D}}$,
respectively. Considering the negative log-likelihood
$\mathcal{L}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})=-\ln
p(\hat{\bm{\eta}}|\bm{\eta}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta}))$, the ML
estimation $\eqref{eq:MLproblemAbstract1}$ can be equivalently written as the
constrained minimization

$\displaystyle\min_{\mathbf{R}_{\mathrm{UE}},\bm{\zeta}}~{}$
$\displaystyle\mathcal{L}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta}),$ (12a)
$\displaystyle\mathrm{s.t.}~{}~{}$
$\displaystyle\mathbf{R}_{\mathrm{UE}}^{\mathsf{T}}\mathbf{R}_{\mathrm{UE}}=\mathbf{I},~{}\mathrm{det}(\mathbf{R}_{\mathrm{UE}})=+1.$
(12b)

where

$\displaystyle\mathcal{L}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})=\nicefrac{{1}}{{2}}(\hat{\bm{\tau}}-\bm{\tau}(\bm{\zeta}))^{\mathsf{T}}\Sigma_{\bm{\tau}}^{-1}(\hat{\bm{\tau}}-\bm{\tau}(\bm{\zeta}))$
$\displaystyle-\bm{\kappa}_{\mathrm{A}}^{\mathsf{T}}\cos(\hat{\bm{\theta}}_{\mathrm{A}}-\bm{\theta}_{\mathrm{A}}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta}))-\bm{\kappa}_{\mathrm{D}}^{\mathsf{T}}\cos(\hat{\bm{\theta}}_{\mathrm{D}}-\bm{\theta}_{\mathrm{D}}(\bm{\zeta})).$
(13)

However, solving the optimization problem (12) using the classical
optimization tools is difficult [33], due to the unitary constraint (12b) on
the rotation matrix $\mathbf{R}_{\mathrm{UE}}$. In addition, the objective
function $\mathcal{L}(\mathbf{R}_{\mathrm{UE}},\bm{\zeta})$ is highly
nonlinear and non-convex, and iterative algorithms for solving (12) might
reach local optima, if initialized by the points far from the global solution.
To address these challenges, we propose a two-step approach: first, we
determine an initial ad-hoc estimate of unknowns from geometric arguments,
being then refined by a gradient descent of the objective function. The
iterative algorithm is explained in the following, while the process to
determine an initial point is deferred until Section IV.

###  III-B An Iterative Algorithm for ML Estimation

As (12) involves optimization over non-Euclidean manifolds, a suitable
optimization tool must be applied. We first present a method for such
optimization before applying that to our 6D localization problem.

####  III-B1 Overview of a Method for Optimization over non-Euclidean
Manifolds

Consider the optimization problem

$\displaystyle\hat{\mathsf{X}}=\arg\min_{\mathsf{X}\in\mathcal{M}}f(\mathsf{X}),$
(14)

where $f\\!:\\!\mathcal{M}\\!\to\\!\mathbb{R}$ is a smooth function over the
(possibly) non-Euclidean manifold $\mathcal{M}$, and $\mathsf{X}$ denotes the
(possibly) non-Euclidean parameter. We exploit the Riemannian gradient descent
algorithm, which is a first-order technique analogous to the standard gradient
descent algorithm, where the Riemannian gradient is obtained by projection of
the classical gradients to the tangent spaces. Starting from the initial point
$\hat{\mathsf{X}}^{(0)}$, the algorithm iterates

$\displaystyle\hat{\mathsf{X}}^{(k+1)}=\mathcal{R}_{\hat{\mathsf{X}}^{(k)}}\left(-\varepsilon_{k}\mathcal{P}_{\hat{\mathsf{X}}^{(k)}}\left[{\partial
f(\mathsf{X})}/{\partial\mathsf{X}}\right]_{\mathsf{X}=\hat{\mathsf{X}}^{(k)}}\right),$
(15)

where $\mathcal{P}_{\mathsf{X}}(\cdot)$ is an orthogonal projection onto the
tangent space at $\mathsf{X}$, $\mathcal{R}_{\mathsf{X}}(\cdot)$ is a
retraction from the tangent space onto $\mathcal{M}$, and $\varepsilon_{k}>0$
is a suitable step size. Intuitively, the gradient is calculated, and
projected to the tangent space (to follow the space of $\mathcal{M}$ as
closely as possible), the value at step $k$ is updated, and then the updated
value is normalized back into the $\mathcal{M}$. Relevant for us is
$\mathcal{M}=\mathrm{SO}(3)$. Example projection and retractions operations
for the Riemannian gradient descent algorithm are given by [50, Eqs. (7.32)
and (7.22)]:

$\displaystyle\mathcal{P}_{\mathbf{X}}(\mathbf{U})$
$\displaystyle=\mathbf{X}(\mathbf{X}^{\mathsf{T}}\mathbf{U}-\mathbf{U}^{\mathsf{T}}\mathbf{X})/2,$
(16a) $\displaystyle\mathcal{R}_{\mathbf{X}}(\mathbf{U})$
$\displaystyle=(\mathbf{X}+\mathbf{U})(\mathbf{I}_{3}+\mathbf{U}^{\mathsf{T}}\mathbf{U})^{-1/2}.$
(16b)

On the other hand, for $\mathcal{M}=\mathbb{R}^{n}$,
$\mathcal{P}_{\mathbf{X}}(\mathbf{U})=\mathbf{U}$,
$\mathcal{R}_{\mathbf{X}}(\mathbf{U})=\mathbf{X}+\mathbf{U}$, leading to
classical gradient descent.

####  III-B2 Solving the ML Estimation Problem

The optimization variable in our algorithm Inspired by the coordinate descent
algorithm [51, Sec. 9.3], we decompose the unknowns, and apply different
optimization algorithms for the estimation of $\mathbf{R}_{\mathrm{UE}}$ and
the rest of unknowns $\bm{\zeta}$, which belong to the Euclidean space. We
then consider the Riemannian gradient descent algorithm to optimize
$\mathbf{R}_{\mathrm{UE}}$ on the $\mathrm{SO}(3)$ manifold, as

$\displaystyle\hat{\mathbf{R}}_{\mathrm{UE}}^{(k+1)}=\mathcal{R}_{\hat{\mathbf{R}}_{\mathrm{UE}}^{(k)}}\Big{(}-\varepsilon_{k}\mathcal{P}_{\hat{\mathbf{R}}_{\mathrm{UE}}^{(k)}}\big{(}\left.{\partial\mathcal{L}}/{\partial\mathbf{R}_{\mathrm{UE}}}\big{)}\right|_{\hat{\mathbf{R}}_{\mathrm{UE}}^{(k)},\hat{\bm{\zeta}}^{(k)}}\Big{)},$
(17)

with the projection and retraction operators as defined in (16), and
$\varepsilon_{k}$ being the step size obtained using a backtracking line-
search [50]. We also use the trust region algorithm [51, Sec. 11.2] to
optimize $\bm{\zeta}$ in the Euclidean space, considering the updated rotation
matrix. Then we iterate this algorithm until a stopping criterion is met. The
partial derivatives ${\partial\mathcal{L}}/{\partial\mathbf{R}_{\mathrm{UE}}}$
and ${\partial\mathcal{L}}/{\partial\bm{\zeta}}$ are obtained using the chain
rule by including the partial derivatives of channel parameters w.r.t.
localization parameters given in Appendix A. The above method requires initial
estimates $\hat{\mathbf{R}}_{\mathrm{UE}}^{(0)}$ and $\hat{\bm{\zeta}}^{(0)}$,
which will be provided by the ad-hoc initial estimator.

##  IV Ad-hoc Initial Estimate

As stated earlier, the ML problem is a nonlinear non-convex optimization, and
the elaborated iterative algorithm might reach local optima if the initial
values are not given properly. In the following, we propose a simple (with
linear complexity in the number of IPs and involving only a 1D line search)
sequential scheme for obtaining initial estimates of localization unknowns
based on the estimates of AoAs, AoDs, and ToAs: First, we estimate
$\mathbf{R}_{\mathrm{UE}}$ only from the estimated AoAs and AoDs, without any
knowledge of $\mathbf{p}_{\mathrm{UE}}$,
$\mathbf{p}_{1},\ldots,\mathbf{p}_{M}$, and $b$. Second, given the estimated
$\mathbf{R}_{\mathrm{UE}}$, we estimate $\mathbf{p}_{\mathrm{UE}}$,
$\mathbf{p}_{1},\ldots,\mathbf{p}_{M}$ from TDoA measurements. Finally, we
estimate the clock bias $b$ using the estimated positions and ToAs. The
algorithm, therefore, takes the measurements $\hat{\bm{\theta}}_{\mathrm{A}}$,
$\hat{\bm{\theta}}_{\mathrm{D}}$, and $\hat{\bm{\tau}}$ as input, but we omit
the “hat” ( $\widehat{.}$ ) of the measurements, only in this section, for the
notational convenience. Note that the values are not confused with the true
_unknown_ channel parameters.

###  IV-A Step 1: Estimation of UE Rotation Matrix

Our approach to estimate the UE rotation matrix from the AoAs
$\bm{\theta}_{\mathrm{A},m}$ and AoDs $\bm{\theta}_{\mathrm{D},m}$ (and
corresponding unit vectors $\mathbf{d}({\bm{\theta}}_{\mathrm{A},m})$ and
$\mathbf{d}({\bm{\theta}}_{\mathrm{D},m})$) is based on an axis-angle
representation of its orientation. Consider the LoS arrival and departure
directions $\mathbf{d}_{\mathrm{A},0}$ and $\mathbf{d}_{\mathrm{D},0}$, which
are along LoS path, while in opposite directions in the global coordinate
frame. Hence,

$\displaystyle\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}=-\mathbf{R}_{\mathrm{UE}}\mathbf{d}_{\mathrm{A},0}.$
(18)

This equation has infinitely many solutions for $\mathbf{R}_{\mathrm{UE}}$,
satisfying both (18) and the orthogonality constraint, among which, one is the
true $\mathbf{R}_{\mathrm{UE}}$.

####  IV-A1 Characterizing the Solutions to (18)

We first find a solution for $\mathbf{R}_{\mathrm{UE}}$ in (18), denoted by
$\widetilde{\mathbf{R}}\in\text{SO}(3)$. Multiplying $\widetilde{\mathbf{R}}$
with a rotation $\psi\in[0,2\pi)$ around $\mathbf{d}_{\mathrm{A},0}$ yields
all rotation matrices that satisfy (18).

######  Lemma 1.

One solution for (18) is given by

$\displaystyle\widetilde{\mathbf{R}}=\mathbf{I}+[\mathbf{d}]_{\times}+\frac{1}{1-\mathbf{d}_{\mathrm{A},0}^{\mathsf{T}}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}}[\mathbf{d}]_{\times}^{2},$
(19)

where
$\mathbf{d}=[d_{1},d_{2},d_{3}]^{\mathsf{T}}\triangleq-\mathbf{d}_{\mathrm{A},0}\times\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}$,
and

$\displaystyle[\mathbf{d}]_{\times}\triangleq\begin{bmatrix}\phantom{-}0&-d_{3}&\phantom{-}d_{2}\\\
\phantom{-}d_{3}&\phantom{-}0&-d_{1}\\\
-d_{2}&\phantom{-}d_{1}&\phantom{-}0\end{bmatrix}.$ (20)

###### Proof.

Let us define
$\tilde{\mathbf{d}}=-\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}$. The
result follows from Rodrigues’ formula, by rotating
$\mathbf{d}_{\mathrm{A},0}$ to $\tilde{\mathbf{d}}$ with rotation axis
$\mathbf{d}_{\mathrm{A},0}\times\tilde{\mathbf{d}}$ [52, Section 9.6.2]. ∎

######  Lemma 2.

The transformation matrix describing rotations by the angle $\psi\in[0,2\pi)$
around the arbitrary unit-norm vector $\mathbf{u}$ is given by
$\mathbf{Q}_{\mathbf{u}}(\psi)=[\mathbf{u}]_{\times}\sin\psi+(\mathbf{I}-\mathbf{u}\mathbf{u}^{\mathsf{T}})\cos\psi+\mathbf{u}\mathbf{u}^{\mathsf{T}}$.

###### Proof.

The result follows from Rodrigues’ formula [52, Section 9.6.2]. ∎

The rotation matrices $\mathbf{R}_{\mathrm{UE}}$ satisfying (18) are thus
characterized as

$\displaystyle\mathbf{R}(\psi)=\widetilde{\mathbf{R}}\mathbf{Q}_{\mathbf{d}_{\mathrm{A},0}}(\psi),~{}\forall\psi\in[0,2\pi).$
(21)

It is easily verified that (18) holds for $\forall\psi\in[0,2\pi)$, since
$-\widetilde{\mathbf{R}}\mathbf{Q}_{\mathbf{d}_{\mathrm{A},0}}(\psi)\mathbf{d}_{\mathrm{A},0}=-\widetilde{\mathbf{R}}\mathbf{d}_{\mathrm{A},0}=\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}$,
where the first transition is due to a rotation around an axis leaving that
axis invariant, and the second transition due to Lemma 1. What remains is now
to determine $\psi$ based on the NLoS paths.

$y_{\mathrm{UE}}$$z_{\mathrm{UE}}$$x_{\mathrm{UE}}$$\mathbf{d}_{\mathrm{A},m}$$\mathbf{d}_{\mathrm{A},0}$LoS$y_{\mathrm{UE}}$$z_{\mathrm{UE}}$$x_{\mathrm{UE}}$$\mathbf{d}_{\mathrm{A},m}$$\mathbf{d}_{\mathrm{A},0}$LoS
Figure 3: The example NLoS arrival direction $\mathbf{d}_{\mathrm{A},m}$
rotates, in the global coordinate frame, while maintaining the same angle
$\bm{\theta}_{\mathrm{A},m}$ in UE coordinate frame, on the lateral surface of
a cone (the gray cone in this figure) with axis $\mathbf{d}_{\mathrm{A},0}$,
and apex angle equal to the angle between $\mathbf{d}_{\mathrm{A},0}$ and
$\mathbf{d}_{\mathrm{A},m}$, when the rotation around axis
$\mathbf{d}_{\mathrm{A},0}$ is applied. The figure illustrates two snapshots
of such rotation.

####  IV-A2 Rotation Estimation Based on NLoS AoAs and AoDs

We now determine the angle $\psi\in[0,2\pi)$, so that the combined rotation
resulting from $\widetilde{\mathbf{R}}$ and
$\mathbf{Q}_{\mathbf{d}_{\mathrm{A},0}}(\psi)$, i.e.,
$\mathbf{R}(\psi)=\widetilde{\mathbf{R}}\mathbf{Q}_{\mathbf{d}_{\mathrm{A},0}}(\psi)$,
leads to the arrival directions $\mathbf{d}_{\mathrm{A},m},~{}m=1,\ldots,M$.
To determine $\psi$, we note the following (see Fig. 1):

  * •

The departure directions $\mathbf{d}_{\mathrm{D},m},~{}m=1,\ldots,M$,
determine the half-lines
$\ell_{\mathrm{D},m}(\mathbf{p}_{\mathrm{BS}},\mathbf{R}_{\mathrm{BS}},\mathbf{d}_{\mathrm{D},m})=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{\mathrm{BS}}+t_{\mathrm{D},m}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m}$,
$t_{\mathrm{D},m}\geq 0\\}$, in the global coordinate frame.

  * •

Under UE rotation represented by $\mathbf{R}({\psi})$, the arrival directions
$\mathbf{d}_{\mathrm{A},m},~{}m=1,\ldots,M$, determine the half-lines
$\ell_{\mathrm{A},m}(\mathbf{p}_{\mathrm{UE}},\mathbf{R}(\psi),\mathbf{d}_{\mathrm{A},m})=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{\mathrm{UE}}+t_{\mathrm{A},m}\mathbf{R}(\psi)\mathbf{d}_{\mathrm{A},m}$,
$t_{\mathrm{A},m}\geq 0\\}$, for any given $\psi\in[0,2\pi)$, in the global
coordinate frame. See Fig. 3.

  * •

With the correct $\psi$, the half-lines
$\ell_{\mathrm{D},m}(\mathbf{p}_{\mathrm{BS}},\mathbf{R}_{\mathrm{BS}},\mathbf{d}_{\mathrm{D},m})$
and
$\ell_{\mathrm{A},m}(\mathbf{p}_{\mathrm{UE}},\mathbf{R}(\psi),\mathbf{d}_{\mathrm{A},m})$
intersect at the incidence point $\mathbf{p}_{m}$ (in the absence of noise).

However, (i) neither $\mathbf{p}_{\mathrm{UE}}$ nor $\mathbf{p}_{m}$ are
known; (ii) the half-lines might not necessarily intersect due to the noisy
measurements.555They form a pair of so-called _skew lines_ , i.e., lines in 3D
that do not intersect, while not being parallel. To tackle the first
challenge, we note that the argument that the half-lines intersect under the
correct value of $\psi$ is true for any scaling of the global coordinate
system. We express
$\mathbf{p}_{\mathrm{UE}}=\mathbf{p}_{\mathrm{BS}}+\rho_{0}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}$,
where $\rho_{0}=\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}\|$. Any
point $\mathbf{p}\in\mathbb{R}^{3}$ can be expressed as
$\mathbf{p}=\mathbf{p}_{\mathrm{BS}}+\rho\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D}}$,
with suitable $\rho$ and $\mathbf{d}_{\mathrm{D}}$, where
$\mathbf{d}_{\mathrm{D}}$ is a unit-norm vector. Given any $r>0$, we can
define a scaled system (with scaling $r/\rho_{0}$) with the BS location as an
invariant point:

$\displaystyle\mathbf{p}(r)$
$\displaystyle=\mathbf{p}_{\mathrm{BS}}+\frac{r}{\rho_{0}}\rho\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D}},$
(22)

and in particular

$\displaystyle\mathbf{p}_{\mathrm{UE}}(r)$
$\displaystyle=\mathbf{p}_{\mathrm{BS}}+\frac{r}{\rho_{0}}\rho_{0}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}.$
(23)

Without loss of generality, we set $r=1$ so that
$\mathbf{p}_{\mathrm{UE}}(r=1)$ is known. Hence, in the scaled coordinate
system with $r=1$,
$\ell_{\mathrm{A},m}(\mathbf{p}_{\mathrm{UE}}(r),\mathbf{R}(\psi),\mathbf{d}_{\mathrm{A},m})$
and
$\ell_{\mathrm{D},m}(\mathbf{p}_{\mathrm{UE}}(r),\mathbf{R}(\psi),\mathbf{d}_{\mathrm{D},m})$
intersect for the correct value of $\psi$, $\forall m$, in the absence of
noise. To cope with the second challenge (measurement noise), we use the
distance between skew lines in the least squares objective. In particular, the
shortest distance $\delta_{m}(\psi)$ between the half-lines
$\ell_{\mathrm{D},m}(\mathbf{p}_{\mathrm{BS}},\mathbf{R}_{\mathrm{BS}},\mathbf{d}_{\mathrm{D},m})$
and
$\ell_{\mathrm{A},m}(\mathbf{p}_{\mathrm{UE}}^{\prime},\mathbf{R}(\psi),\mathbf{d}_{\mathrm{A},m})$
is obtained from the solution of parametric optimization

$\displaystyle\delta_{m}^{2}(\psi)=\min_{\mathbf{t}_{m}\geq\mathbf{0}}D_{m}^{2}(\mathbf{t}_{m},\mathbf{R}(\psi)),~{}m>0,$
(24)

where $\mathbf{t}_{m}=[t_{\mathrm{D},m},t_{\mathrm{A},m}]^{\mathsf{T}}$ and
$D_{m}^{2}(\mathbf{t}_{m},\mathbf{R}(\psi))=\|(\mathbf{p}_{\mathrm{BS}}+t_{\mathrm{D},m}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m})-(\mathbf{p}_{\mathrm{UE}}(1)+t_{\mathrm{A},m}\mathbf{R}(\psi)\mathbf{d}_{\mathrm{A},m})\|^{2}$.
The optimization (24) is a quadratic convex problem, and the solution of that
is provided in Appendix B. Combining the minimum distances, we estimate $\psi$
as

$\displaystyle\hat{\psi}=\arg\min_{\psi\in[0,2\pi)}\|\bm{\delta}(\psi)\|,$
(25)

where
$\bm{\delta}(\psi)=[\delta_{1}(\psi),\ldots,\delta_{M}(\psi)]^{\mathsf{T}}$,
and accordingly, the estimate of $\mathbf{R}_{\mathrm{UE}}$ is given by
$\hat{\mathbf{R}}_{\mathrm{UE}}=\mathbf{R}(\hat{\psi})$, with
$\mathbf{R}(\psi)$ characterized as in (21).

###  IV-B Step 2: Estimation of Positions

We obtain the auxiliary points $\mathbf{p}_{m}(r=1)$ in the scaled coordinate
system induced by (22), as the nearest points to the half-lines
$\ell_{\mathrm{D},m}(\mathbf{p}_{\mathrm{BS}},\mathbf{R}_{\mathrm{BS}},\mathbf{d}_{\mathrm{D},m})$
and
$\ell_{\mathrm{A},m}(\mathbf{p}_{\mathrm{UE}}(1),\hat{\mathbf{R}}_{\mathrm{UE}},\mathbf{d}_{\mathrm{A},m})$
in a least-squares sense (considering the lines from UE and BS as full lines,
rather than half-lines, for computational complexity considerations), using

$\displaystyle\mathbf{p}_{m}(r=1)$
$\displaystyle=\mathbf{A}_{m}^{-1}\mathbf{b}_{m},$ (26a)
$\displaystyle\mathbf{A}_{m}$
$\displaystyle=\mathbf{P}_{\perp}(\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m})+\mathbf{P}_{\perp}(\hat{\mathbf{R}}_{\mathrm{UE}}\mathbf{d}_{\mathrm{A},m}),$
(26b) $\displaystyle\mathbf{b}_{m}$
$\displaystyle=\mathbf{P}_{\perp}(\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m})\mathbf{p}_{\mathrm{BS}}+\mathbf{P}_{\perp}(\hat{\mathbf{R}}_{\mathrm{UE}}\mathbf{d}_{\mathrm{A},m})\mathbf{p}_{\mathrm{UE}}(1)$
(26c)

where
$\mathbf{P}_{\perp}(\mathbf{d})\triangleq\mathbf{I}-\mathbf{d}\mathbf{d}^{\mathsf{T}}$
is the projector onto the subspace orthogonal to the one spanned by a vector
$\mathbf{d}$ (see Appendix C). Then, we define the auxiliary departure
direction $\mathbf{d}_{\mathrm{D},m}^{\prime}$ towards $\mathbf{p}_{m}(1)$ (in
BS coordinate frame), and the corresponding distance $\rho_{m}^{\prime}$ as
(see Fig. 4)

$\displaystyle\mathbf{d}_{\mathrm{D},m}^{\prime}$
$\displaystyle=\mathbf{R}_{\mathrm{BS}}^{\mathsf{T}}\frac{\mathbf{p}_{m}(1)-\mathbf{p}_{\mathrm{BS}}}{\|\mathbf{p}_{m}(1)-\mathbf{p}_{\mathrm{BS}}\|},$
(27a) $\displaystyle\rho_{m}^{\prime}$
$\displaystyle=\|\mathbf{p}_{m}(1)-\mathbf{p}_{\mathrm{BS}}\|,$ (27b)

allowing us to express
$\mathbf{p}_{m}(1)=\mathbf{p}_{\mathrm{BS}}+\rho_{m}^{\prime}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m}^{\prime}$.
What remains is to find $\rho_{0}$, since, with knowledge of $\rho_{0}$, we
can scale the UE and IP positions to their correct place, keeping the BS
fixed. To see this, consider triangles with the vertices
$\\{\mathbf{p}_{\mathrm{BS}},\mathbf{p}_{\mathrm{UE}}(1),\mathbf{p}_{m}(1)\\}$,
$m>1$, and scale them by $\rho_{0}$, yielding

$\displaystyle\mathbf{p}_{m}(\rho_{0})$
$\displaystyle=\mathbf{p}_{\mathrm{BS}}+{\rho_{0}}\rho_{m}^{\prime}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m}^{\prime}$
(28) $\displaystyle\mathbf{p}_{\mathrm{UE}}(\rho_{0})$
$\displaystyle=\mathbf{p}_{\mathrm{BS}}+\frac{\rho_{0}}{\rho_{0}}\rho_{0}\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},0}.$
(29)

$\mathbf{p}_{\mathrm{BS}}$$\mathbf{p}_{\mathrm{UE}}(1)$$\mathbf{p}_{m}(1)$$\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m}$$\mathbf{R}_{\mathrm{BS}}\mathbf{d}^{\prime}_{\mathrm{D},m}$$\mathbf{R}_{\mathrm{UE}}\mathbf{d}_{\mathrm{A},m}$$\rho^{\prime}_{m}$$\delta_{m}(\hat{\psi})$
Figure 4: Visualization of auxiliary direction
$\mathbf{d}_{\mathrm{D},m}^{\prime}$ and auxiliary points
$\mathbf{p}_{\mathrm{UE}}(1)$ and $\mathbf{p}_{m}(1)$ in the scaled geometric
model.

To recover the value of ${\rho}_{0}$, we rely on the TDoA measurements.
Introducing $\bm{\Delta}=[\Delta_{1},\ldots,\Delta_{M}]^{\mathsf{T}}$ with
$\Delta_{m}=c(\tau_{m}-\tau_{0})$, and
$\tilde{\bm{\Delta}}(\rho_{0})=[\tilde{\Delta}_{1}(\rho_{0}),\ldots,\tilde{\Delta}_{M}(\rho_{0})]^{\mathsf{T}}$,
where

$\displaystyle\tilde{\Delta}_{m}(\rho_{0})$
$\displaystyle=\|\mathbf{p}_{m}(\rho_{0})-\mathbf{p}_{\mathrm{BS}}\|+\|\mathbf{p}_{\mathrm{UE}}(\rho_{0})-\mathbf{p}_{m}(\rho_{0})\|-\rho_{0}$
$\displaystyle=\rho_{0}(\rho_{m}^{\prime}+\|\mathbf{d}_{\mathrm{D},0}-\rho_{m}^{\prime}\mathbf{d}_{\mathrm{D},m}^{\prime}\|-1),$
(30)

allows us to formulate

$\displaystyle\hat{\rho}_{0}=\arg\min_{\rho_{0}}\|\bm{\Delta}-\tilde{\bm{\Delta}}(\rho_{0})\|^{2}.$
(31)

Since $\tilde{\bm{\Delta}}(\rho_{0})=\rho_{0}\bm{\beta}$ where
$\beta_{m}=\rho_{m}^{\prime}+\|\mathbf{d}_{\mathrm{D},0}-\rho_{m}^{\prime}\mathbf{d}_{\mathrm{D},m}^{\prime}\|-1$,
the optimization (31) is quadratic in $\rho_{0}$ and gives the closed-form
solution
$\hat{\rho}_{0}={\bm{\beta}^{\mathsf{T}}\bm{\Delta}}/{\bm{\beta}^{\mathsf{T}}\bm{\beta}}$.
Note that $\beta_{m}\geq 0$ since $\tilde{\Delta}_{m}(\rho_{0})\geq
0,~{}\forall\rho_{0}$, according to the Triangle inequality. Hence,
$\hat{\rho}_{0}\geq 0$, and the solution is meaningful. The estimates of
positions are then given by
$\hat{\mathbf{p}}_{\mathrm{UE}}=\mathbf{p}_{\mathrm{UE}}(\hat{\rho}_{0})$ and
$\hat{\mathbf{p}}_{m}=\mathbf{p}_{m}(\hat{\rho}_{0})$, $m=1,\ldots,M$.

###  IV-C Step 3: Estimation of Clock Bias

After estimating the positions for UE and incidence points, we estimate the
clock bias as

$\displaystyle\hat{b}=\arg\min_{b}\|\bm{\tau}-\hat{\bm{\tau}}(\hat{\rho}_{0})+b\mathbf{1}_{M+1}\|^{2},$
(32)

with $\bm{\tau}=[\tau_{0},\ldots,\tau_{M}]^{\mathsf{T}}$,
$\hat{\bm{\tau}}(\hat{\rho}_{0})=[\hat{\tau}_{0}(\hat{\rho}_{0}),\ldots,\hat{\tau}_{M}(\hat{\rho}_{0})]^{\mathsf{T}}$,
and

$\displaystyle\hat{{\tau}}_{m}(\hat{\rho}_{0})=\begin{cases}\hfil\|\hat{\mathbf{p}}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|/c=\hat{\rho}_{0}/c&m=0\\\
(\|\hat{\mathbf{p}}_{m}-\mathbf{p}_{\mathrm{BS}}\|+\|\hat{\mathbf{p}}_{\mathrm{UE}}-\hat{\mathbf{p}}_{m}\|)/c&m\neq
0,\end{cases}$

giving the closed-form solution
$\hat{b}={\mathbf{1}^{\mathsf{T}}(\bm{\tau}-\hat{\bm{\tau}}(\hat{\rho}_{0}))}/{(M+1)}$.

##  V Fisher Information Analysis

In this section, we derive the FIM of the channel parameters and the
localization parameters, and obtain the error bounds for 6D localization,
mapping, as well as UE synchronization.

###  V-A FIM of Channel Parameters

We define the vector of channel parameters as

$\displaystyle\bm{\eta}_{\mathrm{ch}}\triangleq[\underbrace{\bm{\theta}_{\mathrm{A}}^{\mathsf{T}},\bm{\theta}_{\mathrm{D}}^{\mathsf{T}},\bm{\tau}^{\mathsf{T}}}_{\bm{\eta}\in\mathbb{R}^{5(M+1)}},\mathbf{h}_{\mathrm{R}}^{\mathsf{T}},\mathbf{h}_{\mathrm{I}}^{\mathsf{T}}]^{\mathsf{T}}\in\mathbb{R}^{7(M+1)},$
(33)

where
$\mathbf{h}_{\mathrm{R}}=[h_{\mathrm{R},0},\ldots,h_{\mathrm{R},M}]^{\mathsf{T}}$,
and
$\mathbf{h}_{\mathrm{I}}=[h_{\mathrm{I},0},\ldots,h_{\mathrm{I},M}]^{\mathsf{T}}$.
The FIM of $\bm{\eta}_{\mathrm{ch}}$, considering the signal model (1), is
given by the Slepian-Bangs formula [53, Section 3.9] as

$\displaystyle[\mathbf{J}_{\bm{\eta}_{\mathrm{ch}}}]_{i,j}=\frac{2E_{\mathrm{s}}}{N_{0}}\sum_{k=1}^{K}\sum_{n=1}^{N}\Re\left\\{\frac{\partial\widetilde{y}_{k,n}^{\mathsf{H}}}{\partial[{\bm{\eta}_{\mathrm{ch}}}]_{i}}\frac{\partial\widetilde{y}_{k,n}}{\partial[{\bm{\eta}_{\mathrm{ch}}}]_{j}}\right\\},$
(34)

where $\widetilde{y}_{k,n}$ is the noise-free part of the observation
$y_{k,n}$, and the gradients can be found in [16, Appendix. I]. Then we obtain
the equivalent Fisher information matrix (EFIM) of AoAs, AoDs, and ToAs as in
the following:

$\displaystyle\mathbf{J}_{\bm{\eta}}=\left[[\mathbf{J}_{\bm{\eta}_{\mathrm{ch}}}^{-1}]_{1:5(M+1),1:5(M+1)}\right]^{-1}.$
(35)

###  V-B FIM of Localization Parameters

Expressing
$\mathbf{R}_{\mathrm{UE}}=[\mathbf{r}_{\mathrm{UE},1},\mathbf{r}_{\mathrm{UE},2},\mathbf{r}_{\mathrm{UE},3}]$
with $\mathbf{r}_{\mathrm{UE},1}$, $\mathbf{r}_{\mathrm{UE},2}$, and
$\mathbf{r}_{\mathrm{UE},3}$ as columns, we recall the vector of localization
unknowns as

$\displaystyle\bm{\xi}=[\mathbf{r}^{\mathsf{T}},\mathbf{p}_{\mathrm{UE}}^{\mathsf{T}},\mathbf{p}_{1}^{\mathsf{T}},\ldots,\mathbf{p}_{M}^{\mathsf{T}},b]^{\mathsf{T}}\in\mathbb{R}^{3(M+1)+10}.$
(36)

where
$\mathbf{r}=\mathrm{vec}(\mathbf{R}_{\mathrm{UE}})=[\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}},\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}},\mathbf{r}_{\mathrm{UE},3}^{\mathsf{T}}]^{\mathsf{T}}$.
Then we obtain the FIM $\mathbf{J}_{\bm{\xi}}$ by transforming the channel
parameters to localization parameters through the Jacobian matrix
$\bm{\Upsilon}$ as follows [53, Eq. (3.30)]:

$\displaystyle\mathbf{J}_{\bm{\xi}}=\bm{\Upsilon}^{\mathsf{T}}\mathbf{J}_{\bm{\eta}}\bm{\Upsilon},$
(37)

where
$[\bm{\Upsilon}]_{i,j}=\partial[\mathbf{J}_{\bm{\eta}}]_{i}/\partial[\mathbf{J}_{\bm{\xi}}]_{j}$,
and $\mathbf{J}_{\bm{\eta}}$ is given in (35). To obtain the elements of the
transformation matrix $\bm{\Upsilon}$, we need the derivatives of channel
parameters w.r.t. localization parameters, which are obtained in Appendix A.
We note that
${\partial[{\bm{\eta}}]_{i}}/{\partial\mathbf{r}}=\mathrm{vec}\left({\partial[{\bm{\eta}}]_{i}}/{\partial\mathbf{R}_{\mathrm{UE}}}\right)$.

To obtain the error bounds, we need to account for the constraint that
$\mathbf{R}_{\mathrm{UE}}\in\text{SO}(3)$. We obtain the constrained Cramér-
Rao bound (CCRB) [54] giving the lower bound on the estimation error
covariance, for any unbiased estimator subject to the required constraint on
$\mathbf{R}_{\mathrm{UE}}$. The set of constraints to be satisfied due to
orthogonality of the rotation matrix (i.e.,
$\mathbf{R}_{\mathrm{UE}}^{\mathsf{T}}\mathbf{R}_{\mathrm{UE}}=\mathbf{I}_{3}$)
is given by

$\displaystyle\mathbf{h}(\mathbf{\bm{\xi}})=[$
$\displaystyle\|\mathbf{r}_{1}\|^{2}-1,\mathbf{r}_{2}^{\mathsf{T}}\mathbf{r}_{1},\mathbf{r}_{3}^{\mathsf{T}}\mathbf{r}_{1},$
$\displaystyle\|\mathbf{r}_{2}\|^{2}-1,\mathbf{r}_{2}^{\mathsf{T}}\mathbf{r}_{3},\|\mathbf{r}_{3}\|^{2}-1]^{\mathsf{T}}=\mathbf{0}_{6}.$
(38)

Considering
$\mathbf{M}=\mathrm{blkdiag}(\frac{1}{\sqrt{2}}\mathbf{M}_{0},\mathbf{I}_{3(M+1)+1})$
with

$\displaystyle\mathbf{M}_{0}=\begin{bmatrix}\begin{array}[]{rrrr}-\mathbf{r}_{3}&\mathbf{0}_{3}&\mathbf{r}_{2}\\\
\mathbf{0}_{3}&-\mathbf{r}_{3}&-\mathbf{r}_{1}\\\
\mathbf{r}_{1}&\mathbf{r}_{2}&\mathbf{0}_{3}\\\
\end{array}\end{bmatrix}\in\mathbb{R}^{9\times 3},$ (39)

meets $\mathbf{G}(\bm{\xi})\mathbf{M}=\mathbf{0}$ where
$[\mathbf{G}(\bm{\xi})]_{i,j}={\partial[\mathbf{h}(\bm{\xi})}]_{i}/{\partial[\bm{\xi}]_{j}}$,
and gives [54]

$\displaystyle\mathbf{C}^{(\mathrm{CCRB})}_{\bm{\xi}}=\mathbf{M}(\mathbf{M}^{\mathsf{T}}\mathbf{J}_{\bm{\xi}}\mathbf{M})^{-1}\mathbf{M}^{\mathsf{T}},$
(40)

in which $\mathbf{J}_{\bm{\xi}}$ is given in (37). Then any unbiased estimate
$\hat{\bm{\xi}}$ subject to $\hat{\mathbf{R}}_{\mathrm{UE}}\in\text{SO}(3)$
satisfies
$\mathbb{E}\big{\\{}(\hat{\bm{\xi}}-\bm{\xi})(\hat{\bm{\xi}}-\bm{\xi})^{\mathsf{T}}\big{\\}}\succeq\mathbf{C}^{(\mathrm{CCRB})}_{\bm{\xi}}$,
where the expectation is with respect to the noise.

Finally, we define orientation error bound (OEB), position error bound (PEB),
IPs error bound (IPEB), and synchronization error bound (SEB), which show the
lower bound on the RMSE of estimation as

$\displaystyle\mathrm{OEB}$
$\displaystyle=\left[\mathrm{tr}\left(\mathbf{C}_{\mathbf{R}_{\mathrm{UE}}}\right)\right]^{\nicefrac{{1}}{{2}}}$
$\displaystyle,~{}\mathrm{PEB}$
$\displaystyle=\left[\mathrm{tr}\left(\mathbf{C}_{\mathbf{p}_{\mathrm{UE}}}\right)\right]^{\nicefrac{{1}}{{2}}}$
, $\displaystyle\mathrm{IPEB}$
$\displaystyle={\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\left[\nicefrac{{\textstyle\sum_{m=1}^{M}\mathrm{tr}\left(\mathbf{C}_{\mathbf{p}_{m}}\right)}}{{M}}\right]^{\nicefrac{{1}}{{2}}}}$
$\displaystyle,~{}\mathrm{SEB}$
$\displaystyle=\left[\mathrm{tr}\left(\mathbf{C}_{b}\right)\right]^{\nicefrac{{1}}{{2}}}$
,

where $\mathbf{C}_{\mathbf{R}_{\mathrm{UE}}}$,
$\mathbf{C}_{\mathbf{p}_{\mathrm{UE}}}$, $\mathbf{C}_{\mathbf{p}_{m}}$, and
$\mathbf{C}_{b}$ are diagonal sub-matrices in
$\mathbf{C}^{(\mathrm{CCRB})}_{\bm{\xi}}$ corresponding to $\mathbf{r}$,
$\mathbf{p}_{\mathrm{UE}}$, $\mathbf{p}_{m}$, and $b$, respectively. We note
that $\mathbf{C}_{b}$ is a scalar equal to the variance of clock bias
estimation, and that the RMSE of estimation of $\mathbf{r}$ is equal to
$\mathbb{E}\left\\{\|\mathbf{r}-\hat{\mathbf{r}}\|^{2}\right\\}=\mathbb{E}\big{\\{}\|\mathbf{R}_{\mathrm{UE}}-\hat{\mathbf{R}}_{\mathrm{UE}}\|^{2}_{\text{F}}\big{\\}}$.
In addition, the IPEB (in meters) represents the RMSE of all the IP location
estimates, and is a simplified form of the widely used GOSPA metric, from
radar sensing and target tracking [55].

##  VI Numerical Results

BSUEIP1IP2$y$$x$$z$$x$$y$$z$Global Coordinate System Figure 5: The indoor
scenario considered in simulations, with default parameters provided in Table
II.

###  VI-A Simulation Setup

Our simulation scenario consists of an indoor environment shown in Fig. 5,
where the BS is mounted vertically. We employ uniform planar arrays in both BS
and UE, consisting of rectangular configurations of
$\sqrt{N_{\mathrm{BS}}}\times\sqrt{N_{\mathrm{BS}}}$ and
$\sqrt{N_{\mathrm{UE}}}\times\sqrt{N_{\mathrm{UE}}}$ antennas, with half-
wavelength inter-element spacing. Assuming the configurations in the reference
orientation where the planar arrays are parallel to the global XY-axes,
facilitates expressing $\bm{\Delta}_{\mathrm{BS}}$ and
$\bm{\Delta}_{\mathrm{UE}}$, i.e., the matrices containing the positions of
antenna elements in local coordinate frames666The antenna element located at
row $i$ and column $j$ of such configuration for BS, $1\leq
i,j\leq\sqrt{N_{\mathrm{BS}}}$, is $(i-1)\sqrt{N_{\mathrm{BS}}}+j$-th antenna,
which is located at
$\mathbf{x}_{\mathrm{BS},(i-1)\sqrt{N_{\mathrm{BS}}}+j}=[j-(\sqrt{N_{\mathrm{BS}}}+1)/2,-i+(\sqrt{N_{\mathrm{BS}}}+1)/2,0]^{\mathsf{T}}d_{\mathrm{BS}}$,
with $d_{\mathrm{BS}}=\lambda/2$. Similarly, we express the positions of
antenna elements in UE array.. For the channel model, we correspond incidence
points to reflecting surfaces with reflection coefficient
$\Gamma_{\mathrm{ref}}$ 777For the specific IP1 and IP2, at the given
positions $\mathbf{p}_{1}$ and $\mathbf{p}_{2}$, we assume
$\Gamma_{\mathrm{ref},1}=0.2$ and $\Gamma_{\mathrm{ref},2}=0.8$, respectively.
However, for the general IPs at random positions,
$\Gamma_{\mathrm{ref}}=0.7$.. We consider that the channel gains are
proportional to the free-space path loss, with a random phase uniformly
distributed in $[0,2\pi)$, and account for the radiation pattern of antennas
[56, Chapter 4] as follows:

$\displaystyle|h_{m}|^{2}=\begin{cases}\hfil\dfrac{\lambda^{2}\cos^{2}\theta^{(\mathrm{el})}_{\mathrm{A},0}\cos^{2}\theta^{(\mathrm{el})}_{\mathrm{D},0}}{(4\pi)^{2}\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}\|^{2}}&m=0\\\
\dfrac{\lambda^{2}\Gamma_{\mathrm{ref}}\cos^{2}\theta^{(\mathrm{el})}_{\mathrm{A},m}\cos^{2}\theta^{(\mathrm{el})}_{\mathrm{D},m}}{(4\pi)^{2}(\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{m}\|+\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{m}\|)^{2}}&m\neq
0.\end{cases}$

The pilots are set to $x_{k,n}=\sqrt{E_{s}}$ and the components in precoding
and combining vectors $\bm{f}_{k}$ and $\bm{w}_{k}$ are assumed to comprise
unit-modulus elements with random phase, different for each OFDM symbol.

The rotation matrices are also generated with help of Euler angles
$\alpha\in[0,2\pi),~{}\beta\in[0,\pi),~{}\gamma\in[0,2\pi)$ [39], using
$\mathbf{R}=\mathbf{R}_{z}(\alpha)\mathbf{R}_{y}(\beta)\mathbf{R}_{x}(\gamma)$,
where $\mathbf{R}_{z}(\alpha)$, $\mathbf{R}_{y}(\beta)$, and
$\mathbf{R}_{x}(\gamma)$ are transformation matrices for counter-clockwise
rotations around $z$, $y$, and $x$ axes through $\alpha$, $\beta$, and
$\gamma$, respectively [39].

TABLE II: Default simulation parameters. Parameters that vary are marked with $*$. Parameter | Symbol | Value
---|---|---
Propagation Speed | $c$ |  $3\times 10^{8}$ m/s
Carrier Frequency | $f_{c}$ |  $28$ GHz
Subcarrier Spacing | $\Delta_{f}$ |  $120$ kHz
$\\#$ Subcarriers∗ | $N_{f}$ | $3333$
$\\#$ OFDM Symbols | $K$ | $10$
Transmit Power∗ | $P_{\mathrm{TX}}$ |  $10$ dBm
Noise PSD | $N_{0}$ |  $-174$ dBm/Hz
UE Noise Figure | $n_{0}$ |  $13$ dB
BS $\\#$ Antennas∗ | $N_{\mathrm{BS}}$ | $64~{}(8\times 8)$
UE $\\#$ Antennas∗ | $N_{\mathrm{UE}}$ | $4~{}(2\times 2)$
BS Position | $\mathbf{p}_{\mathrm{BS}}$ | $[4,0,4]^{\mathsf{T}}$
BS Orientation | $\mathbf{R}_{\mathrm{BS}}$ | $\mathbf{R}_{x}(-\pi/2)$
UE Position∗ | $\mathbf{p}_{\mathrm{UE}}$ | $[5,4,1]^{\mathsf{T}}$
UE Orientation∗ | $\mathbf{R}_{\mathrm{UE}}$ |  given in (41)
IP Positions∗ | $\mathbf{p}_{m}$ |  given in (42)
Reflection Coefficient∗ | $\Gamma_{\mathrm{ref}}$ | $[0.2,0.8]~{},~{}0.7$
Clock Offset | $b$ |  $100$ ns
$\\#$ Monte Carlo Simulations | $N_{s}$ | 1000

Table II lists all the default simulation parameters, where default
orientation for UE is either of $\mathbf{R}_{1}$ or $\mathbf{R}_{2}$ given by

$\displaystyle\mathbf{R}_{1}=\mathbf{R}_{z}(\nicefrac{{\pi}}{{6}})\mathbf{R}_{y}(\nicefrac{{-\pi}}{{4}})\mathbf{R}_{x}(\nicefrac{{-\pi}}{{36}}),\mathbf{R}_{2}=\mathbf{R}_{x}(\nicefrac{{\pi}}{{2}}).$
(41)

In some cases, we generate the orientation randomly. More precisely, $\alpha$,
$\beta$, and $\gamma$ are generated randomly. Although we note that this is
not equivalent to uniform sampling of $\mathrm{SO}(3)$, it is an easy way to
evaluate different orientations. Also, in some simulations, the positions of
IPs are generated randomly, while the positions of default IPs labeled in Fig.
5 are given in

$\displaystyle\mathbf{P}=[\mathbf{p}_{1},\mathbf{p}_{2}]=\big{[}[8,2,1]^{\mathsf{T}},[0,6,2]^{\mathsf{T}}\big{]}.$
(42)

The simulations are done in MATLAB $2021$b. For the optimization on manifolds,
we utilize the Manopt toolbox [57].

Figure 6: CDF of PEB with and without independence of channel parameters for a
varying number of incidence points.

###  VI-B Obtaining the Likelihood Parameters

To obtain the parameters $\bm{\kappa}_{\mathrm{A}}$,
$\bm{\kappa}_{\mathrm{D}}$, and $\Sigma_{\bm{\tau}}$ presented in Section III,
considering the independence of angles and delays, we derive the covariance
matrix

$\displaystyle\mathbf{C}_{\bm{\eta}}=\mathrm{diag}\big{(}\mathbf{J}_{\bm{\eta}}^{-1}\big{)}=\mathrm{blkdiag}(\mathbf{C}_{\bm{\theta}_{\mathrm{A}}},\mathbf{C}_{\bm{\theta}_{\mathrm{D}}},\mathbf{C}_{\bm{\tau}}),$
(43)

where $\mathbf{C}_{\bm{\theta}_{\mathrm{A}}}$,
$\mathbf{C}_{\bm{\theta}_{\mathrm{D}}}$ and $\mathbf{C}_{\bm{\tau}}$ are
diagonal matrices corresponding to AoAs, AoDs, and ToAs, respectively.
Furthermore, using the independence assumption, FIMs of
$\bm{\theta}_{\mathrm{A}}$ and $\bm{\theta}_{\mathrm{D}}$ are given, with
respect to $\bm{\kappa}_{\mathrm{A}}$ and $\bm{\kappa}_{\mathrm{D}}$,
respectively, by [22]

$\displaystyle\mathbf{J}_{\bm{\theta}_{\mathrm{A}}}=\mathrm{diag}\big{(}\bm{\kappa}_{\mathrm{A}}\odot
I_{1}(\bm{\kappa}_{\mathrm{A}})\oslash
I_{0}(\bm{\kappa}_{\mathrm{A}})\big{)},$ (44a)
$\displaystyle\mathbf{J}_{\bm{\theta}_{\mathrm{D}}}=\mathrm{diag}\big{(}\bm{\kappa}_{\mathrm{D}}\odot
I_{1}(\bm{\kappa}_{\mathrm{D}})\oslash
I_{0}(\bm{\kappa}_{\mathrm{D}})\big{)},$ (44b)

where $I_{1}(\cdot)$ is the modified Bessel function of order $1$. Solving the
above equations gives $\bm{\kappa}_{\mathrm{A}}$ and
$\bm{\kappa}_{\mathrm{D}}$ 888Specially in high SNR regimes, the values in
$\bm{\kappa}_{\mathrm{A}}$ and $\bm{\kappa}_{\mathrm{D}}$ are large, and
therefore, $I_{1}(\bm{\kappa}_{\mathrm{A}})\oslash
I_{0}(\bm{\kappa}_{\mathrm{A}})\rightarrow\mathbf{1}_{2(M+1)}$ and
$I_{1}(\bm{\kappa}_{\mathrm{D}})\oslash
I_{0}(\bm{\kappa}_{\mathrm{D}})\rightarrow\mathbf{1}_{2(M+1)}$, leading to
$\bm{\kappa}_{\mathrm{A}}=\mathrm{diag}\big{(}\mathbf{C}_{\bm{\theta}_{\mathrm{A}}}^{-1}\big{)}$
and
$\bm{\kappa}_{\mathrm{D}}=\mathrm{diag}\big{(}\mathbf{C}_{\bm{\theta}_{\mathrm{D}}}^{-1}\big{)}$.
There are other approximations for the ratio $I_{1}(x)/I_{0}(x)$, for example
[58, Lemma 2].. Obviously, $\Sigma_{\bm{\tau}}=\mathbf{C}_{\bm{\tau}}$.

In order to motivate removing dependencies, we evaluate the CDF of PEB with
and without the independence assumption, using $N_{s}=1000$ Monte Carlo
simulations. In every simulation, we randomize $\mathbf{R}_{\mathrm{UE}}$ as
well as
$\mathbf{p}_{\mathrm{UE}},~{}\mathbf{p}_{1},\ldots,\mathbf{p}_{M}\in[0,8]\times[0,8]\times[0,4]~{}$m3.
Then we obtain the PEB in two cases: the general case using
$\mathbf{J}_{\bm{\eta}}$ in (37) and then (40); the independent case, with
$\mathbf{J}^{\text{ind}}_{\bm{\eta}}=(\mathrm{diag}\big{(}\mathbf{J}_{\bm{\eta}}^{-1}\big{)})^{-1}$
in (37) and then (40). The CDF curves are shown for three different numbers of
IPs, i.e., $M\in\\{1,2,10\\}$. As seen in Fig. 6, the distribution of PEB with
the independence of channel parameters is closely following that of the
general case, meaning that not only the angles and delays of different paths
but also the azimuth and elevation angles of every individual path can be
taken to be independent without a considerable impact. Similar observations
hold for the other bounds, i.e., OEB, IPEB, and SEB. Although in certain cases
the performance with dependencies can differ significantly (up to $50\%$) from
the independent case, the effect is limited on average.

Figure 7: RMSE of UE orientation estimation (top) and UE position estimation
(bottom) vs. $P_{\mathrm{TX}}$, for ML and ad-hoc estimators, with search
granularity $\pi/200$, for $M\in\\{1,2\\}$, with
$\mathbf{R}_{\mathrm{UE}}=\mathbf{R}_{2}$ and the default
$\mathbf{p}_{\mathrm{UE}}$, $\mathbf{p}_{1}$, and $\mathbf{p}_{2}$ (in case of
$M=2$). The figures also include the OEB and PEB.

###  VI-C Results and Discussions

####  VI-C1 Performance Evaluation of ML and Ad-hoc Estimators

The performance evaluation for the proposed estimators is shown in Fig. 7,
where we show the RMSE of UE orientation and position estimation vs. the
transmit power $P_{\mathrm{TX}}$, for two cases, i.e., $M=1$ with IP1, and
$M=2$, with both IP1 and IP2, with the reflection coefficients
$\Gamma_{\mathrm{ref}}=0.2$ for IP1, and $\Gamma_{\mathrm{ref}}=0.8$ for IP2.
We observe that the performance of both estimators improves by increasing the
transmit power, closely following the corresponding bounds. Specifically, the
RMSE of UE position estimation using the proposed ad-hoc routine sees a
negligible gap compared to the CRB, for a large range of transmit powers. This
of course depends on the geometry as well as the granularity of the
1-dimensional search for obtaining $\psi$. Not surprisingly, in the low SNR
regimes, the performance deviates from the bound, but in the case of $M=2$,
the ML estimator is able to reduce the gap. This gap is due to the ignorance
of the distribution of measurements in the ad-hoc estimator, and it is
especially pronounced in unfavorable positions of UE, where the quality of
different paths arriving at UE are remarkably different. In moderate SNR, the
ML estimation yields an RMSEs close to the performance bounds. In very high
SNR regimes, the performance of the ad-hoc estimator saturates due to the
granularity of angle search. It is then refined using the ML, and the gap to
the bounds is substantially reduced. As a benchmark, we compare the
positioning performance of the ad-hoc estimator to that of the only other
available estimator, i.e., [31, eq. (15)–(16)]. We observe that the proposed
ad-hoc estimator achieves identical performance. The strength of [31] is that
it does not require an estimate of the rotation matrix for estimating the UE
position, but at the same time, the one-dimensional search of our method for
estimation of rotation matrix should be done anyway, to initialize the ML
algorithm. The tightness of ML estimator to the CRB, and the negligible gap
between the performance of the ad-hoc estimator and the lower bounds for a
practical range, shows the efficiency of our proposed estimation algorithms.

Figure 8: Impact of bandwidth (left), number of UE antennas (middle-left),
number of BS antennas (middle-right), and number of IPs (right) on (average)
performance error bounds.

####  VI-C2 Impact of System Parameters

In Fig. 8, we evaluate the impact of bandwidth, the number of antennas, and
the number of IPs, using $\mathbf{R}_{\mathrm{UE}}=\mathbf{R}_{1}$ and the
default $\mathbf{p}_{\mathrm{UE}}$. For evaluation of the impact of bandwidth
and number of antennas, we consider one IP at the default position
$\mathbf{p}_{1}$, while we evaluate the impact of the number of IPs, in an
average sense, i.e., the positions of IPs are randomized, and the average
error bounds are obtained.

As it is observed in the left plot in Fig. 8, increasing the bandwidth, which
leads to higher ToA accuracy and improved delay resolution, decreases the
error bounds. This trend, however, saturates at some point ($\approx 100$
MHz), because further improvement is limited by the accuracy of angle
measurements. The OEB is the least benefited from the enhancement of ToA
accuracy, which makes sense since the orientation is determined mainly by
angle measurements and not the delays.

The two middle plots of Fig. 8, show the performance gains achieved when the
angular resolution and accuracy improve thanks to an increase in the number of
UE and BS antennas. PEB and SEB benefit most from additional UE antennas,
while IPEB benefits most from additional BS antennas. Since analog combining
is used with a fixed number of precoders and combiners, there is no array
gain, which leads again to saturation at a larger number of antennas, when
there are no further resolution gains, and the performance is limited by the
bandwidth.

Finally, in the right plot of Fig. 8 we show the impact of the number of IPs.
We see that increasing number of IPs leads to improvements in the OEB, PEB,
and SEB. The reduction of error bounds is especially considerable when the
number of IPs changes from $1$ to $2$. The reason is that, when $M=1$, the
quality of estimating AoA or AoD degrades in certain positions, and this
causes larger error bounds, on average. However, when another IP is added, the
probability of having both IPs at unfavorable positions reduces significantly,
and the average error bounds decrease. For the IPEB, while it decreases at the
beginning with the number of incidence points, it may experience small
fluctuations or even increase at larger $M$. This is due to the increase in
the number of unknowns, i.e., positions of IPs.

Figure 9: CDF of OEB (left), PEB (middle-left), IPEB (middle-right), and SEB
(right), under different levels of side-information. The position and
orientation of UE are randomized, and the default IP1 is included.

####  VI-C3 Impact of Known Parameters

In this part, we assess the impact of known parameters, i.e., we evaluate the
best achievable estimation accuracy, if some of the parameters are known. For
that, we depict the CDF of error bounds in Fig. 9. To set up the Monte Carlo
simulations, we consider only one IP at the default position $\mathbf{p}_{1}$,
while we randomize $[p_{\mathrm{UE},x},p_{\mathrm{UE},y}]\in[0,8]\times[0,8]$
in the $p_{\mathrm{UE},z}=1$ plane (though $p_{\mathrm{UE},z}=1$ is considered
unknown), as well as the UE orientation. The CDF curves are shown for
$N_{s}=10,000$ realizations. In terms of the OEB, position knowledge of either
the UE or IP improves the orientation accuracy, congruent with the findings
from [22] with $2$ BSs. In terms of the PEB, orientation awareness is less
important than the knowledge from the mapping, i.e., the position of the
incidence point. Similarly, the knowledge of UE position can help mapping, and
certainly, if both $\mathbf{p}_{\mathrm{UE}}$ are $\mathbf{R}_{\mathrm{UE}}$
are known, lower IPEB is achieved. Finally, in terms of the SEB, a variety of
cases exist, with and without side information from either or some of
$\mathbf{R}_{\mathrm{UE}}$, $\mathbf{p}_{\mathrm{UE}}$, and $\mathbf{p}_{1}$.
As we have seen on the other bounds, knowledge of orientation is the least
informative, and the UE and IP location awareness provide a large amount of
information on the clock bias.

BSIP1 (a) $10\log(\text{OEB})$

BSIP1 (b) $10\log(\text{PEB})$

BSIP1IP2 (c) $10\log(\text{OEB})$

BSIP1IP2 (d) $10\log(\text{PEB})$

Figure 10: Contour plots of (a) OEB, (b) PEB [m] with IP1; (c) OEB, (d) PEB
[m] with IP1 and IP2, for $p_{\mathrm{UE},z}=1$,
$\mathbf{R}_{\mathrm{UE}}=\mathbf{R}_{2}$.

(a) $10\log(\text{OEB})$

(b) $10\log(\text{PEB})$

(c) $10\log(\text{OEB})$

(d) $10\log(\text{PEB})$

Figure 11: Contour plots of of (a) OEB, (b) PEB [m] with IP1; (c) OEB, (d) PEB
[m] with IP1 and IP2, for $\beta=-\pi/4$.

####  VI-C4 6D Localization Coverage

As the last step in our simulations, we evaluate the localization coverage and
performance robustness, via contour plots of OEB and PEB, in the region around
the BS and IPs. Fig. 10 shows the contour plots of error bounds, when $x$ and
$y$ coordinates of UE position are varied, while the $z$ coordinate is fixed
to $1$, considering $1$ NLoS path (top row) or $2$ NLoS paths (bottom row). As
the UE gets closer to the IP (which has the lower channel gain compared to the
direct path from the BS), the quality of estimation of all parameters
improves, unless if UE approaches the $y=0$ plane resulting in
$\theta_{\mathrm{D},0}^{(\mathrm{el})}$ close to $\pi/2$, which in turn
strongly attenuates the LoS path, degrades the estimation of
$\bm{\theta}_{\mathrm{D},0}$, and accordingly increases the error. Including
IP2 at the default position provides another signal source and improves the
coverage. In summary, good performance is achieved close to IPs, with a
graceful degradation further away. However, if the UE should not be so close
to the IP that the NLoS paths is no longer resolvable from the LoS path.

In a similar fashion, we depict the contour plots of error bounds, for a range
of rotation angles of UE in Fig. 11, by fixing one of the Euler angles
$\beta=-\pi/4$. If only IP1 is present, we observe a continuous set of
orientations for which the bounds are infinite. Specifically, the orientation
caused by the composition of rotations around $z$ and $x$ axes through
$\alpha$ and $\gamma$ respectively, results in the received ray from either
LoS or NLoS paths to hit the UE antenna array on the endfire, and does not
provide a high-quality estimation of either of AoAs. Subsequently, both OEB
and PEB are affected. However, once these specific orientations change, the
signal arrives in more appropriate directions, and the bounds improve. Once a
second IP is added, the problem is non-identifiable for only a reduced set of
configurations. Hence, with $M>1$ NLoS paths, a more uniform 6D localization
coverage can be achieved.

While not shown, the performance as a function of the IP location can also be
evaluated. With a single IP, the bounds become infinite when the IP is on the
3D line between BS and UE, which is a very unlikely configuration.

##  VII Conclusions

In this paper, we considered a single BS transmitting a mmWave OFDM signal and
a multi-antenna UE receiving the LoS path and at least one resolvable NLoS
path. The objective was to solve the snapshot 6D localization problem whereby
the 3D position, the 3D orientation, and the clock bias of the UE have to be
estimated, as well as the positions of the incidence points. Two estimation
routines, namely ML estimation (which is a high-dimensional non-convex
optimization problem over a product of manifolds) and an ad-hoc routine, were
applied and their performance was evaluated. Although the performance of the
former attains the CRB, the latter provides estimates based on geometrical
arguments. These estimates closely follow the CRB for a large transmit power
range, and serve as initialization to the recursive algorithms for solving the
ML complex estimation problem. In contrast, the proposed ad-hoc solution
reduces the complexity to a single 1-dimensional search over a finite
interval, combined with closed-form expressions. After obtaining an efficient
ML-based estimator, we evaluated the impact of different parameters, such as
bandwidth, number of antennas, number of NLoS paths, etc., through evaluation
of CRB. These results indicate that at least 2 NLoS paths are needed to render
the problem identifiable for most geometric configurations.

There are several possible extensions of the current work, which may be
addressed in future studies. First of all, a solution to the obstructed LoS
scenarios should be developed. Secondly, besides single-bounce NLoS paths,
there may double- and multiple-bounce reflections, and the performance of
localization in these conditions should be investigated.

##  Appendix A Partial Derivatives of AoAs, AoDs, and ToAs w.r.t. Localization
Parameters

###  A-A Auxiliary Variables

We define auxiliary variables

$\displaystyle\mathbf{u}_{\mathrm{A},m\neq 0}$
$\displaystyle=\frac{\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}}{\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}\|},\quad\mathbf{u}_{\mathrm{A},0}=\frac{\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}}{\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}\|},$
(45a) $\displaystyle\mathbf{u}_{\mathrm{D},m\neq 0}$
$\displaystyle=\frac{\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}}{\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}\|},\quad\mathbf{u}_{\mathrm{D},0}=\frac{\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}}{\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|},$
(45b)

as well as $\mathbf{u}_{1}=[1,0,0]^{\mathsf{T}}$,
$\mathbf{u}_{2}=[0,1,0]^{\mathsf{T}}$, and
$\mathbf{u}_{3}=[0,0,1]^{\mathsf{T}}$, for later use. Considering
$\mathbf{R}_{\mathrm{UE}}=[\mathbf{r}_{\mathrm{UE},1},\mathbf{r}_{\mathrm{UE},2},\mathbf{r}_{\mathrm{UE},3}]$
and
$\mathbf{R}_{\mathrm{BS}}=[\mathbf{r}_{\mathrm{BS},1},\mathbf{r}_{\mathrm{BS},2},\mathbf{r}_{\mathrm{BS},3}]$
gives $\mathbf{R}_{\mathrm{UE}}\mathbf{u}_{i}=\mathbf{r}_{\mathrm{UE},i}$ and
$\mathbf{R}_{\mathrm{BS}}\mathbf{u}_{i}=\mathbf{r}_{\mathrm{BS},i}$,
$i=1,2,3$.

###  A-B Mathematical Identities

The following mathematical identities are used in obtaining the derivatives:

$\displaystyle\frac{\partial}{\partial\mathbf{X}}\mathbf{a}^{\mathsf{T}}\mathbf{X}^{\mathsf{T}}\mathbf{b}=\mathbf{b}\mathbf{a}^{\mathsf{T}},$
(46a)
$\displaystyle\frac{\partial}{\partial\mathbf{x}}\mathbf{a}^{\mathsf{T}}\mathbf{x}=\mathbf{a},$
(46b)
$\displaystyle\frac{\partial}{\partial\mathbf{x}}\mathrm{acos}(v(\mathbf{x}))=-\frac{1}{\sqrt{1-v^{2}(\mathbf{x})}}\frac{\partial
v(\mathbf{x})}{\partial\mathbf{x}},$ (46c)
$\displaystyle\frac{\partial}{\partial\mathbf{x}}\mathrm{atan}2({v}(\mathbf{x}),{w}(\mathbf{x}))=\frac{{w}(\mathbf{x})\frac{\partial
v(\mathbf{x})}{\partial\mathbf{x}}-{v}(\mathbf{x})\frac{\partial
w(\mathbf{x})}{\partial\mathbf{x}}}{{v}^{2}(\mathbf{x})+{w}^{2}(\mathbf{x})},$
(46d)
$\displaystyle\frac{\partial}{\partial\mathbf{x}}\frac{\mathbf{x}-\mathbf{a}}{\|\mathbf{x}-\mathbf{a}\|}=\frac{\mathbf{I}}{\|\mathbf{x}-\mathbf{a}\|}-\frac{(\mathbf{x}-\mathbf{a})(\mathbf{x}-\mathbf{a})^{\mathsf{T}}}{\|\mathbf{x}-\mathbf{a}\|^{3}},$
(46e)
$\displaystyle\frac{\partial}{\partial\mathbf{x}}\|\mathbf{x}-\mathbf{a}\|=\frac{\mathbf{x}-\mathbf{a}}{\|\mathbf{x}-\mathbf{a}\|}.$
(46f)

In (46c) and (46d), if the derivatives are taken with respect to a matrix, we
replace $\mathbf{x}$ by $\mathbf{X}$, and the equations still hold.

###  A-C Reformulation of AoAs and AoDs

Using the defined auxiliary variables, one can express

$\displaystyle\bm{\theta}_{\mathrm{A},m}$ $\displaystyle=[$
$\displaystyle\mathrm{atan}2($
$\displaystyle\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{A},m}$ ,
$\displaystyle\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{A},m})$ , $\displaystyle\mathrm{acos}($
$\displaystyle\mathbf{r}_{\mathrm{UE},3}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{A},m})]^{\mathsf{T}},$
$\displaystyle\bm{\theta}_{\mathrm{D},m}$ $\displaystyle=[$
$\displaystyle\mathrm{atan}2($
$\displaystyle\mathbf{r}_{\mathrm{BS},2}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{D},m}$ ,
$\displaystyle\mathbf{r}_{\mathrm{BS},1}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{D},m})$ , $\displaystyle\mathrm{acos}($
$\displaystyle\mathbf{r}_{\mathrm{BS},3}^{\mathsf{T}}$
$\displaystyle\mathbf{u}_{\mathrm{D},m})]^{\mathsf{T}}.$

###  A-D Derivatives with respect to UE Rotation Matrix

We make use of (46a), (46c), and (46d),

$\displaystyle\frac{\partial\theta_{\mathrm{A},m}^{(\mathrm{el})}}{\partial\mathbf{R}_{\mathrm{UE}}}$
$\displaystyle=-\frac{\mathbf{u}_{\mathrm{A},m}\mathbf{u}_{3}^{\mathsf{T}}}{\sqrt{1-(\mathbf{r}_{\mathrm{UE},3}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}}},$
(48a)
$\displaystyle\frac{\partial\theta_{\mathrm{A},m}^{(\mathrm{az})}}{\partial\mathbf{R}_{\mathrm{UE}}}$
$\displaystyle=\frac{(\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})\mathbf{u}_{\mathrm{A},m}\mathbf{u}_{2}^{\mathsf{T}}-(\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})\mathbf{u}_{\mathrm{A},m}\mathbf{u}_{1}^{\mathsf{T}}}{(\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}+(\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}}.$
(48b)

Note that AoDs and ToAs have no dependence on $\mathbf{R}_{\mathrm{UE}}$,
leading to partial derivative $\mathbf{0}_{3\times 3}$.

###  A-E Derivatives with respect to UE Position

The derivatives with respect to $\mathbf{p}_{\mathrm{UE}}$ are obtained using
the chain rule. We make use of (46b), (46c) and (46d) to obtain

$\displaystyle\frac{\partial\theta_{\mathrm{A},m}^{(\mathrm{el})}}{\partial\mathbf{u}_{\mathrm{A},m}}$
$\displaystyle=-\frac{\mathbf{r}_{\mathrm{UE},3}}{\sqrt{1-(\mathbf{r}_{\mathrm{UE},3}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}}},$
(49a)
$\displaystyle\frac{\partial\theta_{\mathrm{A},m}^{(\mathrm{az})}}{\partial\mathbf{u}_{\mathrm{A},m}}$
$\displaystyle=\frac{(\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})\mathbf{r}_{\mathrm{UE},2}-(\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})\mathbf{r}_{\mathrm{UE},1}}{(\mathbf{r}_{\mathrm{UE},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}+(\mathbf{r}_{\mathrm{UE},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{A},m})^{2}},$
(49b)
$\displaystyle\frac{\partial\theta_{\mathrm{D},m}^{(\mathrm{el})}}{\partial\mathbf{u}_{\mathrm{D},m}}$
$\displaystyle=-\frac{\mathbf{r}_{\mathrm{BS},3}}{\sqrt{1-(\mathbf{r}_{\mathrm{BS},3}^{\mathsf{T}}\mathbf{u}_{\mathrm{D},m})^{2}}},$
(49c)
$\displaystyle\frac{\partial\theta_{\mathrm{D},m}^{(\mathrm{az})}}{\partial\mathbf{u}_{\mathrm{D},m}}$
$\displaystyle=\frac{(\mathbf{r}_{\mathrm{BS},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{D},m})\mathbf{r}_{\mathrm{BS},2}-(\mathbf{r}_{\mathrm{BS},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{D},m})\mathbf{r}_{\mathrm{BS},1}}{(\mathbf{r}_{\mathrm{BS},1}^{\mathsf{T}}\mathbf{u}_{\mathrm{D},m})^{2}+(\mathbf{r}_{\mathrm{BS},2}^{\mathsf{T}}\mathbf{u}_{\mathrm{D},m})^{2}},$
(49d)

and (46e) to obtain

$\displaystyle{\partial\mathbf{u}_{\mathrm{A},m\neq
0}}/{\partial\mathbf{p}_{\mathrm{UE}}}$
$\displaystyle=\left(\mathbf{u}_{\mathrm{A},m}\mathbf{u}_{\mathrm{A},m}^{\mathsf{T}}-\mathbf{I}_{3}\right)/{\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}\|},$
(50a)
$\displaystyle{\partial\mathbf{u}_{\mathrm{A},0}}/{\partial\mathbf{p}_{\mathrm{UE}}}$
$\displaystyle=\left(\mathbf{u}_{\mathrm{A},0}\mathbf{u}_{\mathrm{A},0}^{\mathsf{T}}-\mathbf{I}_{3}\right)/{\|\mathbf{p}_{\mathrm{BS}}-\mathbf{p}_{\mathrm{UE}}\|},$
(50b)
$\displaystyle{\partial\mathbf{u}_{\mathrm{D},0}}/{\partial\mathbf{p}_{\mathrm{UE}}}$
$\displaystyle=\left(\mathbf{I}_{3}-\mathbf{u}_{\mathrm{D},0}\mathbf{u}_{\mathrm{D},0}^{\mathsf{T}}\right)/{\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|},$
(50c)

and ${\partial\mathbf{u}_{\mathrm{D},m\neq
0}}/{\partial\mathbf{p}_{\mathrm{UE}}}=\mathbf{0}_{3\times 3}$. Also,
considering (46f) gives

$\displaystyle\frac{\partial\tau_{m}}{\partial\mathbf{p}_{\mathrm{UE}}}=\begin{cases}(\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}})/(c~{}\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{\mathrm{BS}}\|)&m=0\\\
(\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{m})/(c~{}\|\mathbf{p}_{\mathrm{UE}}-\mathbf{p}_{m}\|)&m\neq
0\end{cases}.$ (51)

###  A-F Derivatives with respect to Incidence Points Positions

The derivatives with respect to IP positions are also obtained using the chain
rule. We note that
${\partial\mathbf{u}_{\mathrm{A},m}}/{\partial\mathbf{p}_{n}}=\mathbf{0}_{3},~{}n\neq
m$, for $m=0,\ldots,M$ and $n=1,\ldots,M$, with the same case for
${\partial\mathbf{u}_{\mathrm{A},m}}/{\partial\mathbf{p}_{n}}$ and
${\partial\tau_{m}}/{\partial\mathbf{p}_{n}}$, while for $m\neq 0$

$\displaystyle{\partial\mathbf{u}_{\mathrm{A},m}}/{\partial\mathbf{p}_{m}}$
$\displaystyle=\left(\mathbf{I}_{3}-\mathbf{u}_{\mathrm{A},m}\mathbf{u}_{\mathrm{A},m}^{\mathsf{T}}\right)/\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}\|,$
(52a)
$\displaystyle{\partial\mathbf{u}_{\mathrm{D},m}}/{\partial\mathbf{p}_{m}}$
$\displaystyle=\left(\mathbf{I}_{3}-\mathbf{u}_{\mathrm{D},m}\mathbf{u}_{\mathrm{D},m}^{\mathsf{T}}\right)/\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}\|$
(52b) $\displaystyle{\partial\tau_{m}}/{\partial\mathbf{p}_{m}}$
$\displaystyle=(\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}})/(c~{}\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{BS}}\|)$
$\displaystyle+(\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}})/(c~{}\|\mathbf{p}_{m}-\mathbf{p}_{\mathrm{UE}}\|).$
(52c)

###  A-G Derivatives with respect to Clock Bias

The angles have no dependence on $b$, while ${\partial\tau_{m}}/{\partial
b}=1$.

##  Appendix B Solving the Optimization Problem for the Shortest Distance
Between Half-lines

For the half-lines
$\ell_{1}=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{1}+t_{1}\mathbf{d}_{1},t_{1}\geq
0\\}$ and
$\ell_{2}=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{2}+t_{2}\mathbf{d}_{2},t_{2}\geq
0\\}$, the shortest distance $\delta_{\mathrm{min}}$ is obtained from
$\delta_{\min}^{2}=\min_{\boldsymbol{t}=[t_{1},t_{2}]^{\mathsf{T}}}\|(\mathbf{p}_{1}+t_{1}\mathbf{d}_{1})-(\mathbf{p}_{2}+t_{2}\mathbf{d}_{2})\|^{2},~{}\text{s.t.}~{}\boldsymbol{t}\geq\mathbf{0}_{2}$,
which is a quadratic convex optimization problem in $\boldsymbol{t}$, and its
solution is found by writing the K.K.T. conditions [59]. We utilize a simpler
procedure in which we first obtain the unconstrained optimal solution

$\displaystyle t_{1}^{*}$ $\displaystyle=-$
$\displaystyle\mathbf{d}_{1}^{\mathsf{T}}\left(\mathbf{I}-\mathbf{d}_{2}\mathbf{d}_{2}^{\mathsf{T}}\right)\mathbf{p}_{12}/\left(1-(\mathbf{d}_{1}^{\mathsf{T}}\mathbf{d}_{2})^{2}\right),$
(53a) $\displaystyle t_{2}^{*}$ $\displaystyle=$
$\displaystyle\mathbf{d}_{2}^{\mathsf{T}}\left(\mathbf{I}-\mathbf{d}_{1}\mathbf{d}_{1}^{\mathsf{T}}\right)\mathbf{p}_{12}/\left(1-(\mathbf{d}_{1}^{\mathsf{T}}\mathbf{d}_{2})^{2}\right),$
(53b)

with $\mathbf{p}_{12}\triangleq\mathbf{p}_{1}-\mathbf{p}_{2}$. If
$\boldsymbol{t}^{*}>\mathbf{0}_{2}$, the solution is
$\delta_{\min}=|\mathbf{n}^{\mathsf{T}}\mathbf{p}_{12}|$ where
$\mathbf{n}=(\mathbf{d}_{1}\times\mathbf{d}_{2})/(\|\mathbf{d}_{1}\times\mathbf{d}_{2}\|)$.

###### Proof.

Substituting the optimal solution (53) in
$\|(\mathbf{p}_{1}+t_{1}\mathbf{d}_{1})-(\mathbf{p}_{2}+t_{2}\mathbf{d}_{2})\|$
gives $\delta_{\min}=\|\mathbf{P}_{\perp}(\mathbf{D})\cdot\mathbf{p}_{12}\|$,
where $\mathbf{D}\triangleq[\mathbf{d}_{1},\mathbf{d}_{2}]$, and
$\mathbf{P}_{\perp}(\mathbf{D})\triangleq\mathbf{I}-\mathbf{D}(\mathbf{D}^{\mathsf{T}}\mathbf{D})^{-1}\mathbf{D}^{\mathsf{T}}=\mathbf{I}-\big{(}\mathbf{d}_{1}\mathbf{d}_{1}^{\mathsf{T}}\left(\mathbf{I}-\mathbf{d}_{2}\mathbf{d}_{2}^{\mathsf{T}}\right)+\mathbf{d}_{2}\mathbf{d}_{2}^{\mathsf{T}}\left(\mathbf{I}-\mathbf{d}_{1}\mathbf{d}_{1}^{\mathsf{T}}\right)\big{)}/\big{(}{1-(\mathbf{d}_{1}^{\mathsf{T}}\mathbf{d}_{2})^{2}}\big{)}$
is the projector onto the subspace orthogonal to the one spanned by
$\mathbf{d}_{1}$ and $\mathbf{d}_{2}$, which is in turn spanned by the unit-
norm vector $\mathbf{n}$ normal to $\mathbf{d}_{1}$ and $\mathbf{d}_{2}$,
given by
$\mathbf{n}=(\mathbf{d}_{1}\times\mathbf{d}_{2})/(\|\mathbf{d}_{1}\times\mathbf{d}_{2}\|)$.
Hence,
$\delta_{\min}=\|(\mathbf{n}^{\mathsf{T}}\mathbf{p}_{12})~{}\mathbf{n}\|=|\mathbf{n}^{\mathsf{T}}\mathbf{p}_{12}|$.
∎

Otherwise, we obtain
$\check{t}_{1}=-\mathbf{d}_{1}^{\mathsf{T}}\mathbf{p}_{12}$,
$\check{t}_{2}=\mathbf{d}_{2}^{\mathsf{T}}\mathbf{p}_{12}$,
$\check{\lambda}_{1}=\mathbf{d}_{1}^{\mathsf{T}}\mathbf{P}_{\perp}(\mathbf{d}_{2})\mathbf{p}_{12}$,
and
$\check{\lambda}_{2}=-\mathbf{d}_{2}^{\mathsf{T}}\mathbf{P}_{\perp}(\mathbf{d}_{1})\mathbf{p}_{12}$,
where
$\mathbf{P}_{\perp}(\mathbf{d})\triangleq\mathbf{I}-\mathbf{d}\mathbf{d}^{\mathsf{T}}$.
If $[\check{t}_{1},\check{\lambda}_{2}]^{\mathsf{T}}>\mathbf{0}_{2}$, then
$\delta_{\min}=\check{\delta}_{1}$, and if
$[\check{t}_{2},\check{\lambda}_{1}]^{\mathsf{T}}>\mathbf{0}_{2}$, then
$\delta_{\min}=\check{\delta}_{2}$, where
$\check{\delta}_{1}\triangleq(\mathbf{p}_{12}^{\mathsf{T}}\mathbf{P}_{\perp}(\mathbf{d}_{1})\mathbf{p}_{12})^{1/2}$
and
$\check{\delta}_{2}\triangleq(\mathbf{p}_{12}^{\mathsf{T}}\mathbf{P}_{\perp}(\mathbf{d}_{1})\mathbf{p}_{12})^{1/2}$
are both non-negative, due to the Cauchy–Schwarz inequality. Otherwise,
$\delta_{\min}=\|\mathbf{p}_{12}\|$. Obtaining expressions is straightforward.
As the unconstrained solution $\boldsymbol{t}^{*}$ often satisfies the
constraints, especially in high SNR regimes, this approach is more efficient.

##  Appendix C Proof of the Closed-From Expression for the Closest Point to
Skew Lines

We determine $\mathbf{p}_{0}$ to be mutually closest to the half-lines
$\ell_{1}=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{1}+t_{1}\mathbf{d}_{1},t_{1}\geq
0\\}$ and
$\ell_{2}=\\{\mathbf{p}\in\mathbb{R}^{3}:\mathbf{p}=\mathbf{p}_{2}+t_{2}\mathbf{d}_{2},t_{2}\geq
0\\}$, in a least-squares sense, so that
$\mathrm{d}^{2}(\mathbf{p}_{0},\ell_{1})+\mathrm{d}^{2}(\mathbf{p}_{0},\ell_{2})$,
with $\mathrm{d}(\mathbf{p}_{0},\ell_{i})$ denoting the distance of
$\mathbf{p}_{0}$ to $\ell_{i},~{}i=1,2$, is minimized. According to
Pythagorean theorem,
$\mathrm{d}^{2}(\mathbf{p}_{0},\ell_{i})=\|\mathbf{p}_{0}-\mathbf{p}_{i}\|^{2}-\left((\mathbf{p}_{0}-\mathbf{p}_{i})^{\mathsf{T}}\mathbf{d}_{i}\right)^{2}$,
where $(\mathbf{p}_{0}-\mathbf{p}_{i})^{\mathsf{T}}\mathbf{d}_{i}$ is the
projection of $(\mathbf{p}_{0}-\mathbf{p}_{i})$ on line $\ell_{i}$. Taking
gradient of
$\mathrm{d}^{2}(\mathbf{p}_{0},\ell_{1})+\mathrm{d}^{2}(\mathbf{p}_{0},\ell_{2})$
with respect to $\mathbf{p}_{0}$ and setting it to $\mathbf{0}$ results in
$\mathbf{p}_{0}=\mathbf{A}^{-1}\mathbf{b}$, where
$\mathbf{A}=(\mathbf{I}-\mathbf{d}_{1}\mathbf{d}_{1}^{\mathsf{T}})+(\mathbf{I}-\mathbf{d}_{2}\mathbf{d}_{2}^{\mathsf{T}})$
and
$\mathbf{b}=(\mathbf{I}-\mathbf{d}_{1}\mathbf{d}_{1}^{\mathsf{T}})\mathbf{p}_{1}+(\mathbf{I}-\mathbf{d}_{2}\mathbf{d}_{2}^{\mathsf{T}})\mathbf{p}_{2}$.
Setting $\mathbf{p}_{1}=\mathbf{p}_{\mathrm{BS}}$,
$\mathbf{p}_{2}=\mathbf{p}_{\mathrm{UE}}(1)$,
$\mathbf{d}_{1}=\mathbf{R}_{\mathrm{BS}}\mathbf{d}_{\mathrm{D},m}$, and
$\mathbf{d}_{2}=\hat{\mathbf{R}}_{\mathrm{UE}}\mathbf{d}_{\mathrm{A},m}$ gives
(26).

## References

  * [1] T. S. Rappaport, S. Sun, R. Mayzus, H. Zhao, Y. Azar, K. Wang, G. N. Wong, J. K. Schulz, M. Samimi, and F. Gutierrez, “Millimeter wave mobile communications for 5G cellular: It will work!” _IEEE access_ , vol. 1, pp. 335–349, 2013.
  * [2] W. Roh, J.-Y. Seol, J. Park, B. Lee, J. Lee, Y. Kim, J. Cho, K. Cheun, and F. Aryanfar, “Millimeter-wave beamforming as an enabling technology for 5G cellular communications: Theoretical feasibility and prototype results,” _IEEE Communications Magazine_ , vol. 52, no. 2, pp. 106–113, 2014.
  * [3] R. Di Taranto, S. Muppirisetty, R. Raulefs, D. Slock, T. Svensson, and H. Wymeersch, “Location-aware communications for 5G networks: How location information can improve scalability, latency, and robustness of 5G,” _IEEE Signal Processing Magazine_ , vol. 31, no. 6, pp. 102–112, 2014.
  * [4] A. Alammouri, J. Mo, B. L. Ng, J. C. Zhang, and J. G. Andrews, “Hand grip impact on 5G mmWave mobile devices,” _IEEE Access_ , vol. 7, pp. 60 532–60 544, 2019.
  * [5] A. Shastri, N. Valecha, E. Bashirov, H. Tataria, M. Lentmaier, F. Tufvesson, M. Rossi, and P. Casari, “A review of millimeter wave device-based localization and device-free sensing technologies and applications,” _IEEE Communications Surveys & Tutorials_, 2022.
  * [6] S. Thrun, “Probabilistic robotics,” _Communications of the ACM_ , vol. 45, no. 3, pp. 52–57, 2002.
  * [7] S. Bartoletti, H. Wymeersch, T. Mach, O. Brunnegård, D. Giustiniano, P. Hammarberg, M. F. Keskin, J. O. Lacruz, S. M. Razavi, J. Rönnblom _et al._ , “Positioning and sensing for vehicular safety applications in 5G and beyond,” _IEEE Communications Magazine_ , vol. 59, no. 11, pp. 15–21, 2021.
  * [8] K. Witrisal, P. Meissner, E. Leitinger, Y. Shen, C. Gustafson, F. Tufvesson, K. Haneda, D. Dardari, A. F. Molisch, A. Conti, and M. Z. Win, “High-accuracy localization for assisted living: 5G systems will turn multipath channels from foe to friend,” _IEEE Signal Processing Magazine_ , vol. 33, no. 2, pp. 59–70, 2016.
  * [9] A. Albanese, V. Sciancalepore, and X. Costa-Pérez, “First responders got wings: UAVs to the rescue of localization operations in beyond 5G systems,” _IEEE Communications Magazine_ , vol. 59, no. 11, pp. 28–34, 2021\.
  * [10] A. Behravan, V. Yajnanarayana, M. F. Keskin, H. Chen, D. Shrestha, T. E. Abrudan, T. Svensson, K. Schindhelm, A. Wolfgang, S. Lindberg _et al._ , “Positioning and sensing in 6G: Gaps, challenges, and opportunities,” _IEEE Veh. Technol. Mag._ , 2022.
  * [11] J. Wahlström, I. Skog, and P. Händel, “Smartphone-based vehicle telematics: A ten-year anniversary,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 18, no. 10, pp. 2802–2825, 2017.
  * [12] D. Dardari, P. Closas, and P. M. Djurić, “Indoor tracking: Theory, methods, and technologies,” _IEEE Transactions on Vehicular Technology_ , vol. 64, no. 4, pp. 1263–1278, 2015.
  * [13] J. A. del Peral-Rosado, R. Raulefs, J. A. López-Salcedo, and G. Seco-Granados, “Survey of cellular mobile radio localization methods: From 1G to 5G,” _IEEE Communications Surveys Tutorials_ , vol. 20, no. 2, pp. 1124–1148, 2018\.
  * [14] 3GPP, “Study on NR positioning support,” TR 38.855, Technical Report 16.0.0, 2019.
  * [15] S. Dwivedi, R. Shreevastav, F. Munier, J. Nygren, I. Siomina, Y. Lyazidi, D. Shrestha, G. Lindmark, P. Ernström, E. Stare, S. M. Razavi, S. Muruganathan, G. Masini, Å. Busin, and F. Gunnarsson, “Positioning in 5G networks,” _IEEE Communications Magazine_ , vol. 59, no. 11, pp. 38–44, 2021.
  * [16] Z. Abu-Shaban, X. Zhou, T. Abhayapala, G. Seco-Granados, and H. Wymeersch, “Error bounds for uplink and downlink 3D localization in 5G millimeter wave systems,” _IEEE Transactions on Wireless Communications_ , vol. 17, no. 8, pp. 4939–4954, 2018.
  * [17] E. Björnson, H. Wymeersch, B. Matthiesen, P. Popovski, L. Sanguinetti, and E. de Carvalho, “Reconfigurable intelligent surfaces: A signal processing perspective with wireless applications,” _IEEE Signal Processing Magazine_ , vol. 39, no. 2, pp. 135–158, 2022.
  * [18] A. Elzanaty, A. Guerra, F. Guidi, and M.-S. Alouini, “Reconfigurable intelligent surfaces for localization: Position and orientation error bounds,” _IEEE Transactions on Signal Processing_ , vol. 69, pp. 5386–5402, 2021.
  * [19] N. Garcia, H. Wymeersch, E. G. Larsson, A. M. Haimovich, and M. Coulon, “Direct localization for massive MIMO,” _IEEE Transactions on Signal Processing_ , vol. 65, no. 10, pp. 2475–2487, 2017.
  * [20] O. Kanhere and T. S. Rappaport, “Position location for futuristic cellular communications: 5G and beyond,” _IEEE Communications Magazine_ , vol. 59, no. 1, pp. 70–75, 2021.
  * [21] G. Kwon, A. Conti, H. Park, and M. Z. Win, “Joint communication and localization in millimeter wave networks,” _IEEE Journal of Selected Topics in Signal Processing_ , vol. 15, no. 6, pp. 1439–1454, 2021.
  * [22] M. A. Nazari, G. Seco-Granados, P. Johannisson, and H. Wymeersch, “3D orientation estimation with multiple 5G mmWave base stations,” in _IEEE International Conference on Communications_ , 2021.
  * [23] Y. Liu, Y. Shen, and M. Z. Win, “Single-anchor localization and synchronization of full-duplex agents,” _IEEE Transactions on Communications_ , vol. 67, no. 3, pp. 2355–2367, 2019.
  * [24] A. Shahmansoori, G. E. Garcia, G. Destino, G. Seco-Granados, and H. Wymeersch, “Position and orientation estimation through millimeter-wave MIMO in 5G systems,” _IEEE Transactions on Wireless Communications_ , vol. 17, no. 3, pp. 1822–1835, 2018.
  * [25] J. Li, M. F. Da Costa, and U. Mitra, “Joint localization and orientation estimation in millimeter-wave MIMO OFDM systems via atomic norm minimization,” _arXiv preprint arXiv:2203.00892_ , 2022.
  * [26] R. Mendrzik, H. Wymeersch, G. Bauch, and Z. Abu-Shaban, “Harnessing NLOS components for position and orientation estimation in 5G millimeter wave MIMO,” _IEEE Transactions on Wireless Communications_ , vol. 18, no. 1, pp. 93–107, 2019.
  * [27] F. Wen and H. Wymeersch, “5G synchronization, positioning, and mapping from diffuse multipath,” _IEEE Wireless Communications Letters_ , vol. 10, no. 1, pp. 43–47, 2020.
  * [28] A. Guerra, F. Guidi, and D. Dardari, “Single-anchor localization and orientation performance limits using massive arrays: MIMO vs. beamforming,” _IEEE Transactions on Wireless Communications_ , vol. 17, no. 8, pp. 5241–5255, 2018.
  * [29] A. Kakkavas, M. H. Castañeda García, R. A. Stirling-Gallacher, and J. A. Nossek, “Performance limits of single-anchor millimeter-wave positioning,” _IEEE Transactions on Wireless Communications_ , vol. 18, no. 11, pp. 5196–5210, 2019.
  * [30] B. Sun, B. Tan, W. Wang, and E. S. Lohan, “A comparative study of 3D UE positioning in 5G New Radio with a single station,” _Sensors_ , vol. 21, no. 4, 2021. [Online]. Available: https://www.mdpi.com/1424-8220/21/4/1178
  * [31] Y. Chen, J. Palacios, N. González-Prelcic, T. Shimizu, and H. Lu, “Joint initial access and localization in millimeter wave vehicular networks: a hybrid model/data driven approach,” in _2022 IEEE 12th Sensor Array and Multichannel Signal Processing Workshop (SAM)_ , 2022, pp. 355–359.
  * [32] E. Björnson, L. Sanguinetti, H. Wymeersch, J. Hoydis, and T. L. Marzetta, “Massive MIMO is a reality—what is next?: Five promising research directions for antenna arrays,” _Digital Signal Processing_ , vol. 94, pp. 3–20, 2019.
  * [33] S. Shen, S. Li, and H. Steendam, “Simultaneous position and orientation estimation for visible light systems with multiple LEDs and multiple PDs,” _IEEE Journal on Selected Areas in Communications_ , vol. 38, no. 8, pp. 1866–1879, 2020.
  * [34] J. Jiang, G. Wang, and K. C. Ho, “Sensor network-based rigid body localization via semi-definite relaxation using arrival time and doppler measurements,” _IEEE Transactions on Wireless Communications_ , vol. 18, no. 2, pp. 1011–1025, 2019.
  * [35] S. P. Chepuri, G. Leus, and A.-J. van der Veen, “Rigid body localization using sensor networks,” _IEEE Transactions on Signal Processing_ , vol. 62, no. 18, pp. 4911–4924, 2014.
  * [36] Y. Wang, G. Wang, S. Chen, K. C. Ho, and L. Huang, “An investigation and solution of angle based rigid body localization,” _IEEE Transactions on Signal Processing_ , vol. 68, pp. 5457–5472, 2020.
  * [37] Z. Li, F. Jiang, H. Wymeersch, and F. Wen, “An iterative 5G positioning and synchronization algorithm in NLOS environments with multi-bounce paths,” _arXiv preprint arXiv:2209.01562_ , 2022.
  * [38] J. Hong, J. Rodríguez-Piñeiro, X. Yin, and Z. Yu, “Joint channel parameter estimation and scatterers localization,” _IEEE Transactions on Wireless Communications_ , 2022.
  * [39] J.-L. Blanco, “A tutorial on SE(3) transformation parameterizations and on-manifold optimization,” _University of Malaga, Tech. Rep_ , vol. 3, p. 6, 2010.
  * [40] C. Huang, L. Liu, C. Yuen, and S. Sun, “Iterative channel estimation using LSE and sparse message passing for mmWave MIMO systems,” _IEEE Transactions on Signal Processing_ , vol. 67, no. 1, pp. 245–259, 2018.
  * [41] F. Roemer, M. Haardt, and G. Del Galdo, “Analytical performance assessment of multi-dimensional matrix-and tensor-based ESPRIT-type algorithms,” _IEEE Transactions on Signal Processing_ , vol. 62, no. 10, pp. 2611–2625, 2014.
  * [42] F. Bellili, F. Sohrabi, and W. Yu, “Generalized approximate message passing for massive MIMO mmWave channel estimation with laplacian prior,” _IEEE Transactions on Communications_ , vol. 67, no. 5, pp. 3205–3219, 2019\.
  * [43] X. Cheng, C. Tang, and Z. Zhang, “Accurate channel estimation for millimeter-wave MIMO systems,” _IEEE Transactions on Vehicular Technology_ , vol. 68, no. 5, pp. 5159–5163, 2019.
  * [44] Z. Zhou, J. Fang, L. Yang, H. Li, Z. Chen, and R. S. Blum, “Low-rank tensor decomposition-aided channel estimation for millimeter wave MIMO-OFDM systems,” _IEEE Journal on Selected Areas in Communications_ , vol. 35, no. 7, pp. 1524–1538, 2017.
  * [45] R. Thomä, M. Landmann, and A. Richter, “RIMAX-A maximum likelihood framework for parameter estimation in multidimensional channel sounding,” in _Proceedings of the International Symposium on Antennas and Propagation_ , 2004.
  * [46] T. E. Abrudan, Z. Xiao, A. Markham, and N. Trigoni, “Underground incrementally deployed magneto-inductive 3-D positioning network,” _IEEE Transactions on Geoscience and Remote Sensing_ , vol. 54, no. 8, pp. 4376–4391, 2016.
  * [47] H. Wymeersch, J. Lien, and M. Z. Win, “Cooperative localization in wireless networks,” _Proceedings of the IEEE_ , vol. 97, no. 2, pp. 427–450, 2009\.
  * [48] M.-A. Badiu, T. L. Hansen, and B. H. Fleury, “Variational Bayesian inference of line spectra,” _IEEE Transactions on Signal Processing_ , vol. 65, no. 9, pp. 2247–2261, 2017.
  * [49] K. V. Mardia and P. E. Jupp, _Directional statistics_. John Wiley & Sons, 2009, vol. 494.
  * [50] N. Boumal, “An introduction to optimization on smooth manifolds,” Available online, Aug 2020. [Online]. Available: http://www.nicolasboumal.net/book
  * [51] S. Wright and J. Nocedal, _Numerical optimization_. Springer Science, 1999, vol. 35, no. 67-68.
  * [52] J. Vince, _Rotation transforms for computer graphics_. Springer Science & Business Media, 2011.
  * [53] S. M. Kay, _Fundamentals of statistical signal processing: estimation theory_. Prentice-Hall, Inc., 1993.
  * [54] P. Stoica and B. C. Ng, “On the Cramér-Rao bound bound under parametric constraints,” _IEEE Signal Processing Letters_ , vol. 5, no. 7, pp. 177–179, 1998.
  * [55] A. S. Rahmathullah, Á. F. García-Fernández, and L. Svensson, “Generalized optimal sub-pattern assignment metric,” in _2017 20th International Conference on Information Fusion (Fusion)_. IEEE, 2017, pp. 1–8.
  * [56] C. A. Balanis, _Antenna theory: analysis and design_. John wiley & sons, 2015.
  * [57] N. Boumal, B. Mishra, P.-A. Absil, and R. Sepulchre, “Manopt, a Matlab toolbox for optimization on manifolds,” _Journal of Machine Learning Research_ , vol. 15, no. 42, pp. 1455–1459, 2014. [Online]. Available: https://www.manopt.org
  * [58] J. Segura, “Bounds for ratios of modified Bessel functions and associated Turán-type inequalities,” _Journal of Mathematical Analysis and Applications_ , vol. 374, no. 2, pp. 516–528, 2011.
  * [59] S. Boyd, S. P. Boyd, and L. Vandenberghe, _Convex optimization_. Cambridge University Press, 2004.

  *[mmWave]: millimeter-wave
  *[BS]: base station
  *[UE]: user equipment
  *[MIMO-OFDM]: multiple-input multiple-output orthogonal frequency-division multiplexing
  *[LoS]: line-of-sight
  *[IP]: incidence point
  *[ML]: maximum likelihood
  *[IPs]: incidence point
  *[CRB]: Cramér-Rao bound
  *[SNR]: signal-to-noise ratio
  *[GNSS]: global navigation satellite system
  *[IMU]: inertial measurement unit
  *[IMUs]: inertial measurement unit
  *[TDoA]: time-difference-of-arrival
  *[ BS]: base station
  *[RIS]: reconfigurable intelligent surface
  *[NLoS]: non-line-of-sight
  *[MIMO]: multiple-input multiple-output
  *[FIM]: Fisher information matrix
  *[RSS]: received signal strength
  *[CRBs]: Cramér-Rao bound
  *[AoA]: angle-of-arrival
  *[AoD]: angle-of-departure
  *[ToA]: time-of-arrival
  *[RMSE]: root mean square error
  *[RF]: radio frequency
  *[GLRT]: generalized likelihood ratio test
  *[OFDM]: orthogonal frequency-division multiplexing
  *[AWGN]: additive white Gaussian noise
  *[PSD]: power spectral density
  *[AoAs]: angle-of-arrival
  *[AoDs]: angle-of-departure
  *[ToAs]: time-of-arrival
  *[EFIM]: equivalent Fisher information matrix
  *[CCRB]: constrained Cramér-Rao bound
  *[OEB]: orientation error bound
  *[PEB]: position error bound
  *[IPEB]: IPs error bound
  *[SEB]: synchronization error bound
  *[CDF]: cumulative distribution function
  *[FIMs]: Fisher information matrix
  *[RMSEs]: root mean square error
  *[BSs]: base station