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

Open Phys. 2017; 15:182–187

Research Article Open Access

Weidong Zhou, Jiaxin Hou*, Lu Liu, Tian Sun, and Jing Liu Design and Simulation of the Integrated Navigation System based on Extended Kalman Filter

DOI 10.1515/phys-2017-0019 traffic management and other services [1, 2]. The naviga- Received Oct 30, 2016; accepted Nov 14, 2016 tion technology is used to guide an object from one point to another point [3]. Navigation system platforms are able Abstract: The integrated navigation system is used to esti- to measure instantaneous velocity, direction, position and mate the position, velocity, and attitude of a vehicle with posture, and then provides this information to the opera- the output of inertial sensors. This paper concentrates on tor and then provides proper guidance to the carrier [4, 5]. the problem of the INS/GPS integrated navigation system With the rapid progress and development of technol- design and simulation. The structure of the INS/GPS in- ogy, more and more information can be exploited in the tegrated navigation system is made up of four parts: 1) navigation system, and navigation methods have been up- GPS receiver, 2) Inertial Navigation System, 3) Extended dated frequently. Apart from traditional inertial navigation Kalman filter, and 4) Integrated navigation scheme. After- systems, (such as satellite navigation system, doppler nav- wards, we illustrate how to simulate the integrated naviga- igation systems, ground-based radio navigation system, tion system with the extended Kalman filter by measuring and so on) there are many new navigation systems, such as position, velocity and attitude. Particularly, the extended the celestial navigation system, the geomagnetic naviga- Kalman filter can estimate states of the nonlinear system in tion system, the scene matching system, the terrain match- the noisy environment. In extended Kalman filter, the esti- ing system, and the visual navigation system [6]. mation of the state vector and the error covariance matrix As is well known that, the INS/GPS navigation system are computed by steps: 1) time update and 2) measurement has been widely used in integrated navigation system. In- update. Finally, the simulation process is implemented by ertial Navigation System (denoted as INS) refers to a sys- Matlab, and simulation results prove that the error rate of tem which can compute the position, velocity, and attitude statement measuring is lower when applying the extended of a device with the output of inertial sensors [7]. Particu- Kalman filter in the INS/GPS integrated navigation system. larly, the measurements of the inertial sensors include er- Keywords: Integrated Navigation System; Extended rors because of physical limitations. However, errors may Kalman Filter; Inertial Navigation System; Global Posi- be accumulated in the navigation scheme of INS [8]. On tioning System; Nonlinear system the other hand, Global Positioning System (named as GPS) is often utilized as an aiding sensor in INS. Therefore, the PACS: 01.20.+x 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 1 Introduction solve this proposed problem, in this paper, we focus on the INS/GPS Integrated Navigation System simulation. Early navigation theory was used to handle ships sailing Innovations of this paper lie in the following aspects: on the ocean, however, navigation technology has pene- 1. The extended Kalman filter is used to estimate states trated into every conceivable a vehicle in recent years, in- of the nonlinear system in the noisy environment. cluding land search, remote tracking, ship management, 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 *Corresponding Author: Jiaxin Hou: College of Automation, Harbin Engineering University, Harbin 150001, China; Email: yuguo- update process. [email protected] In recent years, the rapid development of computer Weidong Zhou, Lu Liu, Tian Sun, Jing Liu: College of Automation, technology has encouraged the development of Kalman Harbin Engineering University, Harbin 150001, China.

