Institutionen för systemteknik

Transcription

1 main: :41 1(1) Institutionen för systemteknik Department of Electrical Engineering Examensarbete A Tracking and Collision Warning System for Maritime Applications Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Andreas Rönnebjerg LITH-ISY-EX 05/3709 SE Linköping 2005 Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Linköpings tekniska högskola Linköpings universitet Linköping

2 main: :41 2(2)

3 main: :41 i(3) A Tracking and Collision Warning System for Maritime Applications Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Andreas Rönnebjerg LITH-ISY-EX 05/3709 SE Handledare: Examinator: Dr. Rickard Karlsson ISY, Linköpings universitet Prof. Fredrik Gustafsson ISY, Linköpings universitet Linköping, 26 August, 2005

4 main: :41 ii(4)

5 main: :41 iii(5) Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Linköpings universitet S Linköping, Sweden Datum Date Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX 05/3709 SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Målföljning och Kollisionsvarning för Sjöfart A Tracking and Collision Warning System for Maritime Applications Författare Author Andreas Rönnebjerg Sammanfattning Abstract This Master s thesis describes the development of a tracking and collision warning system for maritime applications. The developed system is a part of FOR Shore Navigation, a larger system for simulation of ship movement in an archipelago environment. The thesis introduces the problems of target tracking, and a set of solutions to these problems, in a general manner. A tracking algorithm based on using extended Kalman filters for state estimation is developed and an evaluation of this tracking algorithm performed. The functionality of the collision warning system is then based on using the Kalman filter for prediction, where the future positions of tracked targets are of interest. Since part of the work with this thesis has been to update the functionality of the FOR Shore Navigation system, the beginning of the thesis also functions as an introduction to the functionality in FOR Shore Navigation. Nyckelord Keywords Target tracking, data association, collision warning, FOR Shore Navigation, Kalman filter

6 main: :41 iv(6)

7 main: :41 v(7) Abstract This Master s thesis describes the development of a tracking and collision warning system for maritime applications. The developed system is a part of FOR Shore Navigation, a larger system for simulation of ship movement in an archipelago environment. The thesis introduces the problems of target tracking, and a set of solutions to these problems, in a general manner. A tracking algorithm based on using extended Kalman filters for state estimation is developed and an evaluation of this tracking algorithm performed. The functionality of the collision warning system is then based on using the Kalman filter for prediction, where the future positions of tracked targets are of interest. Since part of the work with this thesis has been to update the functionality of the FOR Shore Navigation system, the beginning of the thesis also functions as an introduction to the functionality in FOR Shore Navigation. v

8 main: :41 vi(8)

9 main: :41 vii(9) Acknowledgements I would first of all like to thank my supervisor Rickard Karlsson for his guidance throughout the work of writing this thesis. I would also like to thank my examiner Fredrik Gustafsson who, together with Rickard, came up with the idea leading to the formulation of this thesis project. vii

10 main: :41 viii(10)

11 main: :41 ix(11) Contents 1 Introduction Problem Specification Limitations Thesis Outline Simulation Environment System Overview How to Create a Simulation in FSN Simulation Process FSN Function Classes Functions in C Vectorized Nautical Chart WGS84 and RT Ship Model Radar Simulator Radar Search Algorithm Range Rate Computation Radar Measurement Noise Model Radar Clutter Model Estimation Methods The Kalman Filter The Extended Kalman Filter Target Tracking and Data Association Tracking Sensor TrackingCoordinateSystem Target Dynamic Models The Constant Velocity Model The White-Noise Acceleration Model The Singer Acceleration Model Track Initiation A Logic-Based Multi-Target Track Initiator Modified Hough Transform Track Initiator Gating ix

12 main: :41 x(12) Ellipsoidal Gates Data Association The Nearest Neighbor Filter The PDA Filter The JPDA Filter The Multi-Target Modified PDA System Design Target Tracking in FSN TrackingSensor Target Dynamic Model Track Initiation Gating Data Association TrackDeletion Collision Warning Simulations TestingtheTrackingFilter SimulationSettings Process Noise Optimization RMSE(t) Collision Warning Simulations Summary Conclusions Future Work Bibliography 53

13 main: :41 1(13) Chapter 1 Introduction Today the global positioning system (GPS) is the main system used for positioning and navigation at sea. The GPS is based on 24 satellites orbiting the earth, each sending out radio signals with their position and a time stamp to determine the distance to the satellite. For a GPS receiver to be able to determine its 2D position, longitude and latitude, it needs to receive signals from at least three of these satellites. For a 3D position, where the altitude of the receiver is presented as well, the required number of satellite signals is four. If these requirements are fulfilled, the receiver can present not only information about its current location, but also speed, heading, and other information of interest when navigating. Figure 1.1 shows how a receiver s 2D position can be triangulated with the help of signals from three GPS satellites. Although the GPS covers the entire globe and works well even in the worst types of weather conditions it is still in some senses vulnerable. Radio signals can be blocked and satellites malfunction, leading to problems for those relying on an accurate positioning system. In [13] an alternative to the GPS system has been developed, it is a system based on particle filters for positioning and only requires the ordinary radar sensor combined with a nautical chart database to compute a ship s position. The limitation of the system is that it will not work on open seas since it needs radar information about the coastline to be able to compute the position of a vessel. During the development of this new method for positioning a testing platform was needed and the FOR Shore Navigation (FSN) system was developed. The FSN system is a platform for simulating ship movement in an archipelago environment. It uses data from vectorized nautical charts as simulation environment and a radar simulator to compute approximations of radar returns along a ship s route in an archipelago. The first FSN testing platform, which used rasterized nautical charts, was created by Prof. Fredrik Gustafsson and Dr. Rickard Karlsson at the division for Control and Communication Systems, Linköping University. M.Sc. Erik Svensson was later employed to implement a version of the system that allowed for the use of vectorized nautical charts. The system handled simulations of a single ship moving along given waypoints in the nautical chart, where the 1

14 main: :41 2(14) 2 Introduction Figure 1.1. The GPS receiver, marked as the black center dot, receives signals from three GPS satellites and uses the signal information, satellite position and time, to determine its own location. The uncertainty of this position, the common area between the three circles, is due to bias in the time stamp of the satellite signal. The normal accuracy for a GPS receiver is about ± 10 meters. radar simulator computed the simulated response from land echoes. These simulated radar returns were then used for testing particle filters developed for the new positioning system. Figure 1.2 shows the graphical user interface (GUI) of the FSN system and the vectorized nautical chart used for simulation. As a part of developing the FSN system further and adding more functionality to it the idea of implementing a tracking and collision warning system into FSN came up. This idea later evolved to the formulation of this Master s thesis. 1.1 Problem Specification The primary goal of this project is to implement a tracking and collision warning system into FSN, where the warning system warns for collisions with moving targets (other ships). Some time will also be spent on reorganizing and structuring the matlab code while adding code comments to facilitate further extensions or updates of the simulation platform. The design of a collision warning system for moving targets also requires that functionality for simulating several ships at the same time is implemented and that the radar is updated to handle returns from other ships and range rate measurements. To make use of this work part of this Master s thesis therefore functions as documentation of how the FSN system is implemented in general, not only how the tracking and collision warning system is designed. 1.2 Limitations Since much time will be spent on updating the simulation environment, implementing a tracking system, and reorganizing the matlab code to allow for easier

15 main: :41 3(15) 1.3 Thesis Outline 3 Figure 1.2. The FSN GUI showing the main chart window where simulation is shown, the overview chart window to the right and the radar plotter in the upper right corner. All radar echoes are also shown in the main chart window, overlapping the chart. updates and extensions of FSN, it was decided to reduce the time spent on implementing the collision warning part of the system, hence, a simplified collision warning algorithm was implemented. 1.3 Thesis Outline Chapter 2 gives an introduction to how the FSN system is implemented and how the different parts of the simulation environment works. Chapter 3 describes the theory of the Kalman and extended Kalman filters used for state estimation in the tracking algorithm. Chapter 4 introduces the reader to the problems of target tracking and describes the different parts of a tracking system. Chapter 5 describes the design of the tracking and collision warning system implemented during the course of this thesis meanwhile Chapter 6 shows the results of the testing performed on the implemented system. The last chapter, Chapter 7, concludes the thesis and gives suggestions of fu-

16 main: :41 4(16) 4 Introduction ture improvements or extensions that can be made to the system.

