Design and Simulation of the Integrated Navigation System based on Extended Kalman Filter

1 Introduction

Early navigation theory was used to handle ships sailing on the ocean, however, navigation technology has penetrated into every conceivable a vehicle in recent years, including land search, remote tracking, ship management, traffic management and other services [1, 2]. The navigation technology is used to guide an object from one point to another point [3]. Navigation system platforms are able to measure instantaneous velocity, direction, position and posture, and then provides this information to the operator and then provides proper guidance to the carrier [4, 5].

With the rapid progress and development of technology, more and more information can be exploited in the navigation system, and navigation methods have been updated frequently. Apart from traditional inertial navigation systems, (such as satellite navigation system, doppler navigation systems, ground-based radio navigation system, and so on) there are many new navigation systems, such as the celestial navigation system, the geomagnetic navigation system, the scene matching system, the terrain matching system, and the visual navigation system [6].

As is well known that, the INS/GPS navigation system has been widely used in integrated navigation system. Inertial Navigation System (denoted as INS) refers to a system which can compute the position, velocity, and attitude of a device with the output of inertial sensors [7]. Particularly, the measurements of the inertial sensors include errors because of physical limitations. However, errors may be accumulated in the navigation scheme of INS [8]. On the other hand, Global Positioning System (named as GPS) is often utilized as an aiding sensor in INS. Therefore, the INS/GPS integrated system has been widely exploited in several navigation fields. Unfortunately, there are many drawbacks in GPS, such as low sampling rate [9, 10]. To solve this proposed problem, in this paper, we focus on the INS/GPS Integrated Navigation System simulation.

Innovations of this paper lie in the following aspects:

1. The extended Kalman filter is used to estimate states of the nonlinear system in the noisy environment.

2. In the extended Kalman filter, the estimation of the state vector and the error covariance matrix are obtained by a time update process and a measurement update process.

In recent years, the rapid development of computer technology has encouraged the development of Kalman filtering model, and Kalman filtering has been successfully used in the integrated navigation system. To solve the problem in standard Kalman filtering, the extended Kalman filter is introduced in this paper. The rest of the paper is organized as follows. Section 2 illustrates related works about the extended Kalman filter. In section 3, we discuss how to design the INS/GPS integrated navigation system. Section 4 proposes the method to simulate the integrated navigation system using the Extended Kalman Filter. In section 5, experimental simulation is given the test the performance of the proposed method. Finally, the whole paper is concluded in Section 6.

2 Related works

The basic function of navigation is to provide real-time status information for the carrier. Due to system noises and measurement noises, how to extract the real states of the information from the noise navigation environment should be solved. Furthermore, the key issue in integrated navigation system is the fusion filtering. In order to effectively model the nonlinear systems, the extended Kalman filter (EKF) has been proposed and widely used.

The core idea of the extended Kalman filter is to approximate nonlinear model by iterative filtering with a linearized method used in EKF is by using the nonlinear Taylor expansion which ignores the rest of the higher order terms. The extended Kalman filter has been exploited in nonlinear filtering application, such as aerospace, navigation, target tracking, and so on. Related works on the extended Kalman filter are listed as follows.

Delgado et al. proposed a model-based method to detect and isolate non-concurrent multiple leaks in a pipeline, and pressure and flow sensors are located at the pipeline ends. Particularly, the proposed method depends on a nonlinear modelling derived from Water-Hammer equations, and the Extended Kalman Filters are exploited to calculate leak coefficients [11].

Bukhari et al. proposed a real-time algorithm to forecast respiratory motion in 3D space and realizing a gating function without pre-specifying a particular phase of the patient’s breathing cycle. The proposed method utilize an extended Kalman filter independently along each coordinate to forecast the respiratory motion and then exploits a Gaussian process regression network to revise the prediction error of the EKF in 3D space [12].

Miljkovic et al. used the EKF coupled with a feedforward neural network for the simultaneous localization and mapping. The main innovations of this paper lie in that the neural extended Kalman filter is used online to estimate an error between the motion model of the mobile robot and the real system performance [13].

Zareian et al. proposed an appropriate approach for computing road friction coefficient. The proposed algorithm utilized measured values from wheel angular velocity and yaw rate sensors of a vehicle. Particularly, vehicle lateral and longitudinal velocities along with yaw rate value are obtained by the extended Kalman filter. Afterwards, lateral and longitudinal tidal forces are calculated with a recursive least square algorithm [14].

