Mems Ins/gps Data Fusion Using Particle Filter

Coupling GPS with Micro-Electro-Mechanical Systems (MEMS) Inertial Navigation Systems (INS) is an challenging way of improving land vehicle navigation performance. MEMS inertial sensors suffer from complex stochastic errors, which are difficult to compensate and model using conventional Kalman Filter, as it provides an effective solution to the linear Gaussian filtering problem. However where there is nonlinearity, either in the model specification or observation process, other method are required. Particle filtering techniques are good candidates to solve the corresponding nonlinear estimation problem associated to MEMS INS/GPS hybridization. Here nonlinear models for accelerometer were successfully implemented using particle filter (PF) . The performance of the resulting algorithm was illustrated through experimental results. Also the position estimation results using PF during GPS signal outages were presented. Ill. 4, bibl. 5 (in English; abstracts in English and Lithuanian). http://dx.doi.org/10.5755/j01.eee.112.6.450


Introduction
Targeting low cost solution for land vehicles, Micro-Electro-Mechanical Systems (MEMS) based inertial sensors are used.These sensors suffer from complex stochastic error characteristics that are difficult to model.Kalman filter (KF) has limited capabilities in providing accurate estimation of such system parameters, because KF is restricted to use only Gaussian linear models for these sensors' stochastic errors.EKF makes linearization of non-linear model, and after solve problem optimally by KF.This first order linearization in EKF introduces additional errors and difficulty in estimating process [1,2].It is becoming important to include elements of nonlinearity in order to model accurately the underlying dynamics of inertial system.To solve the problem of nonlinear filtering, the particle filter (PF) was proposed.It was first introduced by Gordon et al. (1993).PF exploits numerical representation techniques for approximating the filtering probability density function (PDF) of inherently nonlinear non-Gaussian systems.Using these methods, the obtained estimates can be set arbitrarily close to the optimal solution (in the Bayesian sense) at the expense of computational complexity [2,3].

Integration strategy and sensor error characteristics
In this paper total state system and measurement model and loosely coupled integration is used for GPS and IMU systems.The position from GPS navigation solution is input as a measurement to the integration PF filter.In a loosely coupled architecture, there is a standalone GPS navigation solution available in addition to the integrated solution.
MEMS sensors are cost effective with small dimensions, but its performance is seriously affected by errors such as biases, scale factor variations, drifts and sensor noise.
The measurement γ provided by accelerometer may be expressed in terms of the applied acceleration, acting along its sensitive axis (a x ), bias (b) and scale factor (S) by the equation [3] here b const and b random are deterministic and random part of accelerometer bias (m/s 2 ), S const and S random are deterministic and random part of accelerometer scale factor, η-accelerometer sensor noise .Ideally, when no input is applied to the sensor, the output signal received from the sensor should read 0. However, this is not the case, and an offset called the bias exists.An uncompensated accelerometer bias introduces an error proportional to time (t) in the velocity and proportional to t 2 in the position.Moreover every time the sensor switched on, a slightly different bias or scale factor value is observed for low-cost MEMS sensors.As these errors cannot be easily separated from the sensor data, these are generally modelled as stochastic processes.The stochastic model for predicting accelerometer bias was developed basing on 1 st order Gauss-Markov model.Proposed sensor bias error model is defined by the following stochastic difference equation , 1 here β=[0.9…1] is user defined value for predicted bias mean value control , w k -samples from white noise process.This difference equation was used at prediction step for PF.

System and measurement model for GPS/IMU system
A state propagation model is used for a Particle Filter to predict acceleration, velocity and position referenced in the body frame, accelerometer bias and scale factor The 1 D acceleration dynamic model is used [4] with the state vector x k , which comprises 5 states here f(.) represent system linear model, w-process noise which gathers any mismodeling effect or disturbances in the state characterization.Subscript k denotes the discrete time index here d x -position of the vehicle along x axis in the body frame, v x -velocity, a x -acceleration, b -accelerometer bias, S -accelerometer scale factor.The relation between the measurements z k and the states is modelled by following nonlinear function here h(. ) is a known non-linear function and u is measurement noise here γ -accelerometer output signal , d x_GPS -estimation of vehicle position along x axis in the body frame based on GPS measurements.The accelerometer output signal is modelled by nonlinear measurement equation (1).Both process and measurement noise are assumed white, with known statistics and mutually independent.The initial a priori PDF p(x 0 ) of the state vector is assumed to be known.