17 main: :41 5(17) Chapter 2 Simulation Environment This chapter describes the basic functionality of the FOR Shore Navigation (FSN) system and how the environment, ship, and radar models are implemented. It is primarily intended as an introduction to the general system design for those who are interested in developing the system further or for those who wants to know more about the functionality of the simulation environment. The design of the tracking and collision warning system, which is the main part of this Master s thesis, is described in Chapter 5 and only mentioned briefly in Sections and The first section of this chapter, Section 2.1, describes the structure of the FSN system and the main function classes within the system. Section 2.2 gives an overview over how the vectorized nautical chart, used as basis for the simulation environment, is designed. In Section 2.3 the ship model used for simulation is explained and in the last section, Section 2.4, the functionality of the radar simulator is described further. 2.1 System Overview The FSN system is implemented in matlab and is a system for simulating ship movement in an archipelago environment. The main parts of this system is the simulation environment made out of a vectorized nautical chart, the ship route generator used for simulating ship movement, and the radar simulator generating observations that can be used in the particle filter for positioning and in the tracking system. Besides the vectorized nautical chart a rasterized chart is also available for viewing, however, simulation can not be displayed when using this chart. Figure 2.1 shows the difference between the vector and raster charts. The vectorized chart is made up from points defining the edges of for instance land, meanwhile the raster chart consists of and image of an ordinary nautical chart scanned into the computer. To be able to explain the functionality of the system further a quick introduction to simulation in FSN is given in Section The flow of the simulation is then explained in Section and the functionality of the different function classes briefly explained in Section

18 main: :41 6(18) 6 Simulation Environment Figure 2.1. A raster chart shown to the left and the corresponding vector chart shown to the right. The vector chart is made up of points defining the edges of objects in the environment, for instance land, meanwhile the raster chart is an ordinary nautical chart scanned into the computer How to Create a Simulation in FSN To create a simulation in FSN the user needs to create routes of waypoints for the ships that are to take part in the simulation. There are currently functionality for using four ships in the simulator, one of them being the main or own ship that always has to take part in a simulation, but this number can easily be extended to accommodate the needs of the user. Waypoints are added by selecting a ship, for which waypoints should be edited, and then using the mouse to point out waypoints in the main chart window of the GUI shown in Figure 1.2. When a route has been created the user is asked to define which speeds the ship should have while traveling between the defined waypoints. Figure 2.2 shows a part of the main chart window where waypoints for two ships have been added. Now the basis for a simulation has been created. But before starting a simulation, editing of a ships parameters, such as the length and width of a ship and settings for the maneuverability of a ship, is also possible. When the user is satisfied with the simulation settings, the simulation can be started Simulation Process When a simulation is started a route generator takes the defined waypoints and speeds for the different segments between waypoints and uses them to create a ship route, where the ship s dynamics has been applied. A route is a list of coordinates in which the ship will be at some part of the simulation. Figure 2.3 shows the routes that are generated for the waypoints defined in Figure 2.2. The ship speed has in this case been set to 15 knots between all waypoints. During the simulation the different ships are then moved along these routes and the radar, which has its origin in the main ships center coordinate, computes radar echoes from land and other ships along the route. The radar returns are presented in the radar plotter

19 main: :41 7(19) 2.1 System Overview 7 Figure 2.2. Part of the GUIs main chart window, where two ship routes, each defined by four waypoints, have been added. and overlapping the chart in the main chart window, both seen in Figure 1.2. The radars observations are used in both the particle filter for positioning, developed in [13], and in the tracking and collision warning system designed during the work with this Master s thesis FSN Function Classes The originalfsn systemwascontainedin onelargematlab file with all simulation functionality in it. To facilitate further extensions of the system, and to make it easier to understand the functionality of the code, the systems original "main" file was split into several function classes and a lot of code comments were added. The new structure of the platform, and the linkage between the function classes during simulation, is shown in Figure 2.4. A brief explanation of the functionality contained in each function class is now given: FORShoreNav.m The main program which starts the FSN system, initializes global variables, and initiates the GUI (navgui.m). navgui.m File that contains all code that has to do with the FSN GUI functionality. navchart.m The navchart.m file handles all function calls concerning the nautical chart. It contains functionality for loading, updating, plotting, and zooming in and out of the nautical chart. Depending on which type of chart that is currently selected, navchart.m calls the appropriate functions in its subfunc-

20 main: :41 8(20) 8 Simulation Environment Figure 2.3. The route generated from the waypoints in Figure 2.2. The speed of the ships is in this case set to 15 knots between all waypoints. tion classes, vectorchart.m and rasterchart.m. The difference between the two chart types is shown in Figure 2.1. vectorchart.m Contains functionality for loading and plotting the vector chart used in the FSN system. rasterchart.m Contains functionality for loading and plotting the raster chart used in the FSN system. navwaypoints.m This function class contains functionality for editing ship waypoints. The user can edit waypoints and define which speed a ship is to have when a certain waypoint is passed. navroute.m The navroute.m file contains functionality for generation of ship routes from user defined waypoints. The route generation function is the part of the FSN system that simulates ship dynamics, both for changes in heading and speed. These dynamics are explained further in Section 2.3. navsimulate.m This function class controls all actions that are to be taken during simulation in the FSN system. It moves the ships along their routes, starts radar simulation, and engages the tracking and collision warning system. navradar.m This class contains all functionality for radar simulation. It finds both land

21 main: :41 9(21) 2.1 System Overview 9 ForShoreNav.m navgui.m navwaypoints.m navroute.m navsimulate.m navchart.m navradar.m vectorchart.m rasterchart.m navtracker.m navestimate.m readobjects.dll navcollision.m exportbitchart.dll Figure 2.4. FSN System overview. Shows the function classes available in FSN and the linkage between them during setup of simulation and during simulation, where waypoints are defined using the main chart and the radar uses the chart to simulate radar observations. and ships within the radars range and computes the distance, range rate, and azimuth angle to the different observations. navtracker.m This is the class containing the tracking system implemented during the work with this thesis. navcollsion.m This class contains functionality for the collision warning system also implemented as a part of this thesis. navestimate.m Position estimator linked to the particle filter developed in [13] Functions in C++ The FSN system also uses a few functions implemented in C++. One that handles the extraction of vector chart data from the standardized vector chart format, described in Section 2.2, and one that exports the vector chart to a bitmap file used in navestimate.m. However, the functionality of these files are not explained further here since these functions have not been altered or used during the course of this thesis.

22 main: :41 10(22) 10 Simulation Environment 2.2 Vectorized Nautical Chart The vectorized nautical chart used in FSN is based on the S-57 standard, [10]. The S-57 standard describes how digital hydrographic data should be stored when exchanging data between hydrographic offices. This is also the standard used when supplying this data to, for instance, manufacturers and other users. Exactly how the data for the vectorized nautical chart, used in FSN, is stored is not presented here since no alterations has been made to the original data structure. This section instead introduces the general features of a vectorized nautical chart. In general, vector chart information can be said to consist of three types of spatial objects: points, lines, and areas. Each spatial object contains information about the geographic location of the object so that it can be drawn at its correct location in the map. Other types of information can also be supplied with the object and used for plotting in the chart. To explain further three examples are given, one for each type of spatial object. Apoint:An example of a point object is a lateral mark or other types of buoys that can be described with a single map coordinate, this coordinate describes the position of the buoy. Point attributes can then for instance describe which type of mark or buoy the coordinate corresponds to. A line: A line is, as can be expected, a set of at least two points. Object that can be described as lines are for instance tracks marking the center of a shipping lane or lines marking the different sectors of a beacon. An area: One of the most common types of objects described as areas are land objects. Islands in an archipelago and the main land are all described as areas, where the points belonging to the land object marks its outer edges. Attributes that can be supplied with land objects are for instance the names of islands and so forth. For a further explanation of the S-57 standard and how different spatial objects are defined, see [10] WGS84 and RT90 There are two chart coordinate formats available in FSN, the WGS84 and the RT90 formats. The WGS84 format is the format commonly used in nautical charts world wide. A point in space is here defined by its longitude and latitude, each of which is described by degrees, minutes, and seconds. The RT90 coordinate frame on the other hand is a system specifically made for Sweden. The RT90 system consist of a grid with reference points over Sweden and is used by the Swedish surveying office (in Swedish: Lantmäteriet) when making maps. The coordinates in the RT90 format are given as in an ordinary cartesian coordinate frame, where in this case each coordinate corresponds to the distance in meters to a certain reference point (origin).

23 main: :41 11(23) 2.3 Ship Model Ship Model Since ship movement in the FSN system is based on the ship following user defined waypoints, the ship s motion model corresponds to an autopilot following these waypoints. The autopilot currently implemented is based on a simple PI-controller, where the user can modify ship dynamics by altering the controller parameters. When editing waypoints the user is asked to specify a velocity vector, setting the reference speed for the route between two waypoints. Speed dynamics for a ship is also implemented as a PI-regulator. In the ships autopilot a simple form of line-of-sight (LOS) guidance principle is used, this means that each waypoint has a circle of acceptance surrounding it and when a ship enters into this circle, the next waypoint is activated. The circle of acceptance is expressed as a number of ship lengths from the current waypoint. The definitions for ship heading, reference heading, and a waypoint circle of acceptance are shown in Figure 2.5, meanwhile the structure of the autopilot is shown in Figure 2.6. Worth mentioning is that N a wp δ ψ ref ψ k E Figure 2.5. A ship with heading ψ k and the waypoint wp seen at heading ψ ref. The waypoint has a circle of acceptance with radius a and the course error of the ship is marked δ. polar angles are used internally in the system, but the course presented in the GUI is the ordinary ship course with north at 0,eastat90, and so forth. In addition to setting parameters for ship movement the user can also define the length and width of all ships. The length and width setting defines the ships edges when calculating radar echoes. 2.4 Radar Simulator The radio detection and ranging (radar) sensor is used to scan and provide information about the environment surrounding the main ship. Each observation made by the radar results in information about the distance, range rate, and azimuth angle of the observation. The radar sensor in FSN is situated in the center of the

