# NR5G-SAM: A SLAM Framework for Field Robot Applications Based on 5G New Radio
## 3 Methodology
### Front-End Block (FEB)  
Factor graph optimization is a method for estimating the MAP state estimate of a system by modeling it as a graph composed of nodes and edges. A factor is a function that defines the relationship between graph variables. Nodes represent the variables (such as robot poses and landmark positions) and edges represent the constraints (such as the measurements between landmarks and robot poses or between different landmarks). The factorgraph is used to update the beliefs about the variables based on new measurements and to estimate the most likely values for all variables.  
The inputs of the FEB are the UE pose estimates and IMU data. The factor graph models the relationships between the estimated position, velocity, and orientation of the system based on the pre-integrated IMU measurements and the nodes from the UE pose data.  
The process of factor graph optimization involves iteratively minimizing the objective function that measures the discrepancy between the predicted and measured state vector of the system. This is typically conducted using a numerical optimization algorithm such as Levenberg-Marquardt [80] or Gauss-Newton. In our case, we perform it via smoothing and mapping (iSAM2), where the aim is to obtain the posterior pose distribution of the robot \(X^{*}\) given the noisy sensor measurements and further model them through the iterative factorization of a set of factors \(\phi(X)\), as per Equation (1). The objective function is described by Equation (2) and performs the MAP inference step of the factorization:  
\[P(X|Z)\propto\prod_{i}\phi_{i}(X_{i}). \tag{1}\]  
\[X^{*}=\operatorname*{arg\,max}_{X}\prod_{i}\phi_{i}(X_{i}). \tag{2}\]  
Factor graph optimization typically includes a state transition model that describes how the state of the system evolves over time and a measurement model that is based on the sensor updates, provided by the IMU, UE, and RSSI pose data. The objective function often includes terms that penalize deviations from the observed states and encourage smooth trajectories of the estimated state variables. Equation (2) can be formulated as the equivalent Least Squares (LS) problem as represented in Equation (3), where \(l(X_{i})\) is the sensor model likelihood, \(z\) is the measurement, and \(\Sigma\) is the covariance, which are associated with \(X_{i}\). By minimizing this equation, we iteratively obtain the updated state estimates:  
\[X^{*}=\operatorname*{arg\,min}_{X}\sum_{i}\lVert l(X_{i})-z_{i}\rVert_{\Sigma_ {i}}^{2}. \tag{3}\]  
In our system, UE poses represent the state vector as nodes and the constraints between successive poses as edges. The UE poses are modeled as NR5G factors and the edges of the graph are constraints, which are modeled as IMU and RSSI factors. The IMU factors are used to model the motion constraints through pre-integration and the RSSI factors correspond to the output of the BEB and serve as a correction pose.  
The robot state vector \(\mathbf{x}\) is modeled similarly to the factor graph formulation in [15], consisting of the rotation matrix \(\mathbf{R}\in SO(3)\), position \(\boldsymbol{p}\in\mathbb{R}^{3}\), velocity \(\boldsymbol{v}\) vectors, and the IMU biases \(\boldsymbol{b}\):  
\[\mathbf{x}=\left[\mathbf{R}^{\intercal},\mathbf{p}^{\intercal},\mathbf{v}^{ \intercal},\mathbf{b}^{\intercal}\right]^{\intercal}. \tag{4}\]  
A transformation, \(T\in SE(3)\), from the robot base \(B\) to the world frame \(W\) is represented as \(T=[R|p]\).  
The output of the FEB prediction process is the full pose of the robot, namely the NR5G-SAM localization, consisting of position \([p_{x},p_{y},p_{z}]\), with the IMU orientation represented in quaternions \([q_{x},q_{y},q_{z},q_{w}]\) and pre-integrated linear and angular velocities \([v_{x},v_{y},v_{z},\omega_{x},\omega_{y},\omega_{z}]\). A description of the NR5G, IMU, and RSSI factors is presented below, and an illustration of the NR5G-SAM factor graph is shown in Figure 3.