Joint Localization and Environment Sensing of Rigid Body With 5G Millimeter Wave MIMO

Accurately localizing a target in three-dimensional (3D) space assisted with the fifth generation (5G) wireless systems in an indoor environment could enable a wide variety of new applications, including precise control for factory automation, self-maneuver of the vehicle and so on. However, controlling a target in 3D space relies on a rigid body modelling with six degrees of freedom, which dramatically increases the localization difficulty and complexity. Furthermore, for radio-based localization methods, the lack of line-of-sight (LOS) and the existence of reflection points in the environment will also influence the rigid body localization process. To improve the rigid body localization accuracy as well as unravel useful environmental information from the received signal, a novel rigid body joint active localization and environment sensing scheme is proposed in this article. Specifically, the multi-path effect of millimeter wave (mm-wave) signal with a single reflection can be exploited to enhance the rigid body localization accuracy, and it can also be utilized to locate the reflection points, which further enables a new way for environmental sensing. Hence, we first propose a two-step hierarchical compressive sensing algorithm to extract the angular and distance information of the LOS (if available) and single-bounce specular reflections. Then a particle swarm optimization (PSO) based method is derived to recover the posture of the rigid body and the location of reflection points. The Cramér-Rao lower bound (CRLB) on angle, rigid body posture and reflection points position uncertainty is also analyzed. The simulation results demonstrate that the proposed scheme can achieve high accuracy rigid body localization and locate the reflection points around the rigid body even under obstructed-line-of-sight (OLOS) conditions in an indoor scene.


