Tracking of Ground Vehicles : Evaluation of Tracking Performance Using Different Sensors and Filtering Techniques

(1)

Master of Science Thesis in Electrical Engineering

Department of Electrical Engineering, Linköping University, 2018

Tracking of Ground Vehicles

Evaluation of Tracking Performance Using

Different Sensors and Filtering Techniques

Marcus Homelius

(2)

Master of Science Thesis in Electrical Engineering

Tracking of Ground Vehicles

Evaluation of Tracking Performance Using Different Sensors and Filtering Techniques

Marcus Homelius LiTH-ISY-EX--18/5123--SE Supervisor: Oskar Ljungqvist

isy, Linköpings universitet

Johan Holmqvist

Kapsch TrafficCom AB

Examiner: Isaac Skog

isy, Linköpings universitet

Division of Automatic Control Department of Electrical Engineering

(3)

Abstract

It is crucial to find a good balance between positioning accuracy and cost when developing navigation systems for ground vehicles. In open sky or even in a semi-urban environment, a single global navigation satellite system (GNSS) constella-tion performs sufficiently well. However, the posiconstella-tioning accuracy decreases dras-tically in urban environments. Because of the limitation in tracking performance for standalone GNSS, particularly in cities, many solutions are now moving to-ward integrated systems that combine complementary sensors. In this master thesis the improvement of tracking performance for a low-cost ground vehicle navigation system is evaluated when complementary sensors are added and dif-ferent filtering techniques are used.

How the GNSS aided inertial navigation system (INS) is used to track ground vehicles is explained in this thesis. This has shown to be a very e↵ective way of tracking a vehicle through GNSS outages. Measurements from an accelerom-eter and a gyroscope are used as inputs to inertial navigation equations. GNSS measurements are then used to correct the tracking solution and to estimate the biases in the inertial sensors. When velocity constraints on the vehicle’s motion in the y- and z-axis are included, the GNSS aided INS has shown very good per-formance, even during long GNSS outages.

Two versions of the Rauch-Tung-Striebel (RTS) smoother and a particle filter (PF) version of the GNSS aided INS have also been implemented and evaluated. The PF has shown to be computationally demanding in comparison with the other approaches and a real-time implementation on the considered embedded system is not doable. The RTS smoother has shown to give a smoother trajectory but a lot of extra information needs to be stored and the position accuracy is not significantly improved.

Moreover, map matching has been combined with GNSS measurements and es-timates from the GNSS aided INS. The Viterbi algorithm is used to output the the road segment identification numbers of the most likely path and then the es-timates are matched to the closest position of these roads. A suggested solution to acquire reliable tracking with high accuracy in all environments is to run the GNSS aided INS in real-time in the vehicle and simultaneously send the horizon-tal position coordinates to a back office where map information is kept and map matching is performed.

(4)

(5)

Acknowledgments

First, I would like to thank my examiner Isaac Skog and my supervisor Oskar Ljungqvist for all their help and guidance during this thesis work. They have helped out a lot with filtering choices and implementation solutions.

Second of all I would like to thank Johan Holmqvist and Marcus Bäckman at Kapsch TrafficCom AB in Jönköping for giving me the opportunity to perform this thesis work. I would also like to thank Stefan Josefsson and Mathias Breuss for their help with hardware related issues.

Finally, I also want to show my appreciation to my family for their endless sup-port and encouragement.

Jönköping, May 2018 Marcus Homelius

(6)

(7)

Notation

Acronyms

Acronym Meaning

EKF Extended Kalman filter

GNSS Global Navigation Satellite System GPS Global Positioning System

IMU Inertial Measuring Unit INS Inertial Navigation System

MEMS Micro-Electro-Mechanical Systems

OBU On-Board Unit

PF Particle filter

RMSE Root Mean Square Error

RTS Rauch-Tung-Striebel

UKF Unscented Kalman filter

WGS84 World Geodetic System 1984

(10)

(11)

1

Introduction

In an urban environment the position accuracy is decreased dramatically com-pared to open sky or a semi-urban environment when a single global navigation satellite system (GNSS) constellation is used for positioning. This is because fewer satellites are visible and the measurements can su↵er from multipath ef-fects in urban environments where taller buildings are present. In this master thesis the improvement of tracking performance will be evaluated when di↵er-ent sensors are added and di↵erdi↵er-ent filtering techniques are used.

1.1 Background

On-Board Units (OBUs) with a built-in satellite tracker are used for distance, point or time based tolling. The actual charging transaction can either be per-formed directly in the OBU or in a central system. In the latter case, GNSS po-sitions are first collected and transferred to a host system. This allows for very flexible tari↵ and road segment schemes since no large amount of map data or algorithms need to be downloaded to the OBU. Drivers do not always want to have the OBU in their vehicle. However, they are often forced by law. Therefore, it is important that the OBU can operate standalone in a vehicle for a long period of time. When chargeable services, for example road usage, are established the technology has to guarantee a certain service level with a minimum of reliability, accuracy and integrity [19].

A satellite tolling system normally contains 30,000 to 2,000,000 OBUs. This makes it crucial to find a good balance between positioning accuracy and OBU cost. In open sky or even in a semi-urban environment, a single GNSS constella-tion performs sufficiently well. In an urban environment however, the posiconstella-tion-

(12)

2 1 Introduction

ing accuracy decreases drastically. Because of the limitation in tracking perfor-mance for standalone GNSS, particularly in cities, many solutions are now mov-ing toward integrated systems that combine complementary sensors [11], [19]. Nowadays, a Micro-Electro-Mechanical System (MEMS) and odometer are com-monly used in inertial navigation systems. An odometer is used to measure the traveled distance. However, the use of external sensors limits the application ar-eas of these navigation systems and also incrar-eases the system design cost [11], [16].

1.2 Problem Formulation

In this master thesis the improvement of tracking performance for a low-cost ground vehicle navigation system will be evaluated when di↵erent sensors are added and di↵erent filtering techniques are used. The primary sensors to be used are GNSS and MEMS. Questions to be considered are

• How much is the accuracy increased when adding a sensor?

• In what environment does the added sensors contribute the most/least? • Which technique is best suited for an embedded system with limited

pro-cessor and memory capacity?

• Which technique is best suited for real-time positioning accuracy improve-ment and which is more suited for post-processing?

1.3 Literature

In [11] two di↵erent methods to improve the performance of an inertial naviga-tion system (INS) in dense urban areas based on low cost MEMS Inertial Mea-suring Units (IMUs) are evaluated. The first method exploits the behavior of a typical ground vehicle to derive di↵erent constraints on its movement. The other method uses the Rauch-Tung-Striebel (RTS) smoother [10] and is more suitable for post-processing or near real-time applications.

In [19] an approach is described to ensure high accuracy and reliability for future positioning systems. The approach includes new functionalities for map match-ing as well as the consideration of integrity conditions and confidence circles. Car positioning by map matching among other things have been studied in [13]. Since the performance of the particle filter degrades quickly when the state di-mension increases, a marginalized particle filter was used. In this way, a Kalman filter can be used to estimate the position derivatives and the particle filter is only applied to the part of the state vector that contains the position.

In [6] advantages and disadvantages with di↵erent filters such as extended Kalman filters (EKFs), unscented Kalman filters (UKFs) and particle filters (PFs) are dis-cussed. Nonlinear filters can provide better position accuracy than EKFs.

