Localization and Outdoor Navigation of a car-like robot Dissertation

Localization and Outdoor Navigation of a car-like robot
Dissertation (Phase-2)
Submitted in partial fulfillment of the requirements for the degree of
Master of Technology
In
Mechanical Engineering
(Robotics)
By
AJAY KUMAR
(Reg. No: 16-06-08)
Under the Guidance of
Mr. S. D. Adlinge, Scientific Officer (Internal Supervisor)
Mr. K. Rajkumar (Sc C,External Supervisor)
Center for Artificial Intelligence and Robotics (CAIR)

Defense Institute of Advanced Technology
(Deemed University)
Girinagar, Pune 411025
AcknowledgementI express my sincere gratitude and indebtedness to the thesis supervisor Dr. Mr. S. D. Adlinge, Scientific Officer, Material Management Group for their initiative in this field of research, for their valuable guidance, encouragement and affection for the successful completion of this work. Their sincere sympathies and kind attitude always encouraged me to carry out the present work firmly.

I sincerely thank the management and the teaching staff of DIAT for giving an opportunity to many people like me to enhance their academic qualifications along with professional experience. I would thank my mentor, Mr. K. Rajkumar, Scientist C, Center for Artificial Intelligence and Robotics, for constantly encouraging throughout the course.

I thank my colleagues for all the interactions and knowledge sharing we had relating to the domain of work.
Above all, I thank the almighty and my parents for what I am today. ABSTRACT
Navigation is a tool, which has gained popularity for land vehicles in recent years. However, achieving navigation with high accuracy is still a desire to be accomplished. In pursuit of achieving this goal in a low-cost budget, multi-sensor data fusion has been seen as a promising solution.

The Global Positioning System (GPS) and an Inertial Navigation System (INS) are two basic navigation systems. Due to their complementary characters in many aspects, a GPS/INS integrated navigation system has been a hot research topic in the recent decade. The estimation accuracy of a low-cost inertial measurement unit (IMU) is limited by the accuracy of the used sensors and the imperfect mathematical modelling of the error sources. By fusing the IMU data with GPS data, the errors can be bounded and the accuracy increases considerably. For fusion of data and minimization of error, a Kalman filter based approach has been developed in this work. The work puts forward a cost-benefit solution for achieving accurate navigation through sensor fusion.

TABLE OF CONTENTS
Acknowledgment…………………………………………………………………………i
Abstract………………………………………………………………………………….ii
List of figures…………………………………………………………………………….v
Introduction …………………………………………………………………………1
Background……………………………………………………………………..1
Integration Methodology…………………………………………..….3
Research Aim and Objective…………………………………………………….4
Problem definition………………………………………………………………5
Review of Literature…………………………………………………………….…..6
Theory ……………………………………………………………………………..12
Inertial Navigation………………………………………………………….…12
Co-ordinate Frames……………………………………………………13
Motion of Land Vehicle…………………………………………….…15
Accelerometer………………………………………………………….15
Acceleration and force……………………………………………16
Gyroscope………………………………………………………………17
MEMS Gyroscope……………………………………………….18
FOG Gyroscope………………………………………………….19
Laser Gyroscope…………………………………………………19
Global Positioning System…………………………………………………….20
Sources of error……………………………………………………………….20
Bias…………………………………………………………………….21
Scale factor…………………………………………………………….21
Repeatability………………………………………………………..….21
Resolution…………………………………………………………..….22
Stability ………………………………………………………………..22
Noise …………………………………………………………………..22
Methodology……………………………………………………………………….23
Integration methods……………………………………………………………23
Loose Integration………………………………………………………23
Tight Integration……………………………………………………….24
Estimation techniques…………………………………………………………26
Estimation of Dynamic systems……………………………………….26
Test Case…………………………………………………………30
Estimation of Non-Linear systems…………………………………….30
Test Case…………………………………………………………31
Results……………………………………………………………………………..33
Conclusion and Future Scope……………………………………………………..40
Conclusion……………………………………………………………………40
Future Scope………………………………………………………………….40
References…………………………………………………………………………41
Appendix I………………………………………………………………………………45
LIST OF FIGURES
Figure Title Page No.

Figure 1.1 IMU (Microstrain 3DM-GX1) 1
Figure 1.2 GPS 2
Figure 3.1 Relations between Different Co-ordinate Frames 14
Figure 3.2 Land Vehicle Navigation Co-ordinate Frame 15
Figure 3.3 Accelerometer 16
Figure 3.4 Mechanical Gyroscope 17
Figure 3.5 MEMS Gyroscope 18
Figure 3.6 Tuning Fork Configuration of MEMS Gyroscope 19
Figure 4.1 Loose Integration 24
Figure 4.2 Tight Integration 25
Figure 4.3 Estimation Block Diagram 26
Figure 4.4 Algorithm for Estimation-prediction using Kalman filter 29
Figure 4.5 Kalman filter test 30
Figure 4.6 Extended Kalman filter test 31
Figure 5.1 Comparison of various AHRS algorithm with filtered data 32
Figure 5.2 Plot between GPS, INS, Novatel, Spatial output and Kalman Filter output 33
Figure 5.3 Output with 60s GPS outrage 34
Figure 5.4 Velocity estimation and comparison for 60s GPS outrage 35
Figure 5.5 Northing Position 36
Figure 5.6 Easting Position 36
Figure 5.7 Plot between GPS, Odometer, and Kalman Filter output 37Figure 5.8 Yaw angle comparison 38
Chapter 1
Introduction
1.1 Background
Inertial Navigation Systems (INS) became widely used in military vehicles and civilian aircraft since the origin of inertial guidance during the second world war. Primarily, the high cost of such systems has restricted the use of INS for other navigation and surveying applications. However, in the last two decades, two significant developments have broadened the use of INS: integration of INS with the Global Positioning System (GPS) and the development of lower cost inertial components.

Combining INS with other navigation systems such as GPS has gained significance due to both systems having complimentary error characteristics. GPS provides consistent accuracy position and velocity provided that there is line-of-sight between the GPS receiver and the orbiting GPS satellites. The latest generation of receivers are capable of providing Real-Time Kinematic (RTK) positioning to centimetre level accuracy by using relative positioning over short baselines. In order to achieve such accuracy, integer ambiguity resolution is required in order to resolve the unknown number of integer carrier phase ambiguities between the GPS receiver and satellite.

Fig.1.1 IMU (MicroStrain 3DM-GX1)
On the other hand, INS is an autonomous system that does not require measurements to external signals. It provides high accuracy short-term position, velocity and attitude at a high data rate. INS does, however, require accurate knowledge of the initial position and attitude, and the accuracy reduces over time due primarily to the imperfections in the inertial sensors.

Combining GPS and INS measurements can greatly reduce the shortcomings of each standalone system to provide increased reliability and accuracy. The GPS estimates of position and velocity are used to restrict the growth of the INS errors over time and allow estimation of the inertial sensor errors. The INS provides high accuracy short term navigation information at a high data rate which can also be used to aid GPS ambiguity resolution, correct cycle slips and bridge periods when there is no signal reception.

Micro Electro-Mechanical Sensors (MEMS) have emerged in the last 20 years as a low cost method for mass producing small, lightweight accelerometers and gyros. MEMS accelerometers have developed primarily from the automotive industry where they are used for triggering safety devices. MEMS accelerometers and gyros have diverse applications in other fields such as virtual reality, robotics, and machine control and monitoring, and are increasingly being used in navigation applications. However, they generally provide only short-term navigation capability due to large sensor errors that require continuous calibration. GPS measurements can be used to estimate these sensor errors.

Fig. 1.2 GPS
The use of lower cost inertial components with GPS has opened up a diverse range of applications. Originally, GPS and INS has been confined to high accuracy navigation and geo-referencing applications due to the high cost of the technology, and also due to government regulation. Now, due to the potential for high accuracy mass-fabricated low cost MEMS sensors, GPS and INS applications have expanded to fill a range of new navigation applications such as personal and vehicular navigation where increased performance and robustness is required over standalone GPS systems.

1.1.1 Integration methodology
The Kalman filter has emerged as the dominant data fusion method for integrating the two systems as it allows estimation of time-dependent variables termed states. The Kalman filter uses mathematic and stochastic models to estimate the INS error using measurements provided by GPS. The mathematical model comprises of a functional model to relate the measurements to the states to be estimated, and a dynamic model to describe the way in which the INS errors develop over time. These models are considered to be well defined for GPS and INS systems.

The stochastic information describing each system is used by the Kalman filter in order to correctly weight the new measurements to update the states. The stochastic information is obtained using the specification of the GPS and an INS system provided by the manufacturer, or is estimated from empirical testing in a process termed tuning. This information is defined a priori and generally remains constant throughout processing runs. This has been identified as a deficiency in the conventional Kalman filter algorithm for navigation applications where the stochastic properties of the sensors change over time. These errors depend on factors such as vehicle dynamics and environmental conditions and are consequently difficult to estimate. One example of the time varying nature of low cost INS errors is highlighted during the initialization and alignment of the INS using GPS updates. Initially, the INS navigation and sensor errors can be large and they vary each time the sensor is switched on. Using a low noise estimate for the INS errors results in a precise, yet most likely biased, estimate which results in a long transition to the correct value. Conversely, a larger noise estimate results in a quicker transition to the correct value but results in a noisier estimate which increases the navigation errors. This highlights the need for adapting the stochastic information used in the filter as the inertial sensor errors are reduced through alignment and the stochastic properties of the INS change.