24 main: :41 12(24) 12 Simulation Environment Estimated position (GPS, particle filter) Waypoint Speed ψ r v r ψ k Ship dynamics v k Figure 2.6. Ship autopilot. If the ships current position is within the circle of acceptance of a waypoint the next waypoint is activated, leading to a new reference heading and speed. The reference heading is always computed using the last known ship position, i.e., it is updated at each time step since the ship moves. main ship and is defined by two variables, the range and lobe angle. The range is the longest distance in which the radar can find observations, meanwhile the lobe angle defines the resolution of the radar. For instance, if tracking two targets that are close to each other, a large lobe angle can result in the radar only receiving one observation, i.e., the two targets are seen as one since the radar only retrieves one observation within each lobe. In Figure 2.7 the lobe angle is seen as the width of the radar beam sent towards land. Both the range and lobe angle can be defined by the user in the FSN system. Land β Land r Land Figure 2.7. The radar, situated in the center of the main ship, represented by the black center dot and the radars range shown as the dotted circle. The square marks the edges of the search area in which the radar simulator searches for land or ship vectors. The lobe angle β is defined as the width of the radar beam.

25 main: :41 13(25) 2.4 Radar Simulator 13 Land Figure 2.8. Shows how a vector is gradually rotated towards one of the axises. When the vector crosses the axis, the range r to the crossing and the azimuth angle ϕ of the observation can be computed Radar Search Algorithm The radars search algorithm is applied to the vector chart to obtain observations from the main ships surrounding environment. When searching for observations the user defined radar range setting is used to limit the search. A search square, seen in Figure 2.7, is set up to mark the outer limit of the search area. Since the position of the own ship is assumed to be known, and the radar is positioned in the center of the ship, the coordinates defining the edges of the search square can be computed and used when searching for observations. Using the edge coordinates, the radar simulator checks the database of the vectorized nautical chart and retrieves all land vectors within the given area. The simulator also checks for other ships within the search area and if it finds any, the edges of these ships, i.e., the vectors that define their outer limits, are also added to the list of found vectors. Having gathered all vectors of interest, the simulator starts to go through this list to compute the range and azimuth angle of the observations that can be found on each vector. This is done by rotating the vectors around the origin in steps of 0.5 at a time, until a rotation of 90 is achieved, and checking if any of the vectors crosses one of the positive or negative X-Y axises. If a vector crosses an axis the range can be computed, and since it is known which axis the vector crossed, this can easily be found out by checking the coordinates of the points defining the vector, the azimuth angle, given as a polar angle, of the observation can be computed by checking how many degrees the vector has been rotated. Figure 2.8 shows how a vector is rotated down towards the positive X-axis. Since the search area has the form of a square, vectors further away from the radar

26 main: :41 14(26) 14 Simulation Environment than the specified radar range might sometimes be found. To remove observations from these vectors, only observations within the radars range are allowed to be stored. Also, if there are several vectors having the same azimuth angle to an axis crossing, the observation having the smallest distance is stored. When the 90 rotation of all vectors has been done the observations made can be found in a list where the minimum difference in azimuth angle between the observations is 0.5. The reason for rotating the vectors at a such small angle each step is to be able to find small targets at long distances. Using a larger rotational angle could lead to vectors passing one of the axes without ever crossing in, resulting in a lost observation. To lower the resolution of the radar the lobe angle is now applied to the list of found observations, picking out the observation closest to the radar within each lobe. For instance, applying a lobe angle of 2 to the list means removing three out of four observations, since each lobe contains four 0.5 sectors. Figure 2.9 shows the difference between the search angle α and the lobe angle β. The reduced list of radar observations is then passed on to the radar plotter, the positioning system and the tracking system. α Land β Figure 2.9. The difference between the search angle α and the radar lobe angle β. The smaller search angle is used to prevent lost updates when searching for observations on small vectors at long ranges Range Rate Computation All vectors found within the radar search area are marked so that the radar system can see from what type of object the vector originates. The mark can either be set to indicate a land vector or to indicate a specific ship. This marking is used to when computing the range rate measurement belonging to the observation. When computing the range rate it is assumed that the effect of the own ship speed on the range rate is compensated for, i.e., a land echo should have a range rate of approximately zero meters per second no matter what speed the own ship has. The range rate of a moving ship on the other hand depends on the ships heading. Figure 2.10 shows a scenario where another ship is in the vicinity of the main ship.

27 main: :41 15(27) 2.4 Radar Simulator 15 The range rate computations for an observation originating from this ship looks like: v ς ṙ λ Figure The azimuth angle of the observation, shown as λ, and the speed and heading of the ship for which and observations range rate should be computed shown as v and ς. The resulting range rate measurement marked with r. ṙ = v cos(ς λ) (2.1) where v is the speed and ς the heading of the ship. The azimuth angle of the observation is shown by λ and the result of the range rate computation indicated by the ṙ vector Radar Measurement Noise Model The measurement noise accounts for uncertainties in the observations that the radar makes. The values obtained from the search algorithm and range rate computation are exact values, something that a real radar seldom generates because of disturbances from the surrounding environment. To make the computed measurements seem more like the ones received from a real radar, white noise with the following standard deviations are added to their values: Radar range (r) : ±10 m. Radar range rate (ṙ) : ±1 m/s. Radar azimuth angle (ϕ) : ±1. These values are based on the assumption that although the specifications of radars usually present a larger error in for instance the range measurement, these errors are due to a constant bias in the radar sensor and can not be interpreted as the standard deviation of the measurements.

28 main: :41 16(28) 16 Simulation Environment Radar Clutter Model A radar sometimes generates observations that do not originate from either land or other ships within the radars range, these observations are called clutter. Clutter observations can originate from a lot of different sources. Two of the more usual ones are breaking waves or rain reflecting the radars search beam and causing an observation to be registered. Since the nature of clutter is random, the clutter model in FSN is based on the clutter being uniformly distributed over the radars search area. The clutter level can be set by the user and the parameter corresponds to the number of clutter observations the user wants the radar to receive within a square kilometer. When the clutter observations has been distributed over the search area, the range and azimuth angle of the observations are computed and a range rate ranging from ±2 m/s is added before adding the clutter to the list of radar observations. The level of the range rate has been set so that an observation sometimes can be mistaken for that of a ship, which is good when for instance testing the tracking algorithm developed in Chapter 4.

29 main: :41 17(29) Chapter 3 Estimation Methods In target tracking applications a large part of the tracking problem consists of estimating target parameters such as target position, speed, and acceleration in a good way given noisy measurements. There are several different methods to do this, one of the more common is the use of a Kalman filter for state estimation. This chapter presents, in short, the Kalman filter algorithm used for estimation of linear state-space models and then shows how this algorithm can be modified to handle nonlinear state-space models, an approach called the extended Kalman filter. 3.1 The Kalman Filter For linear systems, where the state-space model is subject to Gaussian process and measurement noise of known statistics, the Kalman filter (KF) constitute the optimal filter for state estimation and state prediction, [9]. Consider the statespace model: x k+1 = F k x k + G k w k (3.1a) y k = H k x k + e k (3.1b) where w k and e k are assumed to be zero-mean Gaussian noise with covariance matrixes Q k and R k. The optimal solution to the state estimation problem is then given by Algorithm 1, where ˆx k+1 k represents the predicted state estimate and P k+1 k is the covariance matrix for the prediction error. S corresponds to the covariance matrix, P, transformed into the measurement domain, i.e., the covariance, or uncertainty, of the incoming measurement, and y k is the set of measurements received at time k. The flow of the algorithm is that given an initial value, the next filter state is predicted using (3.4a) and (3.4b). From these predictions the measurement covariance, S k, and Kalman filter gain, K k, can be computed and used to update the state estimates (3.6a) and to adjust the covariance matrix P (3.6b) when the next set of measurements arrive. To initialize the filter algorithm the expected 17

