Design of Accurate Navigation System by Integrating INS and GPS using Extended Kalman Filter

Design of Accurate Navigation System by Integrating INS and GPS using Extended Kalman Filter

Santhosh Kumar S. A1,

1M.Tech student,

Digital Electronics and Communication Systems, PES institute of technology, Bangalore, Karnataka, India.

MS. Suganthi 2 2Assistant professor, Department of TEC,

PES institute of technology, Bangalore, Karnataka, India.

Abstract MEMS based Inertial Measurement Unit (IMU) has been the primary sensor for measuring attitude of Unmanned Aerial Vehicles. However inertial measurements drift in time because the basic parameters have been obtained by dead reckoning wherein the parameters are obtained from integration, so an external aid like the GPS (Global Positioning System) has been traditionally used to correct the measurements periodically. This project proposes to demonstrate this application in real time using GPS as an external aid in the correction process. Towards this Extended kalman filter (EKF) is proposed to be realized in real time. Integration and testing can be done in Matlab environment.

Key words: Navigation, INS, IMU, GPS, and EKF.

The layout of this paper is as follows. Section I gives Introduction of project. Section II explains the navigation using GPS / INS integration through block diagram representation. Sections III and IV describe the modelling of INS and GPS respectively. Section V explains the integration of GPS and INS using Extended Kalman Filtering technique as has been modelled in this work. Section VI presents the entire modelling done in this work, in block diagram form. Section VII presents the results of the Extended Kalman Filter based integration of GPS / INS for navigation and the conclusions drawn from them.

1. INTRODUCTION

   The navigation of unmanned vehicle (UV) can be done by various systems like, inertial navigation system (INS), global positioning system (GPS), radio navigation system, vision based navigation system (VBNS) and air-data dead reckoning system. Among these systems our interest is to integrating INS and GPS systems to provide the accurate navigation of UV. If we use the INS alone for navigation of UV we cannot achieve the accurate navigation because output of INS (position, velocity and attitude) drift with the time due to sensor errors like accelerometer bias of offset, accelerometer scale factor error, gyro drift or bias (due to temperature changes) and gyro scale factor errors. If we use the GPS alone for the navigation of UV we can achieve the accurate navigation only when GPS signal available, when the GPS signal outages occurs due to the error like Ephemeris errors, Ionosphere and troposphere delays, and

   Multipath errors the entire navigation system will broken down. The main aim of this project is to integrate INS and GPS to provide the navigation system with high accuracy in position and attitude, and navigation possible even when the GPS outages occur. The integration of this to system can be done by using the best dynamic estimator called kalman filter. There are three types of kalman filters are available one is conventional kalman filter and other two is extended kalman filter and unscented kalman filters. The conventional kalman filter is a linear filter it is used for linear systems and other two filters are non-linear filter they are used for non linear systems. Since most of the real time systems are non-linear systems therefore in this project we used EKF filter tor the integration of INS and GPS systems. The state vector estimation using INS is carried out at 100 Hz whereas the GPS measurement is taken at 1 Hz. Thus after every 100 propagations of the state vector, the state vector is corrected using the GPS measurement. In the absence of GPS signal, only INS data is used for navigation.

