Particle Filter

With an unknown starting pose, Markov localization produces a belief with several modes, one for each place that matches the robot’s observations. EKF localization represents the belief by a single Gaussian and cannot express it. The particle filter represents the belief by a set of samples. The samples can form any number of modes, and the motion and measurement models do not have to be linear or have a closed-form density.

​

Particles as a belief

The filter keeps $M$ samples of the state, called particles: $$\mathcal{S}_t = \left\{ s_t^{[1]}, s_t^{[2]}, \ldots, s_t^{[M]} \right\}$$ Each particle $s_t^{[m]}$ is a hypothesis of the state at time $t$. Ideally the particles are drawn from the posterior, $$s_t^{[m]} \sim p(s_t \mid z_{1:t}, a_{1:t}),$$ and the fraction of particles that fall in a region of the state space approximates the probability that the state lies there. $M$ is typically in the hundreds or thousands. The approximation improves as $M$ grows, and the cost of each step grows linearly with $M$.

​

The particle filter algorithm

Each step takes the previous particle set, the action $a_t$ and the measurement $z_t$, and returns the new particle set: $$\begin{array}{lll} 1: & \textbf{Algorithm Particle\_filter}(\mathcal{S}_{t-1}, a_t, z_t): & \\ 2: & \quad \bar{\mathcal{S}}_t = \mathcal{S}_t = \emptyset & \\ 3: & \quad \text{for } m = 1 \text{ to } M \text{ do} & \\ 4: & \quad\quad \text{sample } s_t^{[m]} \sim p(s_t \mid a_t, s_{t-1}^{[m]}) & \text{(prediction)} \\ 5: & \quad\quad w_t^{[m]} = p(z_t \mid s_t^{[m]}) & \text{(importance weight)} \\ 6: & \quad\quad \bar{\mathcal{S}}_t = \bar{\mathcal{S}}_t + \langle s_t^{[m]}, w_t^{[m]} \rangle & \\ 7: & \quad \text{endfor} & \\ 8: & \quad \text{for } m = 1 \text{ to } M \text{ do} & \\ 9: & \quad\quad \text{draw } i \text{ with probability} \propto w_t^{[i]} & \text{(resampling)} \\ 10: & \quad\quad \text{add } s_t^{[i]} \text{ to } \mathcal{S}_t & \\ 11: & \quad \text{endfor} & \\ 12: & \quad \text{return } \mathcal{S}_t & \end{array}$$ Line 4 samples a new state for each particle from the motion model. The filter only has to draw samples from $p(s_t \mid a_t, s_{t-1})$. It does not need the density in closed form or a Jacobian. After line 4 the particles represent the predicted belief $\overline{\mathtt{bel}}(s_t)$. Line 5 computes the importance weight of each particle: the likelihood of $z_t$ given the particle’s state. The measurement model is evaluated at the particle and is not linearized. Lines 8 to 11 resample. The filter draws $M$ particles with replacement, each with probability proportional to its weight. Particles with high weight tend to be duplicated and particles with low weight tend to be dropped. The new set is approximately distributed according to the posterior $\mathtt{bel}(s_t) = \eta \, p(z_t \mid s_t) \, \overline{\mathtt{bel}}(s_t)$. These are the prediction and measurement update of the Bayes filter, applied to samples (Thrun, Burgard and Fox, 2005, Table 4.3). Resampling adds variance. When the weights are nearly equal, the random draw changes the particle set without using any new information. In practice the weights are often carried from step to step, each multiplied by the new likelihood, and the particles are resampled only when the weights become uneven. The effective sample size, computed with normalized weights, $$\hat{N}_{\mathrm{eff}} = \frac{1}{\sum_{m=1}^{M} \left(w_t^{[m]}\right)^2},$$ equals $M$ when all weights are equal and 1 when one particle holds all the weight. The filter resamples when $\hat{N}_{\mathrm{eff}}$ falls below a threshold.

​

The weights as importance sampling

The weights and the resampling step are an instance of importance sampling. The target is the posterior belief $$f(s_t) = \mathtt{bel}(s_t) = \eta \, p(z_t \mid s_t) \, \overline{\mathtt{bel}}(s_t).$$ The filter cannot sample from this directly. It can sample from the predicted belief: take a particle from the previous set, which is distributed according to $\mathtt{bel}(s_{t-1})$, and push it through the motion model $p(s_t \mid a_t, s_{t-1})$. The proposal is therefore $$g(s_t) = \overline{\mathtt{bel}}(s_t) = \int p(s_t \mid a_t, s_{t-1}) \, \mathtt{bel}(s_{t-1}) \, ds_{t-1},$$ and the importance weight is $$w_t = \frac{f(s_t)}{g(s_t)} = \frac{\eta \, p(z_t \mid s_t) \, \overline{\mathtt{bel}}(s_t)}{\overline{\mathtt{bel}}(s_t)} = \eta \, p(z_t \mid s_t).$$ The predicted belief cancels, and the weight of each particle is the measurement likelihood. The constant $\eta$ does not matter, because the weights are normalized when the filter resamples. Thrun, Burgard and Fox (2005, Sections 4.2.2 and 4.2.3) give this derivation over whole state sequences. The failure modes of importance sampling carry over. When the sensor is much more accurate than the motion, $p(z_t \mid s_t)$ is narrow compared to the predicted belief, few particles land where it is large, and the effective sample size drops. When no particle is near the true state, the weights can be even and the belief still wrong.

​

Drones in GNSS-denied airspace

