1. INTRODUCTION
The reliable discrimination of Unexploded Ordnance by a detection system depends on the employed sensor technology as well as on the data processing methods that invert the collected data to infer the UXO. The various detection system platforms such as handheld, man-portable units, vehicle-based units, and airborne systems mostly use total field magnetometer and electromagnetic induction (EMI) sensors (Defense Science Board, 2003). These detection systems require very accurate positioning (or geolocation) and orientation in order to discriminate the candidate Unexploded Ordnance from non-hazardous clutter when inverting magnetic or EMI data. For example, Bell (Reference Bell2005) argued that centimetre-level position accuracy was required for successful detection and discrimination using EM61 Hand Held (EM61-HH) data. Billings et al. (Reference Billings, Pasion, Beran, Oldenburg, Sinex, Song and Lhomme2007) showed good discrimination performance given position errors of the order of 2 to 5 cm.
The typical requirements of position accuracy for UXO geolocation and detection are classified according to levels of accuracy (Simms and Carin, Reference Simms and Carin2004). Position tolerance of 0·5 m (standard deviation) is needed at the screening level where areas of interest are determined using airborne or ground-based sensors. Area mapping should be performed with positioning tolerance of 0·05 m (standard deviation) using man-portable and towed array systems. Position tolerance of 0·02 m or better (standard deviation) is desired for highly accurate, dense data acquisition systems that are used to interrogate, characterize, and discriminate previously located targets.
The primary geolocation system considered by several investigators is based on dual-frequency GPS integrated with a three dimensional inertial measurement unit (IMU); i.e. the IMU/GPS system. Selecting the appropriate estimation method is a key aspect in obtaining precise geolocation using the IMU/GPS systems to aid the UXO detection performance in dynamic environments. For this purpose, the Extended Kalman Filter (EKF) has been used as the conventional algorithm to optimally integrate inertial and GPS positioning data (Farrell and Barth, Reference Farrell and Barth1998; Rogers, Reference Rogers2000; Jekeli, Reference Jekeli2000; Titterton and Weston, Reference Titterton and Weston2004). However, newly introduced non-linear filters such as the Unscented Kalman Filter (UKF) and the Unscented Particle Filter (UPF) can deal better with the non-linear nature of the positioning dynamics as well as the potential non-Gaussian statistics of the instrument errors (Lee and Jekeli, Reference Lee and Jekeli2009).
The proposed integration algorithm in this study is a combination of a linear filter (KF) and a non-linear, also non-Gaussian Particle filter (PF), in the framework of the Rao-Blackwellized Particle Filter (RBPF, also called Marginalized Particle Filter). Although the RBPF was originally developed to overcome the inefficiency of the PF, it can be utilized for precise UXO geolocation because it may yield more accurate estimates of certain linear states than the PF and it provides a basic frame to apply various hybrid filtering methods (Doucet et al., Reference Doucet, de Freitas and Gordon2001; Nordlund, Reference Nordlund2002).
In this paper, a Handheld Geolocation System (HGS) using an IMU was designed to obtain precise position information in relatively small areas. The HGS is based on a tactical-grade IMU (Honeywell's HG1900) to satisfy low-cost and low-weight requirements. The test of system was conducted in a closed environment where an automatic position tracking system was designed and implemented as a substitute for GPS as external position control. This paper focuses on the new optimal estimation method (RBPF) compared to alternative methods (EKF and UPF) for precision geolocation obtained from an IMU integrated with external control and applied to typical UXO handheld system dynamics.
2. RAO-BLACKWELLIZED UNSCENTED PARTICLE FILTER (RBUPF)
The dynamics equations for the inertial system errors are a set of differential equations that model the system error behaviour with time (Jekeli, Reference Jekeli2000). The system states represent the dynamic equations' variables such as position, velocity and orientation errors, and systematic IMU errors. These system states are estimated at any time with or without external information (observations of the system).
If the process and measurement noises are assumed to be additive, the general non-linear models of the error dynamics and the measurement errors can be written as:
where k is a time index, x k and y k are, respectively, the state and measurement vectors. w k and v k are the corresponding process and measurement noise vectors with respective covariance matrices Q k and R k. The functions f k(·) and h k(·) are time-varying vector functions that define the system and measurement equations.
The Kalman filter is an optimal (in the minimum mean square error sense), linear estimation method that recursively estimates the state of the system as time progresses on the basis of available external observations (Kalman, Reference Kalman1960; Brown and Hwang Reference Brown and Hwang1992; Haykin, Reference Haykin2001). If f k(·) and h k(·) are linear in the states, equations (1) can be formulated recursively, as follows:
where F(t k+1, t k) is the state transition matrix from time k to k+1 and H k is the measurement matrix. In general, the Kalman filter based on a Bayesian setting assumes that the a posteriori density is Gaussian, so that the two parameters (mean and covariance) of the states can characterize the a posteriori density completely and can be computed with the prediction and update steps.
The particle filter, on the other hand, represents the statistical distribution of the states by using sequential importance sampling and re-sampling, requiring neither Gaussian approximation nor linearization as does the (extended) Kalman filter. In theory, any non-linear system model can be handled to any desired accuracy using a sufficient number of particles (Sarkka, Reference Sarkka2006). Van der Merwe et al. (Reference Van der Merwe, Doucet, de Freitas and Wan2000) combined the PF with the unscented Kalman filter (UKF) in order to refine the initially generated particles, and called it the unscented particle filter (UPF). Since this approach can yield a better approximation for the conditional probability density function of the states and has yielded improved accuracy for various positioning-related applications, the UPF is often employed instead of the PF. However, sampling with the PF (and UPF) in high-dimensional space can be inefficient because a large number of samples are required to represent the a posteriori statistics accurately (Doucet, Reference Doucet2008).
To overcome the computational inefficiency of the PF, the states can be divided into two subsets (linear and non-linear) and this type of filter is called the Rao-Blackwellized particle filter (RBPF) (Nordlund and Gustafsson, Reference Nordlund and Gustafsson2001). The RBPF applies the Kalman Filter to the linear and Gaussian states of the system model. The remaining state variables, which suffer from severe non-linear and/or non-Gaussian structure, are solved using the unscented particle filter. Partitioning the state vector x k into non-linear, x k1, and linear, x k2, parts the system and measurement models become:
The system noise vector is divided into w k1 and w k2 that are uncorrelated, and have known covariance matrices, Q k1=E[w k1(w k1)T] and Q k2=E[w k2(w k2)T], respectively.
To compute the a posteriori pdf of the filter, we estimate p(x k|y k)=p(x k1, x k2|y k) by factoring it into two parts according to Doucet et at. (Reference Doucet, de Freitas and Gordon2001) and Nordlund (Reference Nordlund2002),
and by first computing each pdf on the right side according to the appropriate filter methods.
2.1. Unscented Particle Filter
The second pdf on the right hand side of (4) can be rewritten using Bayes' rule:
Because of the nonlinear state and measurement equations for x k1, the unscented particle filter is applied to solve (5), as follows:
For i=1, …, N, as initialization of the unscented particle filter, we randomly generate x 01,i~p(x 01) and set the initial importance weights to ω0(i)=1/N. The p(x 01) is the prior (initial) distribution at k=0 with covariance P 01,i=E[(x 01,i−
01,i)(x 01,i−01,i)T]. The UKF was employed to update (or refine) each particle (see Appendix A) and the updated states and covariance, ○ k1,i and P k1,i, were used in the sampling step of the particle filter.
For each i=1, …, N, if the measurement is available, the update step of the particle filter is started from the conditional density, p(y k|x k1), which is based on the measurement equation h(x k1,i) and the pdf of the measurement noise v k (Simon, Reference Simon2006).
The particles will be sampled according to the density function for the transition step of the particle filter, p(x k1,i|x k−11,i) (Gorden et al., Reference Gordon, Salmond and Smith1993; Avitzour, Reference Avitzour1995; Kitagawa, Reference Kitagawa1996). For example, if we, in fact, assume an additive Gaussian noise model, the transition pdf is given by:
and the importance weights are calculated and normalized as follows:
Then, if the effective sample size is less than a pre-defined threshold, a selection step (re-sampling) is performed. In case of re-sampling, we re-generate a new set of samples, x k1,i, i=1, …, N, by multiplying samples x k1,i with importance weights 
, respectively. If a particle has a high importance weight, the particle is duplicated a number of times according to its weight. Otherwise, the particle with small importance weight is eliminated. At the end, we have the same number of particles, and then set 
for i=1, …, N. Finally, the minimum mean square (MMS) estimate of x k1 and its covariance are estimated according to:
![\hskip-6pt\eqalign{\tab \hat{x}_{k}^{\setnum{1}} \equals E\left[ {x_{k}^{\setnum{1}} } \right] \approx \sum\limits_{i \equals \setnum{1}}^{N} {\tilde{\omega }_{k}^{\lpar i\rpar } \cdot x_{k}^{\setnum{1}\comma i} } \equals {1 \over N}\sum\limits_{i \equals \setnum{1}}^{N} {x_{k}^{\setnum{1}\comma i} } \cr \tab P_{k}^{\setnum{1}} \equals E\left[ {\left( {x_{k}^{\setnum{1}} \minus \hat{x}_{k}^{\setnum{1}} } \right)^{\setnum{2}} } \right] \approx \sum\limits_{i \equals \setnum{1}}^{N} {\tilde{\omega }_{k}^{i} \left( {x_{k}^{\setnum{1}\comma i} \minus \hat{x}_{k}^{\setnum{1}} } \right)} \left( {x_{k}^{\setnum{1}\comma i} \minus \hat{x}_{k}^{\setnum{1}} } \right)^{T} \equals {1 \over N}\sum\limits_{i \equals \setnum{1}}^{N} {\left( {x_{k}^{\setnum{1}\comma i} \minus \hat{x}_{k}^{\setnum{1}} } \right)} \left( {x_{k}^{\setnum{1}\comma i} \minus \hat{x}_{k}^{\setnum{1}} } \right)^{T} \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171118-03674-mediumThumb-S0373463310000548_eqn9.jpg?pub-status=live)
2.2. Kalman filter
For each particle ○ k1,i the Kalman filter is applied to estimate ○ k2,i and P k2,i. Since we already obtained ○ k1,i, we can solve p(x k2,i|○ k1,i, y k) for i=1, …, N using the Kalman filter.
Let z k1=○ k+11−f(○ k1) and 
k=y k−h(○ k1); then the system of equations (3) is:
The optimal solution of equations (10) can be obtained by usual Kalman filter and z k1 provides the same information as ○ k+11 as far as x k2 is concerned.
The recursive estimates of p(x k2|x k1, y k) which follow the Gaussian distribution are given by the Kalman filter:
If a new measurement y k is available, the Kalman filter's measurement update step.
If we can assume that z k1 is a known input, the recursive estimate of time update is given by:
3. SMOOTHING
The filtering as described above is a recursive algorithm based on the conditional expectation of the state given all observations and states up to the current time step k. In contrast, smoothing estimates the states by using also available observations in the future. Since the RBPF consists of two filters, the Kalman Filter and the Unscented Particle Filter, the well-known Rauch-Tung-Striebel (RTS) fixed-interval smoother was applied to both the aforementioned forward filters, yielding the Rao-blackwellized Particle Smoother (RBPS).
3.1. Kalman Smoother (KS)
From given observations over the interval 0<k⩽N for fixed N, if the forward and backward estimate (○ kf and ○ kb) and their error covariances (P kf and P kb) are available, the smoothed estimate, ○ ks, and its covariance, P ks, are obtained. Since it is assumed that the process noise w k and measurement noise v k are independent, we may formulate the smoothed estimate and its a posteriori error covariance matrix as:
![\eqalign{\tab \hat{x}_{k}^{s} \equals P_{k}^{s} \left( {\left[ {P_{k}^{f} } \right]^{ \minus \setnum{1}} \hat{x}_{k}^{f} \plus \left[ {P_{k}^{b} } \right]^{ \minus \setnum{1}} \hat{x}_{k}^{b} } \right) \cr \tab P_{k}^{s} \equals \left[ {\left[ {P_{k}^{f} } \right]^{ \minus \setnum{1}} \plus \left[ {P_{k}^{b} } \right]^{ \minus \setnum{1}} } \right]^{ \minus \setnum{1}} \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000548_eqn14.gif?pub-status=live)
This smoother (three-part smoother) has three components; a forward filter, a backward filter and a separate smoother which combines results embodied in the forward and backward filters (Haykin, Reference Haykin2001). However, the Rauch-Tung-Striebel (RTS) smoother differs from this smoother in that the measurements are processed by the forward filter and then a separate backward smoothing pass is used to obtain the smoothing solution (Sarkka, Reference Sarkka2008). Also, the Rauch-Tung-Striebel smoother is more efficient than the three-part smoother in that a single entity can perform smoothing by incorporating the backward filter and separate smoother (Rauch et al., Reference Rauch, Tung and Striebel1965).
3.2. Particle Filter Smoothing
In a literature review on particle smoothing or Monte Carlo Smoothing, it is (interestingly) noted that although particle filter theory and applications are frequently treated, there are few published papers on particle filter smoothing because the smoothing algorithms such as the two-filter smoother (TFS), the forward-backward smoother (FBS), and a maximum a posteriori (MAP) smoother typically incur high computational costs, namely of the order of O(N 2), compared to the PF which has costs of the order of O(N) (Klass et al., Reference Klass, Briers, de Freitas, Doucet, Maskell and Lang2006).
The basic idea of particle smoothing is that the particle filter can provide a smoothed result automatically if the whole history of each particle of states is stored (Kitagawa, Reference Kitagawa1996). That is, from the filtered particles of the UPF, each particle is smoothed by using the unscented RTS (Sarrka, Reference Sarkka2006). Each smoothed particle can be defined as ○ ks(i), i=1, …, N (N is the number of particles) and then the mean value of the smoothed estimate can be found from a simple average (Kitagawa, Reference Kitagawa1996; Sarkka, Reference Sarkka2008):
3.3. Rao-Blackwellized Particle Smoother (RBPS)
For the smoothing version of the RBPF we can use the same method as for the UPS, which employed the unscented RTS algorithm to smooth each particle. The RTS smoother was applied to each of the mean and covariance histories of the particles ○ 1:N(i), P 1:N(i), i=1, …, N to produce the smoothed mean and covariance ○ 1:NS, P 1:NS.
4. HANDHELD GEOLOCATION SYSTEM (HGS)
The HGS developed for our tests has a tactical-grade IMU (HG1900) and is integrated with an automatic target tracking system that updates its position (Figure 1) at a specified rate. The manufacturer error specifications of the HG1900 are described in Table 1. A red circle marked on the front head of the HGS was used as a target that could be tracked for accurate positioning by using real-time colour imaging. This external tracked position served the same purpose as would GPS in the field.
Figure 1. The hardware system for locating geolocation tests (A: Handheld Geolocation System, B: Laptop, C: IMU Run-box, D: Web Cam, E: Target (red dot) with black box, F: HG1900, G: Target tracking software, and H: PCMCIA converter).
Table 1. The basic error specification of HG1900.
The HGS was tested over a small area with its position tracked using a webcam (at 5 Hz update rate). In order to obtain accurate positions of the HGS, an automatic target position extraction system was implemented using real-time colour imaging. First, the colour image is converted to a binary image which has only two values, 0 or 255, for each R, G, and B band (only the R band was employed in this test). Second, the target object in the binary image is extracted from the boundary noise using geometric circularity and knowledge of its size. Finally, the screen coordinates of (the centre of the circle) are converted to real world coordinates. (See figure 2.)
The transformed binary image is captured and displayed in the right-top section with a pre-defined but adjustable threshold value determined with a slide bar. The minimum size of the target and the circularity to differentiate the target from other objects are defined as 100 and 5, respectively. Since the actual map size (128 cm by 960 cm) will be used to convert the target position in the window coordinate system (640×480 [pixel]) to the position in the real-world system, the actual dimension of one pixel is 0·2 cm by 0·2 cm. The position of image plane of the camera (levelled using bubble levels) and the test area (laboratory floor) are assumed parallel, with the camera located 1 m above the centre of the test area An error of less than 0·5 degree due to the misalignment between the camera and the test area implies a final position error that is less than 0·87 cm (=0·5°×1 m, or about 4 pixels).
Figure 2. Target position tracking software.
5. TEST
5.1. RBPF Design
In this test, the RBPF and the previously developed filters/smoothers (KF/KS and UPF/UPS) were evaluated using the handheld geolocation system (HGS). Without loss in generality, the loosely coupled INS/GPS integration scheme based on the decentralized filter architecture was employed. The state vector for the IMU/imaging system comprised 19 states: two horizontal position errors (δx N, δx E) in the navigation frame; two velocity errors (δv N, δv E), three orientation errors in a local north-east-down frame (ψN, ψE, ψD); and, biases (b A and b G) and scale factor errors (κA and κG) for the accelerometers and gyros.
The state vector can be divided into two parts according to Nordlund (Reference Nordlund2002) and Hektor (Reference Hektor2007) who argued, based on simulations and airborne test data, that only the position states are highly-nonlinear and all other states can be assumed as linear without significant loss in position accuracy. However, this state division is incorrect in our case because not only the position error states but also the orientation and velocity error states experience high nonlinear dynamics in a ground-based UXO detection system such as the Hand-held Geolocation system. Therefore, the division of states should be separated into the navigation related states (position errors, velocity errors and orientation errors) and all other states (bias and scale factor error of the gyros and accelerometers):
![\left( {x_{k}^{\setnum{1}} } \right)^{T} \equals \left[ {\matrix{ {\delta x_{N} } \tab {\delta x_{E} } \tab {\delta v_{N} } \tab {\delta v_{E} } \tab {\psi _{N} } \tab {\psi _{E} } \tab {\psi _{D} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000548_eqn16.gif?pub-status=live){kind=small}
![\hskip-10pt\left( {x_{k}^{\setnum{2}} } \right)^{T} \equals \left[ {\matrix{ {b_{G_{N} } } \tab {b_{G_{E} } } \tab {b_{G_{D} } } \tab {b_{A_{N} } } \tab {b_{A_{E} } } \tab {b_{A_{D} } } \tab {\kappa _{G_{N} } } \tab {\kappa _{G_{E} } } \tab {\kappa _{G_{D} } } \tab {\kappa _{A_{N} } } \tab {\kappa _{A_{E} } } \tab {\kappa _{A_{D} } } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000548_eqn17.gif?pub-status=live){kind=small}
5.2. Test Description
The typical dynamics of a hand-held UXO detection platform can be classified into four categories; linear, curved, sweep, and swing (Bell and Collins, Reference Bell and Collins2007). However, since the linear and curved sections are included in the sweep and swing dynamics, only two test trajectories (sweep and swing) were considered. The position accuracy of the HGS system was tested along five sweep and five swing trajectories. The sweep trajectory had six straight lines and five curved sections. The swing trajectory had five straight lines and four curved sections. The total distance of sweep (swing) of the trajectory is about 7·2 (5·6) m and the HGS took about 22 (14) seconds to complete total trajectory. Therefore, the speed of the HGS was about 0·33 (0·4) m/s, respectively. Figure 3 shows the trajectories of the first sweep and swing of five separate tests.
Figure 3. The first sweep and swing trajectory from five tests of the designed handheld UXO Geolocation platform.
5.3. Test Result and Analysis
Various update intervals of the imaged target positions were implemented to analyze the filtered/smoothed position estimates with respect to the requirements of position accuracy for MEC geolocation and characterization. In each test case, 5 Hz, 2·5 Hz, 1 Hz, 0·5 Hz, and 0·25 Hz update rates were employed in the integration. The accuracy of the HGS was tested by comparing the estimated position to the target positions (available at 5 Hz) which were not used as updates in the filtering process. For example, if the update rate of the filter is 1 Hz, every fifth point of the automatically tracked target position is used as external control (the accuracy of measurement is assumed as 2~4 pixel (0·4 cm)2~(0·8cm)2 in north or east direction) in the filter and the other four points are compared to the estimated points of the filter/smoother. The standard deviations of the total 2-D errors (from the EKS, UPS, and RBPS) were computed for the straight and curved sections. The curved section is defined by a pre-defined threshold of absolute value of heading change angle (⩾20°).
5.3.1. Sweep Test
Figure 4 shows the average standard deviations of the error for each of the filters implemented and for each update rate of the control points. The averages represent the results of the five separate sweep tests. In the straight section, the UPS and RBPS can achieve the discrimination level of position accuracy (better than 2 cm standard deviation) for update rates ranging from 5 Hz to a 0·5 Hz, and they can achieve the area mapping level of accuracy (better than 5 cm for standard deviations) at the 0·25 Hz update rate. The EKS yields comparable (slightly worse) results at 0·5 Hz update rate and significantly worse results when the update rate is 0·25 Hz. In the curved section, every filter approached the discrimination level of accuracy (less than 2 cm std. dev.) up to 1 Hz update rate and achieved 5 cm (std. dev.) accuracy at 0·5 Hz. The position errors of the RBPF are comparable with the UPS up to 0·5 Hz but slightly lower than all other smoothers at 0·25 Hz. Therefore, the RBPS is expected to provide superior performance when the external imaging solution is blocked or degraded for longer than 4 seconds.
Figure 4. The average standard deviations of position errors according to various smoothing methods (unit: cm) for the sweep tests.
5.3.2. Swing Test
Overall, the results of the swing test were worse than those from the sweep test, especially when the update rate is less than 1 Hz. Similar to the sweep test, the non-linear filter/smoothing techniques demonstrated better performance than the EKS. However, compared to the sweep test, there was less of a difference between straight and curved sections in all estimation methods. Figure 5 shows the average standard deviations of position errors according to the various smoothing methods and the update interval between control points. The position accuracy of the swing test was degraded compared to the sweep test, especially when the update rate is less than 1 Hz in both straight and curved sections. However, it is noted that the position error of the UPS and RBPS is significantly smaller at 0·25 Hz when compared to the EKS in the straight and curved sections.
Figure 5. The average standard deviations of position errors according to various smoothing methods (unit: cm) for the swing tests.
In the straight section, the UPS and RBPS attained the discrimination level of position accuracy up to the 1 Hz update rate and achieved the area mapping level of accuracy at 0·5 Hz update rate (compared to the sweep test where the update rates for the discrimination and area mapping level of accuracy were 0·5 Hz and 0·25 Hz, respectively). In the curved section, similar to the straight section, the UPS and RBPS achieved the discrimination level up to 1 Hz and the area mapping level up to 0·5 Hz.
In this swing test, the RBPS performs best at all update rates. Therefore, although the swing operation can obtain position data for the UXO detection in shorter time duration and therefore with fewer control points, the sweep operation is preferred because the position errors of the sweep test were smaller than that of the swing test due to the linear dynamics of the sweep motion.
The UPS needs at least 200 particles to yield optimal position accuracy in this test. However, the RBPS utilized only 20 samples (particles) for the nonlinear (particle) filter part. Although there is still room for some position accuracy improvement by increasing the number of particles, it will not yield significant improvements (Lee and Jekeli, Reference Lee and Jekeli2009). Therefore, the RBPS can produce (slightly) better or comparable results compared to the UPS with only 10% of the number of particles used by the UPS.
6. CONCLUSION
A Handheld Geolocation System (HGS) using a tactical-grade IMU was designed to obtain precise positions of a UXO detection system applied in relatively small areas (1·0 m by 1·0 m). Since the test is operated in a closed environment that has no GPS signal available, an automatic position tracking system was designed and implemented. This could be comparable to GPS if we can set the GPS antenna on top of the handheld system.
Generally, the UPF performs best among all other (linear and nonlinear) filters. However since the UPF needs a large number of samples to represent the a posteriori statistics in high-dimensional space, the Rao-Blackwellized Particle Filter (RBPF) can be used as an alternative to increase the efficiency of the particle filter. The nonlinear smoothers that are based on forward/backward filtering techniques (EKS, UPS, and RBPS) were tested and analyzed for two typical local handheld detection platform trajectories (sweep and swing).
On the whole, position accuracy improvements were achieved by applying nonlinear filter-based smoothing techniques (UPS and RBPS) in both the straight and curved section. The handheld geolocation system with a nonlinear filter-based smoother achieved the characterization and discrimination level of accuracy if the update rate of control points is less than 0·5 Hz and 1 Hz for the sweep and swing modes respectively. Therefore, although the data collection using the swing operation can be done in shorter time, the sweep operation is generally better than the swing test because the dynamics of swing operation is too highly nonlinear.
ACKNOWLEDGEMENTS
This study was supported by SERDP project MM-1565, contracts W912HQ-07-P-0013 and W912HQ-08-C-0044.
7. APPENDIX A UNSCENTED KALMAN FILTER (UKF) UPDATE FOR THE PARTICLE FILTER (PF)
For each particle, the UKF sigma points (A.1) with covariance (A.2) and the weights (used later for the prediction and update steps) (A.3) are calculated:
![\chi _{k \minus \setnum{1}}^{\setnum{1}\comma i} \equals \left[\bar{x}_{k \minus \setnum{1}}^{\setnum{1}\comma i} \hskip6pt\bar{x}_{k \minus \setnum{1}}^{\setnum{1}\comma i} \pm \alpha \sqrt {\left( {n_{x} \plus \kappa } \right)P_{k \minus \setnum{1}}^{\setnum{1}\comma i} } \right]\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000548_eqnA1.gif?pub-status=live)
![P_{k \minus \setnum{1}}^{\setnum{1}\comma i} \equals E\left[ \left( {x_{k \minus \setnum{1}}^{\setnum{1}\comma i} \minus \bar{x}_{k \minus \setnum{1}}^{\setnum{1}\comma i} } \right)\left( {x_{k \minus \setnum{1}}^{\setnum{1}\comma i} \minus \bar{x}_{k \minus \setnum{1}}^{\setnum{1}\comma i} } \right)^{T} \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000548_eqnA2.gif?pub-status=live){kind=small}
where and P x1 are the mean and covariance of the n x dimensional random variable x. α and κ are scaling parameters. The parameter α(10−4⩽α⩽1) controls the spread of the sigma points around . The parameter β is used to increase the accuracy of the higher-order moments (β=2 is optimal for Gaussian variables).
Second, the particles are propagated (A.4) and updated (A.5) using UKF:
where χk1,−,i and ○ k1,−,i are, respectively, sigma points propagated through the system equations and the states predicted from the sigma points. Yk−,i and ŷk−,i are updated sigma points and the states by measurement, y k, respectively.
The particles and their covariances are updated according to:
