1. INTRODUCTION
The integration of an inertial navigation system (INS) and the global positioning system (GPS) can provide overall improved navigation performance due to their complementary characteristics. With the emergence of the micro-electromechanical system (MEMS) based low-cost strapdown inertial measurement unit (IMU), cost-effective INS/GPS integrated navigation systems become a reality (EI-Sheimy and Niu, Reference EI-Sheimy and Niu2007). The INS/GPS integration is often referred to the data fusion of measurements obtained from a GPS receiver antenna and an INS and formulated as a state space estimation task where the Kalman filter (KF) or a modified version of it is applied to obtain the (sub)-optimal solutions. Nevertheless, the multi-antenna GPS system using double difference carrier phase measurements is highly recommended for the integration due to its redundant attitude information derived from the GPS side (e.g., Knedlik et al. (Reference Knedlik, Zhou and Loffeld2009)). The INS/GPS integration can be briefly classified as loosely-coupled, tightly-coupled and deeply-coupled. For the loosely-coupled integration, the implementation is simpler with a lower real-time computational burden. However, in that case, one local KF is used for GPS and one KF for integration, so cascaded filtering problems may occur which must be carefully accounted for (e.g., Groves, Reference Groves2008). Besides, when less than four satellites are tracked, the INS predicted estimates cannot be updated from the available GPS measurements. These problems can be easily solved in the tightly-coupled integration because in such a more centralized Kalman filtering scheme, the GPS pseudorange and delta pseudorange (or Doppler) measurements are used as observations to update the navigation filter. The INS estimated solution can be continuously updated by the measurements from the remaining satellites, even if the number of the tracked satellites drops below four. However, the system observation model turns out to be non-linear. As the core of the integrated navigation system, this non-linearity should be carefully treated in the design of the integration KF. Deeply-coupled integration requires access to the GPS receiver tracking loops, which is not given for the majority of commercial GPS receivers. Thus, it is not in the scope of this paper.
Regarding the INS/GPS tightly-coupled integration, the EKF is considered as state-of-the-art for fusing the INS and GPS data. The principle of the EKF is to linearize the non-linear system and observation models around the updated dynamic trajectory using a Taylor series expansion. It relies on the first order linearization of the non-linear models with the assumption of Gaussian noise. In certain circumstances, the EKF may not be able to present reliable system estimation solutions. The UKF was first proposed by Julier et al. (Reference Julier, Uhlmann and Durrant-Whyte1995) as another non-linear filter, which propagates a fixed number of sigma points through the non-linear system and observation models to capture the system a posteriori mean and covariance estimates. Its estimation accuracy can reach to the third order of the Taylor series expansion. Nevertheless, the algorithm still treats the noise as Gaussian distributed. The PF neither makes assumptions on the linearity of the system models, nor on the characteristics of the noise. Therefore, it is considered as a non-parametric estimation method for non-linear/non-Gaussian applications. With the recent advances in computer technology, it turns out to be more attractive for navigation applications (Gordon et al., Reference Gordon, Salmond and Smith1993). However, its large computational burden is still its main obstacle in practical use. Moreover, the system performance degeneracy and sample impoverishments (Arulampalam et al., Reference Arulampalam, Maskell, Gordon and Clapp2002) are other main implementation issues of the basic SISR PF algorithm. To overcome these problems, some engineers propose to combine the PF with the EKF or UKF algorithm to form the extended particle filter (EPF) or the unscented particle filter (UPF) (Haug, Reference Haug2005; Simon, Reference Simon2006). The method mentioned by Simon is to use a bank of EKFs or UKFs (one for each particle) with the likelihood function to derive the system a posteriori estimates. In the proposal from Haug, only one EKF or UKF is required. Its a posteriori mean and covariance estimates are used to specify the PF importance density function to generate particles more intelligently. After calculating the normalized importance weights of the particles, the system a posteriori estimates are refined. This method does not require the re-sampling step, because the filter does not reuse the particles. Such an implementation is a variant of the so-called Gaussian particle filter, which is asymptotically optimal in the number of particles when the additive Gaussian noise assumption holds true. However, this Gaussian assumption can be relaxed and in general be non-Gaussian in the case that the importance density distribution can be approximated as Gaussian and the analytical expression for the likelihood function can be specified (Kotecha and Djuric, Reference Kotecha and Djuric2003). Both methods present highly accurate estimation solutions with a reduced number of particles as compared to the SISR PF algorithm. However, the method proposed by Haug is more attractive, because only one EKF or UKF is used in the algorithm. And hence, it is considered in this paper.
In order to obtain the accurate a posteriori estimates from the EKF or UKF algorithm to build the importance density function for generating particles more intelligently, the prior statistical knowledge on the system process and measurement noises must be known (Hide et al., Reference Hide, Moore and Smith2003; Mehra, Reference Mehra1971; Mohamed and Schwarz, Reference Mohamed and Schwarz1999). Insufficient or incorrect knowledge about these statistics may lead to the degradation of the system performance, or even provoke the filter to diverge. In this paper, the residual-based covariance matching technique is used. Accordingly, the adaptive unscented particle filter (AUPF) algorithm is proposed.
In the remainder of this paper, the content is organized as follows. In Section 2, the UKF and the UPF algorithms are given. In Section 3, the innovation-based and its near relative, the residual-based adaptive Kalman filtering techniques are presented. The flowchart of the proposed AUPF algorithm is shown in Section 4. In Section 5, the mathematical models developed for the low-cost INS/GPS tightly-coupled integration are specified. The experimental setup is introduced in Section 6, and in Section 7, simulation results are compared and analyzed to illustrate the concept of the proposed AUPF algorithm. Last but not least, Section 8 presents the summary for this paper.
2. UNSCENTED PARTICLE FILTER
2.1. Unscented Kalman Filter
The idea behind the UKF comes from the assumption that it is much easier to approximate a Gaussian distribution, rather than to simulate any arbitrary non-linear functions (Julier et al., Reference Julier, Uhlmann and Durrant-Whyte1995, Reference Julier, Uhlmann and Durrant-Whyte2000). Consider the case that the system process and measurement noises are additive and Gaussian distributed, the UKF algorithm from (e.g., Simon, Reference Simon2006) is summarized in Table 1.
Table 1. Summary of the UKF algorithm.
In Table 1, n denotes the dimension of the system state vector. 
is the matrix square root of n Pk−1+, such as 
, which can be obtained from the Cholesky factorization ‘CHOL’ in MATLAB. 
is the i-th row of 
. Qk is the covariance matrix of the additive system process noise. Rk is the covariance matrix of the additive measurement noise.
2.2. Unscented Particle Filter
In the proposed UPF algorithm, the first two moments of the UKF estimates are used to form the importance density function 
for generating particles. The first 7 steps of the UPF algorithm are the same as those of the UKF. Only the extra steps are listed in Table 2. As shown in the Table 1 and 2 equations, the particles of the UPF are generated from the importance density function formed by the UKF a posteriori estimates. The importance weights of particles are then computed and normalized through the likelihood function. The refined a posteriori mean and covariance estimates are calculated in step 11, which are recursively used to generate sigma points in the next epoch (step 2). The whole process repeats until the end of the trajectory.
Table 2. Additional steps in the UPF algorithm.
ADAPTIVE ESTIMATION METHODS
The Kalman filtering methods can provide reliable a posteriori state estimates only when the prior statistics of the system process and measurement noises are known. In practice, these error statistics are difficult to know exactly, because they are strongly related to the type of application at hand and may infrequently be non-Gaussian and usually be non-stationary due to the changing of the physical environment. In order to obtain the accurate system state estimates, prompt reflection of changes from external influences in noise statistics is required. In this paper, the residual-based covariance matching technique is used in the algorithm to monitor and track the changes in the noise sources and to adaptively adjust its parameters to maintain the system estimation accuracy.
3.1. Innovation-based Adaptive Kalman Filtering
The innovation sequence in the KF is the difference between the incoming measurements and the a priori estimated measurements. It represents the additional information available to the filter as a consequence of the incoming new measurements. Hence, it is considered as the most relevant information for the filter adaptation (Mehra, Reference Mehra1971), which is computed as:
As derived in Mohamed and Schwarz (Reference Mohamed and Schwarz1999), the system process and measurement noise error covariance matrices are adaptively updated as:
where 
in Equation (2) is the estimated innovation covariance matrix, which is computed as:
where N is the window size (a fixed number of epochs); zj is the innovation sequence at time instant j; j 0=k−N+1 is the first epoch of the estimation.
3.2. Residual-based Adaptive Kalman Filtering
For the innovation-based adaptive filtering method, the subtraction operation in Equation (2) may cause numerical problems in the calculation of the measurement error covariance matrix 
k. That is, if there are negative parameters in the main diagonal of k, the filter will suddenly diverge. This problem can be easily tackled using the residual-based adaptive filtering method. The residual sequence is computed as the difference between the incoming measurements and the a posteriori estimated measurements, as shown in Equation (4).
The k is calculated by Equation (5), which guarantees the positive definite of the matrix. The detail derivation of this equation can be found in Mohamed and Schwarz (Reference Mohamed and Schwarz1999), and it is omitted here.
where 
is the residual sequence covariance matrix at time instant k.
In Equation (5), the computation of k is on the basis of the a posteriori state error covariance matrix Pk+. The EKF measurement update equations are given here for convenience:
As shown above, the derivation of Pk+ contains the measurement error covariance matrix Rk at time instant k. In order to estimate k using Equation (5), a pre-defined error covariance Rk must be known. This problem can be solved by the recursive EKF algorithm. Namely, the k−1 can be used to approximate the error covariance Rk at time instant k. This approximation is not restricting. That is, the k is varying slowly over time. It is calculated on the basis of 
, which comes from the average of the accumulated residual sequence covariance over a sliding window size.
4. ADAPTIVE UNSCENTED PARTICLE FILTER
For the INS/GPS integrated navigation system, the IMU measurements are normally used in the system model, and their error statistics, i.e., the sensor turn-on biases, temperature related variations in biases and noises, can be found in the product specifications, and most of them can be detected through the self-alignment and calibration processes. The residual accumulated sensor errors will be estimated and corrected by the GPS data on the frequency of GPS measurement update rate. Therefore, the parameters in the system error covariance matrix Q can be approximately given, especially for short-time applications, for instance the 68 s UAV trajectory used in this paper. Nonetheless, the GPS measurements used in the system observation model are strongly related to the external influences. When the UAV is manoeuvring under highly reflective signal environments, the error statistics may vary dramatically. In this paper, we focus on the adaptive estimation of the measurement error covariance under the assumption that the statistical information about the process noise error covariance Q is approximately known.
Equation (5) is derived based on the EKF algorithm. For the UKF, a modification is required:
where Pkyy+ is calculated as:
![{\bf P}_{k}^{yy \plus } \equals {1 \over {2n}}\sum\limits_{i \equals \setnum{1}}^{\setnum{2}n} {\left[ {{\bf h}_{k} \lpar {\bf \hat{x}}_{k\comma i}^{ \plus } \rpar \minus {\bf \hat{y}}_{k}^{ \plus } } \right]} \left[ {{\bf h}_{k} \lpar {\bf \hat{x}}_{k\comma i}^{ \plus } \rpar \minus {\bf \hat{y}}_{k}^{ \plus } } \right]^{T} \comma \ {\rm where}\ \ {\bf \hat{y}}_{k}^{ \plus } \equals {1 \over {2n}}\sum\limits_{i \equals \setnum{1}}^{\setnum{2}n} {{\bf h}_{k} \lpar {\bf \hat{x}}_{k\comma i}^{ \plus } \rpar }.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022040837268-0721:S0373463310000068_eqn8.gif?pub-status=live){kind=small}
The ○k,i+ represent the sigma points drawn from the UKF a posteriori estimates, i.e., ○k+, Pk+, as:
Figure 1 shows the flowchart of the proposed AUPF algorithm.
Figure 1. AUPF algorithm flowchart.
5. TIGHTLY-COUPLED INS/GPS INTEGRATED KALMAN FILTER
5.1. Inertial Mechanization and INS Error Modelling
In the state-of-the-art INS/GPS integrated navigation systems, the INS error propagation model is widely used as the system model (Farrell and Barth, Reference Farrell and Barth1999; Li et al., Reference Li, Rizos, Wang, Mumford and Ding2008). The INS navigation solution errors and IMU sensor errors are commonly chosen to form the system state vector. For the low-cost MEMS-based IMU, the sensor errors have the non-Gaussian characteristics and must be modelled. For instance, the bias errors in the gyroscope measurements will be integrated to form the Euler angles, which are used to build the vector transformation matrix for transforming the accelerometer measurements from the body frame to the navigation frame. After integrations, these errors turn out to be the position and velocity drift in the navigation frame.
For the EKF, the usage of error states or total states presents similar system estimation accuracy in INS/GPS tightly-coupled integration. The mutual derivation between the total states and error states can be found in Brown and Hwang (Reference Brown and Hwang1997). However, for the UKF, using the total states with non-linear system model, the system strapdown processing must be repeated on every sigma point at each IMU data output instant. This will introduce significantly increased computational burden. Therefore, in this paper, the error states are used, as shown in Equation (10). They represent the difference between the estimated values and true values, i.e., δx=○estimated−xtrue.
![\delta {\bf x} \equals \left\{ \openup3{\matrix{ {\delta {\bf p}_{\rm n} \equals \left[ {\delta x_{n} \;\delta x_{e} \;\delta x_{d} } \right]^{T} } \hfill \tab {\left( {Position\ errors\ in\ NED\ frame} \right)} \hfill \cr {\delta {\bf v}_{\rm n} \equals \left[ {\delta \dot{x}_{n} \;\delta \dot{x}_{e} \;\delta \dot{x}_{d} } \right]^{T} } \hfill \tab {\left( {Velocity\ errors\ in\ NED\ frame} \right)} \hfill \cr {\delta {\bimpsi} \equals \left[ {\delta \phi \;\delta \theta \;\delta \varphi } \right]^{T} } \hfill \tab {\left( Attitude\ errors} \right)} \hfill \cr {\delta {\bf f}_{b} \equals \left[ {\delta f_{x} \;\delta f_{y} \;\delta f_{z} } \right]^{T} } \hfill \tab {\left( {Accel\ bias\ errors\ in\ body\ frame} \right)} \hfill \cr {\delta {\bimomega} _{b} \equals \left[ {\delta \omega _{x} \;\delta \omega _{y} \;\delta \omega _{z} } \right]^{T} } \hfill \tab {\left( {Gyro\ bias\ errors\ in\ body\ frame} \right)} \hfill \cr {c\delta t} \hfill \tab {\left( {Receiver\ clock\ bias\ error\ in\ distance} \right)} \hfill \cr {c\delta \dot{t}} \hfill \tab {\left( {Receiver\ clock\ drift\ error\ in\ distance} \right)} \hfill \cr} } \right.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022040837268-0721:S0373463310000068_eqn10.gif?pub-status=live)
The INS mechanization we use is that which is often seen for low-cost IMU. A low-cost IMU is insensitive to the Earth rotation. Moreover, in the strapdown processing and in the system model for the KF, the transport rate, coriolis terms and the lever-arm effects are neglected for simplicity. The simplified mechanization model can then be expressed in NED frame as:
![\eqalign{\tab {\bf p}_{n\comma k \plus \setnum{1}} \equals {\bf p}_{n\comma k} \plus {\bf v}_{n\comma k} \cdot \rmDelta t \cr \tab {\bf v}_{n\comma k \plus \setnum{1}} \equals {\bf v}_{n\comma k} \plus \left[ {{\bf R}_{b\setnum{2}n\comma k} \cdot {\bf f}_{b\comma k} \plus {\bf g}_{n} } \right] \cdot \rmDelta t \cr \tab {\bimpsi} _{k \plus \setnum{1}} \equals {\bimpsi} _{k} \plus {\bf E}_{b\setnum{2}n\comma k} \cdot {\bimomega} _{b\comma k} \cdot \rmDelta t \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022040837268-0721:S0373463310000068_eqn11.gif?pub-status=live)
where fb is measurement vector of specific force; ωb represents the angular rate vector; gn is the acceleration vector due to the local gravity; ψ contains the Euler angles; Rb2n is the rotation matrix from body frame to NED navigation frame; Eb2n is the rotation rate transformation matrix between the body and navigation frame, as shown in Equation (12).
where c(x), s(x) and t(x) represent the mathematical operations of cos(x), sin(x) and tan(x) respectively.
The simplified linear INS error model is given in Equation (13). Its derivation can be found in Titterton and Weston (Reference Titterton and Weston2004) and Sukkarieh (Reference Sukkarieh2000). It is worth noting that this model uses approximations. It is not the exact linearized model from Equation (5). To use this model, the IMU self-alignment and calibration processes must be carefully conducted before the experiment starts.
![\left[ {\matrix{ {\delta \dot{\bf {p}}_{n} } \hfill \cr {\delta \dot{\bf {v}}_{n} } \hfill \cr {\delta \dot{\bimpsi}} \hfill \cr {\delta \dot{\bf{f}}_{b} } \hfill \cr {\delta \dot{\bimomega} _{b} } \hfill \cr {c\delta \dot{t}} \hfill \cr {c\delta \ddot{t}} \hfill \cr} } \right] \equals \mathop {\underline {\left[ {\matrix{ {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf I}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {\bf 0} \tab {\bf 0} \cr {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {\lsqb {\bf f}_{n} \times \rsqb } \tab {{\bf R}_{b\setnum{2}n} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {\bf 0} \tab {\bf 0} \cr {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab { \minus {\bf R}_{b\setnum{2}n} } \tab {\bf 0} \tab {\bf 0} \cr {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {\bf 0} \tab {\bf 0} \cr {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {{\bf O}_{\setnum{3} \times \setnum{3}} } \tab {\bf 0} \tab {\bf 0} \cr {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab 0 \tab 1 \cr {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab {{\bf 0}^{T} } \tab 0 \tab 0 \cr} } \right]} }\limits_{\bf F} \cdot \left[ {\matrix{ {\delta {\bf p}_{n} } \hfill \cr {\delta {\bf v}_{n} } \hfill \cr {\delta {\bimpsi} } \hfill \cr {\delta {\bf f}_{b} } \hfill \cr {\delta {\bimomega} _{b} } \hfill \cr {c\delta t} \hfill \cr {c\delta \dot{t}} \hfill \cr} } \right] \plus {\bf w}\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022040837268-0721:S0373463310000068_eqn13.gif?pub-status=live)
where [fn×] is the skew-symmetric matrix of fn=Rb2nfb; the unknown gyroscope and accelerometer slowly time-varying biases δfb and δωb are modelled as constant plus random-walk processes; the receiver clock drift error cδ⃛ is modelled as random walk, while the receiver clock bias cδt is modelled as the integral of the clock drift error.
With this simplified linear error model, the UKF performs the same time-update procedure as the EKF algorithm. Thus, a large amount of computational burden can be saved. It is implemented in continuous-time domain and can be transformed to its equivalent discrete-time domain using Equation (14), where equivalent means that the discrete and the continuous-time model can predict the same system state at specified discrete time instants.
In this paper, only the first order term is considered. The other higher order terms are neglected. The functional flowchart of INS/GPS tightly-coupled integration is given in Figure 2.
Figure 2. Functional flowchart of INS/GPS tightly-coupled integration.
5.2. Derivation of GPS Observation Model
In INS/GPS tightly-coupled integration, the observation models are heavily non-linear. For UKF and UPF algorithms, non-linear Equations (15) and (16) are used. For the EKF algorithm, the Jacobian matrix in Equation (17) is exploited.
The predicted pseudorange measurements are related with error states δx n, δx e, δx d, cδt as:
where 
(n=1,…, N) is the INS predicted pseudorange measurement from the n-th satellite; x nsv(n), x esv(n) and x dsv(n) are the n-th satellite position coordinates expressed in NED navigation frame; ○ nins, ○ eins and ○ dins are the INS estimated position in NED navigation frame.
The INS predicted delta pseudorange measurements are related with error states δ&xdot; n, δ&xdot; e, δ&xdot; d, cδ⃛ as
where
For the EKF algorithm, the Jacobian observation matrix is calculated as
![{\bf H} \equals \left[ {\matrix{ {l_{n}^{sv\lpar n\rpar } } \tab {l_{e}^{sv\lpar n\rpar } } \tab {l_{d}^{sv\lpar n\rpar } } \tab 0 \tab 0 \tab 0 \tab {...} \tab 0 \tab 1 \tab 0 \cr {0} \tab {0} \tab {0} \tab {l_{n}^{sv\lpar n\rpar } } \tab {l_{e}^{sv\lpar n\rpar } } \tab {l_{d}^{sv\lpar n\rpar } } \tab 0 \tab {...} \tab 0 \tab 1 \cr} } \right]_{\setnum{2}N \times \setnum{17}} \comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022040837268-0721:S0373463310000068_eqn17.gif?pub-status=live)
where l nsv(n), l esv(n), l dsv(n) (n=1,…, N) denote the unit direction vectors pointing from the n-th satellite position in the navigation frame to the INS estimated user position ○ nins, ○ eins, ○ dins in navigation frame.
6. EXPERIMENTAL SETUP
In the following experiments, a 3D UAV trajectory is used as shown in Figure 3. The velocity dynamic profiles in NED navigation frame are plotted in Figure 4. For the majority of time, the UAV is flying at a velocity of 40 m/s, which is 144 km/h. In the north and east directions, the velocities vary dramatically from approximately +40 m/s to −40 m/s in just a few seconds.
Figure 3. UAV trajectory.
Figure 4. Trajectory dynamics.
This trajectory is derived from the strapdown processing of the field collected high quality IMU raw data from the University of Sydney. It is considered as the reference for generating GPS measurements (i.e., pseudorange and Doppler, where the Doppler measurement is derived from the first order central difference of the carrier phase data d k(n)≈(φk+1(n)−φk−1(n))/(2·Δt)) using the SATNAV toolbox from GPSoft LLC. It needs to be noted that such an approximation of instantaneous Doppler measurement using carrier phase data at low output rate (i.e., 1 or 5 Hz) is very poor for high dynamic applications. In order to tackle this problem, 50 Hz carrier phase data are used to derive the Doppler measurements.
Under the assumption that the DGPS mode is used, the majority of GPS measurement errors can be corrected or minimized to small values, i.e., the ionospheric and tropospheric delays, satellite clock errors, and errors in satellite ephemeris predictions. Therefore, they are not considered in the simulation. For the INS/GPS integration, we assume that a NovAtel DL-4plus GPS receiver is used. From the specification, we know when the receiver is navigating in a benign signal environment, in DGPS mode, the typical values for position and velocity estimation errors are 0·45 m CEP and 0·03 m/s RMS respectively. Hence, we reasonably simulate 0·5 m (1σ) thermal noise errors on the pseudorange, and 0·02 m/s (1σ) thermal noise errors on the Doppler measurements. For the multipath, we use the SATNAV toolbox from GPSoft LLC with minor modifications to generate the errors. The basic idea is to generate the time-correlated Gaussian noises in horizontal plane and project them onto the line-of-sight directions from the satellites to receiver through the cosine of the elevation angles (non-linear operation on Gaussian noises). The parameters we have chosen are 0·4 m and 0·01 m/s for generating multipath errors on pseudorange and Doppler measurements. Time constant is chosen to be 15 s.
An automotive level LandMark™ 20 MEMS-IMU from Gladiator Technologies Inc. is simulated for the IMU. The main sensor errors are simulated according to the specification shown in Table 3. The IMU raw data (simulated errors plus the field collected high quality IMU raw data) in the body frame are plotted in Figure 5.
Figure 5. Simulated LandMark™ 20 MEMS IMU raw data.
Table 3. Error characteristics of the simulated LandMark™ 20 MEMS-based IMU (1σ).
The integration is accomplished by using the aforementioned state space error modelling. In such a manner, the three design schemes, i.e., EKF, UKF and UPF, share the same time-update procedures. An ideal time-synchronization has been assumed for the GPS and INS data processing. Flexure and vibrations have not been taken into consideration. For the integrated KF, the system time-update happens at the IMU measurements update rate of 50 Hz, while the measurement-update happens at the GPS measurement update rate of 5 Hz. The simulation is conducted with eight satellites in view. The program is implemented in MATLAB 7.1 using the Intel Core 2 Duo T7600 CPU and 2GB RAM.
7. SIMULATION & RESULTS
Four groups of tests with well-controlled signal environments are conducted in this section to illustrate the concept of the AUPF algorithm. In Section 7.1, the adaptive optimization of the INS/GPS system performance under benign signal environment is investigated and in Section 7.2, highly reflective signal environments are simulated. The advantages of using the adaptive filtering technique can be readily seen. In Section 7.3, system performances from the AEKF, AUKF and AUPF are compared using one group of IMU and GPS data. In Section 7.4, Monte Carlo experiments are conducted to verify the improved system performance from the AUPF algorithm. The position and velocity errors analyzed in the following experiments represent the norm of position and velocity errors (i.e., ‖Δp‖ and ‖Δv‖) in north, east and down directions, which is calculated as 
.
7.1. System performance comparison between the UKF and AUKF algorithms under benign signal conditions
The benign signal environment is simulated according to the parameters given in Section 6. The simulated measurement error variances on pseudorange measurements are about 0·25 and for Doppler, they approach 0·001. The computational burden of the UKF is 10·65 s whilst for the AUKF, 17·26 s is required. This extra time is caused by the additional estimation block as marked by the dashed-line box in Figure 1.
The four out of eight pseudorange and Doppler measurement error variance parameters are chosen and depicted in Figure 6. They approximately match the prior given statistical information, so the UKF and AUKF algorithms yield comparable system performances, as shown in Figure 7.
Figure 6. Adaptively updated measurement error variance parameters under benign signal environment.
Figure 7. Performance comparison between the UKF and AUKF under benign signal environment.
7.2. System performance comparison between the UKF and AUKF algorithms under highly reflective noisy signal environment
For stationary applications, the multipath errors are strongly related to the geometry between the receiver antenna and the satellites, and the errors are time-correlated. Nevertheless, for a UAV, which is manoeuvring in the air with velocity of 40 m/s from one GPS measurement update epoch to the next, the environment will be changed dramatically (i.e., the geometry between the GPS antenna on the UAV with the buildings or trees nearby), and it is usually not possible to relate the multipath errors from one epoch to the next. Thus, we consider the multipath errors to be time-uncorrelated. To simulate such highly reflective multipath effects, we choose 5 m and 0·1 m/s error parameters (1σ) to generate multipath errors on pseudorange and Doppler measurements (typical values according to Misra and Enge (Reference Misra and Enge2007)) using the SATNAV toolbox with minor modifications as introduced in Section 6. They are added onto the GPS measurements randomly without reference to the time. A scale factor is introduced, which changes from 0 to 1 over time. It is used to control the impact of the noises (thermal noises plus multipath errors) to approximate the case that the UAV navigates from the open sky environment into the highly reflective noisy signal environment.
The system performance from the UKF and AUKF algorithms are compared as shown in Figure 8. The residual-based adaptive covariance matching technique improves the system estimation accuracy by adaptively adjusting the weights to the measurements on the basis of the incoming residual sequences. For the measurement error variance parameters, the prior given data fit the signal environment only in the first few epochs. After that, the measurement noises grow steadily over time due to the increased thermal noises and multipath effects, which turn the prior error statistics to be much smaller than the practical ones. Large estimation errors from the UKF algorithm (blue curves) are readily seen in the figure.
Figure 8. Performance comparison between the UKF and AUKF under highly reflective signal environment.
The position estimation errors in north, east and down directions are given in Figure 9, where substantial improvements are obtained using the residual-based AUKF algorithm. It also shows that for the residual-based AUKF, the perfect a priori knowledge of the measurement noises is not of the main importance. The adaptively updated measurement error variance parameters from the randomly chosen four out of eight pseudorange and Doppler measurements are plotted in Figure 10. The window size N is chosen to be 30. Therefore, in the first 30 epochs, the initial statistical parameters are used.
Figure 9. Position error comparisons in north, east and down directions between the AUKF and UKF.
Figure 10. Adaptively updated measurement error variances under highly reflective signal environment.
7.3. System performance comparison between the AEKF, AUKF and AUPF algorithms
For comparison purpose, one set of IMU and GPS data is used for all three filters. The signal environment was the same as that introduced in Section 7.2. Three comparisons are made: (1) compare the system performances between the AEKF, AUKF and AUPF algorithms using one group of GPS and IMU data; (2) use different numbers of particles in the AUPF algorithm to investigate the changes in its estimation accuracy, and (3) summarize the simulations conducted in this Section.
7.3.1. System performance comparison in highly reflective noisy signal environments
Figure 11 plots the norm of position and velocity estimation errors from the AEKF, AUKF and AUPF algorithms using one group of GPS and IMU data. 100 particles are used in the AUPF algorithm. As shown in the figure, in the first few seconds, the introduced thermal noise and multipath effects are very small. The system is working with the prior given measurement error statistics which match the simulated signal environment at the start of the integration and hence, the estimation accuracies from three filters are comparable. After approximately 10 seconds, the noise and multipath errors grow steadily over time. It takes some time for the corrupted measurements to become less weighted. In this period, the magnitude of innovation sequence is increasing, which accordingly turns the non-linearity problem to be more critical. That is, the neglect of the higher order terms of Taylor series expansion may lead to the degrading of system estimation accuracy. Besides, the multipath errors simulated in the trajectory are not exactly Gaussian-distributed. From the figure, the AUPF presents the best estimation accuracy when the norms of position and velocity errors are compared. Nevertheless, for the AUPF, the particles are generated randomly, and the system estimation accuracy varies after each simulation run. Hence, its system performance can only be evaluated in the sense of statistics instead of just from one run of the simulation. For a thorough investigation of its estimation accuracy, in the next Section, 100 runs are conducted using different numbers of particles.
Figure 11. Performance comparison of AEKF, AUKF and AUPF under highly reflective signal environment.
7.3.2. Comparisons of the AUPF with different numbers of particles
In this test, 25, 50, 100, and 500 particles are used. 100 simulation runs are conducted using the same group of GPS and IMU data. The relationship between the improved system estimation accuracy and the increased number of particles is illustrated in Figure 12.
Figure 12. System performance analysis of the AUPF algorithm with different number of particles.
Using 25 particles in the AUPF algorithm, the system diverges frequently. With 500 particles, the system estimation accuracy is much improved compared with the use of fewer particles. It can be expected that further improved system performance can be achieved when 1000 or more particles are used at the expense of significantly increased processing time. It is often the case that slightly improved estimation accuracy requires huge extra computational burden. In practice, the choice of the minimum number of particles is always a trade off between the increased processing load and the improved estimation accuracy, which should be discussed on the basis of the specific application at hand.
The relationship between the system performance and the number of particles is revealed in Figure 13. If ten particles are used, only one particle may be significantly weighted and the others are much less weighted. However, the estimates from this particle may still be far away from the true values. Only with greater numbers, for instance, 50 or 100 particles, will more particles be significantly weighted through the likelihood function, and the system a posteriori estimates are calculated from the normalized weighted sum of particle estimates, which is expected to be much closer to the true values.
Figure 13. Normalized importance particle weights.
7.3.3. Summary of the system performances from the AEKF, AUKF and AUPF algorithms
The system estimation accuracy and processing time from simulations conducted in Section 7.3.1 and 7.3.2 are summarized in Table 4. All the filters are processing the same group of GPS and IMU data so that the differences are caused by the algorithms.
Table 4. System performance comparison between adaptive non-linear filtering methods.
In Table 4, m‖Δp‖ and m‖Δv‖ represent the averaged mean of ‖Δp‖ and ‖Δv‖ errors from realizations, while mσ(‖Δp‖) and mσ(‖Δv‖) denote the averaged sigma values on ‖Δp‖ and ‖Δv‖ errors from realizations. The time represents the number of seconds elapsed in the processing of the whole GPS and IMU data from the 68 s UAV trajectory. It is worth mentioning that this is just an indication to show the computational cost, because it depends largely on the style of programming in MATLAB and the computational power from the CPU. For exact knowledge of the computational burden, the number of numerical operations, i.e., additions and multiplications, should be counted.
In Table 4, the simulation results from the AUPF with 1000 particles are included. Due to its large computational burden, the results are the average values from ten simulation runs, while for the other AUPF algorithms, i.e., with 25, 50, 100 and 500 particles, the results are the average values from 100 runs. For the AEKF and AUKF algorithms, using the same prior statistical parameters, i.e. P, Q and R, the system performance will be unchanged when the same group of GPS and IMU data is processed. Thus, one simulation run for each is performed. From the table, the AUPF with 100 particles is highly recommended due to its improved estimation accuracy and affordable computational burden. Further increasing the number of particles yields very limited improved system estimation accuracy, but requires significantly increased processing load.
7.4. Comparison between the AEKF, AUKF and AUPF algorithms using 100 sets of GPS and IMU data
In this test, 100 groups of GPS and IMU data are simulated to test the system performances from the AEKF, AUKF and AUPF (100 particles) algorithms. The signal environments are the same as the ones used in Sections 7.2 and 7.3.
In Figure 14, improvements from the AUPF algorithm with 100 particles are readily seen, not only with system estimation accuracy, but also in the robustness of the system performance. In the experiments, the position estimation errors mainly arise from the multipath effects introduced onto the pseudorange measurements. However, for the velocity estimation errors, the multipath errors added onto the Doppler measurements are relatively small. That is, the Doppler measurement noises are nearly Gaussian-distributed. And hence, the benefit from the AUPF to AUKF is small. Nevertheless, both the AUKF and AUPF use non-linear observation equations directly and yield improved estimation solutions as compared with the AEKF algorithm.
Figure 14. System performance comparison between the AEKF, AUKF and AUPF algorithms under highly reflective signal environment with 100 Monte Carlo simulations.
CONCLUSIONS
In this paper, the AUPF algorithm is proposed as a non-linear/non-Gaussian estimation method for the INS/GPS tightly-coupled integration. It outperforms the AEKF and AUKF algorithms when a sufficient number of particles are generated. The residual-based adaptive estimation technique is used in the filter to track the changes in the system observation data in an adaptive manner, which makes the system more robust. The simulation tests verify the benefits of the AUPF algorithm in comparison to the AEKF and AUKF algorithms. The computational burden is also analyzed and shows that the AUPF with 100 particles is of highest interest for the simulation conducted in this paper.
ACKNOWLEDGMENTS
Part of the work reported herein has been funded by the German Research Foundation (DFG), grant number KN 876/1-2, which is gratefully acknowledged.
