Abstract

Any vehicle such as vessel has three attitude parameters, which are mostly defined as pitch, roll, and heading from true north. In hydrographic surveying, determination of these parameters by using GPS or INS technologies is essential for the requirements of vehicle measurements. Recently, integration of GPS/INS by using data fusion algorithm became more and more popular. Therefore, the data fusion algorithm plays an important role in vehicle attitude determination. To improve attitude determination accuracy and efficiency, two improved data fusion algorithms are presented, which are extended Kalman particle filter (EKPF) and genetic particle filter (GPF). EKPF algorithm combines particle filter (PF) with the extended Kalman filter (EKF) to avoid sample impoverishment during the resampling process. GPF is based on genetic algorithm and PF; several genetic operators such as selection, crossover, and mutation are adopted to optimize the resampling process of PF, which can not only reduce the particle impoverishment but also improve the computation efficiency. The performances of the system based on the two proposed algorithms are analyzed and compared with traditional KF. Simulation results show that, comprehensively considering the determination accuracy and consumption cost, the performance of the proposed GPF is better than EKPF and traditional KF.

1. Introduction

It is very important to provide accurate and reliable attitude determination data since the performance of a vessel is highly reliant on the attitude determination system [1]. Due to their complementary features of global positioning system (GPS) and inertial navigation system (INS), the GPS/INS integrated navigation systems have been extensively investigated and increasingly used for highly accurate attitude determination [2], especially in position and orientation systems [3]. The GPS/INS integrated systems can be classified into loosely coupled, tightly coupled, and ultratightly coupled systems [4]. Due to the high cost and complexity of tightly-coupled and ultra-tightly-coupled systems, most GPS/INS systems are loosely-coupled because they are easier to build [5]. Therefore, loosely-coupled GPS/INS is employed in our study. The block diagram of loosely-coupled GPS/INS is shown in Figure 1.

In the process of attitude determination, filtering method performs a very important role to achieve high accuracy determination results with high efficiency [68]. Kalman filter (KF) [913] is a popular data fusion algorithm in handling optimal estimation problems which has been widely investigated in vehicle positioning or attitude determination for a long time. However, the optimality of KF highly depends on the data linearity. The first solution to the estimation of nonlinear system is extended Kalman filter (EKF) [1417]. The EKF is based on approximation to the first-order linearization of the nonlinear process or observation equations, so large errors can be introduced which make EKF algorithm no longer effective in many special applications, such as the complex systems environments which demand higher estimation accuracy. Recently, unscented Kalman filter (UKF) [18, 19] and particle filter (PF) [2022] are presented through improvement and innovation. Compared with traditional filtering algorithms, PF is more suitable for non-Gaussian and nonlinear systems. However, because of the so-called sample impoverishment phenomenon, the implementation complexity of PF is prohibitively high with limited computing resources, especially for dynamical systems. So the improvement of PF has been concerned by many scholars for a long time. Extended Kalman particle filter (EKPF) [23], which uses EKF to determine the importance density, has been proposed to improve the performance of PF. In addition, with the development of simulated annealing algorithm, genetic algorithm [24], particle swarm optimization [25], and some other intelligent evolutionary algorithms [26] have been used to improve the diversity of particle groups for accessing to the global optimum estimation, and certain effects have been reached indeed through the improvements above.

The major contributions of this paper are in investigating the EKPF and genetic particle filter (GPF) and using them to improve not only the accuracy of GPS/INS attitude determination but also the computation efficiency. The simulation results show that, compared with traditional KF, more highly accurate attitude determination results can be reached by using EKPF. The comparison results between EKPF and GPF show that GPF has higher filtering accuracy and more efficient computation ability than EKPF.

2. System Model and Data Collection

2.1. System Model

The state model for KF is presented as where denotes state variables, denotes system noise, denotes system matrix, and denotes system noise matrix. The state vector is parameters errors with 15 dimensions:where, , , , , , , , , are, respectively, the 3-dimension position errors, velocity errors and attitude errors of INS system. The 9 parameters mentioned above are estimated by filter, then the determination accuracy can be calibrated by the compensation of the 9 estimated parameters, and the measurement accuracy can be improved after calibrating. , , , , , are, respectively, the 3-dimension errors of accelerometers and gyros. The 6 parameters mentioned above are determined by experience or device introduction, and the accuracy of GPS/INS system can be improved by putting the 6 parameters into the state vector. The system noise matrix iswhere , , , , , , , , are, respectively, the stochastic noise of 3-dimension position, velocity, and attitude errors.

The following are the measured state variables:

The measurement model is defined as

2.2. Data Collection

A set of vessel dynamic data are selected to verify the proposed filtering algorithms. The experiment parameters are as follows: gyro constant drifts of east, north, and up direction are all 1°/h; gyro random drift is 0.3°/h; accelerometer constant and random biases both are 0.08 mg; original heading, pitch, and roll angle are, respectively, 79.36°, 0°, and 0°; original velocity is 0.01 m/s and original velocity error is 0.01 m/s; and original longitude is 126.682234° and original latitude is 45.776563°. The GPS/INS system is shown in Figure 2. The INS and GPS data acquisition interfaces in PC are shown in Figures 3 and 4, respectively.

3. Extended Kalman Particle Filter

3.1. Traditional Kalman Filter

The Kalman filter is a set of mathematical equations which can provide an efficient computational method to recursively estimate the state and error covariance of a process, in a way that minimizes the mean of the squared error covariance. The estimate process contains two steps: prediction and update. Consider a state space dynamic equation of a time-variant system model and a measurement model as where the subscript stands for the iteration time ; is the state of system at time ; is the measurement at time ; and are the state and measurement transition matrices, respectively, which in practice can be considered as invariable; denotes the process noise; denotes the measurement noise; and denotes the process noise matrices.