In order to reduce the dependence on a priori information and allow the filter to adapt on-line, several extensions to the conventional Kalman filtering algorithm exist. These techniques use information from the innovation or residual sequence in the conventional Kalman filtering algorithm to supply additional stochastic information which is used to adapt the a priori information. This adaptation of the stochastic properties of the filter is generically termed adaptive Kalman filtering. The primary potential benefit of adaptive Kalman filtering is improved performance in terms of reducing the navigation errors of position, velocity and attitude. This includes reducing the time required for INS alignment and resolution of the initial sensor errors.

1.2 Research Aims and Objectives
The aim of the research undertaken in this thesis is to analyse and improve the system performance of GPS integrated with a low cost Inertial Measurement Unit (IMU).Novel research is also carried out into the use of GPS and low cost IMU as a potential replacement for higher cost systems.

The broad aims of this research are therefore summarized as follows:
Undertake research into improving the system performance of integrated GPS and low cost INS.

Develop and investigate the performance of an integrated GPS and low cost INS for marine applications.

To investigate the performance of conventional Kalman filtering for GPS and low-cost INS.

1.3 Problem definition
For exact position on the Earth of an object different types of Navigation Systems are used. In the case of outdoor applications, like for navigation or geodetic measurements, mainly Global Navigation Satellite Systems (GNSS) is used. With the help of different types of equipment, it is very easy to determine the position of anyone on the Earth’s surface with accuracy of few centimetres. However, consumer grade receivers have accuracy in the order of 15-100 meters. Furthermore, it implies a line-of-sight connection to the satellites. But in the case of urban areas it gives very poor performance. Apart from urban areas, its performance in normal areas also not up to the mark due to the high buildings, which block the signals. As well as in forests, the quality of the position estimates degrades or even leads to a signal blocking. Another drawback of this system is the slow update rate, it gives output usually once in a second. In applications such as driver training, car racing, driving exercises, and in performance measurements, more frequent position estimation is required for an accurate trajectory. In addition to that, to know the exact position, more information such as the velocity, position and attitude of the vehicle is desired.

Inertial measurement units (IMU) are a common complement to GPS, although it is technically more correct to state that GPS augments an inertial navigation system (INS). The advantage being that together the GPS and inertial sensors can provide a continuous navigation solution, where GPS alone cannot.

Chapter 2
Review of literature
This chapter describes a brief history of previous work done by various researchers and scientists that made efforts to in the direction of integration of inertial sensors and GPS data for accurate as well as cost-effective navigation.

O. M. Maklouf et al 6, discussed the GPS and INS integration for a low cost sensors which can be used for navigation for a car like robot. The data fusion process is done with an Extended Kalman filter in cascade configuration mode. Loosely coupled Kalman filter configuration is considered for the fusion. The result obtained, demonstrates that a low cost INS/GPS navigation system is partially capable of meeting the performance requirements for a land vehicle navigation.

Hamad Ahmed et al 5, presented a method for localizing a land vehicle using low cost MEMS IMU sensor and a terrain map in GPS denied environment. A Kalman filter is used to compensate the external acceleration acting in the lateral direction using a gyroscope and odometery providing an accurate estimate of the vehicle’s roll angle. A particle filter is then uses roll angle estimated by the kalman filter along with pre-determined terrain map of the driving region to localize the vehicle.

Ming-Han Lee et al 7, presets the design and implementation of a sensor fusion system for navigation and control of an autonomous electric vehicle. The system integrates the signals of laser range finder, magnetometer and IMU. The system uses the kalman filter to estimate the state of the vehicle and fuzzy logic controller to decide the behaviour of the vehicle.
Markus Langer et al 4, describes non-coherent deeply coupled GPS/INS integration in a pedestrian navigation system to improve position accuracy and availability in weak signal conditions. The system consists of a torso mounted IMU and is used for step detection and step length and heading detection. A barometer, magnetometer and GPS is used for absolute positioning. Using deeply coupled integration system, tracking of GPS signals under weak signal conditions are possible and a seamless transition between indoor and outdoor situations is achieved.

Xiufeng He et al 3, presents an interval filtering algorithm which is applied to the uncertain integrated system. The system parameters uncertainties are described by intervals. The Inteval Adaptive Kalman Filter (IAKF) algorithm is established tfor the uncertain integrated system. The IAKF algorithm has the same structure as the standard extended Kalman filter. The results shows that the IAKF is effective for the uncertain nonlinear integrated system and it can be used to test the chosen parameters of an integrated GPS/IMU system.

Hong-Viet Nguyen et al 2, presents a tightly coupled GPS/INS integration algorithm for attitude estimation using low cost sensors. The attitude estimation is independent to provide accurate Eular angles for INS system and the magnetic sensor reduce the growth-rate of navigation error. The tightly coupled INS/GPS model is built with 23 state variables, bias and scale factor of IMU are estimated to correct INS system. The position, velocity and time information of GPS is used as measurement model in the Kalman filter.results shows that the tightly coupled algorithm has higher accuracy and more robust than the loosely coupled algorithm when GPS signal is corrupted or when there are less than 4 satellite signals are received.

Douglas Spares dos Santos et al 1, presents the autonomous navigation of a small boat. The Kalman filter is applied to estimate the real time position and heading using the measurement from a low cost IMU, a standard GPS and a digital compass. A look-up table is used to decide the commands for the boat actuators such that it tracks the way-points autonomously.

Dorota A. Grejner-Brzezinska et al 24, proposed the design of a high-accuracy geolocation methodology that address centimeter-level relative accuracy requirements of a man-portable electromagnetic (EM) sensor system in open and impeded environments. The proposed system design is based on the tight quadruple integration of the Global Positioning System (GPS), the inertial measurement unit (IMU) system, the terrestrial radio-frequency (RF) system pseudolite (PL), and terrestrial laser scanning (TLS) to support high-accuracy geolocation for a noncontact EM mapping system in GPS-challenged environments. The key novel component of the proposed multisensor system is the integration of TLS that can provide centimeter-level positioning accuracy in a local frame and thus enables a GPS/IMU/PL-based navigation system to achieve both high absolute and relative positioning accuracy in GPS-impeded environments.

Seong-Baek Kim et al 19, presents a bimodal approach for integrating IMU and DGPS, taking advantage of positioning and orientation data calculated from CCD images based on photogrammetry and stereo-vision techniques. The positioning and orientation data from the photogrammetric approach are fed back into the Kalman filter to reduce and compensate IMU errors and improve the performance.

Debo Sun et al 16, reduced inertial measurement unit (RIMU) is ultratightly (UT) integrated with a vector-based Global Positioning System (GPS) receiver using two approaches; vector delay and frequency lock loops (VDLL/VFLL), and a VDLL with a cascaded Phase Lock Loop (CaP). A Cascaded PLL plus a frequency discriminator (CaPF) composite loop is developed to provide reliable Doppler measurements and improves navigation performance by up to 20% compared with other methods using real world data.

Farhad Aghili el at 15, integration of inertial measurement unit (IMU) with two real-time kinematic global positioning system (GPS) units in an adaptive Kalman filter (KF) for driftless estimation of a vehicle’s attitude and position in 3-D. The observability analysis reveals that integration of a single GPS with IMU does not constitute an observable system and integration of two GPS units with IMU results in a locally observable system provided that the line connecting two GPS antennas is not parallel with the vector of the measured acceleration, i.e., the sum of inertial and gravitational accelerations. It makes it possible to compensate the error in the estimated orientation due to gyro drift and its bias without needing additional instrument for absolute orientation measurements, e.g., magnetic compass. In order to cope with the fact that GPS systems sometimes lose their signal and receive inaccurate position data, the self-tuning filter estimates the covariance matrix associated with the GPS measurement noise.

Mohammad Abdel Kareem Jaradat et al 26, presented an architecture based on an adaptive neuro-fuzzy inference system is proposed for fusing the GPS/IMU measurements. This integration incorporates the variable delay between the IMU and GPS signals as an additional input to the fusion system. In addition, once the GPS signal becomes available, the measurement is used as a correction reference value to
provide an enhancement to the estimation accuracy.

Giovanni Fusco et al 21, demonstrated a novel computer vision-based localization approach that is tailored to the street intersection domain. The technique harnesses the availability of simple, ubiquitous satellite imagery (e.g., Google Maps) to create simple maps of each intersection. The technique scale naturally to the great majority of street intersections in urban areas, but it has the added advantage of incorporating the specific metric information. The key approach used is the integration of IMU (inertial measurement unit) information with geometric information obtained from image panorama stitchings.

Pinar Oguz-Ekim et al 22, proposed an algorithm to self-localize a vehicle in a GNSS-denied environment. The algorithmis designed for vehicles driving on inner city streets, which areusually made up of an amalgamation of straight and curved trajectories. Data from an inertial measurement unit and map information are fused with radio frequency time-of-arrival measurements, obtained from a single 802.11p Dedicated Short-Range Communication systems (DSRC) roadside unit, to track the vehicle along both the straight and curved portions of a trajectory.