30 main: :41 18(30) 18 Estimation Methods means: ˆx 0 1 = E[x(0)] = x 0 (3.2a) P 0 1 = E[(x(0) x 0 )(x(0) x 0 ) T ]=Π 0 (3.2b) are used as initial values. Algorithm 1 Kalman filter (KF) Initial values: Time update: ˆx 0 1 = x 0 P 0 1 =Π 0 ˆx k+1 k = F k ˆx k k (3.4a) Filter gain computation: P k+1 k = F k P k k F T k + G kq k G T k (3.4b) S k k 1 = H k P k k 1 Hk T + R k K k = P k k 1 Hk T S 1 k k 1 (3.5a) (3.5b) Measurement update: ˆx k k =ˆx k k 1 + K k (y k H k ˆx k k 1 ) P k k =(I K k H k )P k k 1 (3.6a) (3.6b) where Q k = Cov(w k ) R k = Cov(e k ) (3.7a) (3.7b) 3.2 The Extended Kalman Filter The previous section described the KF for a linear state-space model. However, in many cases the state-space model consists of nonlinear functions in either the state update or measurement update equations and the regular KF can not be applied directly. One solution to this problem is to linearize the system around its latest state estimate and then applying the KF to the linearized state-space model. This approach is referred to as the extended Kalman filter (EKF), [7, 11].

31 main: :41 19(31) 3.2 The Extended Kalman Filter 19 Consider the state-space model: x k+1 = f(x k )+g(x k )w k (3.8a) y k = h(x k )+e k (3.8b) where the functions f(x k ), g(x k ),andh(x k ) represent nonlinear functions of the state vector and noises w k and e k are assumed to be zero-mean and Gaussian with covariance matrixes Q k and R k. To obtain a practical estimation algorithm f(x k ) and h(x k ) are approximated using first order Taylor series around the current state estimate ˆx k, meanwhile g(x k ) is expanded using a zeroth order expansion resulting in the following approximations: f(x k ) f(ˆx k k )+F k (x k ˆx k k ) (3.9a) h(x k ) h(ˆx k k 1 )+H k (x k ˆx k k 1 ) (3.9b) g(x k ) g(ˆx k )=G k (3.9c) where F k = f(x) x H k = h(x) x x=ˆxk k x=ˆxk k 1 (3.10a) (3.10b) Using these approximations, (3.8a) and (3.8b) can be expressed as: x k+1 = F k x k +(f(ˆx k k ) F k ˆx k k )+G k w k y k = H k x k +(h(ˆx k k 1 ) H k ˆx k k 1 )+e k which constitute a linear state-space model for x k. For the EKF the state update and measurement update now looks like: ˆx k+1 k = F k ˆx k k +(f(ˆx k k ) F k ˆx k k )=f(ˆx k k ) ˆx k k = ˆx k k 1 + K k (y k (h(ˆx k k 1 ) H k ˆx k k 1 ) H k ˆx k k 1 ) = ˆx k k 1 + K k (y k h(ˆx k k 1 )) which represent linear equations that can be used in the KF. The complete set of computations for the EKF or Schmidt EKF is given in Algorithm 2.

32 main: :41 20(32) 20 Estimation Methods Algorithm 2 ExtendedKalmanfilter(EKF) Initial values: Time update: Filter gain computation: ˆx 0 1 = x 0 P 0 1 =Π 0 ˆx k+1 k = f(ˆx k k ) (3.12a) P k+1 k = F k P k k Fk T + G k Q k G T k (3.12b) S k k 1 = H k P k k 1 Hk T + R k K k = P k k 1 Hk T S 1 k k 1 (3.13a) (3.13b) Measurement update: where and ˆx k k =ˆx k k 1 + K k (y k h(ˆx k k 1 )) P k k =(I K k H k )P k k 1 F k = f k(x) x H k = h k(x) x x=ˆxk k x=ˆxk k 1 Q k = Cov(w k ) R k = Cov(e k ) (3.14a) (3.14b) (3.15a) (3.15b) (3.16a) (3.16b)

33 main: :41 21(33) Chapter 4 Target Tracking and Data Association Target tracking is the problem of establishing tracks and estimating the states of the targets being tracked. Areas where target tracking is of great importance is for example within commercial air traffic, where flight controllers need to keep track of the airplanes currently present in their airspace to avoid collisions. Another area is within the military industry where finding and tracking enemy movement is of interest. Usually the target states to be estimated involve position and speed, but sometimes acceleration is also of interest. The sensors used for tracking targets are often different type of radar sensors, but there are several other types of sensors that can be used for tracking targets as well, for instance the passive sonar and IR sensors. The vast area of target tracking and data association offers many approaches and theories for creating a good tracking system. In this chapter some of the more commonly used theories and solutions to the tracking problems are presented. In Sections 4.1 and 4.2 the tracking sensor and tracking coordinate system used throughout this Master s thesis is introduced, meanwhile Section 4.3 offers an introduction to different ways of modelling target movement. In [2] a track is said to have three life stages: tentative, confirmed, and deleted, each associated with its own set of problems. A solution to the problem of track deletion is presented in Chapter 5, meanwhile the problem of track initiation using tentative tracks, and the problem of data association for confirmed tracks, are described in Sections 4.4 through 4.6 of this chapter. 4.1 Tracking Sensor The radio detection and ranging (radar) sensor is one of the most commonly used sensors in target tracking. A good book describing the functionality of different radar systems is [17]. The functionality of the radar used in the FSN system is described in detail in Chapter 2 and only the measurement equations are presented 21

34 main: :41 22(34) 22 Target Tracking and Data Association (Y) N (40,50) (0,0) E (X) Figure 4.1. The relative Cartesian coordinate system used for tracking showing the own ships position in the origin, the positive Y-axis along the northernly direction, the positive X-axis in the easterly direction and an example of another ships position in the first quadrant, 40 meters east and 50 meters north of the own ship. in this section, being: y k = ṙ r ϕ = h(x k )+e k (4.1) where r is the range to an observation, ṙ is the range rate measurement (derived from the Doppler shift), and ϕ is the azimuth angle to the observation. There are, as said in the introduction to this chapter, many different types of tracking sensors available, but since only the radar sensor has been of interest considering the application developed, no other sensors are presented here. 4.2 Tracking Coordinate System The Cartesian coordinate system in two dimensions is used in the tracking system developed. The coordinates used are relative coordinates, meaning that a targets position is given relative to the own ship position, which is always set to the origin. The Y-axis corresponds to the northerly direction, meanwhile the X-axis is set in the easterly direction. Figure 4.1 shows the definition of the relative Cartesian coordinate system. The advantage of using the Cartesian coordinate system for tracking is that this allows for the use of linear target dynamic models for extrapolation, [2]. For example, given the estimates of a targets position and velocity, one can easily predict the path of a target since the velocity estimates Ẋ and Ẏ combined reveals the heading of the target. A prediction of the targets next position can then easily

35 main: :41 23(35) 4.3 Target Dynamic Models 23 be done via the simple linear equations: X k+1 = X k + T Ẋk (4.2a) Y k+1 = Y k + T Ẏk (4.2b) where T is the time step of the prediction. There are however a couple of disadvantages of using the Cartesian coordinate system for tracking. One of them is that the measurement errors are coupled since an error in the measurements radar range, r, or radar angle, ϕ, effect both the relative X- andy -coordinate through the equations: X k = r k cos ϕ k (4.3a) Y k = r k sin ϕ k (4.3b) The effect this coupling has on the measurement covariance matrix is described further in [2]. Despite the previously stated disadvantage the Cartesian coordinate system was chosen for tracking since the pros of being able to easily predict movement overcame the cons of the coupling in the measurement errors. 4.3 Target Dynamic Models One of the more important choices that has to be made when designing a tracking system is the choice of maneuvering model. Without a good model describing the dynamics of how the objects being tracked can move in tracking space, any attempt to predict target movement would be futile. A good survey of different target dynamic models is given in [15] and in this section some of the simpler models from this survey are presented The Constant Velocity Model The constant velocity model is a non-maneuver model based on the assumption of a target having a constant velocity during the time it is being tracked, i.e., the target is not accelerating in any direction and heading along a straight path. When tracking in two dimensions this corresponds to Ẋ and Ẏ having constant values. Using this assumption, the target states can then be computed using (4.2a) and (4.2b), resulting in the following Cartesian state-space model: x k+1 = F k x k + w k (4.4) where x k = X Y Ẋ Ẏ and F k = 1 0 T T

