target="_blank" rel="nofollow" href="#fb3_img_img_c8aeeb21-0be4-5269-b7ae-797d64e4a74a.png" alt="equation"/>
where κ is the normalization factor required such that the sum of weights is unity, the posterior density function is expressed by the collection of particles and weights
(36.116)
or, equivalently
(36.117)
Our goal is to estimate the posterior density at time k, p(xk| ℤk), by incorporating the statistical process model and the observation at time k. The density function of interest can be written as
(36.118)
Assuming our proposal density can be factored:
the posterior particle locations can be sampled from
(36.121)
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.