The equations for the KF are divided into two groups: time update equations and measurement update equations. The time update equations also can be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. If , , and can be assumed as constant, the time update equations are as follows: where is the process noise covariance matrix. While the measurement update equations are

3.2. Extended Kalman Particle Filter

PF algorithm is based on sequential importance sampling (SIS) step, which forms the basis for most sequential Monte Carlo filters. Compared with the traditional KF, PF is more suitable for nonlinear and non-Gaussian systems, so PF is gradually used in signal tracking, robot control, navigation and positioning, and many other fields. But it also has many drawbacks, such as degeneracy phenomenon, large amount of computation, and sample impoverishment caused by re-sampling. A more detailed description of PF is beyond the scope of this paper. The reader is encouraged to consult one of the many papers about the PF, such as [2022].

In order to improve the performance of PF, EKF algorithm is introduced. It uses the local linearization method for approximation of the importance density moving the particles to the high likelihood region, and then the optimal importance density can be approximated. This filtering algorithm is called EKPF. The EKPF algorithm is summarized as follows.(i)Initialization: (ii)Update the particle by EKF: , the specific steps as follows: (iii)Particle importance weight after updating: (iv)Normalize the importance weights: (v)State estimation: (vi)Re-sampling:

The flow diagram of EKPF is shown as Figure 5.

3.3. Comparison Results

Figures 6, 7, and 8 show the performances comparison between KF and EKPF algorithms. Data are collected during 3200 seconds. In order to evaluate the performances of different algorithms, standard deviation of errors (SDE) is introduced. Figure 9 is the comparison of SDE; it can be concluded that EKPF performs much better than KF in filtering accuracy.

Another evaluation criterion of algorithm performance is computation burden. In this paper, the calculation time of each output is used as evaluation indicator, which is shown in Table 1. From Table 1 we can see that due to the complexity of EKPF algorithm, the computation consumption is highly increased from 423.2 μs to 1095.98 μs. Therefore, comprehensively considering the filtering accuracy and instantaneity, it can be concluded that EKPF is not the most available algorithm. In order to take both filtering accuracy and computation burden into consideration, the combination of genetic algorithm and particle filter is proposed in Section 4.

4. Genetic Particle Filter

4.1. Algorithms

Genetic algorithms (GAs) are search methods based on the mechanization of natural selection and natural genetics. GAs combine survival of the fittest among string structures (chromosomes) with randomized information exchange. A simple GA consists of three stages: selection, crossover, and mutation. Selection is the process in which individual chromosomes are being selected according to their fitness function. By this process, the more likely chromosomes will contribute offspring in the next generation with higher probability. Crossover is the process that changing genetic information between two reproduced chromosomes occurs in. Even though the population can be improved by reproduction and crossover process, they can become overzealous and lose potentially important genetic information. Mutation process can protect against such an irrecoverable loss by simply altering a character with small probability every once in a while.

Aiming to solve the particle degeneracy phenomenon existing in the standard PF algorithm, GPF uses genetic algorithm as the re-sampling method of PF algorithm. The genetic selection, crossover, and mutation operations are introduced into GPF to improve the re-sampling process, which can not only reduce the particle degeneracy phenomenon but also decrease the computation time. The key point of GPF is using GA to improve the re-sampling process of PF; the diagram is shown in Figure 10.

Here we only discuss the genetic algorithm scheme of re-sampling process. The details can be seen as follows. (i) Genetic Crossover OperationProcessing the selected particles by crossover algorithm, assume that the number of particles which need to be crossover is , then the implementation process can be shown as in Algorithm 1. (ii) Genetic Mutation OperationThe equation of mutation operation is where, .After GA re-sampling, the state and variance can be estimated as

for from 1 to
  for from 1 to
    find particle with the biggest weight from particles */This is the best particle;
    find particle with the biggest weight from particles */This is the worst particle;
  end
     */Replace the worst particle by the linear combination of and , ;
  Re-calculate the weight of and then normalized it;
end

4.2. Comparison Results

Figures 11, 12, and 13 show the performances comparison between EKPF and GPF algorithms. From the comparison results we can see that all the heading, pitch, and roll errors are decreased by using GPF algorithm. Figure 14 also proves that GPF has better performance in filtering than EKPF since the SDEs are reduced effectively.

Table 2 is the comparison results of consumption burden; compared to EKPF, the calculation time of GPF is significantly decreased. The calculation time of GPF is still longer than KF, but comprehensively considering the filtering accuracy and computation burden, it can be concluded that GPF is the most available filtering algorithm among the three algorithms mentioned above.

5. Conclusions

The problem of choosing a suitable filter for attitude determination application is studied here. Due to the low filtering accuracy of KF and the particle degeneracy phenomenon of PF, two improved filters are presented in this paper, which are EKPF and GPF, respectively. And then the three filtering methods (KF, EKPF, and GPF) for attitude determination using GPS/INS system are studied, and their performances are compared.

The presented algorithms are tested with vessel attitude data, and the simulation results demonstrate that GPF yields the best accuracy under the same condition. In addition, the computation cost of the three filtering methods is analyzed in this paper; it shows that KF requires the lowest computation time, while EKPF requires the largest computation time. Comprehensively considering the filtering accuracy and computation cost, it can be concluded that the GPF is the most available filter among the three presented filtering methods.

Acknowledgments

This work is supported by Ocean special funds for scientific research on public causes (no. 201205035-09), Specialized Research Fund for the Doctoral Program of Higher Education (no. 20110092110039), National Natural Science Foundation of China (no. 50975049), 973 Program (no. 2009CB724002), Research Innovation Program for College Graduates of Jiangsu Province (no. CXZZ_0144), and The Scientific Research Foundation of Graduate School of Southeast University (no. ybjj1130).