36 main: :41 24(36) 24 Target Tracking and Data Association The white process noise, w k, is assumed to be zero since the target is moving at a known constant velocity. In practice however, the ideal equation is often modified so that w k 0 to account for unpredictable modeling errors and slight changes in target velocity The White-Noise Acceleration Model The white-noise acceleration model is the simplest of the models for tracking maneuvering targets and can be seen as an extension of the constant velocity model described in Section The only difference is that now the noise level, w k,is set to a higher value to accommodate the fact that a target might be accelerating. Updating the constant velocity model by allowing process noise to account for target acceleration results in the following two dimensional state-space model: where x k = X Y Ẋ Ẏ, F k = x k+1 = F k x k + G k w k (4.5) 1 0 T T , and G k = T 2 /2 0 0 T 2 /2 T 0 0 T and w k 0. Although the assumption of acceleration being an independent process with respect to time is far from correct in most cases, the model works well when tracking targets whose maneuvers are small or random The Singer Acceleration Model White noise processes are time-invariant, i.e., the value of the process at time t is uncorrelated with the value at time t + τ. Acceleration is however, as said in the previous section, usually not an independent process with respect to time. For instance, consider a target that is accelerating at a time t, then, assuming that τ is a small increment in time, the most likely event is that the target is still accelerating at time t + τ. This type of process, where the value at time t + τ can be assumed to be correlated with the value at the previous time t, is called a Markov process. The Singer model utilizes this correlation assumption by modeling acceleration a(t) as a zero-mean first-order stationary Markov process with autocorrelation: E[a(t)a(t + τ)] = σ 2 e α τ (4.6) where σ 2 is the variance of the targets acceleration and α =1/τ is the reciprocal of the maneuver time constant τ and therefore depending on how long the maneuver lasts. Translating (4.6) into a state process of a linear time-invariant system leads to: ȧ(t) = αa(t)+w(t), α > 0 (4.7)

37 main: :41 25(37) 4.4 Track Initiation 25 which in discrete time corresponds to: a k+1 = βa k + w a k (4.8) where w a k is a zero-mean white noise process with variance σ2 (1 β 2 ) and β = e αt. Adding acceleration to the tracking states and using (4.8) yields the following Singer state-space model for one coordinate: x k+1 = F k x k + G k w k (4.9) where x k = X Ẋ Ẍ, F k = 1 T (αt 1+e αt )/α (1 e αt )/α 0 0 e αt, G k = T 2 /2 T 1 where the exact expression for the covariance of w k can be found in [16]. Depending on how the maneuver time constant τ is chosen the Singer model can vary between being a nearly constant velocity model as in Section 4.3.1, small τ resulting in a large αt, or a constant acceleration model, large τ resulting in a small αt, were the acceleration i assumed to be "nearly constant", [15]. 4.4 Track Initiation Before any tracking can begin targets have to be identified and tracks initiated to follow these targets. This is usually a difficult task, especially if tracking in a cluttered environment, and a variety of solutions to this problem are available, both simple and more complex. In this section two methods for track initiation will be described. The first is a combined variant of the logic-based multi-target track initiator and the 2/2&m/n track initiation method, both presented in [12]. The second is a modified Hough transform initiator, introduced in [3] A Logic-Based Multi-Target Track Initiator In this approach, which is a combination of the two logic based multi-target track initiators presented in [12], the track initiator consists of a logic for evaluating tentative tracks and establishing tracks that pass this logic. An outline of the 2/2&m/n initiation logic is shown in Figure 4.2, where m =2and n =3. The notation is that D marks that a measurement has been detected and N implicates a missing or lost measurement detection. The nodes S 0 S 7 represent the possible whereabouts in the decision logic, where the gray nodes indicate that a decision about whether or not to continue track initiation, node S 2, or whether or not a track can be established, node S 7, can be made. The logic works as follows: For each measurement at scan k, nodes 0, an acceptance region is set up for associating measurements from scan k +1,nodeS 1, using prior knowledge about a targets possible maximum and minimum velocity along a coordinate l, vmax l and vmin l. The process can be described as follows.

38 main: :41 26(38) 26 Target Tracking and Data Association N N S 3 D S 6 N D S 0 D S N 1 S 2 N D S 4 N D D S 7 D N S 5 Figure 4.2. The 2/2&m/n track initiation logic (m=2, n=3). Let y (i),l k be coordinate l of measurement i at time k and consider only two successive scans, k =0, 1. Then, having a priori knowledge of the maximum and minimum velocities along coordinate l the association distance can be computed as: d l ij = max{y (j),l 1 y (i),l 0 v l maxt,0} + max{ y (j),l 1 + y (i),l 0 + v l mint,0} where T is the time interval between the scans and l =1, 2,..., m, m being the dimension of the measurement vector. When this is done, assuming the measurement errors to be independent and zero-mean with covariances R (i) 0 and R (j) 1, the test statistic will be: D ij = d T ij(r (i) 0 + R(j) 1 ) 1 d ij (4.10) where the measurement from scan k+1 will be associated to the measurement from time k if: where γ is a χ 2 distributed probability threshold. D ij <γ (4.11) For each pair of measurements from time k and k +1 passing the association test, a Kalman tracking filter is set up at node S 2,usingthesetwo measurements for initiation. The track is now said to be tentative. The tentative track is then tested further and out of the next three measurements, at least two must lead to an observation being so close to the track that it is associated, otherwise the track is dropped. If the tentative track passes this two-out-of-three test, the track is said to be established, or confirmed, and is promoted out of the initiation logic, node S 7.

39 main: :41 27(39) 4.4 Track Initiation 27 Y (1,5) C (2,4) B ρ (5,1) A θ X Figure 4.3. Normal parametric representation of a straight line passing through points A, B, and C. Figure 4.4 shows the result of applying the Hough transform to these points Modified Hough Transform Track Initiator The Hough transform is a well known technique for finding straight lines in pictures. In [3] a detailed description of a modified version of the Hough transform track initiator is presented. In this thesis however, only the main idea behind the Hough transform and its use in track initiation is presented. The Hough transform is based on the normal parametrization of a line, where a line is specified by its distance ρ from the origin and the angle θ of its normal. Figure 4.3 shows the normal parametric representation of a straight line. The transform maps points (x, y) in the Cartesian coordinate system into the ρ θ parameter space through the relationship: ρ = x cos θ + y sin θ (4.12) where θ is ranging from 0 to π. Doing this to several points, or in the case of a radar sensor, several position measurements, will show that points on a straight linesharethesameρ and θ parameters. A point in the Cartesian coordinate system will be described as a curve in ρ θ parameter space, meanwhile a point in ρ θ space will represent a straight line in the Cartesian coordinate system. Figure 4.4 shows the curves generated by the Hough transform applied to the points in Figure 4.3. It can here be seen that their intersection point at ρ 4.25 and θ 45 correspond to the correct parameters of the straight line in Figure 4.3. To find these intersections the ρ θ space is dicretized into a 3-D histogram using θ and ρ as step size, making N θ = π/ θ the number of increments along the θ-axis while N ρ is dependent on the maximum possible distance to the origin. Each point in Cartesian space is then transformed and for each θ i,where i =1, 2,...,N θ, and where: θ 1 = θ 2,θ 2 = 3 θ 2,...,θ n θ =(N θ 1 2 ) θ

40 main: :41 28(40) 28 Target Tracking and Data Association 6 4 A B C Range ρ in Hough Transformation Intersection corresponding to the line passing through points A, B, and C in Figure Angle θ in Hough Transformation (degrees) Figure 4.4. The points from Figure 4.3 transformed into Hough parameter space. Point A as a solid line, point B as a dotted line and point C as the dashed line. The curves intersection corresponds to the line in Figure 4.3. represents the center point of the histogram bins, the value of the bin corresponding to the resulting ρ is incremented. After a few scans, the bins in the histogram corresponding to straight line relationships will have accumulated more points than those who only get hits from clutter. An example of a histogram is shown in Figure 4.5. Peaks in the histogram indicate possible tracks and by applying a suitable threshold to the histogram, bins that exceed this threshold can be assumed to correspond to tracks along the straight line defined by the ρ and θ parameters of that particular bin. For a further explanation of the Hough transform the reader is referred to [3] where several testing criterions for tentative tracks are presented. 4.5 Gating When a track has been initiated the next problem kicks in, track maintenance. With each new radar scan the tracking algorithm receives observations of which only a part usually can be associated with running tracks, the rest is clutter or candidates for new tracks. To reduce the risk of faulty track updates, a procedure called gating is used. Gating eliminates unlikely observation-to-track pairings before data association is done. This procedure also reduces the amount of computation that has to be done each time new measurements arrive since only relevant data is processed. There are several gating techniques available and in this section one of the more adaptive gating techniques, the use of ellipsoidal gates, [1, 2], is presented.

41 main: :41 29(41) 4.5 Gating ρ range (m) θ angle (degrees) Figure 4.5. A Hough histogram generated over ten scans clearly showing a peak representing a straight line relationship between some of the measurements. The data was generated from a target heading in a northeasterly direction at a distance of about 200 meters Ellipsoidal Gates The ellipsoidal gating technique, described in [2], utilizes the covariance matrix S of the measurement prediction (3.13a) to eliminate unlikely observations-to-track pairings. The gate is set up as a maximum likelihood gate so that only observations that satisfy the gating criteria: d 2 =ỹ T S 1 ỹ G (4.13) where ỹ is the residual vector for the observation, are taken into evaluation during data association for a track. Figure 4.6 shows how elimination of unlikely observations is done with the help of an ellipsoidal gate. In [2], two ways to define G are presented. The first is more adaptive but require some knowledge about the probability of detection, P D, and the new source density β. The second method is instead based on the assumption of Gaussian process- and measurement noise leading to d 2 having a χ 2 M distribution, M being the measurement dimension. For the first method G is computed as: ( ) P D G 0 =2ln (1 P D )β(2π) M/2 (4.14) det(s) where det(s) is the absolute value of the determinant for the residual covariance matrix. Using the simpler χ 2 M method and allowing a probability P G =(1 P G ) of a valid observation falling outside the gate, the value of G can be determined from a χ 2 M -table as follows: Prob{χ 2 M >G} = P G (4.15)