© 2017 W. Zhou et al., published by De Gruyter Open. This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 License. Design and Simulation of INS Ë 183 filtering model, and Kalman filtering has been success- that the neural extended Kalman filter is used online to es- fully used in the integrated navigation system. To solve timate an error between the motion model of the mobile the problem in standard Kalman filtering, the extended robot and the real system performance [13]. Kalman filter is introduced in this paper. The rest ofthe Zareian et al. proposed an appropriate approach for paper is organized as follows. Section 2 illustrates related computing road friction coefficient. The proposed algo- works about the extended Kalman filter. In section 3, we rithm utilized measured values from wheel angular veloc- discuss how to design the INS/GPS integrated navigation ity and yaw rate sensors of a vehicle. Particularly, vehi- system. Section 4 proposes the method to simulate the cle lateral and longitudinal velocities along with yaw rate integrated navigation system using the Extended Kalman value are obtained by the extended Kalman filter. After- Filter. In section 5, experimental simulation is given the wards, lateral and longitudinal tidal forces are calculated test the performance of the proposed method. Finally, the with a recursive least square algorithm [14]. whole paper is concluded in Section 6. 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 2 Related works 5-input 3-output and 3 states estimation system, which utilized both the arbitrary nonlinear model error and the white Gauss measurement noise [15]. The basic function of navigation is to provide real-time Apart from the above works, the extended Kalman Fil- status information for the carrier. Due to system noises ter has been utilized in other fields, such as hemodynamic and measurement noises, how to extract the real states state estimation [16], Acoustic velocity measurement [17], of the information from the noise navigation environment Proton Exchange Membrane Fuel Cell [18], structural pa- should be solved. Furthermore, the key issue in integrated rameters estimation [19], and Radar Tracking [20]. In this navigation system is the fusion filtering. In order to effec- paper, we try to introduce the extended Kalman Filter to tively model the nonlinear systems, the extended Kalman estimate the states of the INS/GPS integrated navigation filter (EKF) has been proposed and widely used. system. The core idea of the extended Kalman filter is to ap- proximate nonlinear model by iterative filtering with a lin- earized method used in EKF is by using the nonlinear Tay- lor expansion which ignores the rest of the higher order 3 Overview of the INS/GPS terms. The extended Kalman filter has been exploited in integrated navigation system nonlinear filtering application, such as aerospace, navigation, target tracking, and so on. Related works on the ex- Inertial Navigation System (INS) refers to a system which tended Kalman filter are listed as follows. can compute the position, velocity, and attitude using Delgado et al. proposed a model-based method to inertial sensors. The measurements of the inertial sen- detect and isolate non-concurrent multiple leaks in a sors may include errors because of its design limitations, pipeline, and pressure and flow sensors are located at the and the errors are accumulated in the navigation process. pipeline ends. Particularly, the proposed method depends Hence, if the error is not compensated with other types on a nonlinear modelling derived from Water-Hammer of sensors, the information of INS can be used in a short equations, and the Extended Kalman Filters are exploited time. Currently, the Global Positioning System (GPS) can to calculate leak coefficients [11]. provide helpful information for INS. On the other hand, Bukhari et al. proposed a real-time algorithm to fore- GPS also has some disadvantages, such as low sampling cast respiratory motion in 3D space and realizing a gating rate and satellite signal loosing. Structure of the INS/GPS function without pre-specifying a particular phase of the integrated navigation system is shown in Figure 1. patient’s breathing cycle. The proposed method utilize an As is shown in Figure 1, INS system obtains inputs extended Kalman filter independently along each coordi- from the gyros and accelerometers, and a state vector is nate to forecast the respiratory motion and then exploits a defined as follows. Gaussian process regression network to revise the predic- T tion error of the EKF in 3D space [12]. X = [ϕE , ϕN , ϕU , δVE , δVN , δVU , δL, δλ, δh] (1) Miljkovic et al. used the EKF coupled with a feed- where parameters ϕE , ϕN , ϕU represent errors of the plat- forward neural network for the simultaneous localization form misalignment respectively, and δVE , δVN , δVU refer and mapping. The main innovations of this paper lie in to the velocity errors respectively. Furthermore, δL, δλ, δh 184 Ë W. Zhou et al.

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 Figure 1: Structure of the INS/GPS integrated navigation system stochastic linear state-space model as follows [21–23].

denote position errors of latitude, longitude and altitude X (t) = AX (t) + Bu (t) + W (t) (7) respectively. Based on the definition of state vector, the er- The above equation refers to that the continuous state ror state at time t is represented as follows. to control system dynamics. X̂︀ (t) = A (t) · X (t) + G (t) · W (t) (2) Yk = CXk + Vk (8) where W (t) is the model vector, and it is computed by the where this equation denotes the discrete measurement following equation. which is corresponding to the system X (t) to the available T Y u t W t W (t) = (εE , εN , εU , ∇E , ∇N , ∇U ) (3) measurements k. Moreover, ( ) is the model input, ( ) is the Gaussian process noise, and Vk is the measurement where εE , εN , εU refer to the errors of gyros drift, noises. ∇E , ∇N , ∇U denote the errors of accelerometer, and sym- Suppose that a time point is represented as tk = k · Ts, bol E, N, U refer to East, North and Up direction in the where Ts refers to a sampling period. The estimation of the geographical frame respectively. Particularly, matrix A is state vector x̂︀k and the error covariance matrix P̂︁k are cal- generated from the error equations of INS system, and culated by two processes: 1) Time update and 2) measure- G is the error transition matrix. Integrating the position ment update. information and velocity information together, and the In the time update process, we utilize a priori to cal- − − measurement equation to represent the observation of the culate the state X̂︀k+1 and the error covariance P̂︀k+1 at time integrated navigation system is described as follows. point tk+1 by the following two equations:

