Position, Navigation, and Timing Technologies in the 21st Century

Substituting Eq. 36.81into the Chapman–Kolmogorov equation ( Eq. 36.12) and simplifying:

(36.82)

(36.83)

(36.84)

(36.85)

The rightmost density in Eq. 36.85is the transition probability function, which can be rewritten as

(36.86)

Substituting Eq. 36.86into Eq. 36.85and simplifying

(36.87)

(36.88)

(36.89)

where the new particle weight is given by

(36.90)

Conceptually, this can be calculated as the sum of all posterior weights at time k – 1 multiplied by the specific transition probability into state l from all possible prior states.

The development of the measurement update function proceeds in a similar fashion. Recalling our definition of the a priori density function (repeated from Eq. 36.89for clarity):

(36.91)

and substituting into the update equation ( Eq. 36.16) yields

(36.92)

which can be simplified by changing the order of integration and using the properties of the delta function

(36.93)

(36.94)

(36.95)

Finally, recalling the grid particle filter form of the posterior density function

(36.96)

and substituting into Eq. 36.95yields

(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.

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.