Thus, the associated particle weights at time k can be calculated in a similar fashion as Eq. 36.108:
Substituting Eqs. 36.119 and 36.120 into Eq. 36.122 yields
Note that this equation is a function of the posterior weights at time k – 1; thus, the right‐hand fraction of Eq. 36.123 can be replaced according to Eq. 36.108, which yields the final particle weight update equation from time k – 1 to time k:
(36.124)
which can be normalized such that the collection of weights sums to one, thus approximating the posterior density as
(36.125)
In this manner, the particle locations and weights can be continuously maintained and updated using a recursive estimation framework.
36.3.9 Sampling Particle Filter Demo
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.
36.3.10 Strengths and Weaknesses of Approaches
In this chapter, we have presented three classes of nonlinear recursive estimation algorithms. While each algorithm offers improved performance over the linear and extended Kalman filter in the presence of nonlinearities and non‐Gaussian systems, it is important to address the “strengths and weaknesses” of each. To accomplish this, we evaluate each estimation from this perspective, starting with the traditional approaches.
Figure 36.19 SIR particle filter state estimate (after 100 observations). Note that the state estimate is almost completely unimodal and has converged to the correct integer ambiguity.
Figure 36.20 SIR particle filter position error and one‐sigma uncertainty. Note that the error uncertainty collapses once sufficient information is available to resolve the integer ambiguity.
As expected, each approach has a set of associated strengths and weaknesses that can greatly influence the results for a given problem. Thus, the choice of estimator must be considered carefully based on the characteristics of the problem at hand. In cases where the constraints of the problem do not readily fit into the generalized categories above, there are many examples of hybrid estimation schemes that seek to synergistically combine the desirable properties of multiple estimator types. While it is beyond the scope of this chapter to explore the range of hybrid filtering approaches, the interested reader is referred to the references (e.g. [4, 5, 6, 9, 10]) for foundational concepts.
36.4 Summary and Conclusions
In this chapter, we have presented an overview of nonlinear estimation approaches suitable for navigation problems. Starting with first principles, three classes of nonlinear, recursive estimators were derived, the performance was demonstrated using a common navigation example application, and comparisons were made between the approaches.
The