⎛ ⎞ ⎛ ⎞ tk δL LI − LG ∫︁ +1 − ⎜ δλ ⎟ ⎜ λ λ ⎟ X̂︀k = ΦX̂︁k + ΦBu (t)dt (9) ⎜ ⎟ ⎜ I − G ⎟ +1 ⎜ ⎟ ⎜ ⎟ Z (t) = ⎜ δh ⎟ = ⎜ hI − hG ⎟ = H (t) · X (t) + V (t) (4) tk ⎜ ⎟ ⎜ ⎟ ⎝δvE ⎠ ⎝vEI − vEG ⎠ δvN vNI − vNG − T P̂︀k+1 = ΦP̂︀k + P̂︀k Φ + Q (10) δL δλ δλ where , , mean the errors in latitude, longitude and where Φ means the state transition matrix. In the measure- height respectively, and δvE, δvN refer to the errors with ment update process, the state X̂︀k+1 and the error covari- the INS velocities ance P̂︀k+1 are computed as follows. H denotes a measurement matrix and V refers to the − (︁ − )︁ measurement errors. Afterwards, the INS/GPS integrated X̂︀k+1 = X̂︀k+1 + Kk+1 yk+1 − Ŷ︀k+1 (11) navigation system can be described the following dis- cretely transforming process. − P̂︀k+1 = (I − Kk+1C) P̂︀k+1 (12) X (k + 1) = A (k + 1) X (k) + Γ (k) W (k) (5) −1 − T(︁ − T )︁ Kk+1 = P̂︀k+1C CP̂︀k+1C + R (13) Z (k) = H (k) X (k) + V (k) (6) As is illustrated in the above, we can see that the Kalman Filter refers to an optimal estimator which re- cursively couples the most recent measurements into the Design and Simulation of INS Ë 185 linear model to update the model states. However, the 5 Experimental simulation Kalman Filter can only measure states of the systems, in which the dynamic and observation functions are lin- According to the mathematical model the INS/GPS inte- ear [24]. To solve the problem of the INS/GPS integrated grated navigation system discussed in the above chapter, navigation system which is belonged to a nonlinear sys- and we utilize the northeast sky coordinates in this sim- tem measuring, an extended Kalman Filter are utilized in ulation. Particularly, longitude, latitude, speed at east di- this paper. rection, speed at north direction and Euler angle are used An extended Kalman filter is able to estimate system as state variables in the system simulation process. We state in the noisy environment [25]. Predicted state esti- exploit the extended Kalman filtering to estimate system mate ̂︀xk |k , filtered state covariance Pk|k , and predicted +1 states. state covariance P are estimated by the following k+1|k We suppose carrier flies horizontally along the equato- equation. rial eastward direction, and flight simulation parameters (︁ )︁ ̂︀xk|k −1 = f ̂︀xk−1|k −1 (14) are set as follows: 1) initial position is located at 50 degrees North latitude and 110 degrees East longitude, 2) the (︁ )︁ (︁ )︁T height is set to 5000 m, 3) the initial rate is set to 170 m/s P F x P F x Q k|k −1 = ̂︀k−1|k −1 k−1|k −1 ̂︀k−1|k −1 + k−1 (15) (for the East direction), and 0 m/s (for the North direction), 4) the initial error of the longitude and latitude are set to where F() refers to the Jacobian matrix of f, and it is calcu- 50 m respectively, 5) the initial velocity error is 0.5 m/s, 6) lated as follows. ′ platform Euler angle error is 0.12 , 7) the Gyro first order ∂f (xi) ∘ Fij(x) = (16) Markoff shift is 0.1 /h, 8) the first order Markov Kraft ac- ∂xj celerometer bias is 10-3 g, and 9) sampled emulation inter- Afterwards, the following equations represent the val and simulation time are set to 1 s and 600 s respectively. measurement update process: The simulation process is implemented by Matlab, (︀ )︀ and then the extended Kalman filter is utilized to test vk = yk − h ̂︀xk|k−1 (17) the performance of filter divergence suppression and the (︀ )︀ (︀ )︀T wide range of adaptive capacity. Furthermore, we main- Sk = H x | P | H x | + Rk (18) ̂︀k k−1 k k−1 ̂︀k k−1 tain the initial model unchanged in 0 s to 100 s. From 101 s to 200 s, measurement noises become five times and (︀ )︀T −1 Kk = Pk|k−1 H ̂︀xk|k−1 + Sk (19) seven times as much as the initial value in the time range [101s, 200s] and [201s, 400s] respectively. Particularly, ̂︀xk|k = ̂︀xk|k−1 + Kk vk (20) the experiments are developed using the MATLAB 7.0.1 simulation environment. T Pk|k = Pk|k−1 − Kk Sk KK (21) Afterwards, the measurement noises restore to its orig- inal value after 401 s. Initial state settings for filter are de- H h where denotes the Jacobian matrix of , and it is com- scribed in Table 1. puted as follows.

