GPS-oscillation-robust Localization and Visionaided Odometry Estimation

GPS-oscillation-robust

Localization and Vision-aided Odometry Estimation

HONGYI CHEN

Examensarbete TRITA-ITM-EX 2019:10

GPS-oscillation-robust lokalisering och visionsstödd odometri uppskattning

Hongyi Chen

Godkänt

2019-02-03

Examinator

Lei Feng

Handledare

Xinhai Zhang CO: Milan Horemuz

Uppdragsgivare

Integrated Transport Research Lab

Kontaktperson

Yuchao Li

Sammanfattning

GPS/IMU integrerade system används ofta för navigering av fordon. Algoritmen för detta kopplade system är normalt baserat på ett Kalmanfilter. Ett problem med systemet är att oscillerade GPS mätningar i stadsmiljöer enkelt kan leda till en lokaliseringsdivergens. Dessutom kan riktningsuppskattningen vara känslig för magnetiska störningar om den är beroende av en IMU med integrerad magnetometer.

Rapporten försöker lösa lokaliseringsproblemet som skapas av GPS-oscillationer och avbrott med hjälp av ett adaptivt förlängt Kalmanfilter (AEKF). När det gäller riktningsuppskattningen används stereovisuell odometri (VO) för att försvaga effekten av magnetiska störningar genom sensorfusion. En Visionsstödd AEKF-baserad algoritm testas i fall med både goda GPS omständigheter och med oscillationer i GPS mätningar med magnetiska störningar. Under de fallen som är aktuella är algoritmen verifierad för att överträffa det konventionella utökade Kalmanfilteret (CEKF) och ”Unscented Kalman filter” (UKF) när det kommer till positionsuppskattning med 53,74% respektive 40,09% samt minska fel i riktningsuppskattningen.

Nyckelord: Adaptivt förlängt Kalmanfilter, lokalisering, stereovisuell odometri, GPS avbrott.

Master of Science Thesis TRITA-ITM-EX 2019:10

GPS-oscillation-robust Localization and Vision- aided Odometry Estimation

Hongyi Chen

Approved

2019-02-03

Examiner

Lei Feng

Supervisor

Xinhai Zhang CO: Milan Horemuz

Commissioner

Integrated Transport Research Lab

Contact person

Yuchao Li

Abstract

GPS/IMU integrated systems are commonly used for vehicle navigation. The algorithm for this coupled system is normally based on Kalman filter. However, oscillated GPS measurements in the urban environment can lead to localization divergence easily. Moreover, heading estimation may be sensitive to magnetic interference if it relies on IMU with integrated magnetometer.

This report tries to solve the localization problem on GPS oscillation and outage, based on adaptive extended Kalman filter(AEKF). In terms of the heading estimation, stereo visual odometry(VO) is fused to overcome the effect by magnetic disturbance. Vision-aided AEKF based algorithm is tested in the cases of both good GPS condition and GPS oscillation with magnetic interference.

Under the situations considered, the algorithm is verified to outperform conventional extended Kalman filter(CEKF) and unscented Kalman filter(UKF) in position estimation by 53.74% and 40.09% respectively, and decrease the drifting of heading estimation.

Key word: Adaptive Extended Kalman Filter, Localization, Stereo visual odometry, GPS

outage

Acknowledgements

Firstly I would like to thank my supervisor Xinhai Zhang to help me along the thesis work and review my thesis report.

Moreover, I really enjoyed working in Integrated Transport Research Lab to- gether with many talented people. Especially, I appreciate the research assistant Jiajun Shi. With his kind help, I can hand on the hardware and software platform of RCV more easily. I also want to thank Mikael Hellgren to help solve the hardware problems when RCV stops working.

Additionally, I would appreciate my co-supervisor Milan Horemuz to give me some suggestions and help verify my results.

Finally, I would express my sincere gratitude to my family and friends. Thanks

to their supports, I can overcome the trouble and complete the thesis work.

1 Introduction 1

2 Background 3

2.1 Research Concept Vehicle . . . . 3

2.2 Sensors Introduction . . . . 4

2.3 State Estimation Techniques . . . . 8

2.4 Estimation Problems in GPS/IMU Navigation System . . . 11

3 Research Design and the State of the Art 15 3.1 Research Questions . . . 15

3.2 Research Method . . . 16

3.3 Limitations . . . 18

3.4 Related Work/State of Art . . . 18

4 INS/GPS Localization Based on AEKF 21 4.1 Vehicle Modeling . . . 21

4.1.1 Single-track Model . . . 21

4.1.2 Double-track Model . . . 22

4.2 Coordinate System . . . 24

4.3 Adaptive Extended Kalman Filter Algorithm . . . 25

4.4 Implementation . . . 26

4.4.1 Hardware Analysis . . . 26

4.4.2 Sensor Placement . . . 29

4.4.3 Software Platform . . . 30

4.4.4 Coordinate Transform . . . 31

4.4.5 Filter parameters & settings . . . 34

4.5 Experiment Results . . . 36

4.5.1 Algorithm Performance on Good GPS Condition . . . 36

4.5.2 Localization on GPS Oscillation . . . 40

4.6 Discussion . . . 44

5 Vision-aided Heading Estimation 47 5.1 Algorithm Introduction . . . 47

5.2 Yaw Rate Test . . . 48

CONTENTS 7

5.3 Results and Discussion . . . 49

6 Conclusion and Future Work 51 6.1 Conclusion . . . 51

6.2 Future Work . . . 52

6.2.1 More Robust Sensor Assembly . . . 52

6.2.2 More Extensive Parameter Tuning . . . 52

6.2.3 Dynamic Vehicle Model . . . 52

6.2.4 Implementation of dual GPS system . . . 52

Bibliography 53

Abbreviations

AEKF Adaptive Extended Kalman Filter AHS Active Heading Stabilization

CEKF Conventional Extended Kalman Filter DOP Dilution of Precision

EKF Extended Kalman Filter

GNSS Global Navigation Satellite Systems GPS Global Position System

IAE Innovation-based Adaptive Estimation ICC In-run Compass Calibration

IMU Inertial Measurement Unit

ISPKF Iterated Sigma Point Kalman Filter MFM Magnetic Field Mapper

ML Maximum Likelihood

NMS Non-maximum Suppression PF Particle Filter

RANSAC Random Sample Consensus RCV Research Concept Vehicle

RMSE Root Mean Square Error ROS Robot Operating System RPF Regularized Particle Filter RTK Real Time Kinematic

SIRF Sampling Importance Resampling Filter SLAM Simultaneous localization and mapping UKF Unscented Kalman Filter

UT Unscented Transform

VO Visual Odometry

WO Wheel Odometry

List of Figures

2.1 KTH RCV (Research Concept Vehicle) . . . . 3

2.2 Positioning from IMU Measurements [5] . . . . 4

2.3 Xsens MTi-G-700 . . . . 5

2.4 GPS Positioning in the Urban Environment[10] . . . . 5

2.5 Good and Poor Satellite Geometry [11] . . . . 6

2.6 Trimble SPS852 Modular GPS Receiver . . . . 6

2.7 Reach RTK Receiver . . . . 6

2.8 RTK Positioning Principal[12] . . . . 7

2.9 ZED camera . . . . 8

2.10 VBOX 3i Dual Antenna . . . . 8

2.11 State Transition Graph . . . . 9

2.12 EKF vs UKF [15] . . . 11

2.13 GPS/IMS Navigation Structure . . . 12

2.14 Linearization on the Sate with Different Uncertainties . . . 12

4.1 Kinematic Bicycle Model . . . 22

4.2 Kinematic Double-track Model . . . 23

4.3 ECEF and ENU Coordinate . . . 25

4.4 AEKF Algorithm Structure [16] . . . 25

4.5 MFM successful result [31] . . . 27

4.6 Yaw Rate calculated from steering encoders . . . 28

4.7 Sensor Placement on RCV . . . 29

4.8 Data Communication in RCV . . . 30

4.9 Rqt Frame Tree . . . 31

4.10 GPS coordinate Transform . . . 32

4.11 Testing Track . . . 36

4.12 Yaw Estimation by AEKF . . . 37

4.13 Estimation at Low Speed . . . 38

4.14 Estimation at High Speed . . . 38

4.15 Position Estimation by AEKF . . . 39

4.16 Yaw Rate Estimation by AEKF . . . 40

4.17 GPS Signal on the Test Road . . . 41

4.18 Position estimation by EKF . . . 42

4.19 Position estimation by UKF . . . 42

4.20 Position Estimation by AEKF (initial σ

= 0.25) . . . 43

4.21 Position Estimation by AEKF . . . 43

4.22 Covariance Estimation by EKF . . . 45

4.23 Path Estimation by AEKF . . . 46

5.1 Yaw Rate Test between VO and WO . . . 49

5.2 Yaw Estimation by AEKF with Vision Aided . . . 50

List of Tables

2.1 Comparison between VO and V-SLAM . . . . 8

2.2 GPS Measurement Methods . . . 13

3.1 AEKF Algorithm Test on Good GPS condition . . . 17

3.2 AEKF Algorithm Test on GPS oscillation . . . 17

3.3 Vision-aided Heading Estimation Test . . . 18

4.1 Message frequency . . . 34

4.2 Observation Input . . . 35

4.3 Yaw Estimation Error . . . 37

4.4 Estimation Error at Low Speed . . . 38

4.5 Estimation Error at High Speed . . . 38

4.6 Position Estimation Error (AEKF, EKF, and UKF) . . . 44

5.1 Yaw Rate Estimation Error between VO and WO . . . 48

5.2 Vision-aided Heading Estimation . . . 50

Chapter 1

Introduction

Autonomous driving is becoming a popular topic and its development grows rapidly with abundant contributions from industry and research organizations. The au- tomation level can be divided into five stages, ranging from Driver Assistance (level 1) up to full automation (level 5) [1]. Currently, a majority of vehicles still depend on drivers’ manual operations but possibly assisted by some safety systems to avoid potential accidents. For instance, automatic braking system could help prevent the collision with the pedestrian or other road users in front. Even more advanced in reality, on-road level 2 driving has already been possible due to ADAS(Advanced Driver Assistance Systems), such as steering and lane control assistance, remote parking systems, etc. The full automation remains under conceptual research so far, however, highly and fully unmanned driving are developed rapidly due to the technique revolution in various areas, such as the great progress in machine learning and artificial intelligence. Without human inference, the perception of surround- ing environment relies on multiple sensors’ detection, such as IMU, GPS, Lidar or camera on the vehicle. For instance, Tesla Model 3 uses eight cameras, a forward radar and twelve ultrasonic sensors for its autopilot/auto-steering features [2]. The camera is responsible for auto-steering, in particular keeping the car in the lane, as well as detecting the surrounding environment such as pedestrian, obstacles and traffic sign. While the radar is primarily used to determine relative speed to the objects in front to achieve "Traffic Aware Cruise Control".

Redundant sensor inputs are fused to estimate more accurate vehicle states, which is of great importance for path planning and vehicle navigation control. As for the localization issue in automotive industry, GPS/IMU coupled fusion is nor- mally adopted and works effectively in the case where GPS reliably measures the position. Moreover, during recent years, camera, lidar or radar are gradually utilized to improve the robustness especially when GPS or IMU works improperly.

The thesis is proposed to develop an algorithm to localize the position and esti-

mate the odometry for an electric vehicle with multiple sensors supported, including

wheel encoders, IMU, GPS, and stereo camera. The algorithm aims to overcome the

localization problem on bad GPS condition, and heading estimation problem under

magnetic interference. The implementation platform is based on the RCV (Re-

search Concept Vehicle), a battery electric vehicle made by KTH. The robustness

for the estimator is concentrated to be researched on, especially its robustness to

poor GPS measurements and magnetic interference. In Chapter 2, sensors and some

common navigation techniques are basically introduced. Then, Chapter 3 clarifies

the research questions and methods. The algorithm and implementation work are

discussed about in Chapter 4 and 5. Eventually, the conclusions derived from the

thesis project and suggestions for the future work are summarized in Chapter 6.

Chapter 2

Background

2.1 Research Concept Vehicle

Research Concept Vehicle is a vehicle prototyping made by KTH for research, con- cept demonstration and verification. The RCV is shown in Figure 2.1 below.

Figure 2.1: KTH RCV (Research Concept Vehicle)

RCV is a steer-by-wire and drive-by-wire electric vehicle with different sensors

attached, to be more specific, two Velodyne lidars, one front ZED camera, encoders,

Trimble GPS, Reach RTK GPS, and Xsens IMU. The communication network in

RCV is divided into a lower and upper layer, which are based on CAN and Ethernet

network respectively. The network on the lower layer is principally responsible for

the interactions among dSpace autobox, actuator drivers, encoders, and Xsense

IMU. While the Ethernet on the upper layer enables the connection from lidars,

camera, GPS receiver to a ROS computer. The mutual data transmission between

this hierarchical network is bridged through UDP communication protocol. More

descriptions of the networks and function assignments in RCV will be discussed

about in Chapter 4.

In terms of basic dynamical properties for RCV, the maximum steering angle is ± 25

actuated by linear actuators [3]. Each wheel is driven by its hub motor with the install torque at 150 Nm and rated maximum speed of 520 rpm, which can speed the vehicle up to 55 km/h [4].

2.2 Sensors Introduction