I. INTRODUCTION
The rapid proliferation of localization-enabled applications including warehouse management, asset tracking and factory automation are significantly boosting the need for highaccuracy target positioning under challenging conditions [1]. Albeit there have been many investigations into real-time localization for many years, this area is attracting growing research interest due to new harsh localization environments and stringent accuracy requirements from many emerging applications. Traditionally, a target is modelled as a point target whose location is only defined by the 3D coordinates with three degrees of freedom while neglecting its orientations [2]. However, to achieve accurate maneuvering for tasks like cargo transportation, robotic control and others, not only the target's position but also its orientations are required. For this purpose, a target has to be modelled as a rigid body, which always maintains its shape and size. Apart from the 3D coordinates, a rigid body has the other three degrees of freedom including three Euler angles.
As a result, the rigid body localization (RBL) problem has been studied recently to determine a rigid body's centre position and orientation. In general, the rigid body localization problem for outdoor scenarios relies on the combined use of global navigation satellite systems (GNSS) and inertial measurement unit (IMU). However, GNSS suffers from limited signal coverage and poor position accuracy in indoor environment, while IMU requires frequent calibration, which rules out most localization-based indoor applications. Meanwhile, some alternative indoor rigid body localization technologies, using radar [3], laser [4], infrared light (IR) [5], ultrasound [6] and cameras [7], are either very sensitive to environmental changes or dependent on expensive hardware.
Recently, the rapid advancements of 5G technologies such as multiple-input and multiple-output (MIMO) and mm-wave bring enormous improvements in network throughput and energy efficiency, as well as their potential for localization [8], [9]. Such technological advancements could be exploited to enhance localization performance. In particular, operating at carrier frequencies beyond 30 GHz with ultra-wide bandwidths, mm-wave based 5G MIMO systems provide extremely high data rates through dense spatial multiplexing by using a large number of antennas [10]. The high spatial resolution and large bandwidth provided by these 5G technologies are expected to bring in revolutionary impact on wireless localization [8]. In addition, 5G mm-wave MIMO not only enables the active localization of the rigid body but also provides the opportunity of positioning the surrounding physical objects serving as reflection points simultaneously to achieve environment sensing. It has been proved that the non-light-of-sight (NLOS) components in the channel estimation results can be turned into benefits to assist the radio map construction [11]. The precise rigid body posture and reflection points location information also in turn contribute to the design, operation and optimization of future 5G beyond and 6G wireless networks. For instance, location information of rigid body and reflection points can be utilized to construct location-specific channel state information (CSI), which can boost the spatial spectrum efficiency of future networks. Motivated by these considerations, joint localization and environment sensing of a rigid 5G mobile terminal equipped with MIMO has become one of the promising research directions.
While 5G mm-wave MIMO assisted rigid body localization and environment sensing have many advantages, several challenges of previous studies have yet to be overcome. On one hand, existing studies on angle information of a target only consider one [12] or two rotation angles [13] in 2D or 3D scenarios, which neglects part of rotation due to the inherent complexity. Solving the translation vector and rotation information of a rigid body from signals transmitted by fixed anchor nodes is a complicated nonlinear optimization problem. For rigid body localization, [14] proposed a range measurement method in a sensor network, in which four anchor nodes and five wireless sensors mounted on the rigid body are used. Jiang et al. [15], [16] presented a refined positioning algorithm using the range measurements between the anchors and several sensors in both 2D and 3D cases. In [17], an angle of arrival (AOA) based method is introduced by utilizing only one base station and four wireless sensors in the rigid body localization problem. However, the aforementioned methods need to guarantee that the topology of how the sensors are mounted on the rigid body is known and the LOS exists. In addition, these methods are based on passive localization which gathers the measurements from an incoming signal transmitted/reflected by sensors mounted on the rigid body and the robustness of distance estimation to the environment noises/shadowing is limited due to the fluctuations of practical path conditions. Therefore, active localization performed at the rigid body with received signals from only one anchor node is more controllable and efficient.
On the other hand, the derivation of NLOS components for reflection points localization has also been studied for years [18], [19], [20], [21], [22]. The reflection points localization problem can be solved by exploiting the AOA and angle of departure (AOD) with the sparsity of channel information. Among several popular algorithms, the distributed compressed sensing-simultaneous orthogonal matching pursuit (DCS-SOMP) [23], [24], [25] and its modifications like CoSaMP/OMP are useful tools by manipulating sparsity of mm-wave channels. [10] demonstrated a three-stage improved DCS-SOMP algorithm by linear antenna arrays with AOA and AOD to estimate the scatters (or reflection points) to improve the moving target localization accuracy only in the 2D scenario. However, the reflection points localization in the 3D scenario has not been investigated and its implementation at the rigid body end is yet to be studied. Thus, an integrated rigid body localization and environment sensing technique with accurate and robust performance in 3D is quite challenging yet demanding.
In this paper, the mm-wave MIMO assisted rigid body active localization and environment sensing scheme is proposed to tackle the aforementioned challenges. In the considered 3D scenario, the six degrees of freedom of rigid body are successfully resolved with only one base station equipped with uniform rectangular array (URA). Three different cases are investigated: with only line-of-sight (LOS), with both LOS and non-line-of-sight (NLOS), and with only the NLOS. Moreover, when there's NLOS, the position of reflection points in the environment is also estimated. Both the rigid body localization and reflection point estimation are achieved at the rigid body end with the one-way signals. To guarantee high rigid body/reflection points localization accuracy, a new strategy of the multi-step hierarchical design is developed. Specifically, the primary contributions of this paper comprise the following three aspects.
1) A novel rigid body active localization and environment sensing strategy is proposed with the assistance of one anchor node. To the best of our knowledge, there are no developed solutions for joint rigid body and reflection points localization estimation under 3D scenarios. Although some works were reported to detect the 2D reflection points, an optimal reflection points localization solution to deal with the 3D situation has never been studied using the mm-wave MIMO at the rigid body end. Our proposed strategy achieves the rigid body location estimation accuracy to the centimetre level and orientation estimation accuracy to 0.02 rd and improves the reflection points localization accuracy to decimeter level with reasonable computational complexity.
2) A novel hierarchical DCS-SOMP algorithm incorporated with iterative maximum likelihood (IML) is presented to resolve the channel information from received signals. In particular, by using the sparsity of the mmwave channel in the angle domain, the AOA/AOD estimation accuracy is increased to 0.01 rd and distance estimation accuracy to the centimetre level. Compared with the original DCS-SOMP, the estimation precision is enhanced greatly.
3) The theoretical CRLB of angular, rigid body posture and reflection points location estimation is derived to evaluate the effectiveness of the proposed algorithms.
Simulations show that our proposed joint rigid body and reflection points localization method approaches the theoretical bounds while the signal-to-noise ratio (SNR) is not less than 0 dB. The rest of the paper is organized as follows. The system models including the environments, rigid body localization and reflection points localization are formulated in Section II. The novel compressive sensing method for channel parameter estimation is investigated in Section III-A. The PSO-based joint rigid body and reflection points localization strategy are proposed in Section III-B. The fundamental bounds are derived in Section IV. Section V presents the simulation results. Finally, Section VI summarizes and concludes the work.

