NAVIGATION comprises of different methodologies to estimate the time varying position and attitude of moving objects by measurements such as Inertial Navigation systems (INS) and Global Positioning System (GPS) (Aggarwal, 2007). GPS provides long term accurate measurements and is highly portable, has low power consumption, and hence is well suited for integration with other sensors. However, GPS does not work in all environments, and provides navigation data only when a direct line of sight between four or more satellites and the receiver antennas exists. This is not always possible for example when vehicle is going through a tunnel or under trees, bridges etc. In contrast, Inertial Navigation Systems (INS) are selfcontained units consisting of gyroscopes and accelerometers and do not require any external signals. Hence INS is capable of working in all environments where GPS has difficulties. It is very useful to bridge GPS outages and provides high frequency data during these periods. However, the accuracy of INS deteriorates in the long-term due to the combined effects of errors like noises, biases, drifts and scale factor instabilities (Schwarz and Wei, 1995). In order to combine the advantages of the two technologies, an integrated INS/ GPS system are used. Accurate positioning information from GPS is typically used to provide frequent updates to the INS to correct its errors; while the GPS signal outages will be compensated by using the short term accuracy of the INS derived position and velocity.

Earlier usage of these integrated sensors were restricted to military and aerospace areas because of high manufacturing cost of INS, but with the development of low cost system, such as Micro Electro Mechanical Systems (MEMS), the possibility of using inertial sensors for automobile industry has been generated. MEMS has enabled the sensor technology to evolve from discrete, expensive and inflexible units to smart, self-calibrating, silicon-based devices which are integrated, low-cost and small. MEMS is an enabling technology with a massive global market, predicted to be at 180 billion US $ in 2007. The MEMS sensors are used for measuring angular velocity and acceleration in these navigation systems but the errors in these sensors are more than that of the higher grade sensors due to their miniature size, and light weight. For MEMS sensors, the errors like the nonlinearity of bias or scale factor with respect to temperature become huge and therefore cannot be neglected, unlike navigation grade sensor errors.

For GPS/INS integration, estimation techniques, such as EKF and UKF, are commonly used for the system error estimation and compensation. In EKF, Taylor series expansion is applied to the nonlinear system and measurement equations to obtain first order linear system (Grewal, 2001). The EKF assumes that the posterior probability density functions along with system and measurement noises are Gaussian distribution. Moreover, the derivation of the Jacobian matrices, i.e., the linear approximations to the nonlinear functions, can be very complex and lead to implementation difficulties (Julier, 2000). Also, only small errors are allowed to be delivered to the EKF and hence the first order approximations can cause inconsistency of the covariance update and lead to filter instability in the presence of higher order effects (Lerro, 1995).

On the other hand, the UKF was proposed in the GPS/INS navigation community to overcome the limitations of EKF instabilities and Jacobians matrices evaluation. UKF is based on the principle that it is easier to approximate a Gaussian distribution than to approximate an arbitrary nonlinear function (Julier, 2000). UKF deterministically samples fixed number of minimal points, named sigma points, from the Gaussian distribution, which are used to estimate the true mean and covariance of gaussian distribution. These sigma points are then individually propagated through the true nonlinear models, to capture posterior mean and covariance more accurately. However, when the non-linearity is pronounced, even the best-fitting Gaussian distribution will be a poor approximation to the posterior distribution (Doucet, 2000).

Therefore use of an Extended Particle Filter (EPF) to approximate the posterior distribution in cases of highly nonlinear integrated navigation systems is proposed here. Particle Filters (PFs) [Maskell, 2001; Gordon, 1993] have a number of characteristics that make them attractive for navigation applications. They are nonparametric, can efficiently deal with non linearities and non Gaussian noises, and are relatively easy to implement. It is proposed here to use an EPF for estimating the state vector with the aim of obtaining more accurate results and efficiently dealing with high non-linearities or non- Gaussian noises. Furthermore, in the PF implementation, there are no assumptions necessary about the form of the posterior distributions. PFs give an approximate solution to an exact model, rather than the optimal solution to an approximate model unlike KFs (Gustafsson, 2002).

