*3.4. Weighting Approach*

The samples in a particle filter are weighted according to the likelihood functions that describe the accuracy of camera readings, given the predicted landmarks and sensor locations. The weight of a particle is inversely proportional to the innovation of observed landmarks **y** [1:*L<sup>p</sup> k* ] *k*in a time step *k* which is measured in pixels, in the image frame:

$$w\_k^p \sim w\_{k-1}^p p\left(\mathbf{y}\_k^{[1:L\_k^{p\top}]} \middle| \mathbf{x}\_k^{[p]}\right) \tag{21}$$

where *L<sup>p</sup> k* is the number of landmarks matched by the particle *p* at a time *k*. The weight of a given particle can be further calculated using the following formula:

$$w\_k^p = w\_{k-1}^p \left| 2\pi \mathbf{S}\_k^{\left[1:L\_k^P\right]} \right|^{-1/2} \exp\left[ -\frac{1}{2} \left( \mathbf{y}\_k^{\left[1:L\_k^P\right]} \right)^T \left( \mathbf{S}\_k^{\left[1:L\_k^P\right]} \right)^{-1} \mathbf{y}\_k^{\left[1:L\_k^P\right]} \right] \tag{22}$$

where **S** [1:*L<sup>p</sup> k* ] *k* is the innovation covariance matrix constructed for all landmarks that were matched with the previously seen scene points at a time step *k*. The particle cloud encoding exemplary estimates in a particle filter in the local body frame is shown in Figure 3.

**Figure 3.** An example of bimodal PDF encoded using weighted particles.

For our algorithm to include particular landmarks in the sample-weight calculation, they have to be correctly associated with previously extracted scene points. The matching procedure consists of two steps. After the visual association of image features, which is based on a comparison of SURF descriptors (and was described in previous sections), we proceed to spatial gating, using the Mahalanobis distance to remove spatial outliers. This step introduces a potentially error-prone dependency. Namely, landmark matches have to be gated in accordance with measurement residuals *y* [*i*] *k*+1 for all the potential associations—for every particle. As the set of samples represents the complete PDF of a camera pose, the differences between predicted and observed landmark positions vary between particles, while the matching threshold remains constant. Consequently, the number of landmarks that pass the gating criterion may differ within the particle set. Hence, using the Mahalanobis distance as a straightforward solution to the outliers issue introduces distinctive ambiguities during the particle weighting and resampling procedures. While every observation is assumed to be independent and normally distributed, Equation (22) could be rewritten as:

$$w\_k^p = w\_{k-1}^p \prod\_{i=1}^{L\_k^p} \left| 2\pi \mathbf{S}\_k^{[i]} \right|^{-1/2} \exp\left[ -\frac{1}{2} \left( \mathbf{y}\_k^{[i]} \right)^T \left( \mathbf{S}\_k^{[i]} \right)^{-1} \mathbf{y}\_k^{[i]} \right] \tag{23}$$

The magnitude of **y** [1:*L<sup>p</sup>k* ] *k*+1 depends on the deviation from the true trajectory of the camera for a given particle. Due to the nature of the computation and system parameters, every product factor of the resulting weight *w<sup>p</sup> k* is significantly smaller than one. Thus, every matched landmark 1 : *L<sup>p</sup> k* causes *w<sup>p</sup> k* to decrease. Consequently, although the number of matched landmarks is, in general, proportional to the accuracy of the trajectory estimation process, it is not unusual that, for two given particles whose poses resulted in matching a different number of landmarks, the one with more matches would have a lower importance factor. This is caused by the gating step, as the negative impact of the magnitudes of landmark-pose innovations on *w<sup>p</sup> k* is often lesser than the presence of additional product factors. A natural way to include the missed landmarks in the weight calculation would be to introduce a probability of incorrect association *Pia* into the Equation (23):

$$w\_k^p = w\_{k-1}^p (1 - P\_{\rm in})^{L\_k^p} \prod\_{i=1}^{L\_k^p} \left| 2\pi \mathbf{S}\_k^{[i]} \right|^{-1/2} \exp\left[ -\frac{1}{2} \left( \mathbf{y}\_k^{[i]} \right)^T \left( \mathbf{S}\_k^{[i]} \right)^{-1} \mathbf{y}\_k^{[i]} \right] P\_{\rm in}^{\rm L\_{max} - L\_k^p} \tag{24}$$

where *Lmax* is the maximum number of matched landmarks by any of the particles during a given time step. The tested value of *Pia* ranged between 0.2 and 0.001 [46]. However, simulations shown in the next section sugges<sup>t</sup> that this idea is insufficient to solve the beforementioned problem—the weights of particles which matched fewer landmarks were often still larger, as the sole adjustments of the *Pia* did not allow one to tune the weighting process accurately. To efficiently address this issue, we introduce the weight stratification scheme. The idea behind this approach is based on adopting the number of matched landmarks as a primary importance factor criterion. After initial weighting, samples are divided into subsets according to the number of correctly matched scene points and their weights are further adjusted. The adjustment is performed with the addition of offsets to particle weights in a way that separates each of the subsets, creating disjunctive strata of particles. The details of the stratification algorithm are described in the flowchart presented in Figure 4. The graphical interpretation of this procedure is illustrated in Figures 5 and 6. In this way, matching more landmarks by a given particle guarantees having a greater weight.

**Figure 4.** The flowchart of the weights stratification algorithm.

**Figure 5.** Weights before stratification.

The offset that separates the strata is equal to the following expression:

$$
\text{offset} = (L\_{\text{max}} - L\_k^s) \ln P\_{\text{ir}} \tag{25}
$$

where *Lsk*is the number of landmarks matched by particles in a given strata *s*.

Applying the algorithm results in the stratification of sample subsets. Of note is the fact that the procedure is performed in such a manner that particle-weight ratios, in distinct subsets, are preserved. The weights adjustment outcome is presented in the figures below.

After the stratification and normalization, the system performs the resampling procedure, though only if the efficient number of particles *Neff* , calculated using the expression below, is less than a quarter of the true number of particles.

$$N\_{eff} = \frac{1}{\sum\_{p=1}^{N} \left(w^p\right)^2} \tag{26}$$

This procedure allows one to replace samples of negligible weights with those representing the most probable state hypotheses only when significant disproportions in particle weights occur.