II. SYSTEM MODEL A. ENVIRONMENT MODEL
We consider a 3D indoor environment with an anchor node AN located at s = [s x , s y , s z ] T ∈ R 3 and a rigid body with posture [p; q] ∈ R 6 . Note that p = [p x , p y , p z ] T ∈ R 3 is the position and q = [q 1 , q 2 , q 3 ] T ∈ R 3 , q 1 , q 3 ∈ [0, 2π ) and q 2 ∈ [0, π], represents the orientation of rigid body. The objects in the environment are characterized by L − 1 reflection points located at κ l = [κ l,x , κ l,y , κ l,z ] ∈ R 3 , l = 1, 2, . . ., L − 1. It is assumed that the anchor node as signal transmitter carries a uniform rectangular array (URA) consisting of N T (N T x × N Ty ) antennas while rigid body as receiver carries URA with N R (N Rx × N Ry ) antennas. The value of s is assumed to be known, while [p; q] and κ l are unknown. Fig. 1 illustrates the rigid body and reflection points localization scenario. In our mm-wave wireless system, the signal transmitted by AN is orthogonal frequency division multiplexing (OFDM) signal [10]. The mm-wave network operates at carrier frequency f c and bandwidth B. In particular, G signals are transmitted sequentially for each subcarrier. The g-th signal is denoted as x (g) [n] ∈ C N s ×1 for n-th subcarrier n = 0, 1, 2, . . ., N − 1.

B. JOINT RIGID BODY AND REFLECTION POINTS LOCALIZATION MODEL
where (2) In (2), h l is complex channel gain and ρ l here is the path loss of the l-th channel (l = 0, 1, 2, . . ., L − 1). T s = 1/B denotes the sampling period. The transmission time τ 0 at LOS is obtained by τ 0 = |p − s|/c (c is the speed of light) while τ l (l = 1, 2, . . ., L − 1) is calculated by τ l = (|p − κ l | + |κ l − s|)/c. The response vectors at transmitter and receiver can be represented as where the sub-vector a t,n (θ t,l , φ t,l ) = χ x (θ t,l , φ t,l ) ⊗ χ y (θ t,l , φ t,l ). And there are where [θ t,l , φ t,l ] T denotes the azimuth and elevation angles of AOD of l-th path at the anchor node side. λ n = c/(n/(NT s ) + f c ) is the signal wavelength at the n-th subcarrier and d o denotes the distance between the antenna elements. The sub-vector a r,n (θ r,l , φ r,l ) can also be derived with similar equations. θ r,l and φ r,l are azimuth and elevation angles of AOA of l-th path.
In this paper, the origin of the global coordinate system (GCS) is the position of the gravity center of URA antennas on the anchor node, and the z-axis of GCS is parallel to the antenna (as shown in Fig. 1). All of our calculations are under GCS. However, since the posture of URA antennas on the rigid body changes as the rigid body moves, φ r,l and θ r,l are measured at the rigid body coordinate system (RBCS). Thus, we need to calculate the transformation relationship from RBCS to GCS as shown In (6), R is the rotation matrix related to the rigid body orientation q. We use φ r,l,g and θ r,l,g to represent the elevation and azimuth angles of AOA at path l under the global coordinate system as where c i = cos(q i ) and s i = sin(q i ), i = 1, 2, 3 for simplicity. Therefore, the received signal at n-th subcarrier and g-th transmission can be expressed as (9).
where n (g) [n] ∈ C N R is a Gaussian noise vector with zero mean and variance N 0 /2.
Our goal is to estimate rigid body posture [p; q] and reflection points position κ l from y.

III. PROPOSED JOINT RIGID BODY AND REFLECTION POINTS LOCALIZATION SCHEME
In this section, a joint rigid body posture estimation and reflection points location estimation scheme is introduced. Firstly, channel parameters including AOD, AOA and transmission time are estimated by exploiting the sparsity of the mm-wave MIMO channel. Secondly, to recover the rigid body posture and reflection points location in both NLOS and OLOS scenarios, we use different expressions of the location of reflection points to build a minimization problem and solve it with the heuristic algorithm.

A. CHANNEL PARAMETERS ESTIMATION
Since the AOA and AOD estimation accuracy are limited to the size of the grid by using the original DCS-SOMP algorithm, we propose a novel two-step channel estimation algorithm. The coarse estimation is based on an improved DCS-SOMP algorithm called hierarchical DCS-SOMP (H-DCS-SOMP) while the further fine estimation is achieved by an iterative maximum likelihood (IML) method.