Particle Filter constructs a point mass representation of a state vector by large number of random samples, called particles that explore the state space. Basic Particle filters are based on the sequential Monte Carlo estimation (SMC) method, which is integration of Monte Carlo and Importance Sampling methods (Arulampalam and el, 2002). The MC method (Doucet, 2000) states that if we can draw out independent, random and equally weighted samples N from the required posterior distribution, then the required probability density function is approximated by the average or collection of these samples or particles. Monte Carlo method assigns equal weights to these generated particles. The PF is used to update the weights of these particles when measurements become available. The location and weight of each particle reflect the value of the posterior density in the region of the state space. However, it is not always possible to draw out independent and uniformly distributed samples from the required density function as it might be multivariate, nonstandard or multimodal. In these cases, the Importance Sampling method is used. Importance Sampling method (Haug, 2005) states that if it is difficult to obtain a sample directly from the required distribution, then samples can be generated from importance or proposed density function, which is similar to the desired distribution. The weights of these generated samples are then corrected to represent the target function as closely as possible. The most popular and easy option is to take the transition prior as the proposal density function. This proposal distribution simplifies the particles weight calculations greatly. The basic Particle filters namely Sequential Importance Sampling (SIS) and Sequential Importance Re-Sampling (SIR) Filters assume the transition prior density as the importance proposal distribution.

The pseudo code for Sequential Importance Sampling (SIS)/Re-Sampling (SIR) Particle Filter is summarized below:

Sequential Importance Sampling/ Resampling (SIS/R) Particle Filter

• Generate independent and identically distributed (i.i.d.) N samples from the previous posterior density function p(x0) according to Monte Carlo principle.
• On receiving the measurements yk, update weights of these generated according to eq 1. (a) Updating weight of each particle i=1.........N
  
  As the importance density function is taken to be equal to the prior density function p(xk|xk–1), the above weight
  update equation simplifies to
  
  i.e. weights of the particles are proportional to the likelihood function.
  (b) Normalized the weights of the particles
  
  However this simple implementation of the SIS particle filter causes particle dispersion because the variance of the particles increases without bound with time. After few iterations, all but one particle has negligible weight. This increase in variance of the particles causes dispersion phenomenon and is called degeneracy of the particle filter. In order to reduce this problem, re-sampling step is incorporated into the above SIS particle filter, leading to Sequential Importance Re- Sampling particle filter (SIR) formation. Re-sampling eliminates the particles with less weight and multiples the particles with higher weights. However resampling at every time step can easily lead to sample impoverishment as similar particles will be repeated number of times. Therefore avoid this additional problem, resampling is only done when degeneracy of the filter is high. To measure this degeneracy of the PF, effective sample size Neff is calculated along with threshold value Nth as stated in Eq. 3. Only if effective sample size is less than the threshold value, re-sampling is performed.
• Compute the effective and threshold weights according to below eq
  
  where N_eff is the computed effective weights and Nth is the threshold value.
• If N_eff > N_th, particles remain as such, else resample particles and assign equal weights to them.
  
• Once resampling is done, time epoch is incremented, new particles are predicted and steps from 2 are repeated.
  
• This process is repeated till end of the test trajectory is reached.

As stated before, re-sampling step decreases degeneracy issue but causes sample impoverishment (Doucet, 2000). Moreover large numbers of particles are required to obtain high level of accuracy of the state vector estimation. In order to overcome the limitations of large number of particles required, different methods were developed to optimize proposal distribution that approximates the importance distribution. One such method is to incorporate the most current observation with the approximation of the state (Merwe, 2000) through local linearization. Usages of this principle lead to the developed of Extended Particle Filter (EPF).

In case of SIS/SIR filters the latest observation is not considered to evaluate the weights of the particles as the importance function is taken to be equal to the prior density function. This choice of Importance Sampling function simplifies the computation but can cause filter divergence. In cases where the likelihood function is too narrow as compared to the prior function, very few particles will have significant weights which might lead to divergence. Hence a better proposal distribution is desired that takes the latest observation into account. Kalman filters like EKF or UKF incorporate the latest observation in the update state. Hence if EKF/UKF is used to generate the proposal distribution, the latest observation can be included. Therefore the incorporation of the most current observation with the approximation of the state is through local linearization of the state vector by EKF/UKF ( Merwe, 2000). This hybrid filter is called the Extended Particle Filter (EPF) where the proposal distribution is generated by the EKF. For our application UKF and EKF gave similar performance, so in order to simply EPF implementation EKF is used instead of UKF. In fact, a separate EKF is used to generate and propagate Gaussian proposal distribution for each individual particle i.e.

where x⁽ⁱ⁾ _k and p⁽ⁱ⁾ _k are the mean and covariance of the ‘ith’ particle generated by the EKF. Once all the particles have been updated by the EKF, sample particles are selected from the proposal distribution and the above process of SIR filter is repeated. The complete algorithm for the EPF is given below