(13)

How-1.3 Literature 3 ever, the real-time computational complexity can often be a problem for nonlin-ear filters.

When di↵erent sensors in a system sample with di↵erent frequencies, the mea-surements originating from these sensors are available at di↵erent times. In [3] a specific particle filtering algorithm is presented that handles asynchronous mea-surements together with detection of Global Positioning System (GPS) failure. The algorithm outperforms an UKF algorithm with detection of sensor failures. In [2] simulations were run to compare a Kalman estimator and a particle es-timator with respect to robustness against di↵erent errors in the accelerometer measurements. It was shown that the estimates from the particle estimator were quite una↵ected by the specific force measurements biases. The estimates from the Kalman estimator were more a↵ected. However, the Kalman estimator is op-timal when no bias is present.

A GPS aided INS is designed in [20] and simulation results of the system are presented to show its superiority over a regular GPS system.

In [7] it is shown that with the use of constraints that govern a ground vehicle’s motion, the duration of time that an IMU can be relied upon as the sole naviga-tion tool can be significantly improved compared to convennaviga-tional techniques. In [17] an approach based on simplifying the full six-sensor MEMS IMU is pro-posed. Inertial sensors that are less important for navigation purposes are re-moved and replaced by pseudo signals that have constant values plus white noise. The results indicate, compared to the full IMU case, that the results are within expected levels and the obtained accuracy meets the requirements of several ground vehicles.

Enhancement of the integrity of a navigation system for ground vehicle applica-tions that uses a combination of GPS and IMU is focused on in [21]. An imple-mentation of fault detection for both low-frequency faults in the IMU caused by bias in sensor readings and high-frequency faults from the GPS receiver caused by multipath errors makes it possible to improve the integrity of the navigation system.

A standalone attitude and heading reference system algorithm which employs the IMU and magnetometer data in an averaging manner is introduced in [18]. The results indicate that the introduced system outperforms the traditional inte-gration scheme in di↵erent situations.

In [16] a combination of using the MEMS error coefficients derived from Allan variance analysis for Kalman filter tuning and providing additional measure-ments based on the knowledge of vehicle kinematic features is proposed. The results demonstrates that the proposed algorithms improves the accuracy of a MEMS/GNSS navigation system in unfavourable areas.

In [22] the superiority of the sampling importance resampling PF over the EKF is shown by running di↵erent simulations. Is is also mentioned that it is important

(14)

4 1 Introduction

to find a good balance between location precision and time consumed in calcula-tion when using the PF.

An EKF that integrates data from GPS and a low-cost dead reckoning system has been developed in [23] and it was shown to meet the required tracking perfor-mance of the device under development.

1.4 Outline

The EKF, the RTS smoother and the PF are described and discussed in Chapter 2. Chapter 3 contains information about the di↵erent coordinate systems that are used and transformations between them. Chapter 4 explains how the GNSS aided INS works. Map matching is described in Chapter 5. In Chapter 6, the di↵erent results are presented. Finally, the conclusions drawn from this thesis are presented in Chapter 7.

(15)

2

Nonlinear Observers

In nonlinear observers, measurements from di↵erent sensors and a motion model of the system are combined to provide reliable estimates of the system’s states. The uncertainty of the estimates are increased in the time update where a motion model of the system is used to provide a guess of the next states given the current states and inputs. The uncertainty is then decreased in the measurement update when information about some of the vehicle’s states are given from di↵erent sen-sors. An advantage with this approach is that the observer can estimate states that are not possible to directly measure with any of the available sensors.

In this chapter, two di↵erent filters and one smoother are presented, where the filters are operating in real-time. In a filter, only information from past measure-ments are used to estimate the current states of the system. A smoother operates di↵erently and uses both past and future measurements to estimate the current states of the vehicle. Advantages and disadvantages with the algorithms are also discussed and some theoretical details are presented.

2.1 Extended Kalman Filter

Models of the system and measurements are used in a Kalman filter in order to estimate the states. The EKF is a way of approaching the problem if there are nonlinear models. Consider the discrete-time nonlinear model with additive noise

x_k+1 = f (x_k,u_k) + v_k (2.1)

yk = h(xk,uk) + ek (2.2)

(16)

6 2 Nonlinear Observers

where (2.1) is the motion model and (2.2) is the measurement equation. The process noise v_k and the measurement noise e_k are modelled as Gaussian distri-butions v_k _{⇠ N(0, Q}_k) and e_k _{⇠ N(0, R}_k), respectively.

The algorithm for the EKF is obtained from [12] and is presented below in Algo-rithm 1. It is divided into two steps: a measurement update and a time update where Hk = h0x(ˆxk|k 1) and Fk = fx0(ˆxk|k,uk), i.e. the measurement equation and the motion model are linearized around the current state estimate. The noise covariances can be tuned to compensate for linearization errors. In the time up-date, a model of the system is used to update the states and the uncertainty is thus increased. Measurements from di↵erent sensors are then used in the mea-surement update to correct the estimates from the time update and therefore the uncertainty of the estimates is decreased.

In an INS, the IMU measurements are dead-reckoned to create a reference trajec-tory. Additional measurements from a GNSS are then used in the EKF to estimate the IMU sensor o↵sets and to avoid long-term drifts.

The initialization of the filter is fairly important. If the initial states x₀ are per-turbed from the actual states and the initial uncertainty P₀is low, the filter prob-ably will not converge.

One drawback of using an EKF is the assumption of the noise distribution. The filter assumes Gaussian noise and some problems may arise if it is not. Although, it is generally a good assumption to use a Gaussian distribution for the noise. Therefore, it might be seen as a limitation rather than a drawback.

The most time consuming step of the EKF is the Riccati recursion of P. From the matrix multiplication FP, the EKF is O(2n3

x) as a first order approximation for large n_x. Here, n_x is the dimension of the state vector x [13].

(17)

2.2 RTS Smoother 7

2.2 RTS Smoother

The RTS smoother is a fixed-interval smoothing method that runs a forward filter and a backward filter. The forward filter is the standard EKF and the results from both the time and measurement update, ˆx_k_|k, ˆxk+1|k,Pk|k,Pk+1|k, are stored.

The smoothed state estimates are then computed by a set of backward recursion equations that propagates the saved statistics from the EKF backward in time [11]. The backward recursion equations in the RTS smoother can be summarized as

Gk = Pk|kFk>Pk+11|k (2.4a)

ˆxk|n = ˆxk|k+ Gk( ˆxk+1|n ˆxk+1|k), k = N 1, ..., 0 (2.4b)

Pk|n = Pk|k + Gk(Pk+1|n Pk+1|k)G>k (2.4c) where G is the smoother gain matrix, F is the transition matrix, N is the final time step, ˆxk|n is the estimated smoothed state vector at time step k and ˆxk+1|n is

the predicted smoothed state vector at time step k + 1. Further, Pk|n and Pk+1|n are

the corresponding state error covariance matrices.

The RTS smoother can be directly applied to the estimates from the GNSS aided INS by simply switching ˆx to ˆx in (2.4b). This means that ˆxk+1|k does not need

to be stored because it will always be zero after the internal compensation that is described in Section 4.6. However, ˆx_k+1_|k have to be stored instead because it will be compensated with the smoothed estimates ˆx_k_|n.