Liu et al. combined the Minimum Model Error criterion with the Extended Kalman Filter to estimate states of 4WD vehicle states. Moreover, this paper constructed a general 5-input 3-output and 3 states estimation system, which utilized both the arbitrary nonlinear model error and the white Gauss measurement noise [15].

Apart from the above works, the extended Kalman Filter has been utilized in other fields, such as hemodynamic state estimation [16], Acoustic velocity measurement [17], Proton Exchange Membrane Fuel Cell [18], structural parameters estimation [19], and Radar Tracking [20]. In this paper, we try to introduce the extended Kalman Filter to estimate the states of the INS/GPS integrated navigation system.

3 Overview of the INS/GPS integrated navigation system

Inertial Navigation System (INS) refers to a system which can compute the position, velocity, and attitude using inertial sensors. The measurements of the inertial sensors may include errors because of its design limitations, and the errors are accumulated in the navigation process. Hence, if the error is not compensated with other types of sensors, the information of INS can be used in a short time. Currently, the Global Positioning System (GPS) can provide helpful information for INS. On the other hand, GPS also has some disadvantages, such as low sampling rate and satellite signal loosing. Structure of the INS/GPS integrated navigation system is shown in Figure 1.

Figure 1

Structure of the INS/GPS integrated navigation system

As is shown in Figure1, INS system obtains inputs from the gyros and accelerometers, and a state vector is defined as follows.

where parameters ϕ_E, ϕ_N, ϕ_U represent errors of the platform misalignment respectively, and δV_E, δV_N, δV_U refer to the velocity errors respectively. Furthermore, δL,δλ, δh denote position errors of latitude, longitude and altitude respectively. Based on the definition of state vector, the error state at time t is represented as follows.

where W (t) is the model vector, and it is computed by the following equation.

where ε_E, ε_N, ε_U refer to the errors of gyros drift, ∇_E, ∇_N, ∇_U denote the errors of accelerometer, and symbol E, N, U refer to East, North and Up direction in the geographical frame respectively. Particularly, matrix A is generated from the error equations of INS system, and G is the error transition matrix. Integrating the position information and velocity information together, and the measurement equation to represent the observation of the integrated navigation system is described as follows.

where δ L, δ λ, δ λ mean the errors in latitude, longitude and height respectively, and δv_E, δ v_N refer to the errors with the INS velocities

H denotes a measurement matrix and V refers to the measurement errors. Afterwards, the INS/GPS integrated navigation system can be described the following discretely transforming process.

4 Integrated navigation system simulation based on the extended Kalman filter

In this section, we discuss how to simulate the Integrated Navigation System using the Extended Kalman Filter by measuring position, velocity and attitude. The Kalman filter is used to estimate states of dynamic systems by a stochastic linear state-space model as follows [21–23].

The above equation refers to that the continuous state to control system dynamics.

where this equation denotes the discrete measurement which is corresponding to the system X (t) to the available measurements Y_k. Moreover, u (t) is the model input, W (t) is the Gaussian process noise, and V_k is the measurement noises.

Suppose that a time point is represented as t_k = k ⋅ T_s, where T_s refers to a sampling period. The estimation of the state vector xk^ and the error covariance matrix Pk^ are calculated by two processes: 1) Time update and 2) measurement update.

In the time update process, we utilize a priori to calculate the state X^k+1− and the error covariance P^k+1− at time point t_{k + 1} by the following two equations:

where Φ means the state transition matrix. In the measurement update process, the state X^k+1 and the error covariance P^k+1 are computed as follows.

As is illustrated in the above, we can see that the Kalman Filter refers to an optimal estimator which recursively couples the most recent measurements into the linear model to update the model states. However, the Kalman Filter can only measure states of the systems, in which the dynamic and observation functions are linear [24]. To solve the problem of the INS/GPS integrated navigation system which is belonged to a nonlinear system measuring, an extended Kalman Filter are utilized in this paper.

An extended Kalman filter is able to estimate system state in the noisy environment [25]. Predicted state estimate x^k+1k, filtered state covariance P_k|k., and predicted state covariance P_{k + 1|k}. are estimated by the following equation.

where F() refers to the Jacobian matrix of f, and it is calculated as follows.

Afterwards, the following equations represent the measurement update process:

where H denotes the Jacobian matrix of h, and it is computed as follows.

We simulate the integrated navigation system based on the extended Kalman filter using Matlab, and the calculation procedure is listed as follows.

Step 1: Suppose that we have X (0) and P (0)

Step 2: Predicting the values of X^k,k−1 and h( X^k,k−1,k)

Step 3: Computing the coefficient Φ_{k,k − 1} and H_k

Step 4: Computing the P_{k,k − 1}

Step 5: Computing the extended Kalman gain K_k

Step 6: Estimating the values: X^k,Pk

Step 7: K++

Step 8: Go to Step 2.

5 Experimental simulation

According to the mathematical model the INS/GPS integrated navigation system discussed in the above chapter, and we utilize the northeast sky coordinates in this simulation. Particularly, longitude, latitude, speed at east direction, speed at north direction and Euler angle are used as state variables in the system simulation process. We exploit the extended Kalman filtering to estimate system states.

We suppose carrier flies horizontally along the equatorial eastward direction, and flight simulation parameters are set as follows: 1) initial position is located at 50 degrees North latitude and 110 degrees East longitude, 2) the height is set to 5000 m, 3) the initial rate is set to 170 m/s (for the East direction), and 0 m/s (for the North direction), 4) the initial error of the longitude and latitude are set to 50 m respectively, 5) the initial velocity error is 0.5 m/s, 6) platform Euler angle error is 0.12′, 7) the Gyro first order Markoff shift is 0.1°/h, 8) the first order Markov Kraft accelerometer bias is 10-3 g, and 9) sampled emulation interval and simulation time are set to 1 s and 600 s respectively.

The simulation process is implemented by Matlab, and then the extended Kalman filter is utilized to test the performance of filter divergence suppression and the wide range of adaptive capacity. Furthermore, we maintain the initial model unchanged in 0 s to 100 s. From 101 s to 200 s, measurement noises become five times and seven times as much as the initial value in the time range [101s,200s] and [201s,400s] respectively. Particularly, the experiments are developed using the MATLAB 7.0.1 simulation environment.

Afterwards, the measurement noises restore to its original value after 401 s. Initial state settings for filter are described in Table 1.

Afterwards, indicators of the system measurement noise variance are listed in Table 2.

Based on the above simulation environment, we utilize the extended Kalman filter to measure parameters of the INS/GPS integrated navigation system. Experimental results are shown in Figure 2 to Figure 8.

Figure 2

Platform error of the east orientation using the extended Kalman filter

Figure 3

Platform error of the north orientation using the extended Kalman filter

Figure 4

Platform error of the up orientation using the extended Kalman filter

Figure 5

Speed error of the east orientation using the extended Kalman filter

Figure 6

Speed error of the north orientation using the extended Kalman filter

Figure 7

Error of latitude using the extended Kalman filter

Figure 8

Error of longitude using the extended Kalman filter

From the experimental results, it can be observed that using the extended Kalman filter in integrated navigation system, the error rate of statement measuring is very low. The standard Kalman filter is very sensitive to the unexpected external perturbations, measurement accuracy is not satisfied by us. On the contrary, we can see that the extended Kalman filter performs robustly and can effective reduce the measurement error rate. Furthermore, from the simulation results, we find that 1) the extended Kalman filter is more stable in anti filter divergence without external calibration, and 2) the extended Kalman filter achieves a better performance for the unexpected perturbation. The reason why our algorithm performs better than other methods lies in that 1) a Kalman filter is used to estimate states of dynamic systems by a stochastic linear state-space model, and 2) we define the state vector and the error covariance matrix to help construct an integrated navigation system, and the above two matrices are computed by two processes: a) Time update and b) measurement update.

6 Conclusion

In this paper, we aims to design and simulate the INS/GPS integrated navigation system, and the main works are to estimate the position, velocity, and attitude of a vehicle with the output of inertial sensors. Firstly, the structure of the INS/GPS integrated navigation system is given. Afterwards, we proposed a novel method to simulate the integrated navigation system with the extended Kalman filter by measuring position, velocity and attitude. In the end, simulation results demonstrate that the extended Kalman filter performs stably in anti filter divergence without external calibration, and performs better than a Kalman filter for the unexpected perturbation.

To extend this work, in the future, we will try to use other initial state settings for filter to test the adaptability of the proposed design.