42 main: :41 30(42) 30 Target Tracking and Data Association x x x x x x Figure 4.6. Observations marked as x, the predicted position of the target marked by the black center dot, and the covariance of the measurement prediction S seen as an ellipsoid around the targets predicted position. Observations within the ellipsoidal gate fulfill the gating criteria given by (4.13). where P G is the probability that a valid observation will fall within the gate. 4.6 Data Association Data association is the process where gated observations are summarized, or weighted, into a single residual that can be used during filter update. As within all areas of target tracking there are several methods for data association available. Two of the more popular ones however is the all-neighbor approach called probabilistic data association (PDA) and its extension to multiple targets called joint probabilistic data association (JPDA). In this section the PDA and JPDA filters are explained further and a modified version of the PDA, which handles the multi-target case better then the ordinary PDA, is also explained. As an introduction to data association the simple nearest neighbor (NN) filter is also presented. Worth mentioning is that both the PDA and the JPDA association methods only can be used on already established tracks and therefore are in need of a separate track initiation logic such as those described in Section The Nearest Neighbor Filter The nearest neighbor (NN) filter, presented in [1], is one of the simplest methods for data association. It is based on associating the observation closest to the predicted target position and using this for filter updating. The distance measure that is used for finding the closest observations is the same as that used in the gating criteria for ellipsoidal gates (4.13): d 2 =ỹ T S 1 ỹ

43 main: :41 31(43) 4.6 Data Association 31 where ỹ is the residual for an observation and S the covariance of the measurement prediction. A variant of the NN filter is the global nearest neighbor (GNN) filter which minimizes the total distance when there are several targets for which data association is performed, i.e., the combination of observation-to-track associations that generates the shortest total distance is the one chosen for association. The NN approach for data association is simple, but performs poorly when used in a cluttered environment since no considerations to the origin of observations are taken into account The PDA Filter The PDA filter is an all-neighbor approach for data association and a good method to use when tracking a single target in clutter. Since the PDA filter considers the fact that observations might originate from other sources then the target being tracked, several different probabilities have to be defined before the filter can be presented: P D = Probability of detection β NT = Probability of new target β FT = Probability of false target (clutter) β = β NT + β FT where β is assumed to be the Poisson density for extraneous returns. Using these probabilities and given N observations that fulfill the gating criteria for a track i, N +1 hypothesis can be formed. One hypothesis for each observation concerning whether or not the observations originate from the track in question, and one hypothesis, H 0, for the case in which none of the observations are valid for track update. As shown in [2], the probability H 0 is proportional to p i0,where p i0 = β N (1 P D ) (4.16) The probabilities for hypothesis H j (j =1, 2,...,N) to represent valid returns are then proportional to the Gaussian density (likelihood) function: 2 y p β N 1 P D e d 2 ij = (2π) M/2, j =1, 2,...,N (4.17) det(s i ) where d 2 y is given by (4.13). The probability for each hypothesis is then computed through the normalization equation: p ij = p ij N l=0 p il (4.18) When all probabilities has been computed it is time to merge the hypothesis. This is done by calculating the weighted sum of the residuals associated with each observation: N ỹ ki = p ij ỹ kij (4.19) j=1

44 main: :41 32(44) 32 Target Tracking and Data Association x x x x x x Figure 4.7. The square represents the weighted sum (4.19) of the observations inside the gate and its residual is used in the Kalman filter update equation. where ỹ kij = y kj H ˆx k k 1 y kj = observation j received at scan k Figure 4.7 shows the result of the PDA computation for the observations inside the gate. Given the weighted sum from (4.19) the standard Kalman filter update equation is used to update filter i: ˆx k k = ˆx k k 1 + K k ỹ ki (4.20) For the covariance matrix however, a modified update equation is used to account for the uncertainties in the update due to the fact that there are several observations inside the gate: P k k = P o k k + dp k (4.21) where Pk k o is the Kalman covariance that would have been used if there were only one observation inside the gate and dp k is an increment added to reflect the effect of the uncertain correlation. The equations for Pk k o and dp k are: Pk k o = p i0 P k k 1 +(1 p i0 )Pk k (4.22) N dp k = K k p ij ỹ kij ỹk T ij ỹ ki ỹk T i Kk T (4.23) j=1 where P k k is defined as the standard Kalman covariance update: P k k = (I K k H) P k k 1

45 main: :41 33(45) 4.6 Data Association 33 x (6) τ 3 x (5) x (2) x (1) τ 1 τ 2 x (4) x (3) Figure 4.8. A scenario showing three tracks, τ, and six observations, x, where observation (2) has fallen inside the gates of both track τ 1 and track τ The JPDA Filter When tracking multiple targets in a dense target environment, where the validation gates of two or more tracks often intersect, it has been shown in [6] that the PDA filter is outperformed by the joint probabilistic data association (JPDA) filter. The difference between the PDA and JPDA filter is the way that the association probabilities p ij are computed. The PDA filter computes the probabilities separately for each target, assuming that all measurements inside a gate can be associated to the target and disregarding the possibility of some of the measurement originating from other targets. The JPDA filter on the other hand computes the probabilities jointly across a set of targets, hereby accounting for the probability of a measurement originating from another track. The JPDA algorithm from [4] and [18] is outlined in Example 4.1. Example 4.1 Figure 4.8 shows a scenario where three targets are being tracked and where the gates of tracks τ 1 and τ 2 are overlapping. Inside the common region of these gates lies observation (2), which is eligible for updating both track τ 1 and τ 2. Applying the JPDA algorithm to this scenario results in the following computations having to be made: The first thing that is done is that a binary validation matrix, Ω, issetup using all observations, M, andalltracks,τ, which in this case, given three tracks (τ =3) and six observations (M =6), correspond to the matrix: Ω=[w ij ]= i =0, 1,...,τ j =1, 2,...,M where w ij =1indicate that observation j lies within the gate of track i

46 main: :41 34(46) 34 Target Tracking and Data Association and w 0j has all units, indicating that any of the observations could have originated from clutter or false alarms. Next, clustering is applied to the validation matrix, pairing all tracks that have overlapping validation regions and common observations into smaller clusters. For tracks where no association conflicts occur, i.e., tracksthat have no common observation with another track, the ordinary PDA filter is applied. In this case track τ 3 is free from conflict and data association for this track can be done via the PDA filter. For tracks τ 1 and τ 2 however, a clustered validation matrix is set up using all observations that passed the gating test for these tracks. The validation matrix for tracks τ 1 and τ 2 now looks like: Ω cluster (τ 1,τ 2 )=[w ij ]= i =0, 1, 2. j =1, 2, BasedontheΩ cluster matrix, data association hypothesis, or feasible events, ε are generated. Each feasible event is represented by a hypothesis matrix ˆΩ, whereˆω has the same size as Ω cluster but is restricted by the following assumptions: 1. An observation can only have one origin, meaning that an observation can only be associated to at most one target or clutter: ŵ ij =1 i i 0 2. No more than one observation can originate from a target: ŵ ij 1 j With the restrictions stated above applied to the Ω cluster matrix eight feasible events, ε(ˆω l )(l =0, 1,...,7), can be generated: ˆΩ 0 = , ˆΩ1 = , ˆΩ2 = ˆΩ 3 = , ˆΩ4 = , ˆΩ5 = ˆΩ 6 = , ˆΩ7 =

47 main: :41 35(47) 4.6 Data Association 35 The probability for each feasible event P (ε(ˆω l ) Z), wherez is the set of all observations received at the last measurement, is then computed using a formula given in [6]. The association probability, p ij, for observation i belonging to track j is then computed by summing the probability of all feasible events where the association, observation i to track j, occurs. For instance, the probability of observation 1 belonging to track 1 in this case is computed as: p 11 = P (ε(ˆω 1 ) Z)+P (ε(ˆω 2 ) Z)+P (ε(ˆω 3 ) Z) Having computed the joint probabilities these are used in the same way as in the PDA method, computing the weighted sum of residuals to get a combined residual for the filter update (4.19)-(4.23). As a last remark it can be said that when there are several tracks with overlapping validation regions, the JPDA algorithm leads to a combinatorial problem where the number of feasible events increase exponentially with the number of measurements and targets. In the case where there are no common observations however, the JPDA works in the same way as the ordinary PDA, under the assumption that a clustering algorithm is used The Multi-Target Modified PDA Since the JPDA filter outlined in Section is computationally complex, a simpler algorithm for data association in a multi-target environment was developed in [5] and further analyzed in [14]. This filter is a modified variant of the ordinary PDA filter, described in Section 4.6.2, handling the case with overlapping validation regions and common observations through a slight modification of the probability computations. represented the Gaussian probability of Remembering from (4.17) that p ij observation j originating from target i and from (4.18) that p ij represented the normalized version of this probability, a new multi-target PDA can be defined as: p ij β ij = (4.24) S ti + S rj where β ij is the new association probability used in the computation of the weighted sum of residuals (4.19) and: S ti = j S rj = i p ij (4.25) p ij p ij (4.26) where S ti is the denominator from (4.18) and S rj is an increment reducing the probability of β ij if observation j is inside several validation regions.

