Road-constrained target tracking using particle filter

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Road-constrained target tracking using particle

filter

Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping

av

Henrik Johansson

LITH-ISY-EX--08/4056--SE

Linköping 2008

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Road-constrained target tracking using particle

filter

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Henrik Johansson

LITH-ISY-EX--08/4056--SE

Handledare: Per Skoglar

isy, Linköpings universitet Mats Ekman

SAAB Systems

Examinator: Thomas Schön

isy, Linköpings universitet

(4)

(5)

Avdelning, Institution

Division, Department

Division of Automatic Control Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

Datum Date 2008-01-10 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-11562 ISBN — ISRN LITH-ISY-EX--08/4056--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Vägbegränsad målföljning med hjälp av partikelfilter Road-constrained target tracking using particle filter

Författare

Author

Henrik Johansson

Sammanfattning

Abstract

In this work a particle filter (PF) that uses a one-dimensional dynamic model to estimate the position of vehicles traveling on a road is derived. The dynamic model used in the PF is a second order linear-Gaussian model. To be able to track targets traveling both on and off road two different multiple model filters are proposed. One of the filters is a modified version of the Efficient Interacting Multiple Model (E-IMM) and the other is a version of the Multiple Likelihood Models (MLM). Both of the filters uses two modes, one for the on road motion and one for the off road motion. The E-IMM filter and the MLM filter are compared to the standard PF to be able to see the performance gain in using multiple models. This result indicates that the multiple model filters have better performance, at least when the true mode switching probabilities are used.

Nyckelord

(6)

(7)

Abstract

In this work a particle filter (PF) that uses a one-dimensional dynamic model to estimate the position of vehicles traveling on a road is derived. The dynamic model used in the PF is a second order linear-Gaussian model. To be able to track targets traveling both on and off road two different multiple model filters are proposed. One of the filters is a modified version of the Efficient Interacting Multiple Model (E-IMM) and the other is a version of the Multiple Likelihood Models (MLM). Both of the filters uses two modes, one for the on road motion and one for the off road motion. The E-IMM filter and the MLM filter are compared to the standard PF to be able to see the performance gain in using multiple models. This result indicates that the multiple model filters have better performance, at least when the true mode switching probabilities are used.

Sammanfattning

Den här arbetet presenterar ett partikelfilter som använder sig av en endimensionell dynamisk modell för att skatta positionen på fordon som befinner sig på någon väg. Den dynamiska modellen som används i partikelfiltret är en andra ordningens linjär-gaussisk modell. För att kunna spåra fordon som befinner sig både på och utanför vägen så föreslås två olika multipla filter. Ena filtret är en modifierad variant av Efficient Interacting Multiple Model (E-IMM) och den andra är en version a Multiple Likelihood Models (MLM). Båda filtren använder sig av två moder, en för rörelse på vägen och en för rörelse utanför vägen. E-IMM filtret och MLM filtret jämförs med ett standard partikelfilter för att kunna se förbättringen vid använding av multipla modeller. Resultatet visar att båda multipla modell filtren ger bättre resultat, i varje fall då rätt sannolikheter för modbyte används.

(8)

(9)

Acknowledgments

I would like to thank my supervisor Mats Ekman at Saab System for all help and support for my master thesis work. I would also thank Anders Jörmgård, who also did his master thesis at Saab System, for all good discussions around particle filters.

Finally I would like to thank all staff at Saab System in the Datafusion group for making my time during the thesis work enjoyable.

Stockholm, Mars 2008 Henrik Johansson

(10)

(11)

Introduction

In this chapter a background to this thesis is presented together with its purpose and goal. An introduction to Saab Systems Unattended Ground Sensor Network is also presented.

1.1 Background

Ground target tracking is the problem of estimating the position, velocity and acceleration of moving ground targets. This is performed using several different types of sensors. Some sensors only give the bearing to the target whereas others give both the bearing and the distance to the target. Examples of sensors used for ground target tracking applications are radar, acoustic and vision.

To estimate the true position of the target from sensor measurements different kind of filters can be used. Particle filters have become more and more common to use in different ground target tracking applications. The reason for this is that constraints and other non-linearities can be properly and conveniently handled in the particle filters (PF). There are several constraints on targets - like position, ve-locity and/or acceleration - that are often incorporated in the PF in ground target tracking applications. Other types of constraints may be included by additional terrain information, for instance road constraints.

At Saab Systems a research project, Integrated Detection and Estimation AL-gorithms Solutions for data processing and fusion (IDEALS), investigates among other things the use of PF in ground target tracking applications. An important topic in this project is to utilize additional information from the environment such as roads, forests, water and terrain in the PF to improve the estimate. It is also of interest to study different vehicle models. When the targets vehicle type is known some parameters can be changed to improve the estimation. Those parameters can for example be speed limits in the vehicle model or the probability of a target being on or off a road within a multiple model PF framework.

(14)

2 Introduction

1.2 Purpose and goal

How much can you gain by using more sophisticated vehicle models together with road information in ground target tracking applications? This application should use PF´s to estimate the state of ground targets. To see the gain, particle filters, that make use of road information and an appropriate vehicle model, are compared to a PF without any extra information. A convenient vehicle model should be able to handle the motion of different vehicle types such as trucks, cars, military vehicles with wheels and tracks. The PF should also be able to handle both on-road and off-road targets. In the IDEALS project, a simulation environment is already built in matlab and the new PF should be tested in this simulator.

1.3 Unattended Ground Sensor Network

The algorithms in this thesis can be tested on real data where the data comes from an Unattended Ground Sensor Network (UGSN). The UGSN is a prototype developed by Saab Systems, the Swedish Defence Research Agency (FOI), former Aerotech Telub (now an integrated part of Saab AB) and the Swedish Defence Material Administration (FMV). Almost all text in this section are influenced by [5].

In its current state the UGSN consist of 12 nodes and 2 central nodes, see Figure 1.1. The central nodes act as mobile control sites where data from all nodes are collected, processed and displayed. The information is processed in one of the central nodes using a multi sensor tracker (MST) which is a platform for data fusion developed at Saab Systems. The MST is based on Interacting Multiple Model (IMM) with different Kalman filters and Multiple Hypothesis Tracking (MHT) data association techniques. The processing in this central node is performed to get a global ground situation picture which is compared to a mosaic ground situation picture in the other central node. A mosaic ground situation picture is a picture where the information is processed in each node and the processed data are sent to the central node. The information from all nodes can easily be extracted before it is processed and this data can be saved and used as real testing data for external users.