CHEN Jingjing et al 14, analyzed the influence of time synchronization errors on INS/GPS tightly integrated navigation, studies the case where the time synchronization errors are not included in the integration filter, which leads to an increased error covariance and the inaccuracy of IMU bias estimation, and proposes the countermeasure in which the time synchronization errors are regarded as a part of the estimation problem in the data integration, besides, uses two different measurements in Kalman filter
Yuquan Wang et al 10, presented Vehicle model based concept for state estimator setup that will use signals that are available on modern vehicles. An extension of a commonly used Bicycle representation of the vehicle is applied with an automated tuning for signal disturbances. For coping with different update frequencies from GNSS and motion sensors a Bezier extrapolation is used. The data obtained was then integrated using Adaptive Kalman Filter. The resulting Adaptive Kalman Filter approach was compared to recorded signals from driving tests.

Ling Chen et al 20, investigated IMU and GPS integration in pedestrian localization. The position calculation is achieved in sequence by three different strategies, namely basic double integration of IMU data, Zero-velocity Update (ZUPT) and Extended Kalman Filter(EKF) based fusion of IMU and GPS data. Instead of using IMU alone, we integrate ZUPT strategy of IMU with GPS by designing an EKF algorithm to optimally estimate the position and attitude of a person while walking.

Slawomir Romaniuk et al 27, presented a Kalman Filter design which was programmed and evaluated on STM32 dedicated platform. The work was performed to achieve accelerate estimation of position and attitude, which was further used for unmanned arial vehicle autopilot. Raw data of IMU and GPS were used to integrate using Kalman Filter. The data obtained from integration was then compared with the raw data from IMU and GPS.

Chapter 3
Theory
This chapter gives details about the inertial navigation systems also; it discusses fundamentals important in understanding integration of sensors for navigation.

3.1 Inertial Navigation
In this chapter we will discuss about the principles of inertial navigation system. At first we describe about the basics of modelling motion in land vehicle. Second an introduction to inertial sensors is given and then discuss about some useful information like position, velocity, what we get from inertial navigation system as output. Finally we will discuss the drawbacks of inertial navigation systems and try to understand why inertial navigation system is better to use with other navigation system than using it alone.

Basically, inertial sensors have two main implementations one is the gimballed arrangement, and another is the strap-down arrangement. In gimballed type of arrangement, the sensors are mounted on a gimballed mechanized platform, which is always remain aligned with the navigation frame. In the case of strap-down arrangement, it is directly mounted to the vehicle. This,however, requires a higher dynamic range and bandwidth, with a higher computational load also. The advantages of strap-down arrangements are less costly, the smaller size, low power consumption, with decreased weight. Therefore, strap-down arrangements are preferred for low-cost applications.

The acceleration is measured by an accelerometer. These sensors used to measure the acceleration due to local gravity as well as acceleration due to an external force. If we know the attitude of the accelerometers then we can easily mathematically remove the local gravity component.

In order to determine the attitude, an angular velocity sensor, i.e. a gyroscope can be employed. Mathematically integrating these values allows us to determine the rotation of the platform and therefore the change in its attitude.

3.1.1 Coordinate Frames
In an inertial navigation system, several information sources usually come from different coordinate systems. Therefore, it is necessary to transform the measured quantities into a common coordinate frame in order to process all the data. Coordinate frames are basically of three types
Local systems
Earth centered systems
Vehicle centred systems
Accelerations are measured in their inertial frame of reference, resolved in their instrumental frame and transformed into the platform frame (vehicle centred).The same applies to the angular rates from the gyros. For the data fusion with other systems, e.g. GPS, the coordinates are transformed into the navigation frame (e.g. Earth centred). Different co-ordinate frames are:
Earth-Centred Earth-Fixed Frame (ECEF, e-frame): As the name indicates, this type of frame has its origin at the center of mass of the Earth and rotates with it. The x-axis points to the reference meridian, the z-axis is parallel to the mean spin axis of the Earth, and the y-axis completes the right-handed orthogonal frame.

Local Geodetic Frame (t-frame): This type of frame is also called as north-east-down or NED system. Because its x-axis pointing towards geodetic north, the y axis completes the right-handed orthogonal frame on the geodetic reference frame, and the z axis pointing towards the origin of the e-frame. It has a coordinate system at its origin coinciding with that of the instrumental frame.

Inertial Frame (i-frame): This type of coordinate frame works on the principle of Newton’s laws of motion. It is not accelerating type coordinate system. It could be chosen arbitrary but it is convenient to let it allows its origin to coincide with the one of the e-frame.

Figure 3.1Relations between Different Co-ordinate Frames
Body Frame (b-frame): This coordinate system has its origin at the centre of gravity of the vehicle. As we seen in figure 3.1 its x-axis goes with the forward direction, the y-axis completes the right-handed orthogonal system, and the z-axis down through the bottom of the vehicle. In case the instrumentation platform is not aligned with the body frame, an additional transformation has to be done.

3.1.2 Motion of a land vehicle
In most of the cases, navigation system is used to detect the position of an object. The general motion of a land vehicle is typically calculated by the values that are given by accelerometer and gyroscope.

Figure 3.2 Land Vehicle Navigation Co-ordinate Frame
3.1.3 Accelerometer
An accelerometer is generally a proof mass that is kept away from a case by a pair of springs. The mass proof will be in a specific calibrated position in zero accelerometer called the equilibrium position. In Figure 3.2 we can see the sensitive axis by accelerometer through an arrow. Any kind of acceleration will take place through the proof mass to be displaced along the axis. It works on the principle of Newton’s 2nd law. The movement of mass proof is proportional to the force of acceleration, that’s why when we measure displacement from the equilibrium position will give the acceleration along the axis.

Figure 3.3 Accelerometer
An accelerometer shown in Figure 3.3 is generally used for measuring the acceleration of a moving vehicle or an aircraft or a gravitational acceleration. When an accelerometer placed in a gravitational field the proof mass will be displayed in positive of the sensitive axis that shows on the below figure. But this is also true that when we are measuring the acceleration of a vehicle in a positive direction the output of vehicle acceleration as well as gravitation acceleration is goes in a positive sign.

3.1.3.1 Acceleration and force
Here in this column, we will discuss the g-force. It is mainly used to measure acceleration, not force. When acceleration is in vector quantity, g-force accelerations are expressed as a scalar. It’s mainly operated on the principle of Newton’s 3rd law, which says that law of reciprocal actions and the direction of forces are pointing to upward and downward with the positive g-forces and negative g-forces respectively. The output from accelerometer measures by vehicle acceleration minus gravitational acceleration, it’s known as specific force.

It’s represented by this formula:
false
where ‘a’ is the acceleration of that vehicle, ‘f’ is the acceleration that is given by non-gravitational forces and ‘g’ is the acceleration due to gravity.

Now a day’s accelerometers are used in so many types of applications in scientific and engineering systems. Accelerometers are can be made by the embedded system or by micro-electro-mechanical system (MEMS). MEMS accelerometer is used because it has a lot of advantages like it is very small is dimension and weight, it’s just one mm in dimension and less than one gram in weight. These types of accelerometers are mainly used in airbags system or in low-cost navigation systems, so the manufacturers are needed to be very cheap to make this type of sensors. But expensive accelerometers are also there, and those accelerometers are used for high-performance purposes like in INS. It is given the output in radian/s2 unit for specific force and in degree/hr for velocity increment due to a specific force.

3.1.4 Gyroscope
The gyroscope is used to measure or maintain the angular velocity or rotation of a vehicle. It was first invented in 1852 by the French physicist Leon Foucault (1819-1868) and it works on the principle of conservation of angular momentum. Primarily, gyroscopes are purely mechanical and worked with a rotor (spinning mass), which is used for the spin of one axis as shown in Figure 3.4.

Figure 3.4 Mechanical Gyroscope
As shown in Figure 3.4 we see that that there is a rotor mounted in two gimbals that are suspended on the base. For these two gimbals it given the gyroscope 3 degrees of freedom to the rotor and makes it possible to keep its orientation while the base is rotated in any direction. An error will always occur because of the friction between the moving parts. Just like accelerometer gyroscope will also work on an inertial frame and output given by the gyroscope is in degrees/hour or in rad/s unit for angular velocity and in degrees or radian for angular rotation.

Three types of gyroscopes available are :
MEMS gyroscope
FOG gyroscope
Laser gyroscope
3.1.4.1 MEMS Gyroscope
The main advantage of MEMS gyroscope is that it is comparatively less expensive than any other mode of gyroscopes. MEMS gyroscopes are working on the principle of Coriolis Effect to measure the angular rate as shown in Figure 3.5.

Figure 3.5 MEMS Gyroscope
From Figure 3.5 we can easily say that when a mass (m) is moving in direction v?and angular rotation velocity ??is applied, then the mass will experience a force in the direction of the Coriolis force. From the capacitive sensing structure we know the resulting physical displacement which is caused by Coriolis force. Most of the available MEMS gyroscopes use a tuning fork configuration as shown in Figure 3.6.

Figure 3.6 Tuning Fork Configuration of MEMS Gyroscope
3.1.4.2 FOG Gyroscope
This type of gyroscope is very recent in gyroscope families, and it is uses an external laser and two beams going opposite directions in long spools or several kilometres of fibre optic cable, with the two beams compared after their travel through the spools of fibre. The basic mechanism of fog gyroscope is dependent upon the monochromatic laser light travelling in opposite paths. It also depends upon sagnac effect. FOG gyroscope is an abbreviation of fibre optic gyroscope.

3.1.4.3 Laser Gyroscope
Just like FOG gyroscope ring laser gyroscope also splits laser light into two beams in opposite directions by a narrow tunnel in a closed optical path.