Inertial Measurement Unit(IMU) is a self-contained electronic device that mea- sures linear and angular motion. Commonly, IMU consists of a triad of gyroscopes and accelerometers, but some types have magnetometers equipped. A 3-axis ac- celerometer measures the acceleration along x, y, z axes of IMU respectively, while a 3-axis gyrosope measures angular velocity. In addition, a magnetometer is able to measure the earth magnetic field intensity and derive the global roll, pitch and yaw.

Unfortunately, since RCV is an electric vehicle, it can generate strong electromag- netic field to distort the measurement of a magnetometer. However, it is possible to derive the position by the angular velocity and acceleration from IMU, which is shown in Figure 2.2.

Figure 2.2: Positioning from IMU Measurements [5]

Deciphered from the figure above, the position is calculated from double inte- gration of accelerations after the subtraction by gravitational acceleration g, which can accumulate the error from a gyro bias. Moreover, when RCV is driven uphill or downhill, the vertical acceleration is no more aligned with the gravity direction, which also leads to inaccurate positions.

In RCV, Xsens MTi-G-700 with GNSS(Global Navigation Satellite Systems) aided is adopted for use, which is shown in Figure 2.3. It can provide precise mea- surements with 10 deg/h gyro bias stability and 0.8 deg yaw measurement accuracy.

In order to improve the yaw measurement, there are two calibration algorithms in this type of Xsens IMU, that are AHS(Active Heading Stabilization) [6] and ICC(In- run Compass Calibration) [7]. AHS is aimed at tackling the magnetic distortions that do not move with the sensor, such as temporary or spatial distortions in the surrounding environment. Some other distortions such as calibration errors, impact from the materials of hard and soft iron cannot be removed by AHS effectively.

Fortunately, those distortions can be estimated by MFM(Magnetic Field Mapper)

2.2. SENSORS INTRODUCTION

[8] calibration. Instead of being north-referenced, the yaw output will be referenced with respect to startup heading after AHS is enabled. According to the specializa- tion of MTi series in the product documentation, with AHS assisted, the drifting error of the yaw measurement can be reduced by 1 deg for the MTi 100-series and 3 deg for the MTi 10-series after 60 min [6]. Furthermore, imperfect MFM calibration and inhomogenous magnetism can also introduce some errors.

Figure 2.3: Xsens MTi-G-700

Global Position System(GPS) is a satellite-based radio-navigation system that provides the geolocation and time information of anywhere on the Earth where there are unobstructed lines of sight to at least 4 satellites [9]. GPS works by con- necting the receiver to multiple satellites around the space and localize the position given radio speed, locations of satellites, and the time for signal travelling. In terms of GPS/IMU navigation problem, IMU works well at measuring accelerations and yaw rate, but diverges the position due to the error accumulated from gyro bias.

Fortunately, this divergence can be corrected by the GPS global positioning.

However, GPS signal can oscillate from time to time due to some environmental factors, such as atmosphere refraction and dense urban regions. Figure 2.4 shows the GPS oscillation when the vehicle is driven at the dense district where crowded buildings stand aside.

Figure 2.4: GPS Positioning in the Urban Environment[10]

In addition, bad satellite geometry also degrades the precision, which can be

expressed by DOP(Dilution of Precision) [11]. DOP can be classified into “math-

ematical” and “geometrical” DOP. The latter one is equal to the reciprocal of the

volume of a tetrahedron shaped by satellites. Generally, if a DOP becomes larger, the worse GPS signal is received, and vice versa. For example, in Figure 2.5, the volume of tetrahedron on the left picture is obviously greater than the right one, which corresponds to a smaller DOP and better GPS measurement. In the worst case, a infinite value of DOP can be derived when satellites are aligned in a straight line. Therefore, even though many satellites are available to be detected, there still exists a possibility of GPS outage due to poor satellite geometry.

Figure 2.5: Good and Poor Satellite Geometry [11]

Trimble SPS852 Modular GPS receiver is equipped on RCV originally. With RTK(Real Time Kinematic) service facilitated, Trimble GPS is able to localize the vehicle in centimeter-level accuracy. Unfortunately, Trimble was no longer able to be set up due to limited knowledge on RCV network and undesirable change on the configuration by previous users. Therefore, it was replaced by Reach RTK(shown in Figure 2.7) during some tests. However, Trimble GPS without RTK setup still can provide accurate position in an open environment with good satellite geometry.

Figure 2.6: Trimble SPS852 Modular GPS Receiver

Figure 2.7: Reach RTK Receiver

2.2. SENSORS INTRODUCTION

The working principal of RTK is demonstrated in Figure 2.8. RTK technique establishes the network amongst GNSS satellite, rover station, and base station. A base station is particularly responsible for post-processing the data received from satellites and transmitting to a rover station. A rover station receives the carrier wave, meanwhile corrects the position using the signal from a base station. With RTK technique assisted, it becomes possible to localize the vehicle in the accuracy up to 1 cm + 1 ppm under good GPS condition.

Figure 2.8: RTK Positioning Principal[12]

Wheel Odometry(WO) calculates the vehicular state only by a vehicle’s kine- matics and speed. The incremental encoder and absolute encoder are commonly used to measure rotational velocity and position. RCV has four incremental en- coders attached to the wheel axle to measure the velocity of front wheels and rear wheels respectively. Apart from that, the other four encoders measure the steering angle of each wheel. In contrast to absolute encoders, the incremental encoders in RCV cannot store the steering angle after power-off, which might introduce initial measurement errors when RCV is restarted.

Visual Odometry(VO) estimates the ego-motion of a stereo camera by asso-

ciated images, which is widely used in autonomous driving in recent years. Visual

odometry derived by stereo camera is named stereo VO. It estimates camera mo-

tion by computing relative rotation and translation between 3D landmarks which

are triangulated from stereo-matched image features. Moreover, the stereo VO

can avoid scale ambiguity in monocular VO and work more efficiently than visual

SLAM(Simultaneous localization and mapping). Table 2.1 compares differences

between VO and SLAM method.

Table 2.1: Comparison between VO and V-SLAM

Vision-based Technique Visual Odometry Visual SLAM Problem Focus Local trajectory Global trajectory, map Optimization range Last poses of the path Loop closure, globally

Computational Cost Low High

Estimation Accuracy Low High

As is compared in the table, visual odometry has no dependency on prior knowl- edge of maps and has lower computational cost than visual SLAM. In the the- sis project, ZED stereo camera(shown in Figure 2.9) is utilized to generate visual odometry with 20 meters maximum detection range and up to 100Hz sampling fre- quency [13]. Moreover, the package of ZED camera provides a simple interface to ROS(Robot Operating System).

Figure 2.9: ZED camera

VBOX 3i Dual Antenna is a high-quality industrial product made by VBOX Automotive company, which is shown in Figure 2.10. It measures the acceleration, velocity and roll/pitch/yaw angle with twin antennas connected. According to the specialization documented in VBOX website, it can measure the heading in 0.1

accuracy with the resolution of 0.01

, and the velocity in 0.1km/h accuracy [14].