To get a mosaic picture all nodes have their own MST and the sensors are the onboard sensor. A node program distributes sensor tracks in the network to their neighboring nodes. Each node has its surveillance area where data from its own sensors and neighboring nodes are used for detection, tracking and classification of targets. In this way false measurements in the association step can be reduced. As a result, it is possible that the same target can have different tracks in each node. Because of this, only tracks closest to its own node are sent to the central node to update the mosaic picture. The motivation for local fusion in each node is to reduce communication and to make UGSN scalable.

The different sensor types that can be connected to a node can be found in Table 1.1 and pictures of them in Figures 1.2, 1.3 and 1.4. The acoustic sensor have three microphones arranged in a circular array with 120 degrees of spreading. From these three microphones a bearing to the target is measured. The seismic

(15)

1.3 Unattended Ground Sensor Network 3

Figure 1.1. One UGSN node, a small PC with touch screen and GPS mounted in a box.

Table 1.1. Sensor types

Sensor Type Number Manufacturer

Acoustic 12 Developed by FOI

Seismic 10 Thales

Magnetic 2 Thales

PIR (Passive IR) 4 Thales

Radar SIRS (millimeter) 1 SAAB Bofors Dynamic

Analog Video 1 Malux

APN (Area Protection Network) 1 SAAB Bofors Dynamic

sensor is a relatively small sensor which is mounted in the ground and is able to detect and classify personnel, tracked or wheeled targets. The seismic sensor is sending the information wireless to a receiver that is connected to a node. The magnetic sensor is a magnetic transducer which measures changes in the magnetic field, this sensor is connected to a seismic sensor. The magnetic sensor can detect vehicles at ranges from 15 to 50 m depending on the vehicles speed and size [17]. The Passive Infrared (PIR) sensor is also connected to the seismic sensor. The information from a PIR sensor is only a detection in the direction of the sensor line of sight. The sensor is also able to detect from which side the target came into the detection zone, from this information the targets movement is given.

The Subsurface Interface Radar System (SIRS) is a millimeter radar that can detect targets at a range from 0 up to 1600 meters. However, to detect targets at 1600 meters the targets need to be in the size of an airplane. It can detect a single person up to a distance of 400 meters. The detection of a target gives both a bearing and a range. The data from the analog video camera needs to be preprocessed before the node can handle the data. From one single video camera

(16)

4 Introduction

only bearings to the target can be measured. The Area Protection Network (APN) is using IR cameras and a SIRS radar to detect the target. The IR cameras are covering a wider area then the radar, thus, this sensor produces only bearings outside the range of the SIRS radar and both bearings and ranges within the area that the IR camera and SIRS radar are covering together.

The communication between all the nodes and the central nodes are based on the IEEE 802.11b standard, which is a wireless standard. The communication in the UGSN is designed to provide redundancy in the routing of data up to the central nodes. Each node is able to communicate with every other node in the network since multicast routing techniques are used to forward data packages. The network is in an ad-hoc mode, where each node has a static IP-address.

Figure 1.2. A box with Seismic, Magnetic and PIR sensors.

Figure 1.3. An acoustic sensor connected to a node with the an-tenna for the wireless network be-side.

Figure 1.4. The APN sensor, to the left IR cameras, in the middle the SIRS radar, and to right the APN software running on a computer.

(17)

1.4 Saab Systems 5

1.4 Saab Systems

Saab Systems is part of Saab’s business segment Defence and Security Solutions and has around 1200 employees in Sweden, Australia, Denmark and South Africa. The headquarter for Saab Systems is situated in Järfälla, Sweden. Saab Systems offers integrated command and control system solutions and civil security solu-tions, along with further development and adaptations of existing command and control systems. Saab Systems is divided into three divisions; Air & Land Sys-tems, Naval Systems and Security Systems. This thesis work was carried out in the Datafusion group, which is a part of the Air & Land Systems division, but it serves Saab globally with data fusion solutions. At the Datafusion group highly advanced target tracking system products have been developed, like the Multi Sen-sor Tracker (MST), Track Data Fusion Engine (TDFE) and a Short Term Conflict Alert (STCA) system. The products are used in air defence and traffic control systems, as well as in sea and land tracking applications around the world. The Datafusion group are also conducting a lot of research in this area.

1.5 Outline

The main topics are dynamic models, digital road maps and particle filtering. Chapter 2 presents three different types of dynamic models, where two of them are used in the following application. The next chapter describes how the world is represented and produced in digital form together with the road map represen-tation for this application. In Chapter 4 the standard particle filter is presented as well as a modified particle filter which uses the road information in the digital map developed in the previous chapter. To be able to track targets both on and off the road the application use two multiple model techniques which are presented in Chapter 5. These two multiple model techniques are thereafter evaluated in Chapter 6, together with the standard particle filter. Finally, conclusion of the work is presented together with future work to be done. A list of abbreviations can be found in the end of the report.

(18)

(19)

Chapter 2

Dynamic models

In this chapter different types of target models are described. One of the models is a simple dynamic model in Cartesian coordinates, whereas the other two models, are a second order speed model and a so called Gauss-Markov model.

2.1 Second order linear-Gaussian model

In order to model the dynamics of road-constrained targets we will use a continues linear-Gaussian model. Using this model the road-map information can easily be transformed and incorporated into the PF. In [15] the following second order model for the target velocity v is given:

¨

v + 2ζ ˙v + ω2(v − vR) = cw , (2.1)

where w is a zero-mean white Gaussian process noise of unit intensity, ω is the natural frequency, ζ is the damping ratio, tc = 2ζ/ω is the time constant and

vR is the expected value of the vehicle speed. It is assumed that 0 ≤ ζ < 1 and

ω > 0. The constant c on the right side determines the magnitude of the driving

noise. This constant is derived from the steady state variance for the target speed

σ2

v= c2/(4ζω3). In continuous time the model is:

˙ x = Ax + Bu + Bww , (2.2) where A =   1 1 0 0 0 1 0 −ω2 _−2ζω  , B =   0 0 ω2  , Bw=   0 0 c   (2.3) 7

(20)

8 Dynamic models

The state vector is x = [s v a]T, entries are distance, velocity and accelaration,

u = vR which is the expected value of the vehicle speed. This model is sampled

according to: xt= F xt−1+ Gu + Gwwt, (2.4) where F = eAT, G = T Z 0 eAtBdt, Gw= T Z 0 eAtBwdt (2.5)

which leads to:

F =          1 1_β(1 − 2ζ2)e−ζωTsin(βT ) + . . . −_ωβζ e−ζωTsin(βT ) + . . . +2ζ_ω(1 − e−ζωTcos(βT )) +_ω12(1 − e−ζωT cos(βT ))

