# A Single RF Emitter-Based Indoor Navigation Method for Autonomous Service Robots
## 4 RF-Based Target Localisation
### Enhancing Angle and Distance Estimation
\[w_{r}=1.0-t_{r}\times(|particle_{r}-sensor_{r}|-c_{r}) \tag{6}\]  
\[w_{\theta}=1.0-t_{\theta}\times(|particle_{\theta}-sensor_{\theta}|-c_{\theta}) \tag{7}\]  
where \(sensor_{r}\) and \(sensor_{\theta}\) are estimates of the current relative distance (in meters) and relative angle (in degrees) of the emitter based on the instantaneous RSS measurements, \(t_{r}\) and \(t_{\theta}\) are tolerance levels(set to 0.1 and 0.01 respectively for our prototype), and \(c_{r}\) and \(c_{\theta}\) are constant offsets (set to 1.0 and 15.0 respectively for our prototype). The tolerance level represents error in these estimates, adjusted according to our expected accuracy of the measurements. In the proposed algorithm, we focused on using direction estimation to compensate for erroneous distance estimation, and therefore \(t_{r}\) is 10 times higher than the value of \(t_{\theta}\), i.e., distance estimation has a higher error tolerance. The constant offsets represent a minimum threshold that must be crossed before the measurements start to affect the weights in order to avoid fluctuations caused by noise. In our case, the distance difference needs to be larger than 1 m (i.e., \(c_{r}=1\)) and the angle difference needs to be larger than 15 degrees (i.e., \(c_{\theta}=15\)) to start influencing \(w_{r}\) and \(w_{\theta}\) respectively. These weight elements are then clamped so that any value larger than 1 is set to 1, and any value smaller than 0.1 is set to 0.1. The particle weight is calculated by determining the square root of the product of the two weight elements as shown in Equation (8). The square root was taken to ensure the scale of the weight distribution is between 0.1 and 1, avoiding extremely small weights (i.e., less than 0.1) that may cause undesirably fast convergence towards suboptimal estimates. An example of how combining the distance and direction weight distributions leads to more accurate overall estimates is shown in Figure 7.  
\[weight=\sqrt{(w_{r}\times w_{\theta})} \tag{8}\]  
The weights of all particles are then normalised into probabilities so that the sum of all particle weights is equal to one, as shown in Equation (9). The final estimated direction and distance are calculated according to Equations (10) and (11) by calculating the weighted estimated direction and distance from all particles to reduce the impact of significant fluctuations caused by raw RSS measurements. In order to avoid a model with a very small number of high probability particles, a resampling step occurs [36]. This is triggered when the inverse of the sum of the squares of the weights is less than half the total number of particles, as indicated in Equation (12):  
\[P_{i}=\frac{weight_{i}}{\sum_{i=0}^{N}weight_{i}} \tag{9}\]  
\[Estimated_{r}=\sum_{i=0}^{N}particle_{r}^{i}\times P_{i} \tag{10}\]  
\[Estimated_{\theta}=\sum_{i=0}^{N}particle_{\theta}^{i}\times P_{i} \tag{11}\]  
Figure 7: Visualisation of the particle weight functions for a ground truth location of (4, 4), meaning a displacement of 4 m in the x and y directions from the receiver, indicated by the dark square. The receiver is in the centre of each image at (0, 0). The weight function based only on distance is shown in the left image, and the weight function based only on the angle (direction) is shown in the middle image. The final combined weight function is shown in the right image. Red indicates a low likelihood, and green indicates a high likelihood of the emitter position matching the particle location. Figure 4.0  
After the resampling step, the number of particles remains the same, \(N\). However, the new particles are sampled from the existing particles, where the probability for each existing particle to be sampled is the weight of that particle. This has the effect of increasing the number of higher weight particles and decreasing the number of lower weight particles, which leads to convergence of the filter.