48 main: :41 36(48) 36 Target Tracking and Data Association In [14] it is shown that this filter perform much better in the case of multiple targets and overlapping validation regions then the ordinary PDA filter does. Although the JPDA filter is still better then this multi-target PDA, the computational load is significantly less for the modified PDA filter then for the JPDA filter.

49 main: :41 37(49) Chapter 5 System Design Chapter 2 gives a description of how the FOR Shore Navigation (FSN) simulation environment is implemented and how the vector chart, ship model, and radar simulator works. In this chapter the implementation of the tracking and collision warning systems is described and motivations given to why certain design decisions were made. Following the structure of Chapter 4 the different subparts of the tracking system, and design decisions concerning these, are explained in Section 5.1, meanwhile Section 5.2 describes the design of the collision warning system. 5.1 Target Tracking in FSN This section explains the design of the tracking system that has been implemented into FSN. Since most of the problems of target tracking and examples of solutions to these are explained in Chapter 4, only the choice of solution and motivation to why this solution was selected is presented here Tracking Sensor An addition to the tracking sensor presented in Sections 2.4 and 4.1 had to be made to differentiate radar observations coming from land from those coming from ships. This was due to the fact that the track initiator, described in Section 5.1.3, otherwise would start up a lot of false tracks where land echoes corresponded to stationary targets. The particle filter used for positioning is also interested in receiving only land echoes, since all other echoes only contribute to unnecessary disturbances leading to an inaccurate position estimation. To solve this differentiation a filter was created to split the different types of observations. Figure 5.1 shows the radar system with the separation filter. Currently the filter is implemented as a straight forward selection using the radar simulators internal marking made for each observation, telling the system from what type of object the observation originates. In this case only land observations are passed on to the particle 37

50 main: :41 38(50) 38 System Design Radar Simulator Land Echo Particle Filter Clutter Seperation Filter Ship Echo Tracking System Figure 5.1. The separation filter implemented into the radar simulator, where land observations are passed on to the particle filter meanwhile observations originating from clutter or other ships are passed on to the tracking system. filter meanwhile clutter and ship observations are passed on to the tracking system. A more realistic way of implementing this separation filter would have been to compute some sort of a likelihood for each observation, indicating from where it most likely originates. For instance, since the own ships position is assumed to be known, the land masses in the vicinity of the ship can be used to match observations originating from land. Another solution might be to differentiate using the range rates of different observations, however, this was not a part of the goal for this thesis and is left as future work Target Dynamic Model Since ships move rather slow compared to other objects often tracked, such as airplanes, the target dynamic model does not necessarily have to be complicated. Merchant ships, for instance, usually travel along straight lines and seldom make fast or sudden turns. Bearing this in mind, and choosing a maneuver model simple to implement, the white-noise acceleration model was chosen for the tracking filter. Remember from Section 2.4 that the radar sensor supplied information about an observations distance, r, range rate, ṙ, and azimuth angle, ϕ. Then choosing the target states to be the position and speed along the X and Y axis results in the equations for the Kalman tracking filter, given the white-noise acceleration model, looking like: x k+1 = Fx k + Ge k (5.1a) where and: x k = X Y Ẋ Ẏ, F = 1 0 T T , and G = T T 2 T 0 0 T y k = h(x k )+w k (5.1b)

51 main: :41 39(51) 5.1 Target Tracking in FSN 39 where y k = ṙ X2 + Y 2 r ẊX+Ẏ and h(x k )= Y X2 +Y ϕ 2 arctan ( ) Y X (5.1c) Since the measurement equation represents a nonlinear function of the target states, the EKF has to be used Track Initiation The choice of track initiator was based on the difficulty of implementation, where the logic-based multi-target track initiator, explained in Section 4.3.2, led to a simpler implementation then the Hough transform initiator. The Hough transform initiator also required more data to make a decision of wether or not to start up a track, five successive scans or more, then the logic-based initiator did, which only needed two observations that could be associated to start up a tentative track. The fact that the tracking filter was used as a part of the initiation logic in the logicbased multi-target track initiator also contributed to this choice, mostly because this leads to the filter converging faster towards the actual target states since it is started at an early stage. Another reason for not choosing the Hough transform initiator was the difficulty of finding a good form of range splitting for the Hough histogram range cells Gating To sort relevant observation-to-track pairings from irrelevant pairings ellipsoidal gates, described in Section 4.5, is used. Although the probability of detection, P D, and the new source density, β, has to be defined for use in the data association algorithm, the simpler χ 2 M based threshold for gating is used to keep down the number of variables effecting tracking performance. The measurement dimension M is in this case 4 and Table 1 shows how the gating value G should be selected foranumberofdifferent P G,where P G is the probability of a valid observation falling outside the gate: Table 1:Aχ 2 4 table showing selection of G given the probability P G. P G G Selecting the probability of a valid observation falling outside the gate to be 0.01, the gating criteria from (4.13) now looks like: d 2 =ỹ T S 1 ỹ which is the gating criteria implemented into the tracking system.

52 main: :41 40(52) 40 System Design Data Association Since the tracking system is to be used primarily for ship collision warning and the distances to targets are relatively short, the multi-target modified PDA described in Section should perform well. Also, given the fact that ships move very slow compared to for instance airplanes, the gates for different targets should be quite small and seldom overlap, not even in a dense target environment. The small risk of overlapping gates is because the Kalman filters uncertainty for a slow moving targets position is quite small since the target does not move long distances between observations. The modified PDA also offers a more direct implementation compared to the JPDA, described in Section 4.6.3, for which the combinatorics of making hypothesis matrices and computing the joint probabilities for the different events results in a complex implementation. The design parameters for the modified PDA algorithm is limited to the new source density, β NT, the probability of a false target (clutter), β FT, and the probability of detecting a target, P D. Since these parameters are unknown they are set to have a low impact on the performance of the tracking system, which in this case leads to the values: P D = 0.99 β NT = 0.01 β FT = 0.02 β = β NT + β FT =0.03 This means that if there is a target, the radar almost always detects it, P D =0.99, and that the chance of an observation inside a gate originating from a new target or clutter is quite low, β NT =0.01 and β FT = Track Deletion Track deletion is based on the number of lost updates a track has and can be defined by the user. A lost update means that a track had no observation inside its association gate during the last radar scan. In a densely cluttered environment the track deletion parameter should be set quite low, since a track might survive due to clutter observations inside the association gate. In a low clutter environment on the other hand the track deletion parameter can be set quite high to allow tracks to pass behind obstacles in the environment without being deleted. The track deletion parameter sets the amount subsequent lost updates a track can have before being deleted. 5.2 Collision Warning The collision warning system uses established tracks and the strength of the Kalman predictor to predict a target s future position. The system assumes that the own ships position, speed, and heading are known variables and at each time step the predictor computes the predicted position for all established tracks and

53 main: :41 41(53) 5.2 Collision Warning 41 T = 15 s. T = 30 s. T = 60 s. τ τ τ Figure 5.2. This figure shows how the Kalman filter predicts the position of a target, τ, and how the prediction error variance can be used to plot an ellipse in which the target can be after 15, 30, and 60 seconds. It also shows the straight line prediction used for the own ship. The rectangles in this case represents the own and tracked ships current position. for the own ship. The length of the prediction, or prediction horizon, is a user defined variable depending on how long before a collision the user wants a warning. During this prediction the speed states, Ẋ and Ẏ, of a target are set to be constant, i.e., a target is assumed to keep its current speed and not slow down or accelerate during the prediction. This assumption is primarily used to avoid getting unnecessarily big variances for the prediction error since the variance of the prediction error is used to form an uncertainty ellipse for the track, and this ellipse is used to see if there is danger of a collision. Since the collision warning algorithm generates new predictions with each time step, the prediction will adjust to speed changes of a target even though it assumes constant speed. For the own ship the collision warning system uses a straight line prediction since no Kalman filter is active for this ship, i.e., the own ship is assumed to keep its current heading and speed for the duration of the prediction. One could have set up a Kalman filter for the own ship as well, but since the idea behind the warning system is to test if the own ship position is within any of the targets predicted position ellipses, this would have complicated things and is left as future work. Let s say that the user has set the prediction horizon to be 60 seconds. Figure 5.2 then shows an example of the predictions the collision warning system does. With this figure in mind the decision process of the algorithm is now outlined in Example 5.1 Example 5.1 Assuming that the tracking system has established a track, τ, and that the own ships position, heading, and speed are known. The collision warning system makes the following computations: First predict what the own ships position will be using 1 second steps until the prediction horizon is reached. Run through all established tracking filters and predict the targets position using 1 second steps until the prediction horizon is reached. Figure 5.2 shows