3.2 Global Positioning System
Currently the Russian Global Orbit Navigation Satellite System (GLONASS) and Global Positioning System (GPS) from the U.S. Department of Defence are two widely used Global Navigation Satellite Systems (GNSS).

With the GPS, the restricted Standard Positioning Service (SPS) only can be used by the civilian users. U.S. military and other authorized users are only projecting The Precise Positioning Service (PPS).

Around the Earth at least 24 satellites are equally spread on six orbits. With a 55 degree angle with respect to the equator the six orbital planes are inclined. About 20200 km above the Earth’s surface the orbits are situated.

3-Dimensional position estimation and the accurate Coordinated Universal Time UTC are chiefly provided by the GPS. To estimate a 3-d position, the signals of at least 4 satellites are need to be received by the receiver.

In turn, to compute the location of the receiver, the place of the satellites must be known. With the GPS almanacks which are transmitted by the satellites can roughly calculate these coordinates. The ephemeris data also transmitted in the satellite message, to get a more accurate satellite message.

3.3 Sources of Error
There are always some errors included in the output of sensors. So the values that are given by accelerometer and gyroscope are subject to errors. There are several types’ errors, which are discussed here.

3.3.1 Bias
Bias is the output that does not has any correlation with the input. It could be asymmetric in nature for both of negative and positive inputs and also has an instability that is given by random variation when the output in compared over a specified sampled intervals. Bias is generally measured in m/s2 of mg for accelerometer and for gyroscopes it gives value in deg/hr or rad/s.

3.3.2 Scale Factor
The scale factor is the ratio of change in the measured output upon intended input. Just like bias error scale factor also can be asymmetric in nature for negative and positive inputs. Sometimes the term scale factor and sensitivity is mixed up by the manufacturers, but sensitivity and scale factor are not same, the difference between sensitivity and scale factor. Sensitivity is related to the secondary input whereas the scale factor is related to the intended primary input.

Through calibration, we can easily measure both drift and scale factor. The calibration method is mainly used to measure accurately three-axial turn, angle rate, and six-position static test. The above-mentioned equipment is used to determine the bias and scale factor by comparing with known parameters like the earth gravity or well-known angles to measure the output.

3.3.3 Repeatability
Repeatability is the closeness of repeated measure values in the same condition by the same input variable and for accelerometer, it is expressed in m/s2 or mg and for gyroscopes, it gives the values in deg/hr or rad/s.

3.3.4 Resolution
Resolution is the also another vital term that should be monitored in inertial navigation sensors. It is the minimum value, which is greater than noise level that gives an output above a certain level and just like repeatability the unit of accelerometer and gyroscope are expressed in m/s2 or mg and deg/hr or rad/s respectively.

3.3.5 Stability
When the sensor giving you the same type of output with a constant input then this ability if that sensor is called stability. It is also expressed as same as repeatability and resolution and measured over a single run.

3.3.6 Noise
Noise is a random type error, which occurs in output it can only be removed by stochastic models. Sometimes it modelled as a 1st order Gauss Markov process. The term random walk is used to describe a stochastic process in evaluating accelerometer and gyroscopes with zero mean and standard deviation, which grows as the square root of time. Angle random walk process is one of the most popular random walk processes, that describes the error built up with time for gyroscopes due to white noise in angular rate. It is generally giving output in deg/h. For both accelerometers and gyroscopes, the parameters for the stochastic models is measured by long time collection of static data.

Chapter 4
Methodology
4.1 Integration methods
The basic things, which is used in all methods in INS and GPS, is that the INS is used for the reference purpose, and GPS is used for the updating the server. The measurement frequency of INS is many times higher than GPS measurement frequency, INS measurement frequency normally around 1-10 Hz for GPS where GPS measurement frequency is only 100-400Hz. The state vector parameters can determine in higher frequency only, but it can update only in low frequency. The main difference between them is in the data flow. Further, some methods provide feedback information to improve its performance while others don’t. Here we will discuss three types of integration technique, loose integration, tight integration and ultra-tight integration.

4.1.1 Loose Integration
This is the simplest and very common type of integration method. Generally, in this method, two different Kalman filters are operated individually, one for 35 GPS measurement and other for INS measurement. But here it is built a three input Kalman filter. Using this we can reduce the step of the integration process. Here the basic in the loose integration method is described in the following steps:
Processing of the raw GPS measurements through Kalman filter in order to obtain the velocity and position from GPS.

Processing of the raw INS measurements through Kalman filter in order to obtain the values of accelerometers and gyroscope and then determine the position and velocity from them.

Figure 4.1 Loose Integration
The main advantage of using loosely coupled integration technique shown in Figure 4.1 is that it’s very simple by its architecture and also robustness. If anyone of them stopped working, still we are able to determine the position through the other. Apart from that there also some other advantages are there. Loose integration can take a short time period and its processing time of the algorithm due to generally smaller state vectors. The disadvantage is mainly that it gives poor GPS result when GPS signally is very low. It happens only when it receives signals less than four satellites. This drawback can be removed by using tight integration method and this is one of the main reasons why tight integration is better than loose integration. It mainly uses in urban areas. We can use two different Kalman filter for loose integration method.

4.1.2. Tight Integration
The tight integration method also uses single Kalman filter. Unlike loose integration method, it will not use two different Kalman filters for two different sensors. There are basic steps of tight integration method is described as follows:
Getting the raw data from INS sensors and through the mechanization equations, it uses to determine the position and velocity.

Same as like INS, using GPS sensor also for getting the information about position and velocity, from step 1 to predict pseudo-range and Doppler measurement.

Use predicted pseudo-range and Doppler measurements from step 2 as input to an INS/GPS Kalman filter. Here Kalman filter takes the difference between the pseudo-range and Doppler in order to determine the error of the position and velocity plus misalignment error.

Using the error estimates from Step 3, we update the position and velocity from Step 1 in order to get a full state vector.

The main advantage of using tight integration method is that it is working properly even when the signal strength is not so good and gives proper output. It can happen because INS can be still updating. So this process is mainly used in urban areas, where this process is use in the absence of strong GPS signal.

The tight integration is comparatively complex process than loose integration, due to state vector increases in size which is its disadvantage.

Figure 4.2 Tight Integration
4.2 Estimation Techniques
Estimation is quietly the procedure of obtaining unknown parameters from a set of observations. Depending on the amount of the observations we can use different type of estimation techniques. One of the most common methods is least square method. If we know the systems dynamic over time then we can easily obtained better estimate of the parameters. Figure 4.3 shows the estimation block diagram.

Figure 4.3Estimation Block Diagram
4.2.1 Estimation of Dynamic Systems
Kalman filter is one of the most common and very useful methods, for sensor integration. It consists of 7 steps, the main reason for using this technique is, because it changes its value with changing time, and updated and corrected your position. As most of the navigation system behaves like non-linear system. In Kalman filter, all subscripts indicate a certain time epoch.

false
Where,
false
Where, P and V are the position in local frame and velocity in local frame, respectfully. In order to compute optical estimates of the state vector, false while using knowledge of the behavior of the system’s dynamic a Kalman filter is used. It is a recursive algorithm that uses a series of prediction and measurement update step with a minimum variance.

According to the principle of Kalman filter, the process can be modeled as
false
false
false
Where, false and false is the transition matrix of a given system, from time period t to t+1, false is the acceleration in local frame at time t and I represents as the identity matrix.

To relate the observation to the unknown parameters we need to know the measurement model. For time t, the measurement model will be written as:
false
false
At zero means white noise sequences with zero correlation, we assume that the measurement noise false and process driving noise, false, an updated estimate of the state vector and its covariance can be found as:
false
false
Where the signs “+” and “-” indicates the state of measurement update after and before respectively, Pt represents the covariance, vt is for innovation sequence, and Kt denotes the Kalman gain.

false
The innovation sequence vt is actually the difference between actual and predicted observation, where actual observation denoted as false, and false is given as predicted observation
false
This is the new part of the system which introduced new information into the system. If values of actual measurement and predicted measurements are same, then the value of innovation sequence is zero, so it means that no new information is introduced in the system. So the values of false and false will also be same, the updated and predicted state vector. Clearly, if some difference creates between actual and predicted values, then it added some new information to the system. Just like measurements contains measurement noise. It is not mentioned that weather the new information will fully accept by the system or not, this thing we know from Kalman gain matrix, it works like a weighting factor with a minimum error variance. The measurement will be very reliable is the value of Kalman gain matrix is greater than noise covariance Rt . But if the value of Rt is small then Kalman will also be small.

false
Predicted state and its covariance forward in time can be expressed by these two equations
false
false
Where we already know that false denotes the transition matrix as the previous equation.

During the GPS outrages only IMU data is available. So during outrages the observation model is calculate using INS position and error from the previous state, given as:
false
Where false denotes the error from the previous state and false denotes the state calculated from IMU.

Figure 4.4 Algorithm for Estimation-Prediction using Kalman Filter
Figure 4.4 describes the whole operational process of Kalman filter. It consists of the process model, and the measurement model, this works exclusively for linear systems only.

4.2.1.1 Test Case
The above Kalman filter was tested usingfalse, while the X and Z is generated from random signal and random white Gaussian noise and the result was generated.

Figure 4.5 Kalman Filter test
Figure 4.5 shows that the Kalman filter is capable of predicting next state with good accuracy. Hence, it can be used for the sensor fusion.