2. BLOCK DIAGRAM OF INS AND GPS INTEGRATING SYSTEM.

   6 DOF

   Matlab

   toolbox

   GY-80 IMU
   IMU Data (Acceleration and Angular Velocity)
   Atmega 328 Controller
   Zigbee Transmitter CC2500
   GPS receiver (S1216)	GPS Position
   Module on Object

   INS Position and attitude

   EKF

   Corrected

   position

   IMU Data

   Corrected

   Zigbee

   receiver

   Attitude

   GPS

   Position

   Ground Station

   Figure 1: block diagram representation of navigation using GPS/INS integration.

   Figure1 shows the block diagram representation of navigation using GPS/INS integration. It mainly contains four blocks namely inertial measurement unit (IMU), INS, GPS receiver, and Extended kalman Filter. INS takes the initial value of position, attitude and velocity and also it takes acceleration and angular rates, measured by the IMU, as inputs and integrates them to determine the position, velocity and attitude of UV. Once INS find the position, velocity and attitude of UV at time tk, it will take that as a initial values to find the position, velocity and attitude of UV at time tk+1. The GPS measures only the position of the UV. The EKF takes the INS state estimate and GPS position as inputs. It correct INS state estimate using the GPS position. In this way accurate navigation can be achieved by INS and GPS integration.

   Fxyz(N)

   Mxyz(N-m)

   Velocity in earth frame Position in earth frame Attitude

   6 DOF

   toolbox

   DCM

   Velocity in body frame

   Angular velocity Angular Acceleration Acceleration in m/s2

   Figure 3: 6DOF simulink toolbox.

3. INERTIAL NAVIGATION SYSTEM (INS).

Input to this toolbox is force and moment. Force can be calculated by using acceleration obtained from accelerometer as given below

Accelerometer

Acceleration

Angular

IMU

Gyroscope

velocity

Position

6

Degree of freedom matlab toolbox

Attitude

Velocity

Initial position, attitude and velocity

Where,

Figure 2: Inertial navigation system (INS) which is shown in figure 1.

The INS block, which is shown in figure 1, consists of Inertial Measurement Unit (IMU) and mechanization block. IMU has 3accelerometers to measure the acceleration in 3dimensions and 3gyroscopes to measure the angular velocities. The Mechanization block converts the IMU measurements from body frame to required navigation frame. Then the navigation frame measurements are integrated to obtain the velocity and the position of the vehicle. Position, velocity and attitude of vehicle from INS are denoted as Rins(deg) , Vins(m/s) and ins (deg) respectively.

Where

Rins = [ ¢ins ins hins ]T

• m is a mass of object in kilogram.

• ax, ay, az: is acceleration from accelerometer in m/s2.

  Moment can be calculated by using angular velocity obtained from gyroscope, the rotational dynamics of the body-fixed frame are given below,

  Where the applied moments are [L M N]T, and the inertia tensor I is with respect to the origin of body. This toolbox requires initial values of position, velocity and attitude to find the next state value of position, velocity and attitude. Initial position can be taken from the GPS, initial velocity when object is in stationary is assumed as zero, and Initial

  Vins

  = [ Vxins

  Vy ins

  Vzins ]T

  attitude when object is in stationary can be obtained from the three equations given below

  ins = [ roll pitch yaw]T

  In this project this hree parameters are obtained by using the 6 degree of freedom (DOF) matlab toolbox, which is shown below

  Roll = atan(ax/(-az))

  Pitch = atan(ay/sqrt(ax^2+az^2)) Yaw=atan((mx*cos(roll)+mz*sin(roll))/(mx*sin(

  roll)*sin(pitch)+my*cos(pitch)- my*cos(roll)*sin(pitch)))

  Where,

• mx, my, mz; are magnetic field obtained from magnetometer.

INS Error Model

Perturbation is a process of linearzing the nonlinear differential equations in order to perform error analysis. The linear position error dynamics can be obtained by perturbing equation 1, which are the dynamics equations for the geodetic positions. Since the position dynamics equations are functions of position and velocity, the position error dynamics equations are obtained using partial derivatives:

r^n = Frr rn + Frr vn

Where Re is the radius of the earth and is considered a constant.

The velocity error dynamics equation is expressed as

b

v(dot)n = Frr rn + Fvv vn + (fn X) n + Cn fb

Where f b is the acceleration of the aircraft in the body frame, and w are earth rotation rate and the rotation due to motion of the vehicle at a constant height above the ground

The attitude error dynamics equation can be written as

(bot)n = Fer rn + Fevvn (( + w)x) n – Cnbwibb

Where GPS has several advantages and disadvantages as following:

Advantages: precision during long term, absolute position and operational conditions.

Disadvantages: multipath problems, dependency to the United States Department of Defence and atmospheric delays.