54 main: :41 42(54) 42 System Design the 15, 30 and 60 second predictions for the own ship and the target τ. Using the same gating technique as when gating observations to tracks, (4.13), check at every step of the prediction if the own ships position is within the gate of the target τ. The difference between this gating test and that of the observation-to-track gating being that the prediction variance of the Kalman filter, P, for the position states are used instead of the measurement error variance, S. d 2 =ỹ T P 1 posỹ G where P pos = ( ) P11 P 12 P 21 P 22 If the own ship position is within the gating ellipse, a warning is given to indicate that there might be a risk of a collision with the track. Only one warning can be given for a track each time the collision warning system runs through the predictions, i.e., even though the own ships position might be inside the gate of a target for several steps of the prediction, only one warning is given. In Figure 5.2 the 60 second prediction shows that the own ship is predicted to be inside the gate of the target τ and a warning is given for the track. If there has been four warnings or more within the time frame of the last six times the collision warning system has tested for collisions, an alarm is set to indicate that the risk of a collision occurring is high. The reason for allowing four warnings to pass before an alarm is set is to prevent an unnecessary amount of alarms going off. For instance, if a ship is turning, the own ship might pass through the position prediction of the track for this ship even though the risk of a collision is very small. By using the lower limit of four warnings in the last six predictions before setting an alarm, the certainty of the alarm originating from a real risk of collision is greater.

55 main: :41 43(55) Chapter 6 Simulations To optimize the performance of the tracking filter used in the ship collision warning system simulations were made and design parameters set based on the results of these simulations. The tracking filter is the one part of the system that can be tested in a good way since the real position, speed, and heading of a ship in a simulation can be stored and used for evaluating the tracking filter. The collision warning part of the system on the other hand offers no such exact knowledge of whether or not a given warning is correct. A presentation of the functionality of the currently implemented warning system is however given, even though no evaluation of the performance of the warning algorithm has been made. 6.1 Testing the Tracking Filter In [8], several performance measures for filter evaluation are presented. One of the more commonly used is the computation of the root mean square error (RMSE) based on Monte Carlo simulations. The RMSE error is defined as: RMSE(t) = 1 M M x o t ˆx (j) j=1 t 2 2 1/2 (6.1) where x o t is the known correct value of the filter states, ˆx(j) t is the estimated value for state j, andm is the number of Monte Carlo simulations performed. The RMSE(t) is an estimation of the standard deviation for the estimation error at a time t of the simulation. A scalar value that can be used to estimate the filter performance over a whole simulation is the RMSE average, which is defined as the square root of the time averaged RMSE(t): RMSE average = 1 t t i=1 1 M M x o t ˆx(j) j=1 t 2 2 1/2 (6.2) 43

56 main: :41 44(56) 44 Simulations Stage Stage Stage Figure 6.1. The route used to test the tracking filter. The black dot in the origin marks the position of the radar used when tracking Simulation Settings Since the white-noise acceleration model was used as maneuver model in the EKF used for tracking, it is of interest to know what level of process noise that is appropriate to use. To be able to test this and optimize the performance of the tracking filter a test route was set up using one ship moving through a couple of sharp turns. The test route can be seen in Figure 6.1. Three test runs were made, each with different settings for the reference speed of the ship. Also, since only one model for target movement is used, i.e, only one level of process noise can be set, it was decided to optimize the performance for targets moving at speeds of 0-20 knots. The speed settings for the three test runs can be seen in Table 2. Table 2: The ship s reference speeds during the three test runs. Test run: Speed Stage 1: Speed Stage 2: Speed Stage 3: Slow 7knots 7knots 7knots Mixed 20 knots 7knots 15 knots Fast 20 knots 20 knots 20 knots Process Noise Optimization To test the overall performance of the filter for different settings of the variance level for the process noise, Q, intheekf,thermse average given by (6.2) was used. Assuming the variance level for both the X and Y directions to be the same and the covariance matrix Q to be diagonal: ( ) 1 0 Q = Cov(e k )=q 0 1

57 main: :41 45(57) 6.1 Testing the Tracking Filter 45 where q is a scalar value, Monte Carlo simulations were made and RMSE average computed for values of q ranging from 0.1 to 3. For each level of q 500 Monte Carlo simulations were performed. The results from these simulations, which can be seen in Figure 6.2, shows that selecting the value of q to be approximately Position RMSE Speed RMSE Heading RMSE Slow Mixed Fast Process noise variance Q. Figure 6.2. RMSE average for q ranging from 0.1 to 3. This plot was used to select the setting of the process noise level. 1.2 should maximize the performance of the tracking filter. This value is mostly based on the results concerning the RMSE average for the position state, which for the fast test run drops to its lowest value when q passes 1.2. Selecting q to be smaller then 1.2 can cause large errors in the position estimate, leading to the tracking filter losing track of targets more often. The RMSE average for ship speed and heading are both quite small for q set to 1.2, where the average error in speed is approximately 1 m/s and in heading approximately 5, and should cause no problem for the tracking filter RMSE(t) Using the process noise setting selected in Section 6.1.2, the ordinary RMSE(t) was computed for the position, speed, and heading of the ship used in the test run. The RMSE(t) for the position estimate can be seen in Figure 6.3, meanwhile the RMSE(t) for the speed and heading estimates can be seen in Figure 6.4 and Figure 6.5 repectively. The plots shows that the position estimate of the tracking filter is rather good in all test runs with a maximum error of about ten meters, however, the position RMSE(t) is computed using the ships center coordinate as

58 main: :41 46(58) 46 Simulations 15 Position RMSE(t) knots 7knots 7 knots Slow Position RMSE(t) knots 7 knots 15 knots Mixed Position RMSE(t) knots 20 knots 20 knots Fast Figure 6.3. RMSE(t) for the position estimate during the three test runs. The plot is based on 1000 Monte Carlo simulations and the vertical black lines indicate the different stages of the route. reference, and the radar observations from the ship comes from the ships edges, which can account for a few meters constant error. With this in mind, the variance of the position estimate error can be assumed to be about 5 meters or less. For the speed estimate, presented in Figure 6.4, the error variance is almost at a constant 1 m/s, but as can be seen in the plot for the mixed route, the error is larger when the ship accelerates or decelerates. This however is quite natural since some time most be allowed for the filter to react to the speed change that occurs. Looking at the plot of the RMSE(t) for the heading estimate, it can be seen that the error variance is larger when the ship is traveling at a lower speed, meanwhile there is almost no error when the ship is moving at 20 knots speed. This error comes from the fact that it is harder to estimate the heading of a target moving at low speed, since it only travels a short distance between observations. The measurement noise also causes larger disturbances in the heading estimate for slow moving targets. 6.2 Collision Warning Simulations As said in the introduction of this chapter it is hard to evaluate the performance of the collision warning system since there is no way of telling whether or not a warning is correct. Also, since the parameters of the warning system is set by the user, there might be differences in when a warning is given even though situations are similar to one another. Hence, one can only test and see if the warnings given

59 main: :41 47(59) 6.2 Collision Warning Simulations 47 4 Speed RMSE(t) knots 7 knots 7 knots Slow Speed RMSE(t) knots 7 knots 15 knots Mixed Speed RMSE(t) knots 20 knots 20 knots Fast Figure 6.4. RMSE(t) for the speed estimate during the three test runs. The plot is based on 1000 Monte Carlo simulations and the vertical black lines indicate the different stages of the route. by the system appear to be reasonable. However, to show the functionality of the warning system a number of plots from a simulation will be presented, Figures 6.6 to 6.10, along with commentary notes explaining what can be seen in the plots. These figures shows what can be seen in the main chart window of the FSN GUI during a simulation and will account for the only form of testing performed on the collision warning system. Further development and testing for this part of the system is left as future work.

60 main: :41 48(60) 48 Simulations 15 Heading RMSE(t) Heading RMSE(t) knots 7 knots 7 knots 20 knots 7 knots 15 knots Slow Mixed Heading RMSE(t) 5 20 knots 20 knots 20 knots Fast Figure 6.5. RMSE(t) for the heading estimate during the three test runs. The plot is based on 1000 Monte Carlo simulations and the vertical black lines indicate the different stages of the route. Figure 6.6. Before starting the simulation routes for three ships are created. The plot shows the route for the main ship starting at point A, meanwhile the routes for the other two ships in the simulation starts at points B and C. The main ship will move north and turn to the east, the ships starting at B will move south, and the ship starting at C will first head south and then turn to the east and later on to north.