1) H-DCS-SOMP
In the coarse reflection points location estimation, assuming the received signal y from the anchor node is the compressive measurement, the reflection points localization can be formulated as compressive sensing (CS) problem. The angle, transmission time measurements and channel gain are solvable by constructing a hierarchical sensing dictionary. Firstly, the mm-wave MIMO channel model is transformed into beamspace to reduce the complexity due to the sparsity of mm-wave signals in the angular domain [26]. In the 3D environment, we introduce the N T × N T transformation matrix, uniformly sampling the virtual spatial angles of AOD. where where The partial virtual representation of the channel concerning the angular domain can be written as (13). Furthermore, the received signal is expressed as where In principle, this is a 3D CS problem, where [n] denotes the sparse dictionary (sensing matrix) andh[n] ∈ C N R N T ×1 is the vectorization ofH[n] that corresponds to the coarse estimation of the AOA/AOD. With the received signalỹ[n], r Find the largest projection of r k−1 [n] on the columns bŷ r Update indices according to (18). r Calculate the k-th orthogonalized basis vector ρ k [n] with Gram-Schmidt process using (19).
r Update the residual vector r k [n]. r Repeat until all the L indices are found.
It is noted that the size of [n] will limit the upper bound on estimation accuracy. However, to improve the accuracy of parameter estimation, the size of the dictionary and the amount of computation will increase exponentially. In this paper, the channel parameter estimation is refined through a hierarchical procedure, which is initialized by the original DCS-SOMP. The structure of the dictionary matrix in H-DCS-SOMP is shown in Fig. 2. A new dictionary is constructed based on the subdivision of the L angular cells in DCS-SOMP. Since the dictionary resolution in DCS-SOMP depends on N T x and N Ty , to make the second layer dictionary resolution the same in two dimensions, we set N a = min{N Tx , N Ty } and consider the width between the adjacent virtual angle index as 2/N a in both dimension of U T x and U Ty for simplicity. Assume the indexes of L angular cells are ( V T can be rebuilt, uniformly sampling the virtual spatial angles of multipath. In the other words, N b depicts the sampling density of the second layer of the dictionary. In principle, the two-dimensional AOA/AOD estimation performance degrades greatly by using the original single layer DCS-SOMP due to the sparsity of antenna numbers. However, by using H-DCS-SOMP, increasing N b will contribute to a higher upper bound on parameter estimation accuracy. We'll demonstrate the effectiveness of the hierarchical process in Section V.A. For the second layer of the dictionary, we have where x l,c represents the center of x l and the sampling interval is 2/(N a N b ) as demonstrated in Fig. 2. The construction of v T x (b) and v Ty (b) are similar to (12). Also, we define the N R × N R matrix V R in the same way. After conducting the DCS-SOMP algorithm again, indices with better precision can be obtained. If there are more than two layers, the construction rule is similar. With the estimatedĥ,ˆ , we can rewrite (17) aŝ where can be separated into the multiplication of channel gain as and f 1 , f 2 are determined by (1) and (2) [10]. θ and φ are azimuth angles and elevation angles of all paths. Therefore, h l and τ l can be resolved by minimizing the following least squares problem. For path l, there is

2) FINE ESTIMATION
In the following environment sensing process, the 3D position estimation error of reflection points is proportional to the square of AOA or AOD estimation. Therefore, to ensure satisfied angular estimation accuracy in applications as well as reduce the computation cost, an iterative approach is developed to further improve the channel estimation performance based on the maximum likelihood principle. The iterative procedure to refine the channel parameter estimation is initialized by the coarse estimation and the search for optimal angles is subject to ±δ around the coarse estimation values.
T be the vector of the parameters under estimation. η l = [θ t,l , φ t,l , θ r,l , φ r,l , τ l , h l ] T ∈ R 6 consists of the angle parameters, channel gain and transmission time for the l-th path. The likelihood function of the random vector y conditioned on η can be written as (25) [27], Generally, the maximum of f (y|η) can be calculated directly using its Jacobian and Hesse matrix. However, due to the multi-dimensional (η ∈ R 6L ) maximization of our problem, the complexity of using the analytical methods is high. The regularly utilized searching approach for ML estimator is not feasible well. Therefore, in this paper, a simplified method is employed by utilizing an iterative algorithm to update the variables one by one. Since there are L paths, we consider beginning the iteration with the LOS followed by the NLOS paths. For instance, considering the l-th path, we firstly update one of the angles by gradient descent algorithm with the remaining parameters fixed to maximize f (y|η), which stops when the variation of f (y|η) gets smaller than or equal to the threshold α. Since the estimation error of transmission time of H-DCS-SOMP is relatively small, in the fine estimation, we consider the refinement of angular parameters. Firstly, the four angles (AOA/AOD azimuth and elevation) in l-th path are updated iteratively until the variation of f (y|η) reaches threshold α. Then the L paths are updated in the loop to ensure the convergence of all the angular variables. The steps are summarized as follows.
r Initialize all the angles using the AOA/AOD measurements obtained from the coarse estimation.  r Repeat the steps above for all the remaining angles until the variation of the likelihood function is less than or equal to threshold α. During the iteration, there's no priority difference among the angular variables since the estimation precision of them cannot be distinguished theoretically. For instance, the iteration of l-th path is carried by using where the results θ r,l , θ t,l , φ t,l , φ r,l are calculated from the iteration results of last step θ r,l , θ t,l , φ t,l , φ r,l .
Here, the gradient descent method is used to obtain each maximum within a certain searching angular range. Taking the θ r,l as an example, with θ r,l as the original value, the steps are shown as follows.