1. EXTENDED KALMAN FILTER (EKF) BASED GPS

   / INS INTEGRATION

   Extended Kalman Filter is used to integrate two systems when the state of the system follows continuous state dynamics and the measurement of the second system is related to the estimates provided by the first system. The second system is used to correct the state estimates provided by the first system to yield an increased accuracy in state estimation. Integration of INS/GPS can be done in two ways one is by feedfoward method and other is by feedback method in this paper we follow the first method as shown bellow.

   INS

   Corrected position and attitude Predicted measurements based

   on the corrected inertial output

   Inertial error

   EKF

   GPS receiver

   +

   ib

   Where w b is the perturbation in the angular rate vector between the inertial frame and the body frame?

   The inertial navigation system presents some advantages and disadvantages as follow:

   • Advantages: complete output solution, good accuracy during short time, high data rate and small size.

   • Disadvantages: accuracy decrease after a long time, gravity sensitivity and Obligatory external aid for initialization.

     So, after have seen generally the inertial navigation system, we will try to define briefly the GPS working as in the follow paragraph.

     IV. GPS MODEL

     In real time GPS receivers, 4 of the visible satellites are selected for receivers position determination. In this work, only 3 of the visible satellites are selected for position determination. The GPS gives the latitude, longitude and altitude of the current location of the receiver. The update rate is 1 second. The GPS program uses WGS-84 approximation in which the earth is considered as an ellipse with a semi-major axis (equatorial radius) of a = 6; 378; 137m, and a semi-minor axis (polar radius) of b = 6; 356; 752:3142m.

     –

     Position obtained from GPS

     Figure 4: direct mode (feedback) integration.

     The first and the second systems represent process and measurement models respectively. The state estimates and the measurements provided by the first and second systems are denoted by x and z respectively. In this work, INS and GPS are considered as process and measurement models respectively. Extended Kalman Filter (EKF) has been used to integrate INS and GPS for an increased accuracy in UV navigation.

2. OVERALL ARCHITECTURE OF GPS / INS INTEGRATION USING EXTENDED KALMAN

   FILTER

   The EKF block takes as inputs the following: the state estimate prior to correction, the GPS measurement and the positions of the 3 satellites considered for the UV position determination in GPS modelling. The state estimate prior to correction and the GPS measurements are used to determine the state error. The Kalman Gain is determined and is used along with the error in state estimate prior to correction to determine the estimate correction. This state

   estimate correction is added to the state estimate prior to correction to obtain the state estimate post correction. The updated state estimate is used to determine the transition matrix and the process noise covariance matrix which are used to propagate the state error covariance matrix to the next time instant of state estimation.

   Prior Estimate Xk-, Pk-

   Compute kalman Gain

   Kk = Pk- HkT[HkPk- HkT+ Rk]-1

   Project ahead:

   Pk+1- = ¢k Pk+ ¢kT + Qk Xk+1- = ¢k Xk

   Update estimate with

   measurement Zk

   Xk = Xk- + Kk[Zk – Hk Xk-]

   Compute error covariance for update state Pk= [1- Kk Hk] Pk-

   Figure 5: The Extended Kalman Filter loop.

3. RESULTS AND CONCLUSION

In this section we discuss the results obtained from the simulation of individual subsystems, i.e. the INS and GPS and the integrated system.

Individual subsystems

Due to mechanical errors existing in the accelerometers and gyroscopes, the INS, individually, does not accurately give the position of the aircraft. As seen in figures 6 to 8,

Figure 6: Latitude calculated by the unaided INS and GPS

Figure 7: Longitude calculated by the unaided INS and GPS

Figure 8: altitude calculated by the unaided INS and GPS

This simulation has been done by modelling the sensors as explained in section VI. The updates from the gyroscopes and accelerometers are taken every 10ms. The above mentioned figures show us the typical output given by the GPS (blue line), with an update taken every second. A standard deviation of 20m has been assumed in modelling the GPS output. The GPS has long term accuracy and the INS has short term accuracy, hence the individual systems by themselves are not enough to give us a good and accurate measure of the location.

