Читать книгу Position, Navigation, and Timing Technologies in the 21st Century - Группа авторов - Страница 30

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.