B. JOINT RIGID BODY AND REFLECTION POINTS LOCALIZATION
In this subsection, based on the estimation results of AOAs/AODs/transmission times, we demonstrate that the posture of the rigid body and the position of reflection points can be recovered from the parameters. Both two scenarios with and without LOS are discussed. The case with LOS is first investigated. As shown in Fig. 1, the distances between AN and rigid body, AN and reflection point l, reflection point l and rigid body is denoted by d 0 , d l,1 and d l,2 (l = 1, 2, . . ., L − 1), respectively. Their estimated values can be expressed with transmission times aŝ Then, the rigid body position can be directly estimated with the AOD of LOS andd 0 by using the geometric relationship asp = s +d 0 · [sinφ t,0 cosθ t,0 , sinφ t,0 sinθ t,0 , cosφ t,0 ] T . (28) However, the orientation and reflection points cannot be resolved directly from channel parameters. Thus, we formulated it as an optimization problem with the following steps. As can be seen in Fig. 1, the position of reflection point κ l can be calculated by θ t,l , φ t,l and d l,1 as κ l = s + d l,1 · [sinφ t,l cosθ t,l , sinφ t,l sinθ t,l , cosφ t,l ] T , (29) and it can also be derived by θ r,l,g , φ r,l,g and d l,2 as κ l =p + d l,2 · [sinφ r,l,g cosθ r,l,g , sinφ r,l,g sinθ r,l,g , cosφ r,l,g ] T .
Hence, the problem is reformulated as the estimation of q and d l,1 , which is to minimize the difference between κ l and κ l subject to certain constraints as is shown in P.
To solve P, we propose to use the particle swarm algorithm (PSO) based approach [28]. PSO is a kind of populationbased search algorithm. The advantage of PSO is that it can quickly find the optimal solution without searching the entire solution space, and it has low complexity to execute. In [29], the effectiveness of the PSO algorithm, especially for non-convex global optimization has been proved. Generally, it simulates the social behaviour of birds/fishes in nature. During the execution, the individuals' positions are changing with the social tendency of the group. In PSO, each individual, called a particle, benefits from the historical experience of its own and that of the other members when searching for food. In particular, each particle i records the best position it has experienced so far as pbest i , and the best position of its neighbours or the global community as gbest . With the iteration, the particle i can update its velocity ν i j and position i j ( j-th dimension of optimization vector) through the personal best position and swarm's best position. In problem P, the position of particle i is denoted as i = [q 1 , q 2 , q 3 , d 1,1 , d 2,1 , . . ., d L−1,1 ] ∈ R L+2 . In Algorithm 1, w is the inertia weight, c 1 and c 2 are learning factors while rand 1 and rand 2 are random numbers between 0 and 1. The iteration is stopped when the gbest is convergent or the maximum iteration number is reached.

C. COMPLEXITY ANALYSIS
In this subsection, we analyze the complexity of the channel parameter estimation algorithm and joint rigid body and reflection points localization algorithm.

1) CHANNEL PARAMETER ESTIMATION
The complexity in performing (18) is on the order of O(N 2 R N 2 T GN ) as same as [10]. During the coarse estimation, the coefficients derived from the second layer dictionary approximately take O(N 2 b GN ). Eq. (24) requires O(NL) operations. Consequently, the maximum complexity for H- In fine estimation, the complexity is mainly caused by iterations of the gradient descent algorithm. Since the gradient descent algorithm is an iterative process, the complexity is subject to initial values and step size factor , threshold α. Assume dimension of optimized vector is N 1 , the maximum complexity from fine estimation will be N 1 log(1/α)/ .

2) JOINT POSITION ESTIMATION
The calculation of rigid body location in the LOS case is easy to implement since it involves only some basic operations. For rigid body posture estimation and reflection points position estimation, the complexity is mainly from the PSO algorithm. Assume the PSO algorithm iterates n 2 times and in each iteration of the algorithm N 3 particles are updated. The (31) is invoked N 4 times in each iteration. Hence the overall time complexity of the joint position estimation algorithm is O (N 2 N 3 N 4 ).

