# Forklift Tracking: Industry 4.0 Implementation in Large-Scale Warehouses through UWB Sensor Fusion
## 2 UWB Forklift Localization Method
### Forklift Motion Model
The accelerations are measured through the IMU and they are considered as affected by a zero-mean additive white Gaussian noise, named as \(v_{a_{x_{k}}}\sim\mathcal{N}(0,\sigma_{a}^{2})\), and \(v_{a_{y_{k}}}\sim\mathcal{N}(0,\sigma_{a}^{2})\), where \(\sigma_{a}\) is the acceleration-noise standard deviation. Similarly, the angular speed noise is \(v_{\omega_{k}}\sim\mathcal{N}(0,\sigma_{\omega}^{2})\), where \(\sigma_{\omega}\) is the angular speed-noise standard deviation. Consider that, since the forklift is a rigid body, the angular speed can be measured in any point of the vehicle, and it is not mandatory to measure it at the rotation center of the vehicle. The OFS measures the displacement of the forklift among two consecutive timesteps, thanks to a computer vision algorithm that analyzes floor images acquired at different timesteps. The velocity measurements can be computed by dividing the displacement for the sampling time \(\Delta t\). For the OFS, the quantization noise is preponderant with respect to other noise sources, so the velocity measurements can be considered affected by two uniformly distributed random variables named \(q_{v_{\lambda_{k}}}\sim\mathcal{U}([-r/(2\Delta t),r/(2\Delta t)])\), and \(q_{v_{y_{k}}}\sim\mathcal{U}([-r/(2\Delta t),r/(2\Delta t)])\), where \(r\) is the sensor spatial resolution and \(\Delta t\) is the sampling time. The dynamic state vector is \(\mathbf{s_{k}}=[x_{k},y_{k},v_{x_{k}},v_{y_{k}},a_{x_{k}},a_{y_{k}},\omega_{k},\theta_{k}]^{T}\in\mathbb{R}^{8\times 1}\). The dynamic state vector at timestep \(k+1\), \(\mathbf{s_{k+1}}\), can be written as a non-linear function of \(\mathbf{s_{k}}\) through the following state transition model:  
\[\mathbf{s_{k+1}}=\mathbf{f}(\mathbf{s_{k}})= \begin{bmatrix}x_{k+1}\\ y_{k+1}\\ v_{x_{k+1}}\\ v_{y_{k+1}}\\ a_{x_{k+1}}\\ a_{y_{k+1}}\\ \omega_{k+1}\\ \theta_{k+1}\end{bmatrix} = \begin{cases}x_{k}+\Delta t(v_{x_{k}}\cos(\theta_{k })-v_{y_{k}}\sin(\theta_{k}))\\ y_{k}+\Delta t(v_{x_{k}}\sin(\theta_{k})+v_{y_{k}}\cos(\theta_{k}))\\ v_{x_{k}}+\Delta ta_{x_{k}}\\ v_{y_{k}}+\Delta ta_{y_{k}}\\ a_{x_{k}}\\ a_{y_{k}}\\ \omega_{k}\\ \theta_{k}+\Delta t\omega_{k}\end{cases}  \tag{1}\]  
The high inertia of the forklift makes reasonable the assumption of a quasi-constant velocity motion model between two consecutive timesteps. Therefore, we can suppose that the process noise only applies on the acceleration quantities and the angular speed. Without loss of generality, we will assume that the process noise statistics have the same statistics of the measurement noise for the same variable. The process noise is not additive. In (2), we made explicit the presence of the state noise vector \(\mathbf{w_{k}}=[v_{a_{x_{k}}},v_{a_{y_{k}}},v_{\omega_{k}},\hat{q}_{v_{x_{k} }},\hat{q}_{v_{y_{k}}}]^{T}\) in the state transition model:  
\[\mathbf{s_{k+1}}=\mathbf{f}(\mathbf{s  
where with the symbol \"'\" we indicate the corresponding process noise variable. At each timestep \(k\), the following set of measurements \(\mathbf{z_{k}}=\mathbf{h}(\mathbf{s_{k}},\mathbf{n_{k}})\in\mathbb{R}^{7\times 1}\) is available, where \(\mathbf{n_{k}}\) represents the set of measurement noises:  
\[\mathbf{z_{k}}=\mathbf{h}(\mathbf{s_{k}},\mathbf{n_{k}})= \begin{bmatrix} \vec{x_{k}}\\ \vec{y_{k}}\\ \vec{v_{x_{k}}}\\ v_{y_{k}}\\ a_{x_{k}}\\ a_{y_{k}}\\ c\vec{\omega}_{k}\end{bmatrix} = \begin{cases}x_{k}^{UIWB}-\Delta x_{UIWB}\cos( \theta_{k})+\Delta y_{UIWB}\sin(\theta_{k})+v_{x_{k}}\\ y_{k}^{TIWB}-\Delta x_{UIWB}\sin(\theta_{k})-\Delta y_{UIWB}\cos(\theta_{k})+v_{y _{k}}\\ v_{x_{k}}+q_{v_{x_{k}}}\\ v_{y_{k}}+q_{v_{y_{k}}}\\ a_{x_{k}}+v_{a_{x_{k}}}\\ a_{y_{k}}+v_{a_{y_{k}}}\\ \omega_{k}+v_{\omega_{k}}\end{cases}  \tag{3}\]  
where \(x_{k}^{UIWB}\) and \(y_{k}^{UIWB}\) are the UWB tag coordinates measured in the plant/warehouse frame through the UWB system and which will be described in the next section. Thanks to these two sets of Equations (2) and (3), a UKF can be adopted to estimate the forklift location in the plant/warehouse reference frame and its orientation.