0 e−ζωT(cos(βT ) + ζω_β sin(βT )) _β1e−ζωT sin(βT ) 0 −ω_β2e−ζωTsin(βT ) e−ζωT(cos(βT ) − ζω_β sin(βT ))

         , K =         1 ω2_β(ζ2_ω2_+β2₎(−2ζωβ + 2ζ e−ζωTωβ cos(βT ) + . . . +ζ2e−ζωTω2_{sin(βT ) + βT ζ}2_ω2_{+ T β}3_{− β}2_e−ζωT_{sin(βT ))} 1 β(ζ2_ω2_+β2₎(β − βe

−ζωT _{cos(βT ) − ζωe}−ζωT_{sin(βT )} 1 βsin(βT )e −ζωT         , G = ω2K, Gw= cK, (2.6)

where β = ωp1 − ζ2_{, x and u are the same as for the continuous-time model. The}

distance s in this model is the distance from t = 0. In Figure 2.1 an example of how the model react when the initial state vector is x0= [0 0 0]T and the expected

vehicle speed is vR= 25m/s. In this example the constant is c ≈ 0.14, which gives

a relative small noise effect.

2.2 Constant velocity Model (CV)

The continuous- and discrete-time CV model is commonly used in different track-ing applications. This model will also be utilized in this theses to model the off road motions of the targets. The state vector in the CV model is X = [x y vxvy]T

where x and y is the position in Cartesian coordinates and vx, vy is the

corre-sponding velocity vectors. The model is described as:

(21)

2.3 Discrete Gauss-Markov process 9

Figure 2.1. Step response of the second order speed model.

where [13] F =     1 0 T 0 0 1 0 T 0 0 1 0 0 0 0 1     , G =     T2 2 0 0 T₂2 T 0 0 T     , (2.8)

and w is white Gaussian process noise with the covariance matrix Q = diag(σ2xσ2y),

and T is the sampling interval. A step response example of this model is shown in Figure 2.2. The distance is calculated according to st=

p

x2

t+ yt2and the speed

as vt=

q

v2

x,t+ vy,t2 .

2.3 Discrete Gauss-Markov process

Another model that is quite often used in tracking applications is the descrete Gauss-Markov process. This model will not be utilized in this theses. However, in order to give a more complete picture of different dynamic models that can be used in different tracking applications, a brief description of the model is given below.

(22)

10 Dynamic models

Figure 2.2. Step response of the constant velocity model.

The state vector in a discrete Gauss-Markov process is given by x = [s v]T. The model is described by

xt= F xt−1+ Gwwt, (2.9)

where the matrices are given in [18] as

F = 1 T 0 e−T /Θ G = 0 v√1 − e−2T /Θ , (2.10)

Where Θ is the maneuver correlation time used to discriminate different target types and v is the standard deviation of the target speed. A step response example of this model is shown in Figure 2.3.

(23)

2.3 Discrete Gauss-Markov process 11

(24)

(25)

Chapter 3

Map information

In this chapter the handling of digital map information is discussed. The digital map is stored in so-called Shape files, and some additional information can also be found in the dBase files. In the Shape file all geometric data for roads and terrain are stored and in the dBase file additional information like the type of road and road number is stored.

3.1 Geographic information system

Most of the text in this section is based on [1, 14]. A geographic information system (GIS) is a system for collecting, storing, analyzing and managing data which is refered to a position on the Earth. The first geographic information system was developed in 1962 in Ottawa, Canada, by the federal Department of Forestry and Rural Development. The system was called Canada Geographic Information System (CGIS). In the early 1980 M&S Computing, Environmental Systems Research Institute (ESRI) and Computer Aided Resource Information System (CARIS) developed a GIS software, ArcGIS, which is well known and used nowadays with features from the CGIS. A GIS is often associated with maps but in a GIS there are three ways of viewing the geographic data - The Database View, The Map View and The Model View, briefley introduced below.

1. The Database View: It is in this unique database all geographic information is stored 3.1. The database is called Geodatabase, a database of the world. In the design of the geodatabase a user can specify how the different geographic objects will be represented. For example an area such as a lake or a forest is typically represented as polygons and roads as lines. In the Geodatabase all geographic objects are stored in different classes depending of the type of object. All objects in one class have the same geographic representation. Beside each objects geographic coordinates, additional information about the objects are stored in the Geodatabase. Each class describes a layer in the database. All geographic data are collected into six different layers, see Figure .

(26)

14 Map information

Figure 3.1. The different layers in the Geodatabase [8]

2. The Map View, or geovisualization as it is called: In this view you look at the geographical data stored in the Geodatabase. GIS maps are similar to a printed map, but in GIS you can do more with the information. You can zoom in and pan the map and also turn on and off different layers to get a clearer view. You can also apply symbols for a map layer based on any attribute or you can for example find stores within a certain distance from an address or point. You can do this if the data are in the Geodatabase. A GIS can in addition to these maps show more interactive maps such as schematic drawings or temporals. The temporal view can for example show people movement in a city or country over years and in the schematic view you can display gas lines on a map or use the view when planning a new road.

3. The Model View: A better word for this view is The Geoprocessing View because processing geographical data is what this view can do, using the tools and processes included in GIS. These tools take an existing data set and apply operations, defined by different scripts such as Phyton, VBsript or Javascript, to generate new data sets. When you want to use a tool you build a model over what you want to do. For example from two data sets apply a tool to generate one new data set and with this new data apply another tool together with an existing data set to provide the resulting data set, see Figure 3.2. GIS models can be used to predict the outcome for an event, for example a growing volcano or to see what happens if the water level rises a couple of meters in a lake.

3.1.1 Geometric data

The Geodatabase can store data about the objects´ shape and geographical loca-tion in two types of data representaloca-tion, raster or vector data.

• Raster data: A digital picture is often of the raster data type. A raster is

built up of small squares in rows and columns. This square is also refered to as a pixel and in every square information about it is stored. In a picture the

(27)

3.1 Geographic information system 15

Figure 3.2. Example of a model in a GIS [9].

information in each pixel is what color this pixel has. In GIS the information depends on what kind of data it is. If for example the postal codes were entered in a GIS in the raster data type the information in each square would be the postal code. In common for all kinds of data entered in a GIS with raster type is the geographical information for each square. The objects resolution, how detailed the geographic object is when reproduced in raster data, depends on the size of the squares. Each square can only keep information about one object. When there are two objects inside one square the operator or program must choose which to keep information about. In Figure 3.3 every square has two objects inside. The most common technique is to keep the object that takes up most of the square. If a lake is being reproduced with raster, the lake´s shape will be natural if very small squares are used. If the squares are a little bit bigger the shape of the lake in the raster format is probably completely different from the shape of the actual lake. If the squares are even bigger, like the squares in Figure 3.3, it can happen that the lake is not being reproduced at all because there is no square that takes up most of the lake.

Figure 3.3. A lake and the surrounding ground within raster squares, this shows the problem with to big squares [14]. The object that take up most of the squares is the surrounding ground.

• Vector data: The objects geometry are stored in the Geodatabase by points.

They are combined using lines, which are called vectors. Vector data always consist of coordinates for these points and information about how the points should be combined. There are three different ways to combine the points - points, lines and polygons. Geographical objects stored by points can be

(28)

expressed by a single grid reference, in other words a location. The locations can for example be wells or peak elevations. A more common shape to store geographical data in is the line, or poly-lines. Geographical features stored in lines are for example roads, rivers or power cables. A poly-line is a line split up into more lines. The last geometric shape is the polygon. Lakes and other kinds of areas are stored using polygons. One big question is how many points do we need to represent the object. Of course, more points yields better resolution of the object but it requires a larger storing quantity. If the number of points is reduced, we lose the detail about the objects real shape.

To every geometric shape there are some attribute data which contains informa-tion about the object´s characteristics. These characteristics can for example be identity, color or age. They can also be logical or topological connections with other objects, data files with free text or even pictures.

3.1.2 Collection of data

To collect the geometric data into the Geodatabase different kinds of techniques are used.

• Manually: At the time when GIS was new this was done by a so-called

digitization table. This table consists of one big plate were the construction is made so the position of a marker with a sight can be automatically registered when the marker is moved around the table. The position from the marker needs to be exact within tenths of a millimeter. One printed map is connected to the table and the work starts with registration of the maps corner points. When this is done, ten points evenly distributed over the map should be registered together with the real coordinates. Hereafter the real digitization can be started. The operator takes the marker to the position of the points to be read and presses the register button. Points connected to different objects are stored in different ways. Naturally it is up to the operator to decide which points to be digitized, because of this the result is in a way subjective. A map can consist of hundreds of objects with thousands of points.

• Automatic: Before the work can begin the map is scanned into a raster data

format. This scanned map is then corrected for papers form changes and ten points are registered with their coordinates. This raster map can now be used as a background in a geodata system. To automatically make vectors from this map it needs to be put into an advanced graphics processing software. This software should be able to separate different objects in the map and calculate the objects geometry using vector graphics. In practice there is no programs that can do this without making errors, which is why an operator needs to correct these errors after the program is finished.

• Visual vectorizing: This method is similar to the manual method, but here

(29)

3.1 Geographic information system 17

the manual method the operator can now choose to zoom in the map before the points are registered. Special software can be used to do the work a little more automatic. The operator marks the beginning of an object to be vectorized, the software automatically follows the contour of the object and registers points along the way. The software stops when the object is finished or when there are alternative ways. Then the operator has to tell the software which way to choose.

• Photogrammetry methods: Nowadays this is the most commonly used method

in creating maps. The photos used are usually aerial photos with stereo cover. They can also be satellite photos with stereo cover. Stereo cover means that all objects on the ground are covered in at least two photos. One photo pair is then processed in a so-called stereo instrument. With this instrument an operator can measure and calculate the object coordinates in three dimensions. The detail in the measurements is dependent on the flight height. Maps with a scale from 1:10000 or less are usually produced from aerial photos aquired at 4600 meters. For a larger map scale the flight height needs to be lower.

• Detailed measuring: To produce maps in scales larger than 1:2000,

mea-suring methods on the ground are an alternative to photogrammetry. The instruments used for measuring on the ground is more exact then the pho-togrammetry methods, which is required for larger scaled maps. To do mea-suring on the ground it is necessary to have a large number of well known coordinates, Lantmäteriet call these “stompunkter” here referred to as base points. There are many techniques for making measuring on the ground, but usually an instrument called total station is used. The instrument is placed on a base point, thereafter angels and distances to objects are regis-tered. The measure values are stored together with information about the geographic object in a field computer. The measure values are expressed in polar coordinates but they are transformed to right-angled values.

Every object in the Geodatabase has two kinds of data, geometric data and attribute data. Aerial and satellite photos are not only used for the geometric data, they can also be used for the attribute data. Often the photos are combined with typical patterns and structures together with other indications. To decide what kinds of trees there are in a forest the photos are combined with the shadow shape, time for leafing and the actual position of the forest in the terrain. When attribute data are collected about the terrain the aerial photos can with advantage be Infra Red (IR) color pictures. These pictures have a larger spectral sensitivity. This means that the area for the pictures sensitivity is beside the visible light also is in the reflected infrared light. In this area the difference in reflection is heavily increased for different vegetation in the terrain. This way it is easier to separate different kinds of vegetation in the terrain compared to regular color pictures. Now the type of the vegetation is inserted in the attribute data for that geometric object. The attribute data for roads are for example what kind of road it is, the road name or road number, information about its direction and so on. These kinds of data are collected from the owner of the road.

(30)

In Sweden we have an authority called Lantmäteriet. It is responsible for pro-ducing and maintaining the geographic databases over Sweden [11, 12]. The first complete database called Grundläggande Geografisk Databas (GGD, fundamental geographic database) was completed in 1994, except for the mountain areas. Now the work is about to update this database and this is done from aerial photos which are corrected geometrically to a true scale. Lantmäteriet call these photos “Ortofoto” or in English Ortopicture. In these pictures it is easy to see where for example the forests are, how old they are and what kind of forest it is. In general, by comparing Ortopictures over the same area from different years, changes in the landscape can be detected.

3.1.3 Shape format

When the data collected in a GIS are exported to be used in another system the data can be in different file formats. Lantmäteriet uses two of these formats, Shape and MapInfo. In this thesis we are using the Shape format simply because it is the easiest one to use in the application.

When using the Shape format geometric data are stored in shape files and attribute data in dBase files. There are different shape files for different kinds of layers. Lantmäteriet has 38 layers and in each layer there is at least one kind of geometric object. More information about the layers can be found in [10]. The shape files use the vector format to store the geometric data. In the specification for the shape file, the points are stored together with a shape type. There are 14 different shape types, for example those described in Section 3.1.1, PolyLine and Polygon. The data is stored in different shape types if the data are in two or three dimensions. For example, if a road is stored without height information it is stored as a PolyLine, but if the data also contains the height the road is stored as a shape type called PolyLineZ. More information about the Shape format can be found in [7].

3.2 Representation of the roads

In this thesis the only interesting GIS data are the roads. There are three layers connected with roads in the shape files exported by Lantmäteriet [10]. These layers are public and private roads, roads not for access and road signs. The layer used in this application is the one with public and private roads. This layer contains all roads where cars are allowed to drive. A road is described by a set of points, which is stored in the Shape file as a so called poly-line. All crossings between roads are at the start and/or at the end of the road. See Figure 3.4 for an illustration. Each road has a category code as part of the attribute data, where the category describes what type of road it is. Lantmäteriet, supplies a lot of different categories such as better car road, car road and worse car road. In addition to the category, some roads have one or more road numbers, a road number can for instance be E4 (European route 4). There is also attribute data for the roads which tells if the road is one directional or not. If it is one directional the direction is the same as the road in the Shape file.

(31)

3.2 Representation of the roads 19

Figure 3.4. The figure shows a road network with several roads, where all roads consist of at least one road segment and all road segments consist of two road points.

When the Shape file has been read into Matlab all points from all roads are stored as two variables, one for the x-coordinates and one for the y-coordinates. Between the points from two roads there is a NaN (Not a Number) value to separate the roads, see Figure 3.5. There is no information about crossings and due to how the road points are stored it is difficult to index the roads. In this thesis the application needs to remember on which road each particle is located and to find this road in a fast manner. Because of this, a new structure is built up where all roads are separated.

All roads are now stored in a Matlab each road is represented by a struct with four data fields. One field is for all points in the x, y-coordinates, and another field contains the information about road number and category code. The third data field is for the crossings where information about all roads that cross the current road is stored. Finally, the last data field is devoted to the category information. The first field, for all coordinate points, consists of a vector that has as many columns as there are points on the road. There are at least two points for a road. The second field also consists of a vector but this has only one row and as many columns as necessary. The vector stores all number codes associated to that information. The third field is a Matlab cell which stores vectors. This cell has one row and as many columns as there are road points. In each cell there is a vector that stores the road numbers of the connected roads in that point. The last field consists of a text string which tells what type of road it is. See Figure 3.6 for an illustration of the new data structure.

(32)

Figure 3.5. Road information (data) when read into Matlab.

Figure 3.6. Illustration of road in-formation in structs.

(33)

Chapter 4

Particle filters

In this chapter the standard PF and the modified version for road vehicles are presented.

4.1 Particle filter

This section is mainly based on [16]. The particle filter was first introduced by Gordon in 1993 and since then the algorithm has been further developed and adapted to many different applications. The particles in PF´s can be considered as a set of samples which describes the state of a target. The idea is quite simple; from this set of particles weights are calculated using the measurement model and the latest measurement. The particle weights correspond to the probability of how likely it is that particles are from the target posterior density. Then those particles that have the lowest weight are pruned and those with the highest weight are duplicated. Now this new set of particles are further propagated through the dynamic model. Finally everything is repeated when a new measurement is available.

Particle filters are a part of a larger set of algorithms called Sequential Monte Carlo (SMC) methods. The fundament in most SMC methods is to apply Monte Carlo integration on integral equations. PF´s are based on recursively estimating the probability density function p(xt | Yt), where Yt corresponds to all

measure-ments until time t, and xtis the state vector at time t. When this density function

is recursively estimated by a set of particles it can be approximated with:

p(xt| Yt) ≈ N X i=1 1 Nδ(xt− x (i) t|t), (4.1)

where δ(·) is the Dirac function and {x(i)_t|t}N

i=1are all particles. The density function

(34)

22 Particle filters

p(xt| Yt) is rewritten using Bayes´ theorem as:

p(xt| Yt) = p(xt| yt, Yt−1) = p(yt| xt)p(xt| Yt−1) p(yt| Yt−1) ∝ p(yt| xt)p(xt| Yt−1) , (4.2) From ( 4.2) we identify p(xt| Yt) | {z } t(xt) ∝ p(yt| xt) | {z } q(xt) p(xt| Yt−1) | {z } s(xt) , (4.3)

where t(xt) is the target density, q(xt) the acceptance probability and s(xt) the

sample density. The sample based solution to ( 4.3) involves that the posterior is approximated by a particle cloud. Within this framework the acceptance proba-bility is the assigned weight to a particle corresponding to how likely that particle was drawn from the target density. Then, the acceptance probability is calculated according to

q(i)_t = q(x(i)_t|t−1) = p(yt| x(i)_t|t−1) (4.4)

˜ q(i)_t = q (i) t PN j=1q (j) t (4.5)

where x(i)_t|t−1is a particle from the sample density p(xt| Yt−1). The sample density

s(xt) is rewritten using the approximation in ( 4.1) as

The transitional density, p(xt| x (i)

t−1|t−1), is realized with:

p(xt| x(i)_t−1|t−1) ∼ x(i)_t|t−1, (4.7)

x(i)_t|t−1= F x(i)_t−1|t−1+ Gwt(i), (4.8)

where the process noise is Gaussian, w_t(i)∼ N (0, Q), with the covariance matrix Q = diag([σ2

xσ2y]), F and G are the matrices described in ( 2.8). From ( 4.6)-( 4.8)

(35)

4.2 Particle filter for road vehicles 23

passing particles from the previous time step (after the resample step, see ( 4.9)) through the dynamic model. According to the Sampling Importance Resample (SIR) algorithm the next thing to do is to generate a new set of particles by the following resample step:

P r(x(i)_t|t = x(j)_t|t−1) =q_e(j)_t , i = 1, . . . , N (4.9) This means that those particles that have low weights are copied to particles that have higher weights. The SIR-PF algorithm is summarized in Algorithm 4.1.

Algorithm 4.1 SIR Particle filter

1. Initialize the particles, {x(i)_0|−1}N

i=1 ≈ px0(x0) and set t = 1.

2. Propagate: predict new particles {x(i)_t|t−1}N

i=1 according to ( 4.6).

3. Measurement update: calculate the acceptance probabilityq_e(i)_t according to ( 4.5).

4. Resampling: Generate a new set {x(i)_t|t}N

5. Set t = t + 1 and iterate from step 2.

4.2 Particle filter for road vehicles

The particle filter discussed in this chapter is developed for targets that travel exclusively on roads. This filter utilizes a one-dimensional dynamic model since all particles must be located on a road. The dynamic model is the second order linear-Gaussian model, which is described in Section 2.1. However, the dynamics are somewhat modified to model the distance of the target from the last time instance instead of modeling the total distance from the initial state value. This is convenient since it is the distance from the particles last position of the particle that is needed when the additional information about the road network is utilized. Hence, the element F (1, 1) in ( 2.6) is set equal to zero.

Each particle has to remember which road it is located on and which point on that road it last passed. It is also necessary to remember the direction of the particle on the specific road. This information is stored in a vector L = {l(i)_}N

i=1,

which will be explained later. To be able to plot the particles on a map all particles need two parallel state vectors. One for the one-dimensional dynamic model, which is the vector

Z = {z(i)}Ni=1, z = [s v a] T_,

(4.10) where s is the distance since last time, v is the speed and a is the acceleration. The other state vector is

(36)

where x and y represent the particle position (in Cartesian coordinates) and vx

and vyare the corresponding velocity vectors. Here we refer to the dimension of a

model in terms of the lateral position. Hence, for the one-dimensional model the distance s along the road curve is the position, whereas for the two-dimensional model the position is given by the (x, y)-coordinates.

The particle filter proposed here is based on the particle filter described in Section 4.1 and is summarized in Algorithm 4.2. In the initializing stage all particles are spread out on the roads near the initial state x0. This is done by

first randomly generate all particles near x0 with the probability px0 and then

projecting these particles on the nearest roads by using the GetParticleOnRoad function described in Section 5.3. After the initialization the information about the location, i.e., the valid road number, the nearest corresponding road point and the direction, is stored in l(i)for each particle. In the propagation step, the vector

Z is updated by passing the particles through the one-dimensional dynamic model,

in other words the transitional density is now realized with:

with F , G, Gwand u as described in Section 2.1 except for the change in F (1, 1),

and where w_t(i)is the process noise. In the same step the state vector X needs to be updated. This is performed by the function FindPoint (described in Section 4.3) which maps the particles, z y x, according to the calculations given in Algorithm 4.3. The last steps are performed in the same way as in a standard SIR-PF.

Algorithm 4.2 SIR Particle filter on road

1. Initialize the particles,

X0|−1= {x (i) 0|−1} N i=1 ≈ px0(x0), [Z0|−1, L] = GetParticleOnRoad(X0|−1, N ) (4.13) and set t = 1

2. Propagate: predict new particles {z_t|t−1(i) }N

i=1 according to ( 4.12) and

gener-ate [x(i)_t|t−1, l(i)_{] = FindPoint(x}(i) t−1|t−1, l

(i)_{, z}(i)

t|t−1, N ) for ∀i.

3. Measurement update: calculate the acceptance probability_eq_t(i)according to ( 4.5).

4. Resampling: Generate a new set {z(i)_t|t}N

i=1 and {x (i) t|t}

N

(37)

4.3 Find Point 25

4.3 Find Point

In this section the main structure of the function FindPoint is described. This function is used in the propagation step (step 2 in Algorithm 4.2)in the particle filter algorithm for road vehicles. The purpose of this function is, for each par-ticle, to calculate new xt and lt from the state vector xt−1, a road network N ,

information vector lt−1, and the state vector zt−1. In other words, from a position

and a distance calculate a new position according to the road network. The road network has to be stored as described in Section 3.2.

The first thing this function does is to call a help function, FindWay which has the purpose of finding the road the particle is propagated to. The help function first calculates the distance from the particle to the end of the current road, according to

d =p(xend− x)2+ (yend− y)2, (4.14)

where xend, yend corresponds to the position of the last road point on the current

road and x, y is the particle position (in Cartesian coordinates). This distance is then compared to the distance z(1), which is the estimated distance after the propagation. If z(1) is larger than d the particle will pass a crossing or the particle is in a dead end. If it is a crossing the function picks a road from all possible roads randomly, i.e. we assume that the probability of a particle choosing a specific road is equal for all connected roads. The distance z(1) is decreased by d. Now this is recursively repeated until the distance, z(1), is less than the road length. The information vector lt is updated with the number of the found road. Next,

FindPoint has to calculate where on the returned road from FindWay the particle should be. To get the valid road segment the function searches through the road points in order to find where z(1) is less than the length of the road segment. This is performed iteratively by comparing z(1) with the length of this road segment. If z(1) is larger than this length the distance is decreased by the length. This procedure is repeated at the next road segment. When the correct road segment is found the information vector is updated with the index of the first road point to the segment. To calculate the particles propagated position on this road segment the function has to calculate the vector between the points that build up this segment like vx= xk+1r − xkr q (xk+1r − xkr)2+ (y k+1 r − yrk)2 vy= yk+1 r − yrk q (xk+1r − xkr)2+ (y k+1 r − yrk)2 V = [vxvy]T (4.15)

(38)

new state vector is then

xpos= [xk_r y_rk]T + z(1) ∗ V

xvel= z(2) ∗ V

xt= [xpos xvel]T (4.16)

The pseudo code for the functions FindPoint and FindWay are given in Algorithm 4.3 and 4.4, respectively.

Algorithm 4.3 FindPoint

1: function [x,l]=FindPoint(x,l,z,N)

2: [x,l,z]=FindWay(x,l,z,N);

3: s=length from particle to next road point

4: while s<z(1) do

5: z(1)=z(1)-s;

6: s=length of next road segment

7: x(1:2)=the first road point of the road segment

8: end while

9: V=normalized vector between points of this segment

10: x(1:2)=x(1:2)+z(1)*V;

11: x(3:4)=z(2)*V;

Algorithm 4.4 FindWay

1: function [x,l,z]=FindWay(x,l,z,N)

2: d=length from particle to end of road

3: if d<z(1) then

4: z(1)=z(1)-d;

5: if crossing then

6: l=randomly pick one of the roads

7: [x,l,z]=FindWay(x,l,z,N);

8: else

9: z(1)=0;

10: x(1:2)=endpoint for this road

11: end if 12: end if

As stated before, this section has described the main structure of the support-ing functions FindPoint and FindWay, the difficulties described below are also implemented in the application, but left out in the Algorithms 4.3 and 4.4. The first thing is that the distance z(1) can be negative and the other difficulty is the connected to directions of the roads. When a particle comes to a crossing it is assumed that it will choose one of the connecting roads and continue to travel in the same direction. The problem is that the roads are stored in such a way that two roads can have the same end point and also that roads can have the same start

(39)

4.3 Find Point 27

points. Due to this each particle must store the traveling direction along the road. When the direction is negative the particle is traveling backwards on that road, where backwards means opposite to how the road points are stored. All possible variants of this problem can be seen in Figure 4.1. If a particle starts at the first point on road 1 the direction is positive. Hence the particle will travel towards the crossing if z(1) is positive. When the particle comes to the crossing and if it continues to travel on road 2, the direction has to be changed to a negative value in order to get the particle in the same direction as before the crossing. The same thing will occur if the particle is moving from the last point on road 4 with a positive distance, z(1), and a negative direction. When the particle continues on road 3 the direction is changed. The direction is changed when the first point of two roads is the same crossing point and if the end points for the two roads are the same. Otherwise the value of the direction is the same as it was before the crossing.

(40)

(41)

Chapter 5

Multiple model filters

In this chapter two techniques for combining filters are presented. The techniques are based on the Interactive Multiple Model (IMM) approach. Moreover, a func-tion that projects particles onto a road is described in detail. The purpose of using IMM is to utilize different dynamic models in order to cover a wider spectrum of target movements.

5.1 Interacting Multiple Model, IMM

The combining technique Interacting Multiple Model (IMM) was first derived for algorithms based on Kalman filters in 1988 [2]. However, here the IMM strategy is combined with the SIR-PF method, briefly described in Section 4.1. A more detailed presentation of the IMM algorithm is given in [3]. In [3] the algorithm is called Efficient IMM (EIMM). This algorithm is based on the IMM technique for PF in [4] but also deals with the problem of degeneracy. In the IMM filter given in [4] the number of particles in each mode is calculated according to the probability for each mode. This requires that the total number of particles is very high to prevent degeneration. The degeneracy is a problem when the number of particles in one mode is too small to handle fast mode changing. The EIMM is more efficient because it allows the user to decide the number of particles in each mode. This allows the total number of particles to be reduced without the risk of degeneration.

The purpose of using IMM is to utilize different dynamic models in order to cover a wider spectrum of target movements. For example, consider an airplane as a target. This target can move in a straight movement, make turns or it can accelerate. A regular particle filter that tracks this target only use one dynamic model. However, using a singel dynamic model increases the risk that the PF incorrectly estimates the target trajectory or even looses the target when it makes an abrupt turn. This is especially true when the number of particles is low. In this case it is better to use an IMM filter where each mode describes the dynamics of different target motions.

The steps in the EIMM algorithm are almost the same as for the regular PF. 29

(42)

30 Multiple model filters

However, some additional steps are included before the propagation step and after the resample step. Before the propagation step a mixing between the modes is performed and after the resample step the posterior mode probabilities are calculated. These probabilities are used in the mixing step in the next iteration. In EIMM the posterior for each mode are estimated using a PF, which makes the other steps easy. They are performed in the same way as for the regular PF for each mode. The EIMM algorithm is described in detail in Algorithm 5.1. Before this algorithm is presented some notations need to be defined. Let M_tj denote

j = 1, . . . , m different modes at time t. The probability for each mode is given by

Bayes´ theorem γ_tj= P (M_tj| Yt) = λj_tP (M_tj | Yt−1) Pm j=1λ j tP (M j t | Yt−1) (5.1) where λj_t = Nj X i=1 ˜ q_t(i),j j = 1, . . . , m (5.2)

where Nj denotes the number of particles in mode j and ˜q is the particle weight

defined in (4.5). The probabilities P (M_tj | Yt−1) are calculated according to

P (M_tj | Yt−1) = m

X

i=1

pi,jγt−1i j = 1, . . . , m (5.3)

where pi,j are the transition probabilities

pi,j= P (Mtj| Mt−1i ), pi,j≥ 0 ∀i, j ∈ M (5.4) m

X

j=1

pi,j= 1, i = 1, . . . , m

In this report the application is tracking of vehicles that can drive both on and off roads. In order to improve the estimation of the targets position, information about the road network is incorporated into the PF algorithm. Two different PF´s are used to resemble the on/off road motion of the target. For the on road motions the PF in Section 4.2 is used and for the off road motions the PF in Section 4.1 is utilized. The two different PFs are mixed according to the EIMM algorithm described in Algorithm 5.1. However, since the particles from the different modes are mixed in the mixing stage and the PF for on road motion uses the one-dimensional model described in Section 2.1 it is necessary to have an additional step where the particles are mapped between the different coordinate systems. Moreover, we need a step that projects particles onto the road. These functions are described in Section 4.3 and 5.3, respectively.

(43)

5.1 Interacting Multiple Model, IMM 31

Algorithm 5.1 EIMM-PF

1. Initialize: For t = 0, generate N samples {x(i)t=0}Ni=1 from the initial

distri-bution px0(x0).

2. Mixing: Perform the mixing described in Algorithm 5.2 to obtain P (M_tj | Yt−1) and the mixing particles ˜x

(i),j

t−1|t−1, for each mode j.

3. Propagate: For i = 1, . . . , N , predict new particles x(i),j_t|t−1 according to (4.6) for each mode j.

4. Update: Calculate the importance weights ˜q(i),jt according to (4.5) and λ j t

using (5.2) for each mode j.

5. Resample: Generate a new set {x(i),j_t|t }N

i=1 by resampling according to (4.9).

6. Calculate the posterior mode probabilities using (5.1). 7. Increase t and continue from step 2.

Algorithm 5.2 Mixing stage of EIMM-PF algorithm

1. For j = 1, . . . , m, update the probability

P (M_tj | Yt−1) = m X i=1 pi,jγt−1i = c j_. _(5.5)

2. For i = 1, . . . , m and j = 1, . . . , m calculate the mixing probability

P (M_t−1j | Mtj, Yt−1) =

1

cjpi,jγ j

t−1. (5.6)

3. According to P (M_t−1j | M_tj, Yt−1), calculate in which mode each particle will

be drawn from, { ˜Mt(i),j}Ni=1.

(44)

5.2 Multiple Likelihood Model, MLM

A second combining technique is used in this application and it was first derived in 2007 [6]. This technique is based on standard Multiple Model (MM) algorithms and PF. Originally, the idea is to let different modes be represented by different likelihood models, whereas the transitional density is assumed to be the same for all models. Hence this is applicable when all modes use the same dynamic model. However, in this application the MLM algorithm is modified to work with different dynamic models. In the original MLM algorithm the mixing is performed before the resampling by weighting the particle weights according to the mode probability. This is now complemented with a mixing step before the propagation step where particles are drawn according to the mode probabilities from the old posterior density.

In the initializing step all N generated particles are split into the different modes. The first step in this modified MLM algorithm is to propagate all particles in each mode according to the corresponding dynamic model. Then the importance weights q_t(i),jare calculated for each mode j = 1, . . . , m. Now the mixing conditions need to be calculated. Let yt be the measurement at time t and Yt−1 all the

measurements up to time t − 1. The probability for each mode is given as

γ_tj= 1 cp(yt| M j t, Yt−1) m X i=1 pi,jγt−1i , j = 1, . . . , m , (5.8)

where c is a normalization factor and pi,j are the transitional probabilities

ac-cording to (5.4). The conditioned likelihood p(yt| M j t, Yt−1) is assumed Gaussian as p(yt| M j t, Yt−1) ≈ l j t = p(yt| ˆx j t, ˆS j t) = N (yt; h(ˆx j t), ˆS j t), (5.9)

where ˆStj is the innovation covariance matrix from a filter estimate with model j.

The sensor model h(·) is a transformation from a state vector to measurement val-ues. Measurement values can for example be azimuth and range relative the sensor position. Moreover, ˆxjt in (5.9) is the Minimum Mean Square (MMS) estimate,

which can be calculated as

ˆ xj_t ≈ Nj X i=1 q_t(i),jx(i),j_t , j = 1, . . . , m , (5.10)

where Nj is the number of particles in mode j. Now the mixing can take place by

combining the weights from the different modes according to

qt(i)= 1 α m X j=1 γjtq (i),j t , i = 1, . . . , N , (5.11)

where α is a normalizing factor. A new set of particles is generated by resampling the old particles according to the weights q(i)_t . The modified MLM algorithm is

(45)

5.2 Multiple Likelihood Model, MLM 33

described step by step in Algorithm 5.3. Compared to the original MLM algorithm in [6] there is an additional mixing step is introduced before the propagation of the particles. In this mixing step the particles are chosen with probability γtjto be

propagated through the dynamic model j. This is similar to the mixing stage of the EIMM-PF algorithm (Algorithm 5.1) but here the probability of which mode the particles will be drawn from is already given by γ_tj. In fact, the mixing step just decides, according to the mode probability, how many particles from the common posterior density will be propagated through the dynamic model j.

Originally the MLM algorithm was designed to use only one dynamic model with different constraints acting on the likelihood function. Thus all particles are predicted using the same dynamic model and then the probabilities for each mode are calculated according to different constrained likelihood models. In Algorithm 5.3 step 2 is included in order to handle different dynamic models, like in the description above.

Algorithm 5.3 MLM algorithm

1. Initialize: For t = 0, generate N samples

{˜x(i),j_t=0}Nj i=1, j = 1, . . . , m (5.12) N = m X j=1 Nj,

from the initial distribution px0(x0).

2. Draw randomly Njnumber of particles from the set, {x (i) t−1|t−1}

N

i=1, according

to the mode probability γ_t−1j

3. Propagate: For j = 1, . . . , m predict new particles,{x(i),j_t|t−1}Nj

i=1, according to

each dynamic model.

4. Mixing: Perform the mixing described in Algorithm 5.4 and obtain the com-bined weights q_t(i).

5. Resample: Generate a new set, {x(i)_t|t}N

i=1, by resampling where

P (x(i)_t|t = x(i),j_t|t−1) = qt(i). (5.13)

(46)

Algorithm 5.4 Mixing stage for MLM algorithm

1. Calculate the importance weights, q(i),jt for each mode j.

2. Compute the estimates ˆxj_t and ˆS_tj according to (5.10).

3. For j = 1, . . . , m, calculate the conditioned likelihood functions

lj_t = p(yt| ˆxjt, ˆS j

t). (5.14)

4. Calculate the mode probability γ_tj for each mode j = 1, . . . , m like (5.8). 5. Calculate the combined weights, q_t(i), according to (5.11).

In this application two modes are used, one with the dynamic model described in Section 2.1 and one with the model described in Section 2.2. The first model is the same one dimensional model used in Section 5.1, and also here the function GetParticleOnRoad, described in Section 5.3, is needed in order to handle projec-tions of particles onto roads. In Algorithm 5.3 the GetParticleOnRoad function is used in the initializing step and in the propagation step. In the initializing step N particles are generated from the initial distribution px0(x0), those particles are

in-put to the function GetParticleOnRoad together with a maximum distance. The output from the function is N particles but those particles that have a distance to any road closer than the maximum distance are projected onto the nearest road. Particles that have a distance to the nearest road larger than the maximum dis-tance are not projected to any road. Those particles that are projected onto a road belong to one on-road mode and those not projected belong to the off-road mode. In the propagation step the function is used in a similar way but input to the function is now the resampled particles that belong to off-road mode.

5.3 Get Particle On Road

In this section the function GetParticleOnRoad, which project particles outside the road onto a specific road segment, is described. Input to the function is a matrix, X, that contains all state vectors, x(i), for all particles. The function also takes the road network, N , as an input argument. Output from the function is a new X and a matrix L. L contains a vector, l(i), for each particle, i ,with elements containing road number, last passed point, direction, and information about the movements of the particles.

The first step in this function is to call for a help function. This help function returns all roads that are closer than 500m to the mean position of the particle clouds. When all roads have been found the GetParticleOnRoad function is going into a loop. The function searches for all road segments having their normal intersecting a particle, this is repeated for all particles. Figure 5.1 illustrates an example of this, where all possible normals to a road segments intersecting a

(47)

5.3 Get Particle On Road 35

particle is displayed.

Figure 5.1. Illustration of road segments normals

Figure 5.2. Illustration of the calculations in ( 5.15), projection of a point onto a line segment.

The computation to find all these road segments is based on vector calculations. We want to project vector B onto vector A to get the projected vector ˜A, see Figure

5.2. Vector A is calculated from the two road points P1= (x1, y1) and P2= (x2, y2)

and vector B from the first road point P1 and the particle P3 = (x3, y3). This

calculation results in a value of how much the projected vector ˜A consist of vector A: u = ˜ A |A|= |B| cos v |A| = 1 |A|· A · B |A| = A · B A · A = (x3− x1)(x2− x1) + (y3− y1)(y2− y1) (x2− x1)(x2− x1) + (y2− y1)(y2− y1) , (5.15)

where the following criterion can be identified    0 ≤ u ≤ 1 on road segment u < 0 outside u > 1 outside (5.16)

Road-constrained target tracking using particle filter

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Road-constrained target tracking using particle

filter

Road-constrained target tracking using particle

filter

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Abstract

Sammanfattning

Acknowledgments

Contents

Chapter 1

Introduction

1.1

Background

1.2

Purpose and goal

1.3

Unattended Ground Sensor Network

1.4

Saab Systems

1.5

Outline

Chapter 2

Dynamic models

2.1

Second order linear-Gaussian model

2.2

Constant velocity Model (CV)

2.3

Discrete Gauss-Markov process

Chapter 3

Map information

3.1

Geographic information system

3.1.1

Geometric data

3.1.2

Collection of data

3.1.3

Shape format

3.2

Representation of the roads

Chapter 4

Particle filters

4.1

Particle filter

4.2

Particle filter for road vehicles

4.3

Find Point

Chapter 5

Multiple model filters

5.1

Interacting Multiple Model, IMM

5.2

Multiple Likelihood Model, MLM

5.3

Get Particle On Road