Particle filter algorithm formulation
Particle filters (PF) are capable of handling highly nonlinear models with any kind of noise distribution.The particle filter is a special version of the Bayesian filter, and is based on sequential Monte Carlo (SMC) sampling.
The goal is to compute filtered estimates of x k taking into account all available measurement up to time k, z 1:k .From a Bayesian point of view, the solution is to recursively obtain the a posterior probability density function p(x 0:k |z 1:k ) of states at time k given all available measurements.The PDF are constantly changing shape when receiving indirect measurements.PF represents the posterior probability density function p(x 0: k | z 1:k ) by a set of random samples with associated weight as follows As the number of samples N becomes very large, this Monte Carlo (MC) characterization becomes an equivalent representation to the usual functional description of the posterior PDF under weak assumptions, according to the Strong Law of Large Numbers [3 ].
For the MC method, independent particles are required from posterior density function.In general, it is not possible to draw samples directly from posterior p(x 0: k | z 1:k ) .Instead, the samples are drawn from a simpler distribution called the importance PDF q(x 0: k |z 1:k ).
Importance density function is similar to desired posterior distribution, but the weights of these generated samples need to be adjusted to represent the target function as closely as possible.[3].If the samples i k : 0 x were drawn from an importance density q(x 0:k |z 1:k ) then the weights in (7) are defined by (9) here q(x 0:k |z 1:k ) -importance density function.
Taking in account that conditional probability distribution of future states, given the present state and all past states, depends only upon the present state and not on any past states and the measurements are assumed to be conditionally independent given the states, the following recursive general equation for weights updating can be obtained [5] , -transition prior distribution.It's necessary to make normalisation of weights in (10), in order sum of all weights will be equal to 1 The choice of importance density function  is one of the most critical issues in the design of a particle filter.The reason of this is that the samples are drawn from the proposed distribution, and the proposed distribution is used to evaluate importantance weights.The most popular suboptimal choice is to use the conditional prior of the state vector as the proposed distribution for importance density function Substituting ( 12) in (10) the weight's update equation is -likelihood function.In this case, the weights depend only on the likelihood function.
The particle filter has a problem that it degenerates quickly over time.In practical terms this means that after a certain number of recursive steps, most particles will have negligible.Degeneracy can be reduced by using a resampling step [5].
Resampling is a scheme to eliminate particles with small weights and to concentrate and replace on particles with large weights.In this paper systematic resample is used.Systematic resampling [5] is preferred resample scheme since it is simple to implement, has computational complexity O (N) and minimizes the variance of importance weights variation.Although resampling helps to overcome the degeneracy phenomenon, it unfortunately introduces other problems.Resampling causes the samples that have high importance weights to be statistically selected many times, thus the algorithm suffers from the loss of diversity .This occurs especially in the case of small process noise since the particles at the same place cannot separate enough.It is also referred as sample impoverishment.
It is also mentioned in [5] that using the particle filter is not appropriate when the process noise is zero.In order to reduce effect of sample impoverishment, additional noise is added in the state model.The summary of implemented particle algorithm for navigation data fusion is presented below: 1) Initialisation.The particle filter is initialized by drawing samples i 0 x , i=1,….,N from the prior density function p(x 0 ) and set weights i w 0 to 1/N, N-is the number of particles.
2) Prediction.Particles at time step k-1 i k 1  x are passed through system model ( 3) to obtain the predicted particles at time step k. 3) Update.In the measurement update the new measurements are used to assign a probability, represented by the normalized importance weight , to each particle.The weights are calculated using (11) and (13).The normalized importance weights and the corresponding particles constitute an approximation of posterior distribution p(x 0: k | z 1:k ).4) Systematic resampling.The resampling step will then return particles which are equally probable.Move to stage 2.

Implementation and Results
The MTi-G -a GPS aided MEMS based inertial measurement unit was used in experiments.The MTi-G was rigidly installed inside moving vehicle.The separated raw data from accelerometer and GPS unit was used for combined processing (fusion).As the result of sensor data processing by PF, we obtain estimations of vehicle acceleration, velocity and position along x-axis in the body frame.Also some extra parameters such as bias and scale factor of accelerometer can be estimated.The algorithm is developed in such way, that during GPS outages it continues to estimate position and velocity of moving vehicle.This was implemented by estimating of two set of normalized importance weights according (13), basing on GPS measurements and IMU (accelerometer) measurements.In case of lack of GPS signal, algorithm uses set of normalized importance weights based on accelerometer measurements for system state estimation.The nonlinear measurement model h(x k , u) is directly used in PF, what is impossible for Kalman filter, as measurement model for KF should be linear ( that is not always the case for real systems) or linearisated.Also particles from uniform PDFs' for bias and scale factor were used for PF initialisation.
The evolution of approximated posterior probability density function according to (7) at different time steps is shown in the Fig. 1 The numbers of particles N=100.From Fig. 1 it can be noticed, that approximated posterior PDF has not Gaussian form.
-0.64 -0.63 -0.62 -0.61 -0.6 -0.59 -0.58 -0.57-0.The estimation of acceleration signal, accelerometer bias and scale factor using PF is shown in the Fig. 2.  The result of sensor data processing using PF during GPS signal outages is represented in the Fig. 4. The calculated position estimation drift during 20 s of GPS signal outage is 12 m.PF immediately correct position of the vehicle, when GPS measurements become available (enlarged fragment of Fig. 4).

Conclusions
Presented PF algorithm for accelerometer and GPS data fusion has following advantages over KF: -no need for nonlinear measurement model linearization, this is especially attractive for real low cost navigation system parameters estimation.
-no restriction for using non Gaussian PDF for estimating system states, -implementing and debugging the PF is not a major issue comparing with EKF.Also, presented fusion algorithm continues to have reliable navigation solution during small GPS signal outages.
7) here δ(x) is Dirac delta function, i k w is associated weight with i k x , N is the particle number, z 1:k denotes the measurements accumulated up to k.The weights' values are always positive i k w >0, and sum of weights is equal to 1

Fig. 2 . 3 .Fig. 3 .
Fig. 2. IMU state parameters estimation using navigation data fusion with PFPosition estimation along x-axis (direction of the vehicle movement) in the body frame versus time t is shown in the Fig.3.This estimation was made using combined processing of IMU and GPS data with PF .Also, only GPS based estimation of position is presented in the Fig.3.

Fig. 4 .
Fig. 4. PF result of position estimation with 20 s GPS signal outage