Thereby, VBOX is sufficient to provide the ground truth for algorithm tests.

Figure 2.10: VBOX 3i Dual Antenna

2.3 State Estimation Techniques

The localization problem substantially is to estimate a dynamic state relying on ob-

servations from various sensors. The estimation is simply based on Bayesian filter,

2.3. STATE ESTIMATION TECHNIQUES

which consists of prediction and update stage. Bayesian filter recursively estimates the state based on Markov model, which is indicated in Figure 2.11. In a Markov chain, the conditional probability of the present state x

only relies upon the latest previous state x

, rather than on the sequence of preceded states. The transition between states goes through the process/motion model interfered by the control signal u

, which is reflected on the prediction stage of Bayesian filter. Meanwhile, There exists a variety of measurements z

observed from the state x

to correct the predicted state on the update stage. The equations in prediction and update step are expressed as followed:

The Prediction Step:

p (x

| z

, ..., z

, u

, ..., u

)

=

p (x

| x

, u

)p(x

| z

, ..., z

, u

, ..., u

)dx

(2.1)

The Correction Step:

p (x

| z

, ..., z

) ∝ p(x

| z

, ..., z

) (2.2)

Figure 2.11: State Transition Graph

Extended Kalman Filter(EKF) is a nonlinear version of conventional Kalman

filter by linearizing an estimate of mean and covariance. In EKF, the motion and

observation model are no longer linear functions but expressed by differentiable

functions instead, which are indicated by equations (2.3), (2.4).

System Description

x

= f(x

, u

) (2.3)

z

= h(k − 1) (2.4)

Prediction:

x

= f(x

, u

) (2.5)

P

= AP

A

+ Q (2.6)

Update:

K = P

H

(HP

H

+ R)

(2.7) x

= x

+ K(z

− h (x

)) (2.8)

P

= (I − KH)P

(2.9)

Innovation:

d

= z

− h (x

) (2.10)

residual:

ε

= z

− x

(2.11)

As is shown in the above equations (2.3) - (2.4), both motion model f(x) and observation model h(x) are nonlinear originally. In order to calculate the state uncertainty, the motion model will be linearized at the mean point of the current state’s Gaussian distribution by Jacobian matrix on the prediction stage, which is denoted as matrix A in the equations. Similarly, matrix H denotes the observation model linearized on the update stage. After that, Kalman gain K is used to balance the weight between the predicted state and observations and derive the mean and uncertainty of the current state estimate.

UKF(Unscented Kalman Filter) is an estimation technique invented to solve lin- earization problems. UKF selects the weighted points in symmetric pairs(so called

“sigma points”) from the Gaussian distribution of the original state and then propa- gates the points directly through the nonlinear model by UT(Unscented Transform).

UT is a method to calculate the statistics of a random variable to approximate the

nonlinear relationship [15]. Therefore, without linearization in UKF, the approxi-

mation error can be reduced. Moreover, UKF is more advantageous to estimate the

state if the transformed state is not distributed as Gaussian, which is demonstrated

in Figure 2.12. In the case where the actual state after transformation is no more

distributed as Gaussian, both mean and covariance of the state estimated by UKF

are much closer to the ground truth than linearized EKF.

2.4. ESTIMATION PROBLEMS IN GPS/IMU NAVIGATION SYSTEM

Figure 2.12: EKF vs UKF [15]

AEKF(Adaptive Extended Kalman Filter) adaptively estimates the covariance of process noise and measurement noise based on innovation (2.10) and residual (2.11) to improve the accuracy of state estimation with covariance varying[16]. In terms of CEKF(Conventional Extended Kalman Filter) , the covariance either of process noise or measurement noise is fixed and unchanged during the estimation proce- dure. Therefore, estimation based on CEKF might lead to some problems when the covariance is not known in prior or varies during the estimation procedure. For instance, when Kalman filter is used to estimates the dynamic state of synchronous machine in order to monitor and control the stability of its power system, wrong selection of the initial covariance matrices can diverge the estimate on rotor speed and angle easily. However, AEKF can be helpful to estimate the unknown covari- ance even though wrong covariances are initialized. Moreover, AEKF also functions better in the case where measurement noise varies from time to time. A typical ap- plication is in GPS/IMU navigation system[17]. The phenomenon that GPS signal oscillates in the urban environment reflects the covariance change. Given a proper GPS measurement covariance in real time, Kalma filter can estimate the position in a more robust way.

2.4 Estimation Problems in GPS/IMU Navigation System

The common GPS/IMU navigation structure is demonstrated in Figure 2.13. The

estimator fuses the odometer, GPS and IMU together to estimate the vehicular

state. In Kalman filter based estimator, the predicted state is derived from the

vehicle’s motion model and corrected by odometer, GPS, IMU observations sequen-

tially on the update stage.

Figure 2.13: GPS/IMS Navigation Structure

The estimation challenges of the vehicle navigation problem mainly includes these two topics: the linearization problem, and GPS oscillation and outage.

1. As is discussed in the previous section, EKF is commonly used in the vehicle localization due to its simplicity for practice and robustness. During the prediction step in EKF, if the motion model is nonlinear, the linearization on the model can introduce the approximation error, such as when a vehicle steers. This error can be impacted by the covariance/uncertainty of the state, which is indicated in Figure 2.14.

Figure 2.14: Linearization on the Sate with Different Uncertainties

Figure 2.14 shows the result when the state of different covariances is linearized.

The Guassian distribution on the bottom represents the uncertainty of the initial state or the state updated from previous estimate. The curve above it describes the nonlinear transform function for the motion model. The diagram on the left top compares the distribution of the transformed state and ground truth, which are expressed as the red and blue plot respectively. As can be observed, the mean of the predicted state approaches the actual mean more with the original state uncertainty increasing. Therefore, it can be concluded that the linearization error in EKF cannot be removed but can be reduced by increasing the state uncertainty.

On the other hand, high nonlinearity of a motion model also incurs great errors in

EKF, such as when a vehicle is driven in a roundabout. Given identical distribution

2.4. ESTIMATION PROBLEMS IN GPS/IMU NAVIGATION SYSTEM

of the initial state, a transform function with higher nonlinearity leads to greater approximation errors.

2. The reliability of GPS measurement is mainly influenced by geometry of satellites, which corresponds to its covariance given to Kalman filter. The GPS covariance plays an important role in outlier detection and state estimation for Kalman filter. When the GPS signal with a large DOP is received, the covariance of GPS observation should be adjusted to be larger at the same time in order to balance the predicted state on the update stage. However, a bad GPS measurement with larger covariance can be more likely to be classified as an inlier by Mahalanobis threshold [18] in Kalman filter, which is not expected in the case of GPS outage. In addition, different measurement methods and GPS receivers also constrain the GPS accuracy, which is shown in Table 2.2. For example, with the receiver of C/A-code and L1 frequency adopted, the GPS measured absolutely with 19 meters deviation can still be considered as valid information for use, otherwise should be categorized as an outlier.