4.2.2 Estimation of Non-Linear Systems
In this section, we will discuss a non-linear process and measurement model. It can be given as
false
false
Where h and f represent the non-linear functions of the process model and relation between observation and the state. For linearizing these two above equations a nominal state vector, X t  is represented as:
false
Where Xt represents the value of perturbation from the nominal state. Let the value of Xt is very small, then a first order Taylor series will perform about the nominal state.

false
Where Zt represents the value of the difference between actual measurements over predicted measurements, and Xt is the linear model of error state vectors.

Linearized Kalman filter is that where the linearization is done about the nominal state vector. If this linearization is done in the last computed solution Xt+1 then it’s called an Extended Kalman filter (EKF).

4.2.2.1 Test Case
The extended Kalman filter war tested by giving X as sine wave of constant frequency and the output was compared with the actual input.

Figure 4.6 Extended Kalman filter test
Figure 4.6 shows the output of extended Kalman filter when a sine wave with random noise and its performance with non-linear input.
Chapter 5
Result
The experimental set up comprised of IMU 3DM-GX, standard GPS, a laptop with ROS for data logging and a moving vehicle.

In order to convert IMU into INS for position and velocity quaternions are required. The quarternions are the used to calculate the position and heading from the accelerometer and gyroscope data from IMU.

Figure 5.1 Comparison of various AHRS algorithm with filtered data
In figure 5.1 two different AHRS algorithm, ie, Mahony and Madgwick, are compared with the filtered quaternion output of IMU. The quaternions are the used to calculate the Euler angles, ie, roll, pitch and yaw. These angles are then used to create Direction Cosine Matrix or transformation matrix to convert the acceleration from body frame to the local frame, ie, NED frame.

While performing that experiment of integrated INS and GPS outrage was created for examine the performance the filter clearly, the duration of gap was up to 60 seconds.

Figure 5.2 Plot between GPS, INS, Novatel, Spatial output and Kalman Filter output
In order to evaluate the values of the IMU measurement, the magnitude of the corrected INS derived position, velocity applied from GPS update shown in below figure. It gives a clear idea about the position and velocity correction of INS data after each update.

Figure 5.3 Output with 60s GPS outrage
Figure 5.2 and fig. 5.3 gives the actual path covered by the vehicle or the ground truth ,i.e, Novatel, the output of the GPS/IMU module, i.e, Spatial, GPS position, position calculated from IMU and the Kalman Filter output all in Local frame. Here in the above figure it described the full 60 seconds gap. Here the different baseline length is taken from master location, so that it can be seen clearly.

Here the gaps created in order to simulate the data in a tunnel like fully data outage and partially outage conditions, where no GPS signals were available in kind of form. Here we intentionally cut off the GPS shield connection in order to create such conditions. A standalone GPS solution was therefore impossible. Inorder to increase the efficiency of the filter during GPS outrages the observation during is calculated as the difference of IMU position at current state with the error in the previous state.

Figure 5.4 Velocity estimation and comparison for 60s GPS outrage
Velocity accuracy is calculated with velocity of vehicle in North axis and East axis with respect to time. We get the values from IMU for acceleration in X, Y and Z axis then converted to NED using Direction Cosine Matrix. So we integrate those values and convert it into velocity and again integrate it to get the displacement of that vehicle. For the case of land vehicle we can neglect that value of Z axis, because in a land vehicle move only in X and Y plane.

Figure 5.4 shows the velocity of vehicle in North and East with respect to time respectfully, to show the change of velocity and angular rotation with respect to time.

Figure 5.5 Northing Position
Figure 5.5 shows the difference between the Northing position of Novatel, GPS and Kalman Filter output.

Figure 5.6 Easting Position
Figure 5.6 shows the difference between the Easting position of Novatel, GPS and Kalman Filter output.

Max. Path Error Duration of Outrage
5s 10s 60s
Position North 2.4599m 3.5263m 11.1724m
Position East 2.9115m 2.9189m 2.9425m
Velocity North 0.6819m/s 0.6824m/s 0.6827m/s
Velocity East 0.4356m/s 0.4359m/s 0.4361m/s
Table 5.1 Along the path error
Table 5.1 shows the maximum error along the path between the Novatel data and the estimated data of Kalman filter.

Figure 5.7 Plot between GPS, Odometer, and Kalman Filter output

Figure 5.8 Yaw angle comparison
As to increase the efficiency of the whole system more sensors can be incorporated like odometer, LASER, map fusion, etc. In figure 5.7, GPS position and odometer position was integrated using multi-rate Kalman filter. Simultaneously the Yaw angle from IMU and odometer was integrated with Kalman filter in figure 5.8.

Chapter 6
6.1 Conclusion
It can be concluded that multi-sensor fusion for land vehicle navigation has been demonstrated. Integration of GPS and INS sensor was done for predicting the trajectory of a land vehicle using an estimation-prediction model. Experimental results demonstrate integration of INS and GPS together through Kalman filter estimation technique.

The main objectives of the data analysis was to access the integrated systems performance hereby determine the position and velocity accuracy during data outages under different conditions. The gaps showed that using GPS with INS can provide sub meter level position accuracy when GPS signal is not available.

6.2 Future Scope of the Work
In this dissertation, focus was to achieve multisensory fusion for land vehicle navigation. Thus, a Kalman filter based integration algorithm was developed. This work reports a linear estimation model for problem at hand. However,handling non-linear systems is a challenging task. Future scope of the work includes modelling of nonlinear systems using better estimation techniques like: extended Kalman Filter, Particle filter etc. to further reduce estimation error and enhance predictability of nonlinear systems as well.

Chapter 7
References
Douglas Soares dos Santos, Cairo Lucio Nascimento Junior, and Wagner Chiepa Cunha “Autonomous Navigation of a small boat using IMU/GPS/Digital compass integration” IEEE (2013).

Hoang-Duy Nruyen, Vinh-Hao Nguyen, and Hong-Viet Nguyen “Tightly coupled INS/GPS integration with magnetic aid” 2nd International Conference on Control and Robotics Engineering (2017): 207-212.

Xinfeng He, Yang Le, and Wendong Xiao “MEMS IMU and Two-Antenna GPS integration navigation system using Interval Adaptive Kalman filter” IEEE A&E systems magazine (2013): 22-28.

Markus Langer, Stefan Kiesel, Christian Ascher, and Gert F. Trommer “Deeply coupled GPS/INS integration in pedestrian navigation system in weak signal conditions” International Conference on Indoor Positioning and Indoor Navigation (2012).

Hamad Ahmed and M. Tahir “Terrain-based vehicle localization using low cost MEMS-IMU sensors” IEEE (2016).

O. M. Maklouf, Y. El halwagy, M. Beumi, and S. D. Hassan “Cascade Kalman filter application in GPS/INS integrated navigation for car like robot” 26th National Radio Science Conference (2009).

Ming-Han Lee, Yu-Jen Chen, and Tuzz-Hseng S. Li “Sensor fusion design for navigation and control of an autonomous vehicle” IEEE (2011).

Davide A. Cucci and Matteo Matteucci “Position tracking and sensor self-caliberation in autonomous mobile robots bt Gauss-Newton optimization” International Conference on Robotics and Automation (2014): 1269-1275.

V. V. Avrutov, P. M. Aksonenko, P. Henaff, and L. Ciarletta “3D-caliberation of the IMU” International Conference on Electronics and Nanotechnology (2017): 374-379.

Yuquan Wang, Jan Mangnus, Deagan Kostic, Henk Nijmeijer, and Sven T. H. Jansen “Vehicle state estimation using GPS/IMU integration” IEEE (2011)
Jeffrey S. Wit, Carl D. Crane III, David G. Armstrong II, and Robert M. Rogers “Integrated INU/GPS for autonomous vehicle navigation” IEEE (1996).

GENG Qingbo, Li Nan, and Li Baokui “The application of GPS/SINS integration based on Kalman filter” Proceedings of the 31st Chinese Control Conference (2012): 4607-4610.

Elias de Souza Goncalves and Paulo Fernando Ferreira Rosa “Cointegration Analysis for IMU in a Fixed-Wing UAV” IEEE (2017).

CHEN Jingjing, MIAO Lingjuan and GUO Yanbing “The analysis of time synchronization errors in INS/GPS tightly integrated navigation” Proceedings of the 34th Chinese Control Conference (2015): 5107-5112.

Farhad Aghili and Alessio Salerno “Driftless 3-D attitude determination and positioning of mobile robots by integration of IMU with two RTK GPSs” IEEE/ASME transaction on mechatronics, Vol. 18, No.1, February 2013.

Debo Sun, Mark G. Petovello and M. Elizabeth Cannon “Ultratight GPS/Reduced-IMU integration for land vehicle navigation” IEEE transaction on Aerospace and Electronic Systems, Vol. 49, No. 3, July 2013.

Simone Chicarella, Vincenzo Ferrara, Frbrizio Frezza, Alssandro D’Alvano and Lara Pajewski “Improvement of GPR tracking by using inertial and GPS combined data”.

S. Amir Mousavi Lajimi and John McPhee “A comprehensive filter to reduce drift from Euler angles, velocities and position using an IMU” IEEE 30th Canadian Conference on Electrical and Computer engineering (2017).

Seong-Baek Kim, Seung-Yong Lee, Ji-Hoon Choi, Kyoung-Ho Choi and Byung-Tae Jang “A bimodal approach for GPS and IMU integration for land vehicle application” IEEE (2003).

Ling Chen and Huosheng Hu “IMU/GPS based pedestrian localization” 4th Computer Science and Electronic Engineering Conference, IEEE (2012).