Integrated system

A nine-state model extended Kalman filter was implemented as described in section V. Figures 9 to 11, show the output of the simulation as well as the GPS output simulated for a period of 200s. The standard deviation chosen for the accelerometers here was 10mGal. By increasing the standard deviations of the accelerometers we can achieve the much better accuracy.

Figure 9: Extended Kalman Filtered output of Latitude

Figure 10: Extended Kalman Filtered output of Longitude

Figure 11: Extended Kalman Filtered output altitude

The update from the accelerometers and gyroscopes was taken every 0.01s, the GPS update was taken every 1s and the extended Kalman filter was run every 0.5s to achieve better accuracy. Every alternate 0.5s instant, when the GPS update is not available; we can predict the error state, using the most recent GPS update as the measurement, i.e. the GPS update is taken constant for that whole one second. This also comes in use when there are GPS outages. Figures 12 to 14 show us the results from running the program when we assume a GPS outage of 8s, from the period t = 30s to t = 40s. For this time period of GPS outage, the GPS values used by the program remain the

same as the last measured values, i.e. the values measured at t = 29s. At t = 41s, the GPS starts reading again, and new values are read by the program. During this time the extended Kalman filter relies totally on the INS and state predictions, and the accuracy is affected as we can see from the graphs. But once the new GPS values are read by the program, the extended Kalman filter takes vry less time, of the order of a few seconds, to settle down towards the actual trajectory.

Figure 12: Latitude calculated with GPS outage between 25s and 33s

Figure 13: Longitude calculated with GPS outage between 25s and 33s

Figure 14: Altitude calculated with GPS outage between 25s and 33s

The graphs for attitude computed and corrected by the extended Kalman filter can also done by the same manner. We cannot expect the extended Kalman filter to correct the attitude given by the INS perfectly as attitude is not a part of the measurement vector. We can only correct the attitude given by the INS using the attitude errors predicted by the state matrix. This corrected attitude forms a part of the integration loop in the whole system.

REFERENCES

1. Schmidt, G.T., \Strapdown Inertial Systems – Theory and Applications, " AGARD Lecture Series, No. 95,1978.

2. Bar-Itzhack, I.Y., and Berman, N., \Control Theoretic Approach to Inertial Navigation Systems, Journal of Guidance, Vol. 11, No. 3, 1988, pp. 237-245.

3. Grewal, M.S., and Andrews, A.P., Kalman Filtering: Theory and Practice usingMATLAB, John Wiley and Sons, New York, 2001.

4. Grewal, M.S., Weill, L.R., and Andrews, A.P., Global Positioning Systems, Inertial Navigation, and Integration, John Wiley and Sons, New York, 2001.

5. Wolf, R., Eissfeller, B., Hein, G.W., \ A Kalman Filter for the Integration of a Low Cost INS and an attitude GPS," Institute of Geodesy and Navigation, Munich, Germany.

6. Grejner-Brzezinska, D.A., and Wang, J., \Gravity modelling for High-Accuracy GPS/INS Integration, Navigation, Vol. 45, No. 3, 1998, pp. 209-220.

7. Srikumar, P., Deori, C.D., \ Simulation of Mission and navigation Functions of the UAV – Nishant," Aeronautical Development Establishment, Bangalore.

8. Randle, S.J., Horton, M.A., \ Low Cost Navigation Using Micro Machined Technology," IEEE Intelligent Transportation Systems Conference, 1997.

9. Gaylor, D., Lightsey, E.G., \ GPS/INS Kalman Filter desing for Spacecraft operating in the proximity of te International Space Station," University of Texas – Austin, Austin.

10. Brown, A., Sullivan, D., \ Precision Kinematic Alignment Using a low cost GPS/INS System," Proceedings of ION GPS 2002, Navsys Corporation, Oregon, 2002.