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

Finally, the collection of particle weights can be normalized via

(36.110)

then Eq. 36.109becomes

(36.111)

which we will exploit to develop a recursive estimator.

In this section, we leverage the previously presented concept of importance sampling to derive the basis for a recursive nonlinear estimator using Monte Carlo integration [4]. This type of filter is generally referred to as a recursive particle filter.

Consider the following general system model:

(36.112)

(36.113)

where x kis the state vector at time k , f(·, ·) is the process model function at time k – 1, w k − 1is the process noise vector, h(·, ·) is the observation function, and v kis the measurement noise vector at time k . The noise vectors are assumed to be independent of each other and in time with a known density function. Note that Gaussian densities are not required or assumed.

Assuming we begin with a known posterior density, p ( x k − 1| ℤ k − 1). If N samples are drawn from an associated proposal density,

(36.114)

With normalized weights given by

(36.115)

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 ( x k| ℤ k), by incorporating the statistical process model and the observation at time k . The density function of interest can be written as

(36.118)

(36.119)

Assuming our proposal density can be factored:

(36.120)

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:

(36.122)

Substituting Eqs. 36.119and 36.120into Eq. 36.122yields

(36.123)

Note that this equation is a function of the posterior weights at time k – 1; thus, the right‐hand fraction of Eq. 36.123can 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.

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.