Giovanni Fusco, Huiying Shen and James M. Coughlan “Self-localization at street intersections” Canadian Conference on Computer and Robot Vision, IEEE (2014).

Pinar Oguz-Ekim, khaled Ali, Zahar Madadi, Francois Quitin and Wee Peng Tay “Proof of concept study using DSRC, IMU and Map fusion for vehicle localization in GNSS-denied environments” 19th International Conference on Intelligent Transportation Systems, IEEE (2016).

Jing Chang, Jerome Cieslak, Jorge Davila, Jun Zhou, Ali Zolghadri and Zongyi Guo “A two-step approach for an enhanced quadrotor attitude estimation via IMU data” IEEE Transaction on Control Systems Technology.

Dorota A. Grejner-=Brzezinska, Charles K. Toth, Homgxing Sun, Xiankun Wang and Chris Rizos “A robust solution to high-accuracy Geolocation: Quadruple integration of GPS, IMU, Psedilite, and terrestrial Laser scanning” IEEE Transaction on Instrumentation and Measurement, Vol. 60, No. 11, November 2011.

Wahyudi and Ngatelan “Design of multi-sensor IMU for land vehicle” 2nd International Conference on Information Technology, Computer and Electrical Engineering, IEEE (2015).

Mohammad Abdel Kareem Jaradat and Mamoun F. Abdel-Hafez “Enhanced, delay dependent, intelligent fusion for INS/GPS navigation system” IEEE Sensor Journal, Vol. 14, No. 5, May 2014.

Slawomir Romaniuk and Zdzislaw Gosiewski “Kalman filter realization for orientation and position estimation on dedicated processor” acta mechanica et automatica, vol.8 no.2 (2014).

Paul D. Groves, “Principles of GNSS, Inertial and multisensor integrated navigation system”, Artech House Inc, 2008.

Patrick Y. C. Hwang and Robert Brown, “Introduction to Random Signals and Applied Kalman Filtering”, Wiley ; Sons, 2012.

Appendix I
MATLAB code for Kalman filter
m = 2; %State Dimension
n = 1; %Observation Dimension
A=0 1;-1 -sqrt(2);
H = eye(n,m);

N = 100;
x = zeros(m,N);
z = zeros(n,N);

% Generation of noise
PN = wgn(m,N,0);
ON = wgn(n,N,0);

% Covariances of noise
Q = cov(PN’);
R = cov(ON’);

% Initial values for the model
x(:,1) = 1 0′; %Initial State
z(:,1) = H * x(:,1) + ON(:,1); %Initial Observation

% Simulate states and observations
for i = 2 : N
x(:,i) = A * x(:,i-1) + PN(:,i);
z(:,i) = H * x(:,i) + ON(:,i);
end

% Kalman filtering…

xh(:,1) = 0.01*randn(m,1); %Initial Estimate
Px = eye(m); %Initial state covariance matrix

for i = 1 : size(z,2)
xh_(:,i) = A * xh(:,i); %Priori estimate
Px_ = A*Px*A’ + Q; %Priori estimate of the state covariance matrix
K = Px_ * H’ * inv(H*Px_*H’ + R); %Kalman filter coefficient(Kalman Gain)
zh_(:,i) = H * xh_(:,i); %Estimated observation
inov(:,i) = z(:,i) – zh_(:,i); %Measurement residual error
xh(:,i+1) = xh_(:,i) + K * inov(:,i); %Posteriori (updated) estimate of the current state
Px = Px_ – K*H*Px_; %Posteriori (updated) state covariance matrix
end
MATLAB code for Extended Kalman
% State-error Jacobian
W = 1 0 ;0 0 ;

% Measurement-state Jacobian
H = 1 0 ;0 1 ;

% Measurement-error Jacobian
V = 1 0 ;0 1 ;

% Number of iterations
N = 500;

% True State
x = zeros(2,N);

% Apriori state estimates
x_apriori = zeros(2,N);

% Aposteriori state estimates
x_aposteriori = zeros(2,N);

% Apriori error covariance estimates
P_apriori = zeros(2,2,N);

% Aposteriori error covariance estimates
P_aposteriori = zeros(2,2,N);

% Measurements
z = zeros(2,N);

% Kalman Gain
K = zeros(2,2,N);

% True initial state
x(:,1) = 0 ;3*pi/500 ;

% Initial aposteriori state estimate
x_aposteriori(:,1) = 1 ;1*pi/500 ;

% Process noise covariance
Q = 0.001 0 ;0 0 ;

% Measurement noise covariance
R = 0.1 0;0 0.01 ;

% Initial aposteriori error covariance estimate
P_aposteriori(:,:,1) = 1 0;0 1 ;

for i = 2:N
% Update true state
x(:,i) = sin(x(2,i-1)*(i-1));x(2,i-1) + sqrt(Q)*randn;randn;

% Update measurements
z(:,i) = x(:,i) + sqrt(R)*randn;randn;

% Update apriori estimate
x_apriori(:,i) = sin(x_aposteriori(2,i-1)*(i-1));x_aposteriori(2,i-1);

% Update state Jacobian
Ai = 0 (i-1)*cos(x_aposteriori(2,i-1)*(i-1);0 1;

Qi = Q;
Ri = R;

% Update aprioiri error covariance estimate
P_apriori(:,:,i) = Ai*P_aposteriori(:,:,i-1)*Ai’ + W*Qi*W’;

% Update Kalman gain
K(:,:,i) = P_apriori(:,:,i)*H’ / (H*P_apriori(:,:,i)*H’+V*Ri*V’);

% Update aposteriori state estimate
x_aposteriori(:,i) = x_apriori(:,i) + K(:,:,i) * (z(:,i) – x_apriori(:,i));

% Update aposteriori error covariance estimate
P_aposteriori(:,:,i) = (eye(2) – K(:,:,i)*H) * P_apriori(:,:,i);
end
MATLAB code for AHRS algorithm comparison
imu=csvread(‘imu_outdooropen.csv’,1,0);
imu(:,1) = (imu(:,1)-imu(1,1))/999759274.9;
imu(:,2:5)=imu(:,5) imu(:,2:4);

GT=csvread(‘spatial_odometry.csv’,1,0);
GT(:,1) = (GT(:,1)-GT(1,1))/999759274.9;
GT(:,5:8)=GT(:,8) GT(:,5:7);
y p r = quat2angle(GT(:,5:8));
Y P R = quat2angle(imu(:,2:5));
Y1 P1 R1 = Madgwick_AHRS(imu);
Y2 P2 R2 = Mahony_AHRS(imu);
MATLAB code Madgwick filter
function Y P R = Madgwick_AHRS(imu)

SamplePeriod = 1/39;
Quaternion = 1 0 0 0; % output quaternion describing the Earth relative to the sensor
Beta = 1; % algorithm gain

g=0 0 1′;

Accelerometer=imu(:,9:11);
Gyroscope=imu(:,6:8);
q = Quaternion; % short name local variable for readability

Q_norm=zeros(length(imu),4);
Q=zeros(length(imu),4);
for i=1:length(imu)
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0)
return;
end % handle NaN
Accelerometer(i,:) = Accelerometer(i,:) / norm(Accelerometer(i,:)); % normalise magnitude

% Gradient decent algorithm corrective step
F = 2*(q(2)*q(4) – q(1)*q(3)) – Accelerometer(i,1)
2*(q(1)*q(2) + q(3)*q(4)) – Accelerometer(i,2)
2*(0.5 – q(2)^2 – q(3)^2) – Accelerometer(i,3);
J = -2*q(3), 2*q(4), -2*q(1), 2*q(2)
2*q(2), 2*q(1), 2*q(4), 2*q(3)
0, -4*q(2), -4*q(3), 0 ;
step = (J’*F);
step = step / norm(step); % normalise step magnitude

% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, 0 Gyroscope(i,1) Gyroscope(i,2) Gyroscope(i,3)) – Beta * step’;

% Integrate to yield quaternion
q = q + qDot * SamplePeriod;
Q(i,:)=q;

Quaternion = q / norm(q); % normalise quaternion
Q_norm(i,:) =Quaternion;
end
Y P R=quat2angle(Q);
end
MATLAB code for Mahony filter
function Y P R = Mahony_AHRS(imu)
SamplePeriod = 1/39;
Quaternion = 1 0 0 0; % output quaternion describing the Earth relative to the sensor
Kp = 1; % algorithm proportional gain
Ki = 0; % algorithm integral gain
eInt = 0 0 0; % integral error
Accelerometer=imu(:,9:11);
Gyroscope=imu(:,6:8);
q = Quaternion; % short name local variable for readability
Q_norm=zeros(length(imu),4);
Q=zeros(length(imu),4);

for i=1:length(imu)
if(norm(Accelerometer) == 0), return; end % handle NaN
Accelerometer(i,:) = Accelerometer(i,:) / norm(Accelerometer(i,:)); % normalise magnitude

% Estimated direction of gravity and magnetic flux
v = 2*(q(2)*q(4) – q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 – q(2)^2 – q(3)^2 + q(4)^2;

% Error is sum of cross product between estimated direction and measured direction of field
e = cross(Accelerometer(i,:), v);
if(Ki ; 0)
eInt = eInt + e * SamplePeriod;
else
eInt = 0 0 0;
end

% Apply feedback terms
Gyroscope(i,:) = Gyroscope(i,:) + Kp * e + Ki * eInt;

% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, 0 Gyroscope(i,1) Gyroscope(i,2) Gyroscope(i,3));