Table 2.2: GPS Measurement Methods

Method Receiver Observation

Time

Horizontal Uncer- tainty(95%)[m]

Vertical Uncer- tainty(95%)[m]

Absolutely C/A-code,L1 1s 19 39

Absolutely P-code,L1& L2 1s 3 6

AbsolutelyPPP Code phase,L1& L2 >1h 0.05 0.10

RelativelyDGPS C/A-code,L1 1s 1 2

RelativelyRTK Code phase,L1 & L2 1s 0.03 0.05 Static

Relatively Code phase,L1 & L2 >1h <0.01 <0.02

Chapter 3

Research Design and the State of the Art

RCV provides multiple sensors including a front stereo camera, Lidars, IMU, GPS, and encoders. It is possible to implement an advanced filter to overcome GPS oscillation by only fusing GPS, IMU, and encoders. Apart from the position esti- mate, heading is also very important information for advanced motion control in autonomous driving. However, the magnetic interference from RCV and the sur- rounding environment can affect the heading estimate. To overcome this heading error, visual odometry could be an alternative.

3.1 Research Questions

GPS/IMU based Localization with conventional Kalman filter has problems of high dependency on the GPS position and IMU yaw. Especially within the urban en- vironment, undesirable GPS oscillation has a negative impact on position estimate due to crowded buildings and bridges. Furthermore, GNSS satellite geometry with large DOP can also degrade the estimator performance on positioning. For path planning and following in autonomous vehicle, a consecutive positioning is of great importance to avoid undesirable control behaviour. GPS, as the only reliable mea- surement to correct the position in Kalman filter, fluctuates with varying covariance.

Therefore, assuming the covariance of GPS signal can be estimated and applied to Kalman filter in real time, the Kalman filter can work better at balancing weights between the predicted state and measurements, which may result in a more con- secutive and accurate position estimate. Adpative extended Kalman filter, which is different from EKF and UKF, adjusts the covariance of measurement and process noise dynamically according to the sequence of previous state estimates. This might improve the localization performance under GPS oscillation.

Question 1: Through some experiments, can AEKF (Adaptive Extended Kalman Filter) outperform CEKF (Conventional Extended Kalman Filter) and UKF (Un- scented Kalman Filter) in terms of estimation accuracy, and robustness to GPS oscillation and outage?

IMU, usually consists of a gyroscope, accelerator and magnetometer. It pro-

vides measurements such as yaw, yaw rate and acceleration on longtitudinal and lateral directions. However, Since RCV is an electric-driven vehicle, the gener- ated magnetic field can distort the magnetometer in IMU, which can influence the yaw estimate indirectly. Fortunately, Xsens IMU has the algorithm - AHS(Active Heading Stabilization) to help measure the yaw angle under magnetic interference.

However, even with AHS assistant and proper isolation, the yaw measured from IMU still drifts due to calibration errors, magnetic interference from RCV and the surrounding environment. Visual odometry, which is independent to the magnetic field, can be an alternative to compensate the error of yaw measurements.

Question 2: Can visual odometry improve the accuracy and constrain the drifting error of heading estimate due to magnetic interference?

3.2 Research Method

The sensors utilized for algorithm design have never been tested, therefore sensor analysis should be done in highest priority. During the tests, IMU should be cal- ibrated and configured to the AHS alogrithm setting, while the accuracy of GPS also needs to be evaluated, including Trimble GPS, Xsens GPS and Reach RTK GPS. After the sensor test completed, it is essential to reattach sensors on proper positions on RCV, for instance, IMU should be fixed away from the RCV motors to reduce electromagnetic disturbance. With the hardware well-prepared, the software can be designed.

The experiment should be divided into three different cases, which are listed as followed:

Experiment 1. AEKF Algorithm Test on Good GPS Condition. The purpose of this experiment is to verify if the AEKF algorithm can perform well in a good GPS condition and improve the state estimation. The test track is selected in an open environment where not many buildings can block GPS signals. During the experiment, the velocity, yaw and yaw rate are compared with the ground truth provided by Vbox. Reach RTK GPS can provide centimeter-level position measurements when the GPS receiver is in“Fix” status [19] under sufficient satellites available. This perfect GPS is fused into the AEKF algorithm and used to provide the ground truth of position as well. If the path estimated by AEKF follows the GPS path, it can verify that AEKF localize the vehicle well on good GPS condition.

The experiment design is summarized in table 3.1.

3.2. RESEARCH METHOD

Table 3.1: AEKF Algorithm Test on Good GPS condition

Experiment Design Content

Test Environment Open environment, cars parked aside the road Fused Sensors Xsens IMU, Reach RTK GPS, Wheel Odometry Ground Truth Vbox, Reach RTK GPS

Test Target Position, velocity, yaw, yaw rate Evaluation Method RMSE (root mean square error)

Experiment 2. AEKF, CEKF, and UKF Test on GPS oscillation. To guarantee different algorithms are tested under the same condition, sensor data is recorded to a ROS bag and replayed offline. The track is selected from google map in prior and includes some regions with crowded buildings aside in order to test the algorithm on GPS oscillation. In the experiment, Reach RTK was replaced by Trimble GPS because Reach RTK GPS(with RTK set-up) can provide precise positions in “Fix”

or “Float” status but can completely give unusable measurements under worse GPS condition due to its single-frequency receiver. However, even though Trimble GPS (without RTK set-up) has lower positioning accuracy but it still can provide good positions in decimeter-level under good satellite geometry, and gives oscillated but consecutive position in dense areas. Therefore, since Reach RTK GPS is not be able to localize in the dense urban environment, GPS regression from Trimble GPS was utilized for the ground truth in the experiment. In addition, RMSE(root mean square error) was used to compare the localization accuracy amongst various algo- rithms. Apart from that, the sensitivity to GPS outliers and the smoothness of a estimated path can also be observed from the results. The experiment design is summarized in Table 3.2.

Table 3.2: AEKF Algorithm Test on GPS oscillation

Experiment Design Content

Test Environment Crowded buildings on both sides of the road Fused Sensors Xsens IMU, Trimble GPS, wheel odometry

Ground Truth GPS regression

Test Target Position

Evaluation Method RMSE, robustness to GPS outliers, path smoothness

Experiment 3. Heading Estimate Test under Magnetic Interference. In order to ensure IMU is exposed to magnetic interference, the experiment was conducted on the road where some other vehicles were parking aside, which can affect IMU by interference from ferromagnetic materials. Because RCV is a battery electric car, it can generate inhomogenous magnetic field when steering and changing speed.

Therefore, a tight turning is selected for the test track. The experiment design is

summarized in Table 3.3.

Table 3.3: Vision-aided Heading Estimation Test

Experiment Design Content