IV. FUNDAMENTAL BOUNDS
In this section, we first derive the Cramér-Rao lower bound (CRLB) of the channel parameters. Then the theoretical lower bounds of rigid body position, orientation, and reflection points position are calculated.
CRLB determines the lower limit of the variance of any unbiased estimator in the parameter estimation problems. The variance of the estimator can only approach the CRLB indefinitely, but cannot be less than it. Therefore, it provides important information for the performance evaluation of algorithms. Here, we derive the fisher information matrix (FIM) and the CRLB for the channel parameters and apply them to investigate rigid body posture and reflection points estimation bounds. To simplify the notation without loss of generality, we consider the case of G = 1 which means only one OFDM signal is transmitted. First, we discuss the situation where both LOS and NLOS exist in the channel.

Taking y[n] = H[n]F[n]x[n] + n[n] into
where x l 1 , x l 2 ∈ {θ t,l , θ r,l , φ t,l , φ r,l , h l , τ l , l = l 1 , l 2 }. A more detailed derivation can be found in APPENDIX A. With the FIM, the CRLB of channel parameters in LOS can be calculated by The averages of the CRLBs of AOD/AOA in NLOS are derived in (40) The transformation matrix T is calculated with The elements of T are calculated by using the geometric relationships in (43), (6), (7) and (8) as (43) Here, we structured T analog to (34) by considering each path in the channel model ([p, q] ⊆ ξ is treated as path 0) as With the Jacobian matrix notation, it's clear that T ∈ C (3L+3)×6L , and the elements in T w,v can be abbreviated as shown as Consequently, the entries for the transmission time and AOD are derived as for LOS terms and for NLOS terms (l = 0) while the rest related to AOD and transmission time are zero. Moreover, the entries regarding the AOA are more complex since the AOA is measured in RBCS. The expression of the same vector at RBCS and GCS is connected by rotation matrix R. Therefore, we introduce the notation ζ l (p, s, κ l ) = [cos(θ r,l,g )sin(φ r,l,g ), sin(θ r,l,g )sin(φ r,l,g ), cos(φ r,l,g )] T , and there is φ r,l = arccos(R(3, :) · ζ l (p, s, κ l )), The remaining entries in T containing AOA terms can be deduced by (50) In the case of OLOS, there is no AOA/AOD for LOS to be detected. Hence, the estimation of the channel becomes η = [η 1 , . . . η L−1 ] T while theξ = ξ since the NLOS channel model is still related to all components in ξ. The following derivation under OLOS is similar to the case with LOS.

V. SIMULATION RESULTS
In this section, simulation results of the proposed 5G mmwave rigid body active localization and environment sensing methods are discussed. First, the environmental setup and performance metric are introduced. Second, we present simulation results of the rigid body posture estimation for both NLOS and OLOS situations. Third, the reflection points localization results are presented including the performance comparison of different algorithms. The CRLB as the theoretical optimal solution is also investigated.

A. SIMULATION SETUP
We consider a 3D indoor scenario with the size of 5 × 5 × 5 m 3 in MATLAB 2020b. The location of AN is s = [0, 0, 0] T . To simplify the problem without loss of generality, we randomly set the real position and orientation of rigid body within far-field regime p = [5, 5, 3] T and q = [π/6, 2π/5, π/5] T . Reflection points can be located anywhere in the cube indoor environment except the positions on the extension line between AN and the rigid body. The URAs with half-wavelength antennas inter-element spacing on both AN and the rigid body consists of N T = N T x × N Ty = N R = N Rx × N Ry = 8 × 4 antennas. We set L = 5, N = 10, c = 0.3 m/ns, B = 100 MHz, f c = 60 GHz. The number of beams sent is 10 and the average reflection loss for the first order reflection is -10 dB similar to [10] introduced. The SNR represents the signal-to-noise ratio of the signal from s. It is related to the channel information and can be defined as (53)

H[n]F[n]x[n]
The root mean square error (RMSE) is used to evaluate the estimation accuracy of the joint rigid body and reflection points localization algorithm. Supposeˆ i is the estimation of true value i in the i-th simulation, there is For the embedded PSO algorithm, the initial population is set to 300 and the inertia weight is 0.8 with both the selflearning factor and group learning factor being 0.5. The total iteration number is set to 50 to ensure convergence.