% Integrate to yield quaternion
q = q + qDot * SamplePeriod;
Q(i,:)=q;
Quaternion = q / norm(q); % normalise quaternion
Q_norm(i,:) =Quaternion;
end
Y P R=quat2angle(Q);
end
MATLAB code GPS/IMU integration
close all
clear
clc

%%
imu=csvread(‘imu_outdooropen.csv’,1,0);
imu(:,1) = (imu(:,1)-imu(1,1));
imu(:,2:5)=imu(:,5) imu(:,2:4);
g=0 0 9.8′;
Aned=zeros(3,length(imu));
for i=1:length(imu)
Aned(:,i)=quat2rotm(imu(i,2:5))*imu(i,9:11)’;
Aned(:,i)=Aned(:,i)+g;
end
del_t=1/20;

%%
GPS=csvread(‘gps_outdooropen.csv’,1,0);
gps=zeros(length(GPS),7);
lla=rad2deg(GPS(:,2)) rad2deg(GPS(:,3)) GPS(:,4);
b c=ll2utm(lla(:,1) lla(:,2),’wgs84′);
b=b-b(1);
c=c-c(1);
gps(:,1)=(GPS(:,1)-GPS(1,1))/1001599360.127974;%999759274.9;
gps(:,2:4)=c b GPS(:,4);
gps(:,5:7)=csvread(‘twist.csv’,1,1);

%%
Novatel=csvread(‘novatel_ins_data.csv’,1,0);
Novatel(:,1)=(Novatel(:,1)-Novatel(1,1));
ecef=lla2ecef(Novatel(:,2:4));
N E D= ecef2ned(ecef(:,1),ecef(:,2),ecef(:,3),Novatel(1,2),Novatel(1,3),Novatel(1,4),wgs84Ellipsoid);

%%
GT=csvread(‘spatial_odometry.csv’,1,0);
GT(:,1)=(GT(:,1)-GT(1,1));
GT(:,2)=GT(:,2)-GT(1,2);
GT(:,3)=GT(:,3)-GT(1,3);
%%
A=eye(3) del_t*eye(3);zeros(3) eye(3);
B=eye(3)*(1/2)*del_t;eye(3)*del_t;
H=eye(6);
z=H*gps(:,2:7)’;
z(:,1003:2205)=NaN; % 60 second outrage
% z(:,1003:1023)=NaN; % 5 second outrage
% z(:,1003:1203)=NaN; % 10 second outrage
xest=zeros(6,length(imu));

%%
position,velocity=IMU_pos_vel2(imu(:,1),Aned’,gps(1,2:4));
x=position(:,2:4)’;velocity(:,2:4)’;
xh(:,1) = gps(1,2:7)’; %Initial Estimate
Px = eye(6); %Initial state covariance matrix
%%
for i = 1 : length(z)

Q=0.003*eye(3) zeros(3);zeros(3) (((0.003)^2)/2)*eye(3);

if ~isnan(z(:,i))
R=GPS(i,5) 0 0 0 0 0;0 GPS(i,6) 0 0 0 0;0 0 GPS(i,7) 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;
xh_(:,i) = A * xh(:,i)+B*Aned(:,i); %Priori estimate
Px_ = A*Px*A’ + Q; %Priori estimate of the state covariance matrix
K = Px_ * H’ * inv(H*Px_*H’ + R); %Kalman filter coefficient(Kalman Gain)
zh_(:,i) = H * xh_(:,i); %Estimated observation
inov(:,i) = z(:,i) – zh_(:,i); %Measurement residual error
xh(:,i+1) = xh_(:,i) + K * inov(:,i); %Posteriori (updated) estimate of the current state
Px = Px_ – K*H*Px_; %Posteriori (updated) state covariance matrix

end

if isnan(z(:,i))

zz(:,i) = x(:,i)-(-xh(:,i)+x(:,i-1));
R=(10^(del_t))*eye(3)/0.003 zeros(3);zeros(3) 0.7*eye(3);
xh_(:,i) = A * xh(:,i)+B*Aned(:,i); %Priori estimate
Px_ = A*Px*A’ + Q; %Priori estimate of the state covariance matrix
K = (Px_ * H’ * inv(H*Px_*H’ + R));
zh_(:,i) = H * xh_(:,i); %Estimated observation
inov(:,i) = zz(:,i) – zh_(:,i); %Measurement residual error
xh(:,i+1) = xh_(:,i) + K * inov(:,i); %Posteriori (updated) estimate of the current state
Px = Px_ – K*H*Px_; %Posteriori (updated) state covariance matrix

end
end

%%
Xh(1,:)=smooth(1:length(xh),xh(1,:),0.05,’sgolay’,4);
Xh(2,:)=smooth(1:length(xh),xh(2,:),0.05,’sgolay’,4);
Xh(3,:)=smooth(1:length(xh),xh(3,:),0.05,’sgolay’,4);
Xh(4,:)=smooth(1:length(xh),xh(4,:),0.05,’sgolay’,4);
Xh(5,:)=smooth(1:length(xh),xh(5,:),0.05,’sgolay’,4);
Xh(6,:)=smooth(1:length(xh),xh(6,:),0.05,’sgolay’,4);
%%
figure(1)
plot(x(2,:),x(1,:),’b.’)
hold on
plot(z(2,:),z(1,:),’r.’)
hold on
plot(GT(:,2),GT(:,3),’m.’)
hold on
plot(Xh(2,1:end-1),Xh(1,1:end-1),’k’)
hold on
plot(E,N)
title(‘Position’)
xlabel(‘Easting(m)’);
ylabel(‘Northing(m)’);
legend(‘IMU’,’GPS’,’Spatial’,’KF’,’Novatel’)

figure(2)
subplot(211)
plot(imu(:,1),x(4,:),’b’)
hold on
plot(gps(:,1),z(4,:),’r’)
hold on
plot(imu(:,1),Xh(4,1:end-1),’k’)
hold on
plot(Novatel(:,1),Novatel(:,5))
title(‘Velocity’)
xlabel(‘Time(s)’);
ylabel(‘Northing Velocity(m/s)’);
legend(‘IMU’,’GPS’,’KF’,’Novatel’);

subplot(212)
plot(imu(:,1),x(5,:),’b’)
hold on
plot(gps(:,1),z(5,:),’r’)
hold on
plot(imu(:,1),Xh(5,1:end-1),’k’)
hold on
plot(Novatel(:,1),Novatel(:,6),’c’)
xlabel(‘Time(s)’);
ylabel(‘Easting Velocity(m/s)’);
legend(‘IMU’,’GPS’,’KF’,’Novatel’);

figure(4)
plot(Novatel(:,1),N)
hold on
plot(imu(:,1),Xh(1,1:end-1),’k’)
hold on
plot(gps(:,1),z(1,:),’r’)
xlabel(‘Time(s)’);
ylabel(‘Northing(m)’);
legend(‘Novatel’,’KF’,’GPS’);

figure(5)
plot(Novatel(:,1),E)
hold on
plot(imu(:,1),Xh(2,1:end-1),’k’)
hold on
plot(gps(:,1),z(2,:),’r’)
ylabel(‘Easting(m)’);
xlabel(‘Time(s)’);
legend(‘Novatel’,’KF’,’GPS’);
MATLAB code calculating position and velocity from acceleration

function position,velocity =IMU_pos_vel2(time, Aned, init_pos)

velocity=zeros(length(time),4);
velocity(:,1)=time;
velocity(1,2:4)=zeros(1,3);
for i=2:length(time)
velocity(i,2)=velocity(i-1,2)+Aned(i-1,1)*(time(i)-time(i-1));
velocity(i,3)=velocity(i-1,3)+Aned(i-1,2)*(time(i)-time(i-1));
velocity(i,4)=velocity(i-1,4)+Aned(i-1,3)*(time(i)-time(i-1));
end

position=zeros(length(velocity),4);
position(:,1)=velocity(:,1);
position(1,2:4)=init_pos;
for i=2:length(time)
position(i,2)=(velocity(i-1,2))*((time(i)-time(i-1)))+position(i-1,2);
position(i,3)=(velocity(i-1,3))*((time(i)-time(i-1)))+position(i-1,3);
position(i,4)=(velocity(i-1,4))*((time(i)-time(i-1)))+position(i-1,4);
end
end
MATLAB code for LLA to UTM
function x,y,f=ll2utm(varargin)
% Available datums
datums = …

{ ‘wgs84’, 6378137.0, 298.257223563 };
{ ‘nad83’, 6378137.0, 298.257222101 };
{ ‘grs80’, 6378137.0, 298.257222101 };
{ ‘nad27’, 6378206.4, 294.978698214 };
{ ‘int24’, 6378388.0, 297.000000000 };
{ ‘clk66’, 6378206.4, 294.978698214 };
;

% constants
D0 = 180/pi; % conversion rad to deg
K0 = 0.9996; % UTM scale factor
X0 = 500000; % UTM false East (m)

% defaults
datum = ‘wgs84’;
zone = ;

if nargin < 1
error(‘Not enough input arguments.’)
end

if nargin > 1 && isnumeric(varargin{1}) && isnumeric(varargin{2}) && all(size(varargin{1})==size(varargin{2}))
lat = varargin{1};
lon = varargin{2};
v = 2;
elseif isnumeric(varargin{1}) && size(varargin{1},2) == 2
lat = varargin{1}(:,1);
lon = varargin{1}(:,2);
v = 1;
else
error(‘Single input argument must be a 2-column matrix LAT,LON.’)
end