Test Environment Open environment, cars parked aside the road

Fused Sensors Xsens IMU, Reach RTK GPS, wheel odometry, visual odometry

Ground Truth Vbox

Test Target Yaw

Evaluation Method RMSE , drifting errors

The detailed experiments are discussed about in Chapter 4, 5 based on the research method design.

3.3 Limitations

In experiment 2, it is not possible to provide the ground truth in dense areas by Reach RTK GPS because the noise due to signal multipath are amplified in a single- frequency receiver. Due to limited knowledge on RCV network and undesirable change on the configuration by previous users, Trimble GPS cannot be setup with RTK service. However, it still can provide GPS measurements in decimeter-level under good GPS condition, and gives consecutive oscillated measurements in dense streets. During the experiment, RCV was driven along the road predetermined on Google map and straightforward on poor GPS regions on purpose. Therefore, the ground truth can be regressed by these good Trimble GPS measurements, which is sufficient for the comparison among AEKF, CEKF, and UKF.

It might be better to record the data and replay it offline to compare the heading estimate with and without visual odometry fused on the same dataset. However, the communication between a ZED camera and ROS could be broken down when image data are recorded into a ROS bag during the experiment 3. Instead, the vision-aided test was conducted right after the test case with no visual odometry fused in order to keep the same preconditions. Without visual odometry fused, yaw estimate highly depends on IMU yaw, therefore the drifting of IMU yaw can cause the error of yaw estimation. Therefore, if visual odometry can prevent the yaw estimate from drifting as IMU yaw and decrease RMSE, it can be sufficient to prove that visual odometry indeed can improve the heading estimation under megnetic interference.

3.4 Related Work/State of Art

A great amount of researches contribute to the localization problem for unmanned

vehicles. Conventionally, a Kalman filter based GPS/IMU coupled system is im-

plemented on navigation. Rezaei, et al. applied extended Kalman filter to fuse the

wheel odometer, yaw rate gyro and GPS measurement [20]. The experiment result

3.4. RELATED WORK/STATE OF ART

from Rezaei’s research indicated EKF can carry out the estimation on more than 60 km distance even under slightly bad GPS condition. In order to cope with the linearization problems in EKF, a bunch of new filters were put forwards. Rudolph, et al. developed a new filter framework, called Sigma-point Kalman filter, which was verified to outperform EKF in GPS/IMU navigation system under the same computational complexity [21]. It improved the position estimate by 30% than EKF through the experiment on an unmanned aerial vehicle. Jeong, et al. implemented SIRF(Sampling Importance Resampling Filter) and RPF(Regularized Particle Fil- ter), which are variants of PF(Particle Filter) [22], on the mobile robot localization and proves that PF performs better than EKF in position and heading estimate.

PF is an estimate technique using Monte Carlo algorithms for dynamic state es- timation, which does not require the prior assumption of a Gaussian distribution.

Instead, a bunch of weighted particles are used to describe the state probability.

This can reduce the linearization error but may have the problems of high compu- tational cost. St-Pierre, et al. compared the performance of UKF and EKF on a navigation system [23]. Principally, the UKF itself takes more advantages to deal with the linearization errors than EKF, but it is not effective in the case where no GPS is available, which is shown in Mathieu’s tests. In addition, low dynamics of the vehicle can make the linearlization error in EKF negligible. Moreover, UKF has a drawback of more computational time than EKF.

Since the vehicle localization relies on GPS to correct the predicted state, CEKF with fixed covariances can diverge the position estimate on GPS oscillation and outage. In order to deal with the noisy measurements in EKF, El-Sheimy, et al.

proposed a fuzzy-augmented Kalman filter to compensate the error from measure- ment update [24]. The fuzzy model needs inputs extracted from the outputs of IMU mechanization equations and the errors of position and velocity to train the model parameters. This method can effectively improve the estimation performance dur- ing GPS outage according to the research, but its complicated structure is hard for implementation. Bistrovs, et al. evaluated the UKF, EKF on poor GPS occasions.

The result came out that UKF could better tolerate unreliable GPS [25].

In stead of using the fixed covariances, another solution can be to estimate the GPS covariance dynamically. According to the introduction from section 2.2 men- tioned, a poor satellite geometry, corresponds to a small DOP, can scale up the poor GPS measurements. Mathematically, DOP is equal to the square root sum of the main diagonal elements in a covariance matrix. Therefore, the GPS oscillation can be reflected by the change on GPS covariance. Mohamed, et al. proposed a approach - IAE(Innovation-based Adaptive Estimation) to estimate variable covari- ances in GPS/IMU navigation systems [26]. This scalar adaptive estimator is based on the ML(Maximum Likelihood) and an innovation V-C matrix that is computed from a previous innovation sequence. However an innovation-based estimator can- not guarantee the measurement covariance derived to be positive. In order to deal with this problem, a AEKF(Adaptive Extendd Kalman Filter) with a residual-based estimation on the measurement covariance are utilized in the research of Wang [27]

and Akhlaghi, et al. [16], which is also applied in this thesis project to overcome

unreliability problems of GPS. More details will be discussed in the following sec- tions.

Visual Odometry estimates frame-to-frame motion by image matching. Kitt,

et al proposed an algorithm to estimate ego-motion based on a trifocal geometry

between image triples [28]. They applied an ISPKF(Iterated Sigma Point Kalman

Filter) combined with a RANSAC-based(Random Sample Consensus) outlier re-

jection scheme to improve the robustness of VO(visual odometry). However, a

real-time VO highly depends on high computational performance of a processing

unit. This problem is improved by Geiger, et al., who proposed an more efficient

visual odometry with faster feature matching [29] than Kitt’s algorithm[28].

Chapter 4

INS/GPS Localization Based on AEKF

4.1 Vehicle Modeling

Vehicle modeling plays a crucial role in wheel odometry. Using speed and steering angle measured by wheel encoders, wheel odometry can be calculated by the vehicle model. Methodology in dynamics modeling can be categorized into kinematic model and dynamic model. In the thesis, only kinematic model is used for computing wheel odometry without considerations of a vehicle’s mass and yaw inertia, and tire model.

Moreover, the modelling methods also can be classified into single-track model and double-track model. The former one, commonly used for vehicle modelling, has less complexity to build dynamics relation. However, this might introduce greater modelling errors in comparison to double-track model for RCV due to different steering dynamics.

In the filter, the wheel odometry provides the observations of longitude and latitude velocity, yaw rate, and yaw for Kalman filter. Especially yaw rate and velocities are highly concerned since the absolute yaw is accumulated from yaw rate based on the previous heading estimate.

4.1.1 Single-track Model

Single-track model is also known as bicycle model. Its description is demonstrated

in Figure 4.1. It can describe vehicle behaviours on various occasions with less

complexity. During modelling, two wheels on each axle can be replaced by one single

wheel for simplified calculation. The nonlinear equations describing this kinematic

bicycle model are listed below [30], from equation (4.1) to (4.4):

β = tan

(( L

L

+ L

)tan(α

)) (4.1)

ω = ˙σ = v

L

sin (β) (4.2)

v

= ˙x = v cos(β) (4.3)

v

= ˙y = v sin(β) (4.4)

Here, (X,Y) is an inertial frame while x and y coordinating the center of the vehicle. σ is the inertial heading and β is the angle between the velocity of the body center, which is v indicated in Figure 4.1, and the longitudinal axis of the vehicle. As deciphered from the geometric relation in bicycle model, the heading (or the absolute yaw angle with respect to the (X,Y) coordinate) ω can be simply calculated by (σ + β), in which β can be derived from the steering angle of a front wheel α

. Then combined with current velocity v, the longitudinal and lateral velocity can be obtained. Note that L

and L

are the distances from a front wheel and rear wheel to the center of the car respectively.

Figure 4.1: Kinematic Bicycle Model

4.1.2 Double-track Model

RCV has high performance on steering mainly due to its capability to turn rear

wheels freely. This special design optimizes the steering system with higher curva-

tures but increases the complexity of the vehicle dynamics. Double-track model can

4.1. VEHICLE MODELING

better reflect the RCV dynamics and thereby mitigates model error. The double- track is demonstrated in Figure 4.2.

Figure 4.2: Kinematic Double-track Model

In the figure, α

still represents the steering angle of a front wheel, however, α

is no more equal to 0 deg since the rear wheel can move. Different from Figure 4.1, β, which is the angle of line OP with respect to longitudinal axis, labels the crabbing angle of the car. In order to calculate β, a new coordinate (a,b) is created with center of the vehicle as origin is created. Line OA, OB, which are perpendicular to the wheels, intersect with line OP to the point O. This point corresponds to the vehicle’s center of rotation with the radius |OP |. For simplification, only two wheels on one side are taken into calculation. Therefore, α

and α

are simplified as the average of the steering angles of the wheels on both side, which is indicated in equations: (4.5), (4.6).

α

= α

+ α

2 (4.5)

α

= α

+ α

2 (4.6)

According to the geometric relation modelled, it gives the position of the point O (a’,b’) with respect to the coordinate (a,b):

a

= (L

tan (α

) + L

tan (α

))

(tan(α

) − tan(α

)) (4.7) b

= a − x

tan (α

) (4.8)

Then, with the position of the point O (a’,b’) determined, the radius R or distance from the axis of rotation can be calculated as below:

R = |OP| =

a

+ b

(4.9)

Eventually, the crabbing angle can be computed in equations below:

β =











−|tan

( a

b

)| , if a

· b

> 0

|tan

( a

b

)|, , if a

· b

< 0

(4.10)

Meanwhile, calculate the yaw omega, longitudinal and lateral velocity ω, v

, v

:

ω = v cos (β)

R (4.11)

v

= v cos(β) (4.12)

v

= v sin(β) (4.13)

Note that heading ˙ω is integrated by yaw rate based on the previous heading estimate to decrease error accumulation.

4.2 Coordinate System

In terms of GPS positioning used in vehicle navigation, the transformation from global to local position coordinates involves five different coordinate systems: the geodetic coordinate system, the (UTM) coordinate system, the (ECEF) coordinate system, the local (ENU) coordinate system, and the body-fixed coordinate system.

The geodetic system is a reference system for describing a points in latitude and longitude globally[32]. Similarly, the universal transverse mercator (UTM) coordi- nate system uses map projection by secant transverse Mercator with 2-dimensional Cartesian coordinate system to give locations on the surface of the Earth [33].

The relation between ECEF and local ENU system can be described in figure 4.3.

ECEF, a coordinate system with the origin defined as the center of mass of Earth, can be transformed from geodetic coordinates, which is similar to the conversion to UTM[34]. The local frame, normally referenced by east-north-up, provides reference for the local position converted from ENU or UTM coordinate and places its origin at the starting point of the vehicle movement. The body-fixed coordinate, located in the same spatial space as local ENU coordinate, takes the center of the mass of the car as an origin and aligns x,y axis to the directions of longitudinal and lateral velocity respectively.

Both Trimble and Reach GPS can provide position information in UTM format.

The location of the vehicle in local frame can be converted from UTM earth frame

through translation and rotation between coordinate systems. More details will be

discussed in the next section.

4.3. ADAPTIVE EXTENDED KALMAN FILTER ALGORITHM

Figure 4.3: ECEF and ENU Coordinate

4.3 Adaptive Extended Kalman Filter Algorithm

There are multiple adaptive Kalman filter algorithms researched for dynamic state estimation. In terms of the AEKF algorithm used in this thesis project, the measure- ment covariance is estimated based on residual, while the process noise covariance is based on innovation. The detailed algorithm structure is demonstrated in Figure 4.4.

Figure 4.4: AEKF Algorithm Structure [16]

AEKF can recursively update measurement covariance based on innovation us-

ing:

R

= S

− H

P

H

(4.14) Here S

refers to the covariance matrix of the innovation. However, this equation cannot guarantee the estimated measurement covariance R

be a positive matrix since subtraction between S

and H

P

H

might lead to a negative result. There- fore, in order to ensure the R

is calculated to be a positive matrix, a residual-based approach proposed by[27] is used as:

ε

= z

− h

(ˆx

) (4.15)

S

= E[εε

] (4.16)

R

= S

+ H

P

H

(4.17) Note that the expression ε

is the residual. In equation (4.16), S

calculated by averaging εε

must be a positive matrix. Consequently, the result of equation 4.17 is always positive. There are basically two methods to calculate the average value of εε

. One is to use sliding window[35], while the other solution is to introduce a forgetting factor α[36], which helps save the memory and is simpler to implement.

Then the equations can be transformed as below.

R

= αR

+ (1 − α)(εε

+ HP

H

) (4.18) With a forgetting factor α (0<α<1) employed, preceding states becomes less and less effective on current covariance estimation along the process. A larger α can increase the weight on previous estimates, which prevents the covariance estimate from frequent oscillation but may incur longer time delays to catch up with changes.

In the paper [16], α = 0.3 is used for the studies.

The process noise Q is estimated based on innovation in the AEKF algorithm.

Similarly, using a forgetting factor to average estimates over time gives the equation as below.

Q

= αQ

+ (1 − α)K

d

d

K

(4.19)

4.4 Implementation

4.4.1 Hardware Analysis IMU Analysis and Calibration

Xsens MTi-G-700 is used for motion tacking integrated with IMU and GPS. How-

ever, since Xsens GPS has considerable bias on position estimates, only acceleration,

4.4. IMPLEMENTATION

yaw rate and yaw angle coming from IMU are fused in Kalman filter. The setting configured in Xsens can be adjusted with a suite of software (so called “MT man- ager”) from Xsens company, which can be downloaded from their official website. In the on-board filter mode setting in “MT manager”, the “General” mode is currently set by default. During the tests, the other mode either the GeneralMag mode or the automotive modes does not result in better performance. There are mainly two calibration algorithms in Xsens, Active Heading Stabilization (AHS) and In-run Compass Calibration (ICC). The working principal of AHS has been introduced in the previous Section 2.3. Here documented the AHS performance in on-vehicle testing. Basically, AHS is enabled to lower down the drifting error of yaw output under magnetic field interference and uses magnetic norm to identify the magnetic distortion. Before using AHS, the magnetometer need to be re-calibrated by Mag- netic Field Mapping (MFM). MFM is helpful to remove the static magnetic field distortion caused by RCV (batteries and ferromagnetic materials like the frame). A software in the suite called “magfieldmapper-gui” provides a simple interface to cali- brate MFM. Figure 4.5 shows the successful calibrated MFM results for RCV. In the top left window in the figure, the rounder the sphere is, the better results of MFM can be. The top right window indicates the corrected MFM with respect to new magnetic field model. Small residual outside the Gaussian distribution represents small error due to unregularity of the magnetic field model. The two figures below demonstrate the comparison between the case before and after MFM calibration.

Figure 4.5: MFM successful result [31]

After calibration by MFM, AHS starts working to stabilize the heading estima- tion well and the drifting is smaller than 0.2 deg/min[31] on the way driving at low speed(around 2m/s) in the environment with limited magnetic field. However, it needs to be mentioned that the MFM calibration is never perfect since it is impos- sible to turn around the RCV in three dimensions to complete the full calibration.

Furthermore, it is not reasonable to attach Xsens to the bottom center of RCV

frame, which is very close to front motors. The varying magnetic field generated from motors, which cannot be removed either by MFM or AHS algorithm theo- retically, can influence the IMU yaw output greatly. Then Xsens IMU was moved upwards and far away from the motors.

In conclusion, even though MFM and AHS can decrease the impact of magnetic interference, the yaw measurements still can drift due to calibration errors and when the car approaches other ferromagnetic materials, changes speed, or steers, which generate inhomogeneous magnetic field.

Steering Encoder Analysis

According to the geometric relation in the kinematic model, the yaw rate is cal- culated from the steering angle of each wheel. Thereby, the measurement error of the steering angle can be accumulated to affect yaw rate and indirectly the heading since the heading is derived from the integration of yaw rate. Therefore, it is neces- sary to verify the reliability of yaw rate from wheel odometry. The result recorded from one of the experiment case is shown in Figure 4.6.

Figure 4.6: Yaw Rate calculated from steering encoders

In comparison to the ground truth(the orange coloured plot), the yaw rate calcu-

lated from vehicle dynamics using steering angles has large errors specifically when

the vehicle moves in a straight line. However, in the case where the car steers with

a large curvature, the yaw rate is very close to the ground truth, which verifies that

the geometric calculation is correct. The reason why the error is bigger when driv-

ing in a straight line is due to strong vibrations of the wheels, not well-performed

suspension system and unrobust hardware assembling in RCV. Therefore, the yaw

integrated from this oscillated yaw rate is not reliable for use. Instead, the yaw

estimate from Kalman filter replaces it to calculate the longitudinal and lateral

velocity.

4.4. IMPLEMENTATION

GPS Property Analysis

Trimble GPS without RTK is used for research on GPS oscillation. With the current configuration from previous work, the GPS positioning is varying within 0.5m in the open environment, where signals from many satellites are received in good condition. However, due to its sensitivity to the satellite geometry, GPS positioning can oscillate in a large scale in dense urban areas.

Reach GPS with RTK supported can position a vehicle precisely but it depends on the GPS status in the receiver. With the configuration of “GPS + GLONASS + GALILEO + SBAS + QZSS” pack and only 5 Hz sampling frequency, it works well at “Fix” or “Float” status and can reach the measurement accuracy to centimeter and decimeter respectively. However, Reach GPS is fragile to the case of reflective buildings due to its single-frequency receiver. Therefore, it is impossible to create a differential GPS scenario by twin Reach RTK GPS to provide absolute heading angle referenced by global north.

Xsens GPS also outputs position which is processed by vehicle odoemtry in- sides Xsens IMU. Observed from the tests, its position estimates are smoother than Trimble GPS after filtering in Xsens but it has a considerable bias comapred to the ground truth.

4.4.2 Sensor Placement

Figure 4.7: Sensor Placement on RCV

1O - ZED camera

2O - Dual antenna of VBOX 3i

3O - antenna of Trimble or Reach GPS 4O - Xsens IMU

5O - Speed encoder 6O - Steering encoder

Figure 4.7 demonstrates the places where sensors are attached to the RCV. The antenna either of VBOX 3i or Reach GPS should be hung over the vehicle in order to receive the GPS signal better. Previously, Xsens IMU was placed at center of the RCV chassis, which means IMU was completely exposed to the strong magnetic field generated from RCV, such as motors, cables, and material. This can easily distort the magnetometer, resulting in incorrect measurements. Having been re-attached to the top and away from the motors in RCV, Xsens IMU performed much better due to declined magnetic interference from RCV. The ZED camera was fixed facing forward with a good sight to obtain as many visual features as possible to be used in visual odometry.

4.4.3 Software Platform

The software design is operated in the environment, ROS(Robot Operating System) on Ubuntu 16.04.5 LTS. The AEKF, EKF and UKF algorithm are programmed in C++ and python supported by ROS. The software framework is supported by the ROS package robot_localization. And the computing platform is a MSI G Series, Core i7 Laptop with GPU supported. The computer works as one of the nodes distributed in the Ethernet network and bridges the communication from dSpace.

The data communication in RCV is indicated in figure 4.8.

Figure 4.8: Data Communication in RCV

RCV real-time software structure basically can be divided into the lower and

upper layer. In the low layer, the computational unit, dSpace autobox, is responsible

for monitoring the state of RCV, including speed, steering, acceleration through

4.4. IMPLEMENTATION

CAN communication, and low-level motor controlling. The IMU and encoder data are collected in lower layer and packed to multiple messages broadcast to the upper layer through UDP protocol. Then, the functions of environmental perception and strategy decision, such as localization and path planning, are realized in upper layer where lidar, camera, and GPS are connected to ROS via Ethernet. Moreover, the communication between low and upper layer has limited delays and good data synchronization in practice, which is important to secure the frequency of each sensor message used for data fusion.

4.4.4 Coordinate Transform Coordinate System

Each sensor has its own frame with respect to the RCV body-fixed frame. The transformation between these frames is supported by tf ROS package. The relation between each sensor frame and body-fixed frame is demonstrated in Figure 4.9, which is identical to the frame map exported from rqt_tf_tree.

Figure 4.9: Rqt Frame Tree