∂h (xi) Table 1: Initial state settings for filter Hij (x) = (22) ∂xj Initial state value We simulate the integrated navigation system based ∘ on the extended Kalman filter using Matlab, and the cal- Position error longitude 110 E ∘ culation procedure is listed as follows. latitude 50 N height 5000 m Step 1: Suppose that we have X (0) and P (0) Gyroscope Zero offset 5 (∘/h) Step 2: Predicting the values of X̂︀k k and , −1 ∘ (︁ )︁ Random value 1·10−2( /h) h X̂︀k,k−1, k Accelerometer Zero offset· 3 10−5g0 Step 3: Computing the coefficient Φk,k−1 and Hk Random value 2·10−5g0 Step 4: Computing the Pk,k−1 Step 5: Computing the extended Kalman gain Kk Step 6: Estimating the values: X̂︀k , Pk Step 7: K + + Afterwards, indicators of the system measurement Step 8: Go to Step 2. noise variance are listed in Table 2. 186 Ë W. Zhou et al.

Table 2: Indicators of the system measurement noise variance

Noise mean square deviation Value Gyro white noise drift 3(∘/h) Accelerometer white noise drift 0.001m/s2 Receiver Precision of pseudo range 2.0m (1σ) technical measurement index Measurement accuracy of 0.1m/s Figure 5: Speed error of the east orientation using the extended Pseudo range rate (1σ) Kalman filter Computation Inertial measurement data 0.001s period update cycle Satellite receiver data 0.1s update cycle Navigation computing cycle 0.1s

Figure 6: Speed error of the north orientation using the extended Kalman filter

Figure 2: Platform error of the east orientation using the extended Kalman filter

Figure 7: Error of latitude using the extended Kalman filter