An advantage with the RTS smoother is that it is quite simple to implement. A disadvantage however, is that all state and covariance matrices from the EKF have to be stored. This requires a lot of memory storage and it is also time consuming to store and retrieve the predicted and updated information.

2.2.1 Online Smoothing

Another disadvantage with the RTS smoother is that it can be considered as an o✏ine smoother and is mostly used for post-processing. A more suitable smoother for real-time applications could however easily be derived from the RTS smoother. The smoothing process is simply activated each time GNSS mea-surements are received instead of waiting until the end of the data set. In this version of the RTS smoother, the smoothing is performed from the current time to the previous measurement updating time using the latest updated and pre-dicted estimates that are stored in dynamic arrays [4].

2.3 Particle Filter

The PF estimates the posterior distribution p(xk|y1:k), where xkis the current state and y_1:k are the measurements {y1, y2, ..., yk} up to time k. The process noise and the measurement noise are arbitrary but assumed to be known. The bottleneck with the PF is that it is computationally intensive if many particles are used. A

(18)

8 2 Nonlinear Observers

general description for how to perform the steps in a PF can be found in [12] and is listed as Algorithm 2.

The PF is divided into four steps: a measurement update, an estimation step, a resampling step and a time update. In the measurement update, the weights of the particles are modified according to the likelihood function of the di↵erence between observed measurements from the sensors and the predicted measure-ments. In the estimation step, the estimated states are computed based on the particles and their weights. In the resampling step, N new samples of the state from the existing set of particles are taken. This is supposed to keep the most likely particles and throw away the rest. In the time update each particle sim-ulates a trajectory from one measurement time to the next by using a dynamic model of the system.

As a first order approximation for large n_x, the PF is O(N n2

x) from the matrix times vector multiplication Fx. Here, N is the number of particles and nx is the dimension of the state vector. This means that the PF is about a factor N/2nx computationally more demanding than the EKF [13]. Thus, the choice of N is a trade-o↵ between performance and time consumption.

To not make the PF too computationally demanding, a low-dimensional state vector is used in many applications. Often, a three-dimensional state-vector consisting of horizontal position and heading is used when running a PF. In a three-dimensional state-space, the PF performs quite well. If higher dimensions are used, the curse of dimensionality soon makes the particle representation too sparse to be a meaningful representation of the posterior distribution. Therefore, the marginalized particle filter is often used in more realistic applications with a high dimensional state-space. The PF is then used to solve the most difficult nonlinear parts of the system and an EKF is used on the remaining states [12]. The main reason for the PF’s wide usage is its simplicity of being implemented with any kind of model or noise [14]. That means that road networks for example could be incorporated within the PF framework.

(19)

2.3 Particle Filter 9

Algorithm 2 Particle filter

1. Initialization: Choose the number of particles, N, and initialize with xi

1 ⇠

p_x₀, i = 1, 2, ..., N , and w_1|0i = 1/N.

2. Measurement update: For i = 1, 2, ..., N, calculate the weights w_ki_|k = 1

c_kw

i

k|k 1p(yk|xik), (2.5a) where c_k is a normalization constant given by

c_k = N X

i=1

wi_k_{|k 1}p(y_k_|xi_k). 3. Estimation: The filtering density is

ˆp(x1:k|y1:k) = N X i=1 w_ki_|k (x_1:k xi_1:k), (2.5b) and ˆx1:k ⇡ N X i=1 w_ki_|kxi_1:k. (2.5c)

4. Resampling: Take N samples from the set {xi

1:k}Ni=1 where the probability of taking sample i is wi

k|k = 1/N.

5. Time update: Generate predictions according to the distribution

x_k+1i ⇠ q(xk+1|xik, yk+1), (2.5d) and adjust the weights according to

w_k+1i _|k = wi k|k p(xi_k+1|xi k) q(x_k+1i _|x_ki, y_k+1). (2.5e) 6. Repeat 2.

(20)

(21)

3

Coordinate Systems

The vehicle’s rotations in yaw, pitch and roll are represented by the angles , ✓ and , respectively. An illustration of the rotations is shown in Figure 3.1. The rotations are done in a predefined order according to the zyx-convention [7]. With the help of a rotation matrix, a vector can be rotated from one coordinate system into another. With the given rotation convention, the rotation matrix Rt_b, i.e. rotation from the body frame to the tangent frame, is given by [7]

Rt_b = 2 6666 6664 C✓C C S + S S✓C S S + C S✓C C✓S C C + S S✓S S C + C S✓S S_✓ S C_✓ C C_✓ 3 7777 7775, (3.1)

where C₍_·₎ and S₍_·₎ are shorthand notations for cos( · ) and sin( · ), respectively. The rotation matrix also has the property of being orthogonal, i.e. (Rt_b)> _{= (R}t

b) 1.

This also means that Rb_t = (Rt_b)>_{where R}b

t is the rotation matrix that represents

the rotation from the tangent frame to the body frame.

(22)