B. RIGID BODY POSTURE ESTIMATION
Firstly, we evaluate the CDF (Cumulative Distribution Function) of rigid body position and orientation estimation against different compressive sensing methods. Both the rigid body position and posture are denoted by the three dimension vector. The i in (54) is the three components of the vector with M = 3. The algorithms are repeated 200 times as is shown in Fig. 3. Here, the DCS-SOMP algorithm is taken from paper [30]. H-DCS-SOMP and our modified H-DCS-SOMP are introduced in Section III. A. In Fig. 3, we set SNR = 20 dB and N b = 4. It can be observed that our proposed modified H-DCS-SOMP algorithm has narrower distribution width for both position and orientation estimation compared with the other two algorithms. Moreover, for rigid body position estimation in Fig. 3(a), the DCS-SOMP algorithm can achieve around 0.65 m RMSE with 90% probability, and with the same condition, H-DCS-SOMP can achieve around 0.25 m RMSE and our proposed method can achieve around 0.08 m RMSE. Thus, the performance of the DCS-SOMP algorithm is much more restricted compared with our method. In addition, for rigid body orientation estimation in Fig. 3(b), our proposed method achieves 0.1 rd RMSE with 90% probability, which is higher than the DCS-SOMP method.
In addition, the localization and posture error are investigated when SNR is 0 dB, indicating a low-to-medium SNR case. The CDF is shown in Fig. 4. When SNR is lower, the localization performance using the three algorithms degrades due to the increased noise level. However, it can be observed

. The rigid body localization RMSE (top) and rigid body rotation angle RMSE (bottom) of DCS-SOMP, H-DCS-SOMP and modified H-DCS-SOMP vs CDF when SNR is 20 dB.
that the performance degradation of modified H-DCS-SOMP is much smaller than the other two methods. The average error of position and posture decreases less than 0.1 m and 0.1 rd, with distribution less than 0.1 m and 0.25 rd, respectively. This demonstrates the great robustness of proposed method in rigid body position and posture estimation.
To further demonstrate the influence of SNR, we compare the variation of rigid body position and orientation RMSE of the three algorithms under different SNRs from −20 dB to 20 dB as shown in Fig. 5. Instead of using the CDF, all the performance is the average value of 200 simulations except for the CRLB. As can be seen in Fig. 5(a), when SNR = 0 dB our proposed method reaches the corresponding theoretical bound. When SNR = 10 dB, the RMSE of the rigid body position is less than 0.1 m. Similar to position estimation results, in Fig. 5(b), the orientation RMSE of our method reaches the corresponding theoretical bound when SNR = 0 dB. When SNR = 20 dB, our proposed algorithm's location and orientation precision could reach 0.04 m and 0.027 rd while the other two algorithms can only achieve 0.072 m and 0.035 rd, and 0.15 m and 0.107 rd. Both the position and orientation results show the effectiveness of our proposed method.
In addition, we also evaluate the performance of our proposed method when there is no LOS path (OLOS situation). It can be observed from Fig. 6, that when SNR = −10 dB,   position estimation RMSE reaches the corresponding theoretical bound and when SNR = 0 dB, orientation estimation RMSE reaches the corresponding theoretical bound. It is also noted that when the LOS is blocked, the theoretical bound of location and orientation estimation is worse than the previous situation due to the less effective information acquisition. In the absence of LOS, only angle and distance information from NLOS can be used to assist rigid body posture estimation. However, the information provided by NLOS has a relatively large error due to the influence of the path loss, which leads to the deterioration of the positioning performance. Compared with Fig. 5, the position and orientation estimation error are doubled. Even though, when the SNR = 10 dB, the rigid body localization and orientation estimation can reach decimeter level and 0.1 rd, respectively.

