# Robot Localisation Using UHF-RFID Tags: A Kalman Smoother Approach +
## 3 Materials and Methods
### Filter Design  
We are interested in determining an estimate of the state of an unicycle-like vehicle (very common in, e.g., industrial autonomous warehouses) \(\hat{s}_{k}\) at time \(kT_{s}\) fusing the sensor readings coming from the wheel encoders and the RFID tag ranging. The odometry measurements are related to the linear and angular velocities increment of the vehicle \(v_{k}T_{s}\) and \(\omega_{k}T_{s}\), respectively, and detailed in (2). The UHF-RFID tag sensed quantities are described in (4) and denoted with \(d_{i,k}^{m}\), which are assumed to be collected up to time \(kT_{s}\). Moreover, we are interested in having an effective estimate of the overall trajectory \(\hat{s}_{0,\dots,k}\) from time \(0\) to \(kT_{s}\) using all the measurements up to time \(kT_{s}\), i.e., _full-length_ smoother named SEKF\({}^{2}\), or the measurements in the interval \([(k-n)T_{s},kT_{s}]\), where \(n>0\) is the length of a fixed-lag in the _fixed-lag_ smoother (SEKF) and represents a tuning parameter that should be optimally determined. In particular, we will focus on an EKF-RTS Smoother estimator for robot positioning through globally observable bended trajectories, i.e., trajectories having \(\omega_{k}\
eq 0\) for a sufficiently long time as derived in [37]. The robot moves in an indoor scenario instrumented with several \(M\geq 3\) RFID tags in known positions. At each time step \(kT_{s}\), the vehicle receives a measurement (3), with an unknown offset. The offset-free RFID phase measurement (8) described in Section 3 are organised in a vector of finite difference of measurements, i.e.,\[h(s_{k},s_{k-1})= \begin{bmatrix}h_{1}(s_{k},s_{k-1})\\ h_{2}(s_{k},s_{k-1})\\ \ldots\\ h_{M}(s_{k},s_{k-1})\end{bmatrix} = \begin{bmatrix}d_{1,k}-d_{1,k-1}\\ d_{2,k}-d_{2,k-1}\\ \ldots\\ d_{M,k}-d_{M,k-1}\end{bmatrix} . \tag{9}\]  
Due to (9), the state space of the filter is represented by  
\[\hat{q}_{k}=[s_{k},\hat{s}_{k-1}]^{T}=[\hat{x}_{k},\,\hat{y}_{k},\,\hat{\theta} _{k},\,\hat{x}_{k-1},\,\hat{y}_{k-1},\,\hat{\theta}_{k-1}]^{T},\]  
whose dynamic is given by  
\[\hat{q}_{k+1}=f(\hat{q}_{k},u_{k},\gamma_{k})= \begin{bmatrix}f_{1}(\hat{s}_{k },u_{k},\gamma_{k})\\ f_{2}(\hat{s}_{k},u_{k},\gamma_{k})\\ f_{3}(\hat{s}_{k},u_{k},\gamma_{k})\\ \hat{x}_{k}\\ \hat{y}_{k}\\ \hat{\theta}_{k}\end{bmatrix}  \tag{10}\]  
where \(u_{k}=[v_{k},\omega_{k}]^{T}\) is the vector of the model velocities, while \(\gamma_{k}=[\gamma_{v_{k}},\gamma_{\omega_{k}}]^{T}\) are the noises affecting the model velocities estimate given the odometry and assumed zero-mean, white and normally distributed with diagonal covariance matrix \(Q_{k}\). Finally, \(f_{i}(\hat{s}_{k},u_{k},\gamma_{k})\) are the system model dynamic expressed in (2). In a standard EKF, the predicted model uncertainty will be given by the respective Jacobians, i.e.,  
\[P_{k+1}^{-}=A_{k}P_{k}A_{k}^{T}+B_{k}Q_{k}B_{k}^{T}, \tag{11}\]  
with  
\[A_{k}= \begin{bmatrix}1&0&\frac{\partial f_{1}}{\partial\theta_{k}}&0&0&0\\ 0&1&\frac{\partial f_{2}}{\partial\theta_{k}}&0&0&0\\ 0&0&1&0&0&0\\ 1&0&0&0&0&0\\ 0&0&1&0&0&0\end{bmatrix} \text{ and }B_{k}= \begin{bmatrix}\frac{\partial f_{1}}{ \partial\theta_{k}}&\frac{\partial f_{1}}{\partial\omega_{k}}\\ \frac{\partial f_{2}}{\partial\theta_{k}}&\frac{\partial f_{2}}{\partial \omega_{k}}\\ 0&T_{s}\\ 0&0\\ 0&0\end{bmatrix} ,\]  
where the superscript \(\cdot\)- notation is used to denote the predicted estimates given the model. Of course, the initial value for the state is \(q_{0}\), while the initial covariance is \(P_{0}\). Since the measurement function is modelled as (9), we have in the filter  
\[z_{k}=h(\hat{s}_{k},\hat{s}_{k-1})+\varepsilon_{k}=h(\hat{q}_{k})+\varepsilon _{k}, \tag{12}\]  
where \(\varepsilon_{k}\) is the measurement uncertainty, modelled as a zero-mean, white and normally distributed sequence with covariance \(R_{k}\). Hence, the EKF equations for the estimates updates  
\[