Figure 3: Platform error of the north orientation 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 un- Figure 4: Platform error of the up orientation using the extended Kalman filter expected external perturbations, measurement accuracy is not satisfied by us. On the contrary, we can see that the extended Kalman filter performs robustly and can ef- Based on the above simulation environment, we uti- fective reduce the measurement error rate. Furthermore, lize the extended Kalman filter to measure parameters of from the simulation results, we find that 1) the extended the INS/GPS integrated navigation system. Experimental Kalman filter is more stable in anti filter divergence with- results are shown in Figure 2 to Figure 8. out external calibration, and 2) the extended Kalman filter Design and Simulation of INS Ë 187 achieves a better performance for the unexpected pertur- [8] Perera L.P.,Guedes S.C., Collision risk detection and quantifica- bation. The reason why our algorithm performs better than tion in ship navigation with integrated bridge systems, Ocean Eng, 2015, 109, 344-354. other methods lies in that 1) a Kalman filter is used to esti- [9] Wang Q.Y., Diao M., Gao W., Zhu M.H., Xiao S., Integrated navi- mate states of dynamic systems by a stochastic linear state- gation method of a marine strapdown inertial navigation system space model, and 2) we define the state vector and the er- using a star sensor, Meas Sci Tech, 2015, 26, 6-75. ror covariance matrix to help construct an integrated nav- [10] Hu S.X., Xu S.K., Wang D.H., Zhang A.W., Optimization Algo- igation system, and the above two matrices are computed rithm for Kalman Filter Exploiting the Numerical Characteristics by two processes: a) Time update and b) measurement up- of SINS/GPS Integrated Navigation Systems, Sensors, 2015, 15, 28402-28420. date. [11] Delgado-Aguinaga J.A., Besancon G. Begovich O., Carvajal J.E., Multi-leak diagnosis in pipelines based on Extended Kalman Fil- ter, Control Eng Pract, 2016, 49, 139-148. [12] Bukhari W., Hong S.M., Real-time prediction and gating of res- 6 Conclusion piratory motion in 3D space using extended Kalman filters and Gaussian process regression network, Phys Med Biol, 2016, 61, In this paper, we aims to design and simulate the INS/GPS 1947-1967. integrated navigation system, and the main works are to [13] Miljkovic Z., Vukovic N., Mitic M., Neural extended Kalman fil- estimate the position, velocity, and attitude of a vehicle ter for monocular SLAM in indoor environment, P I Mech Eng C-J Mec, 2016, 230, 856-866. with the output of inertial sensors. Firstly, the structure of [14] Zareian A., Azadi S., Kazemi R., Estimation of road friction coef- the INS/GPS integrated navigation system is given. After- ficient using extended Kalman filter, recursive least square, and wards, we proposed a novel method to simulate the inte- neural network, P I Mech Eng K-J Mul, 2016, 230, 52-68. grated navigation system with the extended Kalman filter [15] Liu W., He H.W., Sun F.C., Vehicle state estimation based on Min- by measuring position, velocity and attitude. In the end, imum Model Error criterion combining with Extended Kalman Fil- ter, J Franklin I S, 2016, 353, 834-856. simulation results demonstrate that the extended Kalman [16] Aslan S., Comparison of the hemodynamic filtering methods filter performs stably in anti filter divergence without ex- and particle filter with extended Kalman filter approximated pro- ternal calibration, and performs better than a Kalman filter posal function as an eflcient hemodynamic state estimation for the unexpected perturbation. method, Biomed Signal Proces, 2016, 25, 99-107. To extend this work, in the future, we will try to use [17] Le D.A., Plantier G., Valiere J.C., Gazengel B., Acoustic velocity other initial state settings for filter to test the adaptability measurement by means of Laser Doppler Velocimetry: Develop- ment of an Extended Kalman Filter and validation in free-field of the proposed design. measurement, Mech Syst Signal PR, 2016, 70-71, 832-852. [18] Bressel M., Hilairet M., Hissel D., Bouamama B.O., Extended Kalman Filter for prognostic of Proton Exchange Membrane Fuel Cell, Appl Energ, 2016, 164, 220-227. References [19] Pan S.W., Xiao D., Xing S.T., Law S.S., Du P.Y., Li Y.J., A general extended Kalman filter for simultaneous estimation of system [1] Batista P., Silvestre C., Oliveira P., Tightly coupled long and unknown inputs, Eng Struct, 2016, 109, 85-98. baseline/ultra-short baseline integrated navigation system, Int [20] Kulikov G.Y., Kulikova M. V., The Accurate Continuous-Discrete, J Syst Sci, 2016, 47, 1837-1855. Extended Kalman Filter for Radar Tracking, IEEE T Signal Proces, [2] Kong X.Z., Duan X.G., Wang Y.G., An integrated system for plan- 2016, 64, 948-958. ning, navigation and robotic assistance for mandible recon- [21] Wang Y.W., Binaud N., Gogu C., Bes C., Fu J., Determination of struction surgery, Intell Serv Robot, 2016, 9, 113-121. Paris’ law constants and crack length evolution via Extended [3] Yang S.J., Yang G.L., Zhu Z.L., Li J., Stellar Refraction-Based and Unscented Kalman filter: An application to aircraft fuselage SINS/CNS Integrated Navigation System for Aerospace Vehi- panels, Mech Syst Signal Pr, 2016, 80, 262-281. cles, J Aerospace Eng, 2016, 29, 25-34. [22] Zhang S., Zhao J.H., Zhao Y., Li G.L., Gain-Constrained Extended [4] Yun S.C., Lee Y.J., Sung S.K., Range/Optical Flow-aided Inte- Kalman Filtering with Stochastic Nonlinearities and Randomly grated Navigation System in a Strapdown Sensor Configuration, Occurring Measurement Delays, Circ Syst Signal Pr, 2016, 35, Int J Control Autom, 2016, 14, 229-241. 3957-3980. [5] Sasani S., Asgari J., Amiri-Simkooei A.R., Improving MEMS- [23] Peng X.J., Cai Y., Li Q., Wang K., Comparison of reactivity es- IMU/GPS integrated systems for land vehicle navigation appli- timation performance between two extended Kalman filtering cations, GPS Solut, 2016, 20, 89-100. schemes, Ann Nucl Energy, 2016, 96, 76-82. [6] Zhang L., Xiong Z., Lai J.Z., Liu J.Y.,Optical flow-aided navigation [24] Li B., You N., Research on the Dynamic Evolution Behavior of for UAV: A novel information fusion of integrated MEMS naviga- Group Loitering Air Vehicles, Appl Math Nonl Sci, 2016, 1, 353- tion system, OPTIK, 2016, 127, 447-451. 358. [7] Liu Y.T., Xu X.S., Liu X.X., Zhang T., Li Y., Yao Y.Q., Wu L., Tong [25] Vishwanath B. Awati S.N., Mahesh K. N., Multigrid method for J.W., A Fast Gradual Fault Detection Method for Underwater In- the solution of EHL line contact with bio-based oils as lubri- tegrated Navigation Systems, J Navigation, 2016, 69, 93-112. cants, Appl Math Nonl Sci, 2016, 1, 359-368.