if all(numel(lat),numel(lon) > 1) && any(size(lat) ~= size(lon))
error(‘LAT and LON must be the same size or scalars.’)
end

for n = (v+1):nargin
% LL2UTM(…,DATUM)
if ischar(varargin{n}) || (isnumeric(varargin{n}) && numel(varargin{n})==2)
datum = varargin{n};
% LL2UTM(…,ZONE)
elseif isnumeric(varargin{n}) && (isscalar(varargin{n}) || all(size(varargin{n})==size(lat)))
zone = round(varargin{n});
else
error(‘Unknown argument #%d. See documentation.’,n)
end
end

if ischar(datum)
% LL2UTM(…,DATUM) with DATUM as char
if ~any(strcmpi(datum,datums(:,1)))
error(‘Unkown DATUM name ;%s;’,datum);
end
k = find(strcmpi(datum,datums(:,1)));
A1 = datums{k,2};
F1 = datums{k,3};
else
% LL2UTM(…,DATUM) with DATUM as A,F user-defined
A1 = datum(1);
F1 = datum(2);
end

p1 = lat/D0; % Phi = Latitude (rad)
l1 = lon/D0; % Lambda = Longitude (rad)

% UTM zone automatic setting
if isempty(zone)
F0 = round((l1*D0 + 183)/6);
else
F0 = zone;
end

B1 = A1*(1 – 1/F1);
E1 = sqrt((A1*A1 – B1*B1)/(A1*A1));
P0 = 0/D0;
L0 = (6*F0 – 183)/D0; % UTM origin longitude (rad)
Y0 = 1e7*(p1 < 0); % UTM false northern (m)
N = K0*A1;

C = coef(E1,0);
B = C(1)*P0 + C(2)*sin(2*P0) + C(3)*sin(4*P0) + C(4)*sin(6*P0) + C(5)*sin(8*P0);
YS = Y0 – N*B;

C = coef(E1,2);
L = log(tan(pi/4 + p1/2).*(((1 – E1*sin(p1))./(1 + E1*sin(p1))).^(E1/2)));
z = complex(atan(sinh(L)./cos(l1 – L0)),log(tan(pi/4 + asin(sin(l1 – L0)./cosh(L))/2)));
Z = N.*C(1).*z + N.*(C(2)*sin(2*z) + C(3)*sin(4*z) + C(4)*sin(6*z) + C(5)*sin(8*z));
xs = imag(Z) + X0;
ys = real(Z) + YS;

% outputs zone if needed: scalar value if unique, or vector/matrix of the
% same size as x/y in case of crossed zones
if nargout > 2
f = F0.*sign(lat);
fu = unique(f);
if isscalar(fu)
f = fu;
end
end

if nargout < 2
x = xs(:),ys(:);
else
x = xs;
y = ys;
end
function c = coef(e,m)

if nargin < 2
m = 0;
end

switch m
case 0
c0 = -175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1;
-105/4096, 0, -45/1024, 0, -3/32, 0, -3/8, 0, 0;
525/16384, 0, 45/1024, 0, 15/256, 0, 0, 0, 0;
-175/12288, 0, -35/3072, 0, 0, 0, 0, 0, 0;
315/131072, 0, 0, 0, 0, 0, 0, 0, 0;

case 1
c0 = -175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1;
1/61440, 0, 7/2048, 0, 1/48, 0, 1/8, 0, 0;
559/368640, 0, 3/1280, 0, 1/768, 0, 0, 0, 0;
283/430080, 0, 17/30720, 0, 0, 0, 0, 0, 0;
4397/41287680, 0, 0, 0, 0, 0, 0, 0, 0;

case 2
c0 = -175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1;
-901/184320, 0, -9/1024, 0, -1/96, 0, 1/8, 0, 0;
-311/737280, 0, 17/5120, 0, 13/768, 0, 0, 0, 0;
899/430080, 0, 61/15360, 0, 0, 0, 0, 0, 0;
49561/41287680, 0, 0, 0, 0, 0, 0, 0, 0;

end
c = zeros(size(c0,1),1);

for i = 1:size(c0,1)
c(i) = polyval(c0(i,:),e);
end
MATLAB code for GPS, IMU and Odometer integration
close all
clear
clc

%%

gps=load(‘gps.txt’);
gpst=gps(:,1)-gps(1,1);
gpsx=gps(:,2)-gps(1,2);
gpsy=gps(:,3)-gps(1,3);
gpshx=gps(:,4);
gpshy=gps(:,5);

odom=load(‘wheelodom.txt’);
odomt=odom(:,1)-odom(1,1);
odomx=odom(:,2)-odom(1,2);
odomy=odom(:,3)-odom(1,3);
odomhx=odom(:,6);
odomhy=odom(:,7);
odomthetaerror=odom(:,8);
wheel_OdomZERO = zeros (length(odom));
q1=wheel_OdomZERO(:,1) wheel_OdomZERO(:,2) odom(:,4) odom(:,5);
pitch roll yaw = quat2angle(q1);
wheelodom_pitch=pitch;
wheelodom_roll=roll;
wheelodom_yaw=yaw;

imu=load(‘imu.txt’);
imut=imu(:,1)-imu(1,1);
q=imu(:,2) imu(:,3) imu(:,4) imu(:,5);
r1 r2 r3 = quat2angle(q);
imu_pitch=r1;
imu_roll=r2;
imu_yaw=r3;

% TODO
odomx= interp1(odomt, odomx, imut);
odomy= interp1(odomt, odomy, imut);
odomhx= interp1(odomt, odomhx, imut);
odomhy= interp1(odomt, odomhy, imut);
wheelodom_yaw=interp1(odomt, wheelodom_yaw, imut);
odomthetaerror=interp1(odomt, odomthetaerror, imut);

imuthetaerror=0.1*odomthetaerror;
%%

state = 3;
A = eye(state);
H = eye(state);
Px = eye(state);
xh(:,1) = 0 0 0;

j=1;
for i=1:length(imu)
if j>length(gpst)
gps_row(i,:) = NaN;
end
if j<=length(gpst)
imu_time = imut(i);
gps_time = gpst(j);
if imu_time<gps_time
gps_row(i,:) = NaN;
end
if imu_time>=gps_time
gps_row(i,:) = gpsx(j) gpsy(j) gpshx(j) gpshy(j);
j=j+1;
end
end
x(:,i) = odomx(i);odomy(i);wheelodom_yaw(i);
z(:,i) = H*gps_row(i,1);gps_row(i,2);imu_yaw(i);
Px = odomhx(i) 0 0;0 odomhy(i) 0;0 0 odomthetaerror(i);

if ~isnan(z(:,i))
R = gps_row(i,3) 0 0;0 gps_row(i,4) 0; 0 0 imuthetaerror(i);
xh_(:,i) = A * xh(:,i); %Priori estimate
%Px_ = A*Px*A’ + Q; %Priori estimate of the state covariance matrix
K = Px * H’ * inv(H*Px*H’ + R); %Kalman filter coefficient(Kalman Gain)
zh_(:,i) = H * xh_(:,i); %Estimated observation
inov(:,i) = z(:,i) – zh_(:,i); %Measurement residual error
xh(:,i+1) = xh_(:,i) + K * inov(:,i); %Posteriori (updated) estimate of the current state
Px = Px – K*H*Px; %Posteriori (updated) state covariance matrix
end

if isnan(z(:,i))
zz(1:2,i) = x(1:2,i)-(-xh(1:2,i)+x(1:2,i-1));
zz(3,i) = x(3,i);
R = odomhx(i) 0 0;0 odomhy(i) 0;0 0 imuthetaerror(i);
xh_(:,i) = A * xh(:,i); %Priori estimate
K = Px * H’ * inv(H*Px*H’ + R); %Kalman filter coefficient(Kalman Gain)
zh_(:,i) = H * xh_(:,i); %Estimated observation
inov(:,i) = zz(:,i) – zh_(:,i); %Measurement residual error
xh(:,i+1) = xh_(:,i) + K * inov(:,i); %Posteriori (updated) estimate of the current state
Px = Px – K*H*Px; %Posteriori (updated) state covariance matrix
end

end

%%

Xh(1,:)=smooth(1:length(xh),xh(1,:),0.05,’sgolay’);
Xh(2,:)=smooth(1:length(xh),xh(2,:),0.05,’sgolay’);
Xh(3,:)=smooth(1:length(xh),xh(3,:),0.05,’sgolay’);
%%

figure(1)
plot(gps_row(:,1),gps_row(:,2),’r.’)
hold on
% plot(xh(1,:),xh(2,:),’c’)
% hold on
plot(odomx,odomy,’g’)
hold on
plot(Xh(1,:),Xh(2,:),’k’)
title(‘Position’)
xlabel(‘Easting(m)’);
ylabel(‘Northing(m)’);
legend(‘GPS’,’Odom’,’KF’)
%
figure(2)
plot(xh(3,:)*(180/pi),’b.’)
hold on
plot(imu_yaw*(180/pi),’m.’)
hold on
plot(wheelodom_yaw*(180/pi),’g.’)
title(‘Yaw’)
ylabel(‘Yaw (degree)’);
xlabel(‘Number of Samples’);
legend(‘KF’,’IMU’,’Odom’)