Most autopilots in the deployed-drones table correct the inertial prediction with GNSS. GNSS can be jammed or spoofed, and buildings block it in dense urban areas. Without it the inertial estimate drifts. In many of these places the cellular network still works. Many drones carry an LTE or 5G modem for their command link. The drone modem measures the signals of nearby cells as part of normal operation, and these measurements depend on where the drone is. They can serve as the measurement $z_t$ of a particle filter whose motion model is driven by the IMU. A single cellular measurement says little about position. A report holds the received power of one or a few cells, and the same values occur at many places, so the belief after one report has many modes. This is the case the particle filter is built for. The radio environment in the air differs from the one on the ground. In the 3GPP Release 15 study of LTE support for drones, a drone flying above about 50 m received up to 16 neighbor cells within 10 dB of the strongest, against up to 8 for a modem on the ground (Muruganathan et al., 2018). Measurement reports still limit the number of cells they include. A likelihood model trained on street-level drive tests does not describe these measurements, so it would have to be trained on measurements taken at altitude. The two methods below locate a modem from the measurement records it reports to a cellular network. The first uses signal strength in LTE. The second adds the beam indices that a 5G modem reports. A drone version would replace their road-graph motion model with a three-dimensional model driven by the IMU.

​

Modem localization in LTE

In a 4G LTE network, modems report radio measurements to the network during data sessions. Operators store these records to monitor connection quality, throughput and coverage. The records do not include a location, because the LTE protocol stack that generates them has no access to the GPS fix. Ray, Deb and Monogioudis (2016) used a particle filter to estimate where each record was generated. The measurement is the reference signal received power (RSRP) of a cell. A record holds the RSRP of the serving cell and at most one neighbor cell, and many records hold only the serving cell. Triangulation needs at least three cells. Fingerprinting against a radio map is ambiguous with one or two values, because the same values occur at many locations. The method therefore links the records of one modem into a time series and uses a motion model to connect consecutive records. The filter has the following parts:

• The state is the modem’s position and speed, $s_t = (x_t, v_t)$. The position is restricted to the nodes of a road graph.
• The motion model updates the speed as $v_t = e^{-\beta\tau} v_{t-1} + (1 - e^{-\beta\tau}) v_0$, with $v_0 \sim \mathcal{N}(\mu, \sigma^2)$ and $\tau$ the time between records. The modem moves a distance $v_t \tau$ along the road graph.
• The measurement model uses one random forest per cell. It predicts the mean and standard deviation of RSRP from latitude and longitude and is trained on the operator’s drive-test data. The likelihood of a reported RSRP is a Gaussian with the predicted mean and standard deviation.
• The weights are carried across steps, and the particles are resampled when $\hat{N}_{\mathrm{eff}}$ falls below a threshold. The estimate is the path of the particle with the largest final weight.

The cost is $O(TN)$ for $T$ records and $N$ particles, and the particles can be processed in parallel. On drive-test data from an LTE network in New York City, the median error for outdoor records was about 20 m. A separate classifier labeled records as indoor or outdoor with more than 98% accuracy.

​

Modem localization in 5G with beam indices

A 5G base station transmits its synchronization signals in a sweep of narrow beams. Each beam has an index, the SS/PBCH block index, between 0 and 63. A modem reports the indices of up to four of its strongest beams and the received power on each. The beam index indicates the direction of the modem from the cell, which RSRP alone does not. Wang, Hosangadi, Monogioudis and Rao (2019) extended the LTE method to use the beam reports. The filter differs in three places:

• The motion model is a Gauss-Markov model for speed and heading. Each step pulls the speed toward a mean $\bar{S}$ and the heading toward a mean $\bar{d}$, with Gaussian noise. The position advances by the speed times the time step along the heading.
• The measurement at step $t$ is the set of up to four reported beam indices $B^j_t$ and their powers $R^j_t$. A random forest classifier for each cell and rank $j$ gives the probability $p(B^j_t \mid s_t)$ of each beam index at a position. A random forest regressor for each cell and beam gives the mean and standard deviation of the power.
• The likelihood is the product over the reported beams:

$$p(z_t \mid s_t) = \prod_{j=1}^{4} p(R^j_t \mid s_t, B^j_t) \, p(B^j_t \mid s_t)$$ The data came from a system simulation of a millimeter-wave cell with 200 m inter-site distance and up to 56 beams. With a single cell and no road constraint, the median error was about 4 m in line of sight and about 12 m with mixed line-of-sight and non-line-of-sight conditions. Narrower beams gave lower error. A drone modem is often in line of sight of the cell, the condition under which this method reached 4 m. If the beams sweep in elevation as well as azimuth, the beam index also constrains the drone’s elevation angle from the cell. The beam likelihoods would have to be trained on aerial measurements, as the signal strength models would.

​

References

• Muruganathan, S. D., Lin, X., Maattanen, H.-L., Sedin, J., Zou, Z., Hapsari, W. A., and Yasukawa, S. (2018). An overview of 3GPP Release-15 study on enhanced LTE support for connected drones. arXiv:1805.00826.
• Ray, A., Deb, S., and Monogioudis, P. (2016). Localization of LTE measurement records with missing information. IEEE INFOCOM 2016, the 35th Annual IEEE International Conference on Computer Communications.
• Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic Robotics. MIT Press.
• Wang, D., Hosangadi, G., Monogioudis, P., and Rao, A. (2019). Mobile device localization in 5G wireless networks. 2019 International Conference on Computing, Networking and Communications (ICNC), Honolulu.

---