# NR5G-SAM: A SLAM Framework for Field Robot Applications Based on 5G New Radio
## 3 Methodology
### User Equipment Block (UEB)  
The PRS and CSI signals transmitted by the gNB stations are received by the UE on the robot via the DL communication channel and generated post-signal processing as follows: the received signals pass through an Analog-to-Digital (A/D) converter and then through Fast Fourier Transform (FFT) conversion to the frequency domain. The outcomes of this processing step are Orthogonal Frequency-Division Multiplexing (OFDM) signals containing the data. In our case, the CSI-RSSI and PRS signals are the information required and decoded using the cyclic prefix.  
In this study, we consider that LoS signals exist between gNbs and the robot within the agricultural area. Range estimation is based on ToA, and the gNBs are assumed to be synchronized via a global network time reference. The ToA ranges are then used for multilateration to estimate the ranges \([r_{1},r_{2},\ldots,r_{n}]\) from each base station to the robot, which includes additive noise that is assumed to follow a Gaussian distribution. The Channel State Information Reference Signal (CSI-RS) information is extracted from the CSI signals and vectorized as a set of RSSI \([rssi_{1},rssi_{2},\ldots,rssi_{n}]\) values from each gNB station, measured in Decibel Milliwatts (dBm) and available as communication parameters in cellular communications, including 5G NR.  
The locations of the stations in Cartesian space \([s_{1},s_{2},\ldots,s_{n}]\) are obtained from a cell search operation that detects and receives basic location information regarding the gNBs in the surrounding area. An optimization of the ToA ranges is performed using a Genetic Algorithm (GA) to minimize the horizontal error during multilateration. Our previous developments [16] involved benchmarking three meta-heuristics for 5G positioning. The GA implementation provided superior results in terms of the mean execution time and average Euclidean error, thus becoming a natural design choice.  
Station locations and the estimated ranges are used as inputs for the GA, which aims to find the optimal solution within the search space of potential 2D robot positions by considering the locations of the fixed gNB stations. This optimization step is required to account for time differences in signal reception and also for the multilateration errors in ranges.  
The output of the GA's fitness function for each estimated position is used as an uncertainty measurement, and the geometric evaluation of the ranges and analysis of the error correlation is performed using inverse trigonometric functions with the position and the gNBs. Practically, the 3D position estimation improves the GA's performance compared to the 2D GA estimation, so the x- and y-coordinates are obtained from the estimated 3D vector, while the vertical z error is excluded until further mitigation methods are considered (see Section 4). The correlation between the positioning error in x and y and the fitness function's output along each axis, are used for the meaningful estimation of a dynamic covariance matrix required by the EKF.  
An experimental scaling factor is applied to convert the positioning fitness into covariance. This allows the EKF to consider the uncertainty of the 5G positioning estimation and fuse it with onboard IMU and robot velocity measurements. The output of the sensor fusion process includes the full pose of the robot, consisting of 5G-based position \([p_{x},p_{y},p_{z}]\), IMU orientation in quaternions \([q_{x},q_{y},q_{z},q_{w}]\), and wheel odometry linear and angular velocities \([v_{x},v_{y},v_{z},\omega_{x},\omega_{y},\omega_{z}]\). The EKF [79] generates the UE pose by smoothing the noisy 5G positioning signal and serves as an absolute measurement in the world frame \(W\) for the FEB.