# NR5G-SAM: A SLAM Framework for Field Robot Applications Based on 5G New Radio
## 3 Methodology
### Front-End Block (FEB)
#### 3.3.3 IMU Factors  
When new angular velocity and linear acceleration measurements are received from the onboard IMU, which is running at a high acquisition rate, we model them in our approach as [15; 81]:  
\[\hat{\mathbf{\omega}}_{t}=\mathbf{\omega}_{t}+\mathbf{b}_{t}^{\omega}+ \mathbf{n}_{t}^{\omega} \tag{5}\] \[\widehat{\mathbf{a}}_{t}=\mathbf{R}_{t}^{\mathrm{BW}}(\mathbf{a }_{t}-\mathbf{g})+\mathbf{b}_{t}^{\mathrm{a}}+\mathbf{n}_{t}^{\mathrm{a}}, \tag{6}\]  
where \(\hat{\mathbf{\omega}}_{t}\) is the raw angular velocity, computed from the current velocity \(\mathbf{\omega}_{t}\), the slowly varying bias \(\mathbf{b}_{t}^{\omega}\), and its respected noise \(\mathbf{n}_{t}^{\omega}\) at the given time step. The same applies for the raw acceleration \(\widehat{\mathbf{a}}_{t}\), which is computed from the rotation matrix \(\mathbf{R}_{t}^{\mathrm{BW}}\) between the base \(B\) of the robot and the world reference frame \(W\), the current acceleration \(\mathbf{a}_{t}\), and the constant gravity vector \(\mathbf{g}\). The addition of a slowly varying bias \(\mathbf{b}_{t}^{\mathrm{a}}\) and noise \(\mathbf{n}_{t}^{\mathrm{a}}\) complete the equation.  
As such, we are able to estimate the motion of the robot in the next timestep. This includes the position, velocity, and orientation, known as the IMU preintegration (see [81]), described by the following set of equations:  
\[\mathbf{v}_{t+\Delta t} =\mathbf{v}_{t}+\mathbf{g}\Delta t+\mathbf{R}_{t}(\widehat{ \mathbf{a}}_{t}-\mathbf{b}_{t}^{\mathrm{a}}-\mathbf{n}_{t}^{\mathrm{a}})\Delta t \tag{7}\] \[\mathbf{p}_{t+\Delta t} =\mathbf{p}_{t}+\mathbf{v}_{t}\Delta t+\frac{1}{2}\mathbf{g} \Delta t^{2}+\frac{1}{2}\mathbf{R}_{t}(\widehat{\mathbf{a}}_{t}-\mathbf{b}_{t }^{\mathrm{a}}-\mathbf{n}_{t}^{\mathrm{a}})\Delta t^{2}\] (8) \[\mathbf{R}_{t+\Delta t} =\mathbf{R}_{t}\exp\bigl{(}(\widehat{\omega}_{t}-\mathbf{b}_{t}^ {\omega}-\mathbf{n}_{t}^{\omega})\Delta t\bigr{)}, \tag{9}\]  
Figure 3: Overview of the NR5G-SAM factor graph consisting of the NR5G factors, IMU preintegration factors, RSSI factor, and their individual data queues. A prior factor has been assigned to the first node of the graph at startupwhere \(\mathbf{v}_{t+\Delta t}\) is the linear velocity, \(\mathbf{p}_{t+\Delta t}\) is the position, and \(\mathbf{R}_{t+\Delta t}\) the rotation matrix at the inertial frame \(I\), which coincides with the robot base frame \(B\) for simplicity, at each timestep. Their overall differences over time are computed as follows:  
\[\Delta\mathbf{v}_{ij} =\mathbf{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}- \mathbf{g}\Delta t_{ij}\right) \tag{10}\] \[\Delta\mathbf{p}_{ij} =\mathbf{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}- \mathbf{v}_{i}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^{2}\right)\] (11) \[\Delta\mathbf{R}_{ij} =\mathbf{R}_{i}^{\top}\mathbf{R}_{j}. \tag{12}\]  
This process is suitable for real-time applications as the generated FEB pose is provided at a high rate (200 Hz in our setup, as shown in Section 4). The output of the Front-End Block is the NR5G-SAM Localization, which feeds the Back-End block.