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

In this section, we apply a sequential importance sampling particle filter design to our previous nonlinear estimation example. As before, an identical, randomly generated trajectory and measurement set from the MMAE example (Section 36.3.3) are used as inputs to the filter. Once again, 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. For this example, we use 10 000 two‐dimensional particles. Finally, we exercise an importance resampling procedure [6] to ensure that the number of effective particles remains acceptable.

The SIS particle filter global state estimate and density function of position after one observation (t = 1 s) are shown in Figure 36.17. In this example, we show the location of the particles along with the estimated mean and one‐sigma standard deviation calculated using the ensemble of particles. In the figures below, the estimated mean is represented as a magenta “plus,” the true state is a green asterisk, and the estimated 2‐sigma error bounds as a dashed ellipse. Each particle location is shown as a black dot.

After 22 cycles, the density shows a reduced number of peaks (see Figure 36.18) and is clearly multi‐modal. Based on our knowledge of the true density functions developed in the previous examples, 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.19), the filter has converged to a single ambiguity.

Figure 36.17 SIR particle filter initial state estimate and position density function. Note that the density function is extremely multi‐modal due to the limited information available at this point.

Figure 36.18 SIR particle filter state estimate (after 22 observations). Range observations combined with the vehicle dynamics model are eliminating unlikely integer ambiguity values.

The global state estimate and associated standard deviation result for this simulation is shown in Figure 36.20. 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.