(36.97)
The bracketed areas show the final particle weight update equations
(36.98)
In the next section, we illustrate a potential application of the grid particle filter in a navigation context.
36.3.6 Grid Particle Filter Example Application
We return to the example presented in Section 36.3.3; however, in this case, we utilize a grid particle filter solution. The first step in the process is to determine the composition of the grid. In this case, there are two parameters we would like to estimate, position and velocity. Both of the parameters are continuous random variables, so we must quantize both of the parameters.
For this example, we are interested in centimeter‐level positioning accuracy; thus, we divide the domain into 5 mm by 20 mm/s grids. For simplicity, we build a grid that is ±2 m in range and ±0.6 m/s in velocity. The absolute grid location is periodically adjusted based on the current estimated position and velocity of the vehicle.
An identical randomly generated trajectory and measurement set from the MMAE example (Section 36.3.3) is used as the inputs to the grid particle filter. For reference, the system parameters are specified in Table 36.1, and the resulting trajectory, range observations, and phase observations are shown in Figure 36.3.
Figure 36.12 Grid particle filter state estimate and position density function after one observation. Note the density function is extremely multi‐modal due to the limited information available at this point.
The grid particle filter global state estimate and density function of position after one observation (t = 1 s) are shown in Figure 36.12. In this case, we present the probability density function using a two‐dimensional array (position vs. velocity) of probabilities. The resulting pdf is clearly multi‐modal, which accurately represents the range of solutions associated with the phase observation. As expected, the peaks are located as a function of the wavelength and represent the most likely values of integer ambiguity. These peaks indirectly indicate the relative likelihood of the associated ambiguity being correct by exhibiting influence on the overall position density. In each plot below, the calculated mean is represented by a white “plus,” the true state is represented by a green asterisk, and the calculated 2‐sigma uncertainty is represented as a white ellipse.
Figure 36.13 Grid particle filter state estimate (after 22 observations). Range observations combined with the vehicle dynamics model are eliminating unlikely integer ambiguity values.
Figure 36.14 Grid particle filter state estimate (after 100 observations). Note that the state estimate is almost completely unimodal and has converged to the correct integer ambiguity.
After 22 cycles, the density shows a reduced number of peaks (see Figure 36.13). This indicates that the filter is incorporating sensor observations and the statistical dynamics model to effectively eliminate a number of potential ambiguity possibilities.
After 100 cycles (Figure 36.14), the filter has converged to a single ambiguity.
The global state estimate and associated standard deviation result for this simulation are shown in Figure 36.15. The shape of the uncertainty bound clearly shows the effects described above. As the likelihood of each integer ambiguity realization changes, the overall uncertainty changes and eventually collapses to the centimeter level.
In the next section, we will move to our final nonlinear filter algorithm, the sampling particle filter.
36.3.7 Sampling Particle Filter (SIS/SIR)
In a similar manner to the grid particle filter, the sampling particle filter, also known as a the sequential Monte Carlo (SMC) filter, represents the state density function using a weighted collection of particles. However, we seek to address the computational scaling problems inherent in grid‐based approaches by exploiting an approach that focuses computation on the regions of the state space with the highest likelihood. This is accomplished by randomly sampling the state space.
Figure 36.15 Grid particle filter position error and one‐sigma uncertainty. Note that the error uncertainty collapses once sufficient information is available to resolve the integer ambiguity.
The main advantage of this approach is the potential to more completely sample the important areas of the state space, while limiting the total number of particles required. This is a useful advantage over the grid particle filter, which can require unreasonable numbers of particles as the state dimensionality and domain increase. While sampling particle filtering approaches are suboptimal, their computational advantages make them attractive for a larger range of applications.
We begin by describing the concept of Monte Carlo integration, which is subsequently used to develop a basic recursive estimation algorithm.
The fundamental enabling concept for the sampling particle filter is the concept of Monte Carlo integration. Given an integral in the following form:
(36.99)
where Ω is an nx‐dimensional region in
(36.100)
If N independent samples are uniformly drawn from Ω, that is, {x[1], x[2], ⋯, x[N]} ∈ Ω, then the integral can be approximated as
(36.101)
which approaches equality