C. REFLECTION POINTS LOCATION ESTIMATION
In this subsection, we evaluate the performance of our proposed reflection points location estimation method. As introduced in Section III. B, the position of reflection points is estimated from the angle and distance of NLOS using 1. Thus, the NLOS channel parameter estimation results are first evaluated. Each NLOS path has four angle parameters (AOA/AOD azimuth and AOA/AOD elevation, [θ t,l , θ r,l , φ t,l , φ r,l ] T ) and there are L − 1 NLOS paths. We calculate the average RMSE and CRLB of each angle among different NLOS paths, respectively. Fig. 7 illustrates the average azimuth angle estimation RMSE of AODs and AOAs of NLOS paths. And the average CRLB is labelled with red lines. It is observed that the RMSE of both elevation angles and azimuth angles reach 0.1 rd accuracy when SNR is above 0 dB. While the SNR is large than −10 dB, the angular estimation converges to the  theoretical bounds. The results demonstrate the validation of the proposed algorithm to resolve the NLOS channel for the following reflection points localization.
The theoretical lower bound and simulation results of RMSE for the different reflection points localization algorithms under various SNRs are shown in Fig. 8. The well-known genetic algorithm (GA) [31] has also been widely used for optimization problems. Thus, (31) can be potentially solved with the classic GA. Herein, if the optimal solution of (31) is obtained with GA, we denote it as Modified H-DCS-SOMP (GA-based). The cyan dash line is the RMSE of DCS-SOMP algorithm with the PSO algorithm, the orange line with a triangular symbol represents the RMSE of the modified H-DCS-SOMP algorithm with GA while the blue line with a cross symbol is the RMSE of the modified H-DCS-SOMP algorithm with PSO algorithm. The crossover rate and mutation rate are 0.5 and 0.1, respectively in the GA algorithm with an initial population of 500 and a maximal generation of 100. It is observed that in our optimization problem, under the same conditions, the PSO algorithm has better performance than GA and can converge faster to the theoretical lower bound. The performance of the PSO method is improved up to 40% compared with GA. The explanation is that, for the global optimization of continuous variables, the memory parameters gbest and pbest i of PSO make it easier to find the optimal solution while the GA-based method doesn't track the optimal solutions in the generations. Moreover, under restricted iteration number/generations, the PSO method costs less than the GA-based method to get converged. Thus, the PSO-based Algorithm 1 is much more preferred when the time resource is limited.

VI. CONCLUSION
In this paper, a rigid body active localization and environment sensing scheme with the assistance of 5G mm-wave MIMO is proposed. A novel hierarchical compressive sensing algorithm refined by an iterative maximum likelihood step for channel angle and distance estimation is presented. Furthermore, we joint consider the rigid body and reflection points localization problem and a PSO-based optimization algorithm is used to estimate the posture of the rigid body and position of reflection points. We also calculate the theoretical bounds (CRLB) on the rigid body posture, NLOS angular and reflection points position estimate uncertainties. Compared to the traditional rigid body localization approaches, our proposed method using only one anchor node can achieve centimeter-level rigid body posture estimation accuracy under both NLOS and OLOS situations. Simulation results reveal that the rigid body posture and reflection points position estimation of our proposed algorithm approach the corresponding bounds with an increase in SNR value. The results also demonstrate that the proposed scheme can achieve high performance for both rigid body localization and reflection points estimation when SNR reaches 20 dB. EDWARD AU (Senior Member, IEEE) received the Ph.D. degree in electronic and computer engineering from the Hong Kong University of Science and Technology (HKUST), Hong Kong, in 2007. He is a Distinguished Engineer with Huawei Technologies Canada Company, Ltd., Markham, ON, Canada, where he works on product certification, test bed development, and standardization of Wi-Fi. He is currently the Chair of IEEE Technical Activities Board Committee on Standards that oversees the advancement and evangelization of technical standards activities across all IEEE Societies and Councils. He is also the Chair of Computer Society Standards Activities Board standards committee that manage working groups and standards development projects that are under the purview of the Society and for new lines of work or for standards for which a viable standards committee has not been identified. He also actively participates and contributes to IEEE 802 LAN/MAN Standards Committee. He is the Chair of IEEE 802.18 Radio Regulatory Technical Advisory Group supports the work of the IEEE 802 LAN/MAN Standards Committee by actively monitoring and participating in radio regulatory matters worldwide as an advocate for IEEE 802. He is also the Founding Chair of the IEEE 802.11ay, which is aimed at developing an amendment for next generation unlicensed millimeter wave technologies for Wi-Fi, a co-editor of the IEEE 802.11 maintenance task group, which is chartered to develop revisions for the IEEE 802.11 standards, and the Editor of the IEEE 802.11be, which focuses on developing a new major amendment for next generation Wi-Fi (a.k.a. Wi-Fi 7). He was a recipient of IEEE SA Standards Medallion YAN XIN received the Ph.D. degree from the University of Alberta, Edmonton, AB, Canada, in 2002. He is currently a Principal Engineer with Huawei Technologies Canada Company Ltd., Ottawa, ON, Canada, where he works on research and standardization of wireless communications. Prior to joining Huawei Canada, he was a Senior Engineer with BlackBerry and Samsung Electronics. He has been extensively involved in 3GPP and IEEE 802.11 standardization activities, and was a Vice Chair of IEEE 802.11 TGay and the Technical Editor of IEEE 802.11 Full Duplex TIG. He has numerous granted U.S. patents and U.S. patent applications in pending. He has authored and coauthored more than 40 refereed international journals and conference papers. His research interests include physical layer development of wireless communication, signal processing, coding and modulation, MAC protocols and hardware implementation of communication systems.