12 3 Coordinate Systems z''', z'' x''' x'' y''' y'' y'',y' x'' θ x' z'' z' x',x y' Φ y z' z

Figure 3.1: Illustration of transformation of Euler angles from the body frame to the tangent frame.

3.1 World Geodetic System 1984

The World Geodetic System 1984 (WGS84) [1] is the most common coordinate system used by GPS devices. It is an Earth-centered, Earth-fixed coordinate sys-tem with its origin in the Earth’s centre of mass. The WGS84 uses a reference ellipsoid to represent the Earth and it is defined by the parameters

1/f ⌘ 298.257223563, (3.2a) a⌘ 6378137.0 m, (3.2b) b = a(1 f ), (3.2c) e = pf (2 f ), (3.2d) RN( ) = q a 1 e2_sin2_{( )}, (3.2e)

where f is the flattening factor of the Earth, a and b are the major and semi-minor axes of the reference ellipsoid, respectively. Further, e is the eccentricity,

RN is the length of the normal from the surface of the ellipsoid to its intersection with the z-axis and is the latitude [8]. The coordinates given in latitude ( ), longitude ( ) and height (h) can then be transformed to rectangular coordinates according to 2 6666 664 x y z 3 7777 775 = 2 6666 664 (R_N + h) cos( ) cos( ) (R_N + h) cos( ) sin( ) (R_N(1 e2) + h) sin( ) 3 7777 775 . (3.3)

(23)

3.2 Transformations 13

3.2 Transformations

According to [8], the rotation matrix that transforms a vector from the body frame to the tangent frame is given by

Rt_b(q) = 2 6666 6664 q2₁+ q2₂ q2₃ q2₄ 2(q2q3 q1q4) 2(q1q3+ q2q4) 2(q₂q₃+ q₁q₄) q2₁ q2₂+ q₃2 q₄2 2(q₃q₄ q₁q₂) 2(q₂q₄ q₁q₃) 2(q₁q₂+ q₃q₄) q₁2 q₂2 q₃2+ q2₄ 3 7777 7775 (3.4) where q is the quaternion that represents the rotational transformation from the body frame to the tangent frame.

The transformation from the directional cosine matrix to quaternions can be com-puted from (3.4) and is given by [8]

q = 2 6666 6666 6666 6666 6664 0.5q1 + Rt b[1, 1] + Rtb[2, 2] + Rtb[3, 3] Rt b[3,2] Rtb[2,3] 4q1 Rt_b[1,3] Rt b[3,1] 4q1 Rt b[2,1] Rtb[1,2] 4q1 3 7777 7777 7777 7777 7775 (3.5) where Rt

b is the directional cosine matrix and q1 is the first element in q.

The Euler angles can also be calculated from the directional cosine matrix and the transformation is given by [8]

2 6666 664✓ 3 7777 775 = 2 6666 6666 6666 6666 64 arctan✓Rtb[3,2] Rt b[3,3] ◆ arctan p Rtb[3,1] 1 (Rt b[3,1])2 ! arctan✓Rtb[2,1] Rt b[1,1] ◆ 3 7777 7777 7777 7777 75 . (3.6)

(24)

(25)

4

GNSS Aided INS

In an inertial navigation system (INS), measurements from an accelerometer and a gyroscope are used to track the position, velocity and orientation of a vehicle relative to the initial values of these states. This system is used as the backbone of a GNSS aided INS and it continuously provides a six degrees of freedom tracking solution. When GNSS measurements are received, the di↵erence between the position measurement and the estimated position is used as input to a filter. In this case both an EKF and a PF are used as filter. The filter estimates the errors in the tracking solution and the errors in the IMU sensors. The outputs from the filter are then used to correct the tracking solution and to calibrate the IMU sensors.

4.1 Notations

The notations used to describe a ground vehicle with six degrees of freedom are

p, hx y zi>, v, hu v wi>,

q, hq₀ q₁ q₂ q₃i>.

(4.1)

The vectors p, v and q denote the position, velocity and orientation (quaternion representation) in the navigation frame, respectively. The vector q is constrained to the unit sphere, i.e., q>_{q = 1 at all time instances. The quaternion}

representa-tion is used instead of Euler angles since they are more numerically stable [12].

(26)

16 4 GNSS Aided INS

The tracking state vector and the input vector that are used are defined as

xk , 2 6666 664 pk v_k qk 3 7777 775 and uk , " s_k !k # , (4.2)

respectively, where sk[m/s2] and !k[rad/s] denote the specific forces and angular velocities of the navigation system in three dimensions at time instant k. These measurements are received from an IMU.

4.2 Inertial Navigation Equations

The discrete time version of the inertial navigation equations are [5]

pk = pk 1+ Tsvk 1+ T 2 s 2 (Rnb(qk 1)sk g), (4.3a) vk = vk 1+ Ts(Rnb(qk 1)sk g), (4.3b) q_k = (cos(0.5T_s_k!_k_k)I₄+ 1 k!kksin(0.5Tsk!kk)⌦k)qk 1, (4.3c) where ⌦k = 2 6666 6666 6664 0 [!_k]_z [!_k]_y [!_k]_x [!k]z 0 [!k]x [!k]y [!k]y [!k]x 0 [!k]z [!k]x [!k]y [!k]z 0 3 7777 7777 7775, (4.4)

Rn_b is the directional cosine matrix in (3.4) that rotates a vector from the body coordinate frame (b-frame) to the navigation coordinate frame (n-frame), T_s is the sampling time and g is the gravity vector expressed in the n-frame.

Represent (4.3) as x_k = f (x_{k 1},u_k).

4.3 Measurement Equations

This section presents the measurement equations for each sensor that will be utilized within the GNSS aided INS.

4.3.1 IMU Measurements

The IMU measurements are described by [8]

˜uk = uk uk + w1,k (4.5)

where uk is a slowly varying measurement bias and w1,k is the measurement

noise which is assumed to be additive white noise with covariance matrix Q₁. The measurement bias is modelled as a random walk process [8]

(27)

4.3 Measurement Equations 17 where w_2,k is assumed to be additive white noise with covariance matrix Q₂. The measurement noise and the random walk process noise are assumed to be uncor-related.

Define the di↵erence between the true states xk and the estimated states ˆxk as

x_k _, 2 6666 664 pk vk ✏_k 3 7777 775 (4.7)

where pkis the position error pk = pk ˆpk, vkis the velocity error vk = vk ˆvk and ✏_k is the attitude error defined as the small Euler angle sequence that rotates the attitude vector ˆq_kinto q_k. This can be written as q_k = ( ˆq_k,✏k), where is the implicit function defined as [8]

( ˆq, ✏), {q 2 S3 _{| R}n

b(q) = (I3 [✏]⇥)Rnb( ˆq)}. (4.8) Here, [a]_⇥ is the skew-symmetric matrix representation of a, for which [a]_⇥b = a⇥ b. That is, [a]_⇥ = 2 6666 664 0 a₃ a₂ a3 0 a1 a₂ a₁ 0 3 7777 775 (4.9) for a = [a₁, a₂, a₃]>_.

With the error vector in (4.7), the error propagation in (4.3) when fed with the measurement vector ˜u_k can be described by the linear parameter varying system

zk = F(xk,uk)zk 1+ Gk(xk)wk (4.10) for small error perturbations where

zk , " xk uk # and wk , " w1,k w2,k # . (4.11)

The state transition matrix is defined as

F(x_k,u_k) = 2 6666 6666 6666 664 I3 TsI3 03,3 03,3 03,3 0_3,3 I3 Ts[Rn_b(qk)sk]⇥ TsRn_b(qk) 03,3 0_3,3 0_3,3 I₃ 0_3,3 TsRn_b(qk) 0_3,3 0_3,3 0_3,3 I₃ 0_3,3 0_3,3 0_3,3 0_3,3 0_3,3 I3 3 7777 7777 7777 775 (4.12)

and the noise gain matrix is defined as

Gk(xk) = 2 6666 6666 6666 664 0_3,3 0_3,3 0_3,3 0_3,3 T_sRn_b(q_k) 0_3,3 0_3,3 0_3,3 0_3,3 TsRn_b(qk) 03,3 03,3 0_3,3 0_3,3 I₃ 0_3,3 0_3,3 0_3,3 0_3,3 I₃ 3 7777 7777 7777 775 . (4.13)

(28)

18 4 GNSS Aided INS

4.3.2 GNSS Measurements

The measurements from a GNSS can either be the position or the relative dis-tances between the receiver and the transmitters. If the position is given directly, the measurements from the GNSS-receiver are described by

˜pGPS

k = pk+ e(1)_k (4.14)

where e(1)_k is additive white noise with covariance matrix R(1). The observation equation for the INS error propagation state space model can then be written as

y(1)_k , ˜pGPS_k ˆpk = H(1)zk+ e(1)_k , H(1) , h I3 03,12 i . (4.15)

4.4 Motion Constraints

The typical motion dynamics of a ground vehicle can be used to define a set of constraints that can be used within the filter. If the driving conditions would be ideal, a wheeled vehicle would not experience any side slip and no velocity in the direction normal to the road surface. In the vehicle’s coordinate frame (p-frame), the velocity in the y- and z-axis should therefore be zero [7]. This can be written as 0_2,1 = ARpnv, A = " 0 1 0 0 0 1 # , (4.16)

where Rpn is the rotation matrix from the navigation frame (n-frame) to the ve-hicle’s coordinate frame (p-frame). Since there are no ideal driving conditions in reality, these constraints have to be relaxed. If the error e(2) _{is modelled as}

Gaussian white noise with covariance R(2)_{the pseudo-observation can be written}

as y_k(2) , 0_2,1 ARpnˆvk = H(2)zk+ e(2), H(2) , h 0_2,3 ARpn 02,9 i . (4.17)

In total we now have

yk = 2 6666 4y (1) k y(2)_k 3 7777 5 = Hzk + e, H , " I₃ 0_3,3 0_3,9 0_2,3 ARpn 02,9 # . (4.18)

This can also be written as

y_k = " ˜pGPS k 0_2,1 # " I₃ 0_3,3 0_2,3 ARpn # " ˆpk ˆvk # . (4.19)

(29)

4.5 Speed Aiding 19

4.5 Speed Aiding

Information about the vehicle’s speed can also be used to improve the tracking solution if it is available. This information can come from either GNSS measure-ments or a speedometer and the observation equation can be written as

y_k(3) , ˜vk BRpnˆvk = H(3)zk + e(3), H(3) , h

0_1,3 BRpn 01,9

i

(4.20) where ˜v_k is the speed measurement, e(3) _{is the measurement noise modelled as}

Gaussian white noise with covariance R(3) and B, h1 0 0i. If (4.20) is stacked above (4.17) in (4.18), the total measurement equation can be rewritten as

yk = 2 6666 6666 64 y_k(1) y_k(3) y_k(2) 3 7777 7777 75= Hzk+ e, H , " I3 03,3 03,9 0_3,3 Rpn 03,9 # (4.21) or yk = 2 6666 664 ˜pGPS_k ˜vk 0_2,1 3 7777 775 " I3 03,3 0_3,3 Rpn # " ˆpk ˆvk # . (4.22)

Note that if the observations are not available at the same time, the corresponding rows in (4.22) can simply be removed during each asynchronous measurement update.

4.6 Algorithm Description

The Kalman filter based algorithm for the GNSS aided INS is described in Algo-rithm 3 and a conceptional sketch can also be seen in Figure 4.1. It is important to highlight that the entire estimated state vector ˆzkis not propagated in the EKF time update, instead only ˆxk and ˆuk are propagated. This is because the esti-mated tracking state perturbations ˆxk are fed back into the INS and are used to correct the tracking solution. After that the perturbations are considered to be zero.

The GNSS aided INS based on the PF in Chapter 2.3 is described in Algorithm 4. Only the x- and y-position from the state vector are used as states in the PF. The rest of the state vector is updated according to the process in Algorithm 3, i.e. the same way as before.

(30)

20 4 GNSS Aided INS

Algorithm 3 GNSS aided INS (EKF)

k = 1

ˆx = Process{Initial tracking states}

ˆu = Process{Initial sensor bias estimates}

P = Process{Initial Kalman filter state covariance matrix}

loop

Use the current sensor bias estimates to calibrate the sensor measurements. ˆu = ˜uk + ˆu

Time update of the tracking states and the state covariances. ˆx = f ( ˆx, û) P = F( ˆx, û)PF>( ˆx, û) + G( ˆx) " Q1 03,3 0_3,3 Q2 # G>( ˆx)

Measurement update if measurements from GNSS are available. if GNSS measurement available then

Compute the measurement vector.

yk = 2 6666 664 ˜pGPS_k ˜vk 0_2,1 3 7777 775 " I3 03,3 0_3,3 Rpn # " ˆpk ˆvk #

Compute the Kalman filter gain.

K = PH>(HPH>_{+ R)} 1

Update the perturbation state estimates and the state covariances. " ˆx ˆu # = " 0_9,1 ˆu # + K(y 0_6,1) P = (I₁₅ KH)P

Use the current perturbations estimates to correct the tracking states. ˆp = ˆp + ˆp ˆv = ˆv + ˆv ˆq = ( ˆq, ˆ✏) end if k = k + 1 end loop

(31)

4.6 Algorithm Description 21

Algorithm 4 GNSS aided INS (PF)

k = 1

Choose the number of particles, N_p, and initialize with xi

1⇠ px0, i = 1, 2, ..., Np, and wi

1|0 = 1/Np. loop

Time update: Generate prediction of each particle xi

k|k 1and add white

noise vi_.

xi_k_{|k 1} = f (xi

k 1, ˆxEKFk 1, ˆuk 1) + vi

if GNSS measurement available then

Measurement prediction for each particle

y_k_{|k 1} = xi k|k 1

Measurement update of each particle.

Resample: Take N_p samples from the set {xi

1:k}Ni=1 where the probability of taking sample i is wi

k|k = 1/Np. end if end if Estimation: ˆx_1:kPF ⇡ PNp i=1wki|kxi1:k k = k + 1 end loop

(32)

22 4 GNSS Aided INS

(33)

5

Map Matching

Maps of road networks can be used as constraints for where the vehicle can be located. This can enhance the positioning process. Polylines that describe the geometry of the centre line are often used as representation of the roads [19]. A map matching algorithm takes the absolute position of the vehicle as input and outputs the segment identification number and the relative position of the vehicle along this segment.

In map matching for car positioning, h(pk) is used to denote the distance from the estimated position to the nearest road, i.e.

yk = h(pk) + ek (5.1)

should be zero, where ek is modelled as additive white noise [13].

The digital map represenations that have been used in this thesis have been ex-ported from OpenStreetMap1_{. In Figure 5.1, a map of an urban area in Jönköping}

can be seen compared to a digital representation of the map.

5.1 Viterbi Algorithm

The Viterbi algorithm [9] can be used to estimate the sequence of road links that a vehicle moves on based on observed GPS positions. It is a recursive program-ming algorithm that finds the most likely sequence of hidden states. The inputs to the Viterbi algorithm are the state space S = {s1, s2, . . . , sM}, the array of initial probabilities ⇧ = (⇡₁, ⇡₂, . . . , ⇡M) where ⇡i is the probability that x1 = si, the observations Y = (y₁, y₂, . . . , yN), the transition matrix A with dimensions M ⇥ M

1_{https://www.openstreetmap.org}

(34)

24 5 Map Matching -100 0 100 y - East [m] -300 -250 -200 -150 -100 -50 0 50 100 150 x - North [m] Map -100 0 100 y - East [m] -300 -250 -200 -150 -100 -50 0 50 100 150 x - North [m] Digital Map Road network Other

Figure 5.1: A map and a digital representation of the map.

where Aij represents the probability of transition from state si to state sj and the emission matrix B with dimensions M ⇥ N where Bij represents the proba-bility of observing y_j from state s_i [15]. The algorithm outputs the hidden state sequence X = (x₁, x₂, . . . , x_N) with the highest probability. The basic principles of the Viterbi algorithm can be seen in Algorithm 5.

First the two tables are initialized by computing the most probable state based on the initial probabilities and the emission matrix for the first observation. This is done for all states and the probabilities are stored in the first table. The last states that lead to the current states are all 0 because these are the first states in the sequences. Then for each observation the probability of the most likely path so far for each individual state is computed and stored. The state that lead to the new most probable state is also stored. When this is done for all observations, the most likely path can easily be found and also the last state of that path. From that state the path is now backtracked to find the whole sequence.

When each observation has been connected to a segment identification number by the Viterbi algorithm, the observations have to be matched to the closest point on the road with the corresponding identification number. The matched positions

(35)

5.1 Viterbi Algorithm 25 Algorithm 5 Viterbi algorithm

Let µ₁and µ₂be two tables with dimensions M ⇥ N. In µ1[i, j] the probability

of the most likely path so far ˆX = ( ˆx1, . . . , ˆxj) where ˆxj = si is stored and in

µ2[i, j], ˆxj 1 of that path is stored. for each state i 2 {1, 2, . . . , M} do

µ₁[i, 1] = ⇡_iB_i1 µ2[i, 1] = 0

end for

for each observation j = 2, 3, . . . , N do for each state i 2 {1, 2, . . . , M} do

µ₁[i, j] = max k µ1[k, j 1]AkiBij µ₂[i, j] = argmax k µ₁[k, j 1]A_kiB_ij end for end for zN = argmax k µ1[k, N] xN = sz_N for j = N, N 1, . . . , 2 do z_{j 1} = µ₂[z_j, j] xj 1 = szj 1 end for

p_Matched are computed in the following way for each measurement

z₁= p_GNSS p₁ (5.2a) z₂= p₂ p₁ (5.2b) u = z > 1z2 z₂>z₂ (5.2c) p_Matched = p₁+ uz₂ (5.2d) where p₁and p₂ are the Cartesian coordinates of two adjacent nodes of the rode with the corresponding identification number. Further, p_GNSS is the GNSS mea-surement given in Cartesian coordinates and u is the percentage of where along the road from p₁ to p₂, the matched position is located. This is done for all ad-jacent nodes in the selected road segment and the position that minimizes the distance from the measurement to the road is selected as the matched position.

(36)

(37)

6

Results

The results from this thesis are presented in this chapter. Data from two di↵er-ent environmdi↵er-ents have been recorded: open sky and urban canyon. These data sets have then been processed in Matlab with the di↵erent filtering techniques described in Chapter 2 and the GNSS aided INS described in Chapter 4. The computation time required for these algorithms have been studied and an Allan variance analysis has also been performed on the IMU measurements.

6.1 Allan Variance

Data from a low-cost MEMS-IMU was recorded for 3 hours while being static on a flat surface. The sensor includes a axis accelerometer and a triple-axis gyroscope. Sensor outputs were sampled with a frequency of 25 Hz. The data was then processed in Matlab according to the Allan variance method (see Appendix A). A log-log plot for the Allan deviation versus the averaging time is shown in Figure 6.1 for the accelerometer measurements and in Figure 6.2 for the gyroscope measurements. The sensor parameters that were derived from the Allan variance method are listed in Table 6.1 for the accelerometer and in Table 6.2 for the gyroscope.

From the Allan variance analysis results in Figures 6.1 and 6.2, it is clear that the random walk is the dominant error term for the shorter averaging times. The parameter values for the random walk in Tables 6.1 and 6.2 were used to initialize the bias variances in the GNSS aided INS. There is no slope of -1 in the plot of the Allan deviation. Thus the quantization errors are much less than the other errors and can be ignored.

(38)

28 6 Results 10-5 10-4 10-3 10-2 10-1 100 101 10-1 100 101 102

Figure 6.1: The Allan deviation of the accelerometer measurements. Table 6.1: Allan variance analysis results for the accelerometer.

Axis Random Walk [m/s/phr] Bias In-stability [m/s/hr] Rate Ran-dom Walk [m/s/hr/phr] Rate Ramp [m/s/hr/hr] x 0.0725 1.61 0.780 0.628 y 0.0523 5.22 NaN NaN z 0.0683 1.18 4.10 1.95

Table 6.2: Allan variance analysis results for the gyroscope.

Axis Random Walk [deg/phr] Bias In-stability [deg/hr] Rate Ran-dom Walk [deg/hr/phr] Rate Ramp [deg/hr/hr] x 0.266 10.6 31.3 43.1 y 0.246 8.60 30.0 27.6 z 0.233 4.71 3.98 2.76

(39)

6.2 GNSS and Map Matching 29

10-5 10-4 10-3 10-2 10-1 100 101 100

101 102

Figure 6.2: The Allan deviation of the gyroscope measurements.

6.2 GNSS and Map Matching

Data was recorded in an open sky environment for about 4 minutes and the data set that was recorded in an urban canyon was about 5 minutes long. In both cases the vehicle was placed stationary on a flat road when the data started recording. In this way the first IMU measurements could easily be used to initialize the filters.

In Table 6.3, it is shown how many satellites that were available for what per-centage of time in the two di↵erent environments where data was recorded. The coverage was good in both environments and no outage occurred. The reason for that is probably that Jönköping does not have enough tall buildings. But to show what a↵ect the di↵erent algorithms would have on the tracking solution if no GNSS information was available, an 80 seconds outage has been simulated for both data sets for the tests in Section 6.3 and Section 6.4.

Figure 6.3 shows the result from GNSS measurements combined with map match-ing in an open sky environment and Figure 6.4 shows the same result in an urban canyon. Since the GNSS coverage was good, the GNSS measurements are suffi-cient to track the vehicle. Interpolated GNSS measurements have therefore been used to represent the true trajectory in the open sky environment. However, the

(40)

30 6 Results

Table 6.3: Availability of satellites in two di↵erent environments.

Environment 5 6 7 8 9 10

Open sky 100 % 99.6 % 98.3% 98.3 % 91.3 % 0 %

Urban canyon 100 % 90.0 % 54.2 % 34.2 % 0 % 0 %

measurements in Figure 6.4 seem to su↵er from some multipath e↵ects which is the result of the GNSS signals reflecting o↵ of some taller buildings in the urban canyon. This is solved by the map matching algorithm that figures out the most likely path and matches the positions from the GNSS measurements to the road network. If outage was to be simulated here, it would obviously not be possible to track the vehicle since only the GNSS measurements are used to locate the vehicle and no dead reckoning is used.

-700 -600 -500 -400 -300 -200 -100 0 100 200 y - East [m] -300 -250 -200 -150 -100 -50 0 50 100 x - North [m]

GNSS and Map Matching

Map matched positions GNSS measurements

Figure 6.3: GNSS measurements combined with map matching in an open sky environment.

(41)

6.3 GNSS Aided INS 31 -500 -400 -300 -200 -100 0 100 200 y - East [m] -1000 -800 -600 -400 -200 0 x - North [m]

GNSS and Map Matching

Map matched positions GNSS measurements

Figure 6.4: GNSS measurements combined with map matching in an urban canyon.

6.3 GNSS Aided INS

Since the GNSS coverage was good and at least 5 satellites were available at all times in both environments, GNSS outage was simulated for the last 80 seconds to see what kind of a↵ect the GNSS aided INS has. The result can be seen in Fig-ure 6.5 for the open sky environment and corresponds to test 5 in Table 6.4. The estimated trajectory is similar to the true trajectory but the di↵erence in position is significant. Test 7 in Table 6.4 where speed aiding is used is presented in Fig-ure 6.6. Here, the estimated trajectory looks very similar compared to the true trajectory. However, the information about the vehicle’s speed comes from the GNSS measurements and is therefore not realistic to use during GNSS outage. Although, it could correspond to a case where a speedometer is available and could measure the vehicle’s speed instead. In Figure 6.7, the result can be seen when no information about the vehicle’s speed was available but non-holonomic constraints were used. This corresponds to test 6 in Table 6.4. This is a pseudo-observation and could therefore be used even during GNSS outage and the esti-mated trajectory is almost identical to the true trajectory. In Figures 6.9 and 6.10, the estimated biases in the accelerometer and gyroscope measurements are pre-sented from the estimated trajectory in Figure 6.7 and it can be seen that these

(42)

32 6 Results

values have converged. That is the reason for why it is possible to track the ve-hicle during outage for a limited time period. The result of test 6 in Table 6.5 where GNSS outage is simulated and non-holonomic constraints is used in an ur-ban canyon is shown in Figure 6.8. Here, it is shown again that the vehicle can be tracked quite well during an 80 seconds outage compared to the interpolated GNSS measurements. 0 100 200 300 400 500 600 700 800 900 East [m] -400 -300 -200 -100 0 100 200 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory Start point

Outage start

Figure 6.5: GNSS aided INS without speed aiding or non-holonomic con-straints in an open sky environment.

(43)

6.3 GNSS Aided INS 33 0 100 200 300 400 500 600 700 800 900 East [m] -400 -300 -200 -100 0 100 200 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory Start point

Outage start

Figure 6.6: GNSS aided INS with speed aiding in an open sky environment.

0 100 200 300 400 500 600 700 800 900 East [m] -400 -300 -200 -100 0 100 200 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory Start point

Outage start

Figure 6.7: GNSS aided INS with non-holonomic constraints and without speed aiding in an open sky environment.

(44)

34 6 Results -400 -200 0 200 400 600 East [m] 0 100 200 300 400 500 600 700 800 900 1000 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory Start point

Outage start

Figure 6.8: GNSS aided INS with non-holonomic constraints and without speed aiding in an urban canyon environment.

0 50 100 150 200 250 -0.4 -0.2 0 0.2 X-axis bias [m/s 2 ]

Accelerometer bias estimate versus time

GNSS aided INS estimate 3 bound 0 50 100 150 200 250 -0.2 0 0.2 0.4 Y-axis bias [m/s

2 ] GNSS aided INS estimate 3 bound 0 50 100 150 200 250 Time [s] -0.2 -0.1 0 0.1 0.2 Z-axis bias [m/s 2 ]

GNSS aided INS estimate 3 bound

Figure 6.9: The estimated bias in the accelerometer measurements from the GNSS aided INS.

(45)

6.3 GNSS Aided INS 35 0 50 100 150 200 250 -0.4 -0.2 0 0.2 0.4

X-axis bias [deg/s]

Gyroscope bias estimate versus time

GNSS aided INS estimate 3 bound 0 50 100 150 200 250 -0.4 -0.2 0 0.2 0.4

Y-axis bias [deg/s]

GNSS aided INS estimate 3 bound 0 50 100 150 200 250 Time [s] -0.4 -0.2 0 0.2 0.4

Z-axis bias [deg/s]

GNSS aided INS estimate 3 bound

Figure 6.10: The estimated bias in the gyroscope measurements from the GNSS aided INS.

The e↵ect of what information that is available for the GNSS aided INS is sum-marized in Tables 6.4 and 6.5. The second column shows whether an 80 second GNSS outage was simulated. This means that no GNSS measurements are avail-able for the last 80 seconds and the algorithm has to rely solely on the IMU mea-surements to track the vehicle during that time period. In the third column it is shown if the non-holonomic motion constraints in (4.17) are used in the filter. In the fourth column it is shown if the information about the vehicle’s speed from the GNSS measurements in (4.20) are used. Since this information comes from the GNSS measurements, tests 7 and 8 become unrealistic in this case because of the GNSS outage. It could however correspond to a case where a speedometer is available and could measure the vehicle’s speed instead. This would obviously be possible at GNSS outage and it could even give information more often since it could sample faster than 1 Hz. Lastly, the root mean square error (RMSE) of the di↵erent tests are shown in the fifth column. This is the position error in the horizontal plane where interpolated GNSS data has been used as reference in the open sky environment. In the urban canyon environment, the trajectory in Figure 6.4 where GNSS data has been combined with map matching has been used as the true trajectory since the GNSS measurements su↵ered from multipath e↵ects.

(46)

36 6 Results

Table 6.4: Results for the GNSS aided INS with di↵erent settings in an open sky environment.

Test GNSS outage

(80 s)

Non-holonomic constraints

Speed aiding RMSE [m]

1 o↵ o↵ o↵ 1.256

2 o↵ on o↵ 1.258 3 o↵ o↵ on 1.254 4 o↵ on on 1.257 5 on o↵ o↵ 51.15 6 on on o↵ 2.648 7 on o↵ on 6.518 8 on on on 2.520

Table 6.5: Results for the GNSS aided INS with di↵erent settings in an urban canyon environment.

Test GNSS outage

(80 s) Non-holonomicconstraints Speed aiding RMSE [m]

1 o↵ o↵ o↵ 8.606

2 o↵ on o↵ 8.523 3 o↵ o↵ on 8.604 4 o↵ on on 8.521 5 on o↵ o↵ 88.89 6 on on o↵ 16.90 7 on o↵ on 26.07 8 on on on 16.92

It can be seen that the di↵erent settings in the GNSS aided INS has very little a↵ect when no outage is simulated. This is because the algorithm then trusts those measurements and they are used as the main component when tracking the vehicle. That is why the errors are greater in the urban canyon where the GNSS measurements su↵ered from multipath e↵ects. It can be seen in test 1 in Table 6.5 that the RMSE is already 8.606 m when just the GNSS measurements are used to estimate the trajectory. When there is an outage it is clear that the performance is improved drastically with the non-holonomic constraints and the speed aiding. The non-holonomic constraints give the most useful information to the algorithm. The reason for why the speed aiding seems to help the algorithm more in the open sky environment is probably because of the multipath e↵ects that occur in the urban canyon. The multipath e↵ects can disturb the velocity measurements.

The results from the o✏ine version and the online version of the RTS smoother, both described in Section 2.2, can be seen in Figure 6.11 for the open sky envi-ronment and in Figure 6.13 for the urban canyon envienvi-ronment. Here, the case

(47)

6.3 GNSS Aided INS 37 with the 80 seconds GNSS outage has been tested for both environments. It can be seen that that o✏ine version gives a smoother trajectory during the outage whereas the online version coincides with the GNSS aided INS estimates. This is because the smoothing process is not activated in the online version when no GNSS measurements are received. In Figure 6.12, a zoomed-in version of Figure 6.11 is shown. Here, the main advantage of the o✏ine RTS smoother is shown. It removes the big skips that occurs in the solution from the GNSS aided INS whenever a GNSS measurement is received. This still happens for the online RTS smoother because it only uses data from the previous to the current measure-ment update in the smoothing equations instead of the whole data set. At least the skips become smaller with the online RTS smoother. The results from the smoothing algorithms can also be seen in Table 6.6 compared to the GNSS aided INS. 0 100 200 300 400 500 600 700 800 900 East [m] -400 -300 -200 -100 0 100 200 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory RTS smoother (offline) RTS smoother (online) Start point

Outage start

Figure 6.11: The o✏ine and online versions of the RTS smoother compared to the GNSS aided INS.

(48)

38 6 Results 100 120 140 160 180 200 East [m] -50 -40 -30 -20 -10 0 10 20 30 40 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory RTS smoother (offline) RTS smoother (online) Start point

Outage start

Figure 6.12: A zoomed-in version of Figure 6.11 to show the e↵ect of smooth-ing. -400 -200 0 200 400 600 East [m] 0 100 200 300 400 500 600 700 800 900 1000 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory RTS smoother (offline) RTS smoother (online) Start point

Outage start

Figure 6.13: The o✏ine and online versions of the RTS smoother compared to the GNSS aided INS.

(49)

6.3 GNSS Aided INS 39 The results from the PF version of the GNSS aided INS described in Algorithm 4 where the PF is run on only the x- and y-coordinates are presented in Figure 6.14 and Figure 6.15. These Figures represent test 6 in Tables 6.4 and 6.5, respectively. Here, 500 particles were used in the filters. It can be seen that the PF has a hard time tracking the vehicle at GNSS outage since it does not get any information that can help with improving the weights of the particles. The results from the PF are also presented in Table 6.6 together with the other algorithms. The RTS smoothers and the PF performs better than the standard GNSS aided INS without any outage. The PF is worst when there is an outage because of reasons discussed above and the RTS smoothers give results that are similar to the results from the GNSS aided INS. That is because the GNSS aided INS is ran as the forward filter in both smoothers. 0 100 200 300 400 500 600 700 800 900 East [m] -400 -300 -200 -100 0 100 200 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory (EKF) GNSS aided INS trajectory (PF) Start point

Outage start

Figure 6.14: The PF version compared to the EKF version of the GNSS aided INS in an open sky environment.

(50)

40 6 Results -400 -200 0 200 400 600 East [m] 0 100 200 300 400 500 600 700 800 900 1000 North [m] Trajectory GNSS position estimate GNSS aided INS trajectory (EKF) GNSS aided INS trajectory (PF) Start point

Outage start

Figure 6.15: The PF version compared to the EKF version of the GNSS aided INS in an urban canyon environment.

Table 6.6: The RMSE of di↵erent algorithms in two di↵erent environments.

Environment GNSS aided

INS (EKF) RTSsmoother (o✏ine) RTS smoother (online) GNSS aided INS (PF) Open sky 1.26 m 0.699 m 0.842 m 1.02 m Open sky (80 s outage) 2.65 m 2.85 m 2.49 m 11.3 m Urban canyon 8.52 m 8.46 m 8.47 m 8.49 m Urban canyon (80 s outage) 16.9 m 19.0 m 16.9 m 18.9 m

The run times of the di↵erent algorithms are listed in Table 6.7. Here, it can be seen that the PF requires a lot of computational power. That the EKF version of the GNSS aided INS has the shortest run time is expected since all the other algo-rithms are based on this one. They do the same things plus some other equations.

(51)

6.4 GNSS Aided INS and Map Matching 41 Table 6.7: The average run times of di↵erent algorithms on data from two di↵erent environments. Environment GNSS aided INS (EKF) RTS smoother (o✏ine) RTS smoother (online) GNSS aided INS (PF) Open sky 1.96 s 3.06 s 2.99 s 82.1 s Urban canyon 2.74 s 4.39 s 4.67 s 118 s

6.4 GNSS Aided INS and Map Matching

The GNSS aided INS has shown to be able to track the trajectory of a vehicle during GNSS outage quite well. If these estimates are combined with map match-ing, the results in Figure 6.16 and Figure 6.17 are obtained. Here, the Viterbi algorithm has used the estimates received from the GNSS aided INS with an 80 seconds GNSS outage as input. The algorithm outputs the road segment identi-fication numbers of the most likely path. Then the estimates have been matched to the selected roads. As can be seen, the map matching procedure removes the small errors that are left from the GNSS aided INS by matching the estimates to the roads. It also solves the problem that occurs because of the multipath ef-fects in the urban canyon. This way of combining the GNSS aided INS with map matching has shown to be able to track the trajectory of a ground vehicle during a long time period of GNSS outage without any problems.

Figure 6.16: Estimates from the GNSS aided INS combined with map match-ing in an open sky environment.

(52)

42 6 Results

Figure 6.17: Estimates from the GNSS aided INS combined with map match-ing in an urban canyon.

(53)

7

Conclusions

The use of additional sensors in di↵erent environments with application to track-ing of ground vehicles have been studied in this thesis. Di↵erent algorithms and filtering techniques have been evaluated to see what would be the best solution for an embedded system with limited processor and memory capacity and what would be best suited for real-time positioning. In this chapter, the conclusions of this thesis are presented and future work is also discussed.

7.1 Conclusions

The GNSS aided INS described in Chapter 4 and summarized in Algorithm 3 has shown to be a very e↵ective way of tracking a ground vehicle. This is the technique that is best suited for an embedded system with limited processor and memory capacity for real-time positioning improvement. It has shown to be able to track a vehicle for a limited time during GNSS outage with a relative small RMSE with aid of non-holonomic motion constraints. Another advantage with the GNSS aided INS is that it gives estimates with the same frequency as the IMU samples regardless if GNSS measurements are available or not. If only GNSS mea-surements would be used to position the vehicle, the position estimates would be updated once per second and never during a GNSS outage.

The outages that have been simulated in this thesis have been in the end of the data sets. If the outages would occur at some other time, the algorithm would try to track the vehicle in the same way and as soon as a GNSS measurement would be received, this would be trusted to be the vehicle’s position and the estimates would be back on track if it would have diverged. However, the point of the GNSS aided INS is that the estimated positions shall be close to the true position

(54)

44 7 Conclusions

during outage as in Figures 6.7 and 6.8. This means that the additional sensors, accelerometer and gyroscope, contribute the most in environments where GNSS outage occurs, such as tunnels or urban canyons of big cities with a lot of tall buildings. Note that without these sensors it would not be possible to track the vehicle at all during GNSS outage. In open sky environments, the GNSS measure-ments are sufficient to track a ground vehicle.

The o✏ine RTS smoother has not shown to improve the tracking during outage but it can be useful to run if the position estimates and the covariance matrices are sent from the vehicle to a back office where they could be post-processed. The advantage with this is that the skips in the tracking solution that are created when there is a GNSS measurement update are removed by the smoother. The drawbacks are that it does not run in real-time and a lot of information needs to be stored.

In the online RTS smoother, the smoothing process is started every time a GNSS measurement is received and the process runs back to the previous measurement update. This means that it is a fixed-lag smoother and the states are estimated based on past, current and even future information. This should naturally im-prove the tracking solution. The trade-o↵ is that the real-time properties are lost and information about the states and the covariances still need to be stored but not as much as for the o✏ine version.

The PF version of the GNSS aided INS has not shown to be an e↵ective way of tracking a ground vehicle in real-time. It requires a lot of computational power and does not increase the accuracy during outage since no information is avail-able then to change the weights of the particles.

The data that has been used in this thesis has been collected in two di↵erent envi-ronments and then later been post-processed in Matlab. It should however not be a problem to implement these algorithms and run them in real-time, except for the o✏ine RTS smoother.

It has been shown that map matching can be used to correct the small errors that are left from the GNSS aided INS estimates, especially during GNSS outage. One possible solution is to let the GNSS aided INS run in real-time in the vehicle and only send the position estimates to a back office where map information is kept and map matching is performed. Then it should be no problem to track a ground vehicle even through limited time periods of GNSS outage with very high accuracy. Another advantage with map matching is that it solves the multipath problems that can emerge in urban canyons.

7.2 Future Work

In many applications, the only connection to the vehicle is through power supply but if information about the vehicle’s speed would be available, for example from a speedometer, the estimates would probably be even more reliable even though the algorithm have shown good results with just the non-holonomic constraints.

Tracking of Ground Vehicles : Evaluation of Tracking Performance Using Different Sensors and Filtering Techniques

Master of Science Thesis in Electrical Engineering

Department of Electrical Engineering, Linköping University, 2018