Surveillance, and subsequent tracking, of a stationary ...

Road-Map–Assisted StandoffTracking of Moving GroundVehicle Using NonlinearModel Predictive Control

HYONDONG OHLoughborough UniversityLoughborough, United Kingdom

SEUNGKEUN KIMChungnam National UniversityDaejeon, Republic of Korea

ANTONIOS TSOURDOSCranfield UniversityCranfield, United Kingdom

This paper presents road-map–assisted standoff tracking of aground vehicle using nonlinear model predictive control. In modelpredictive control, since the prediction of target movement plays animportant role in tracking performance, this paper focuses onutilizing road-map information to enhance the estimation accuracy.For this, a practical road approximation algorithm is first proposedusing constant curvature segments, and then nonlinearroad-constrained Kalman filtering is followed. To addressnonlinearity from road constraints and provide good estimationperformance, both an extended Kalman filter and unscented Kalmanfilter are implemented along with the state-vector fusion techniquefor cooperative unmanned aerial vehicles. Lastly, nonlinear modelpredictive control standoff tracking guidance is given. To verify thefeasibility and benefits of the proposed approach, numericalsimulations are performed using realistic car trajectory data in citytraffic.

Manuscript received October 28, 2013; revised June 25, 2014; releasedfor publication September 1, 2014.

DOI. No. 10.1109/TAES.2014.130688.

Refereeing of this contribution was handled by W. Koch.

Authors’ addresses: H. Oh, Department of Aeronautical and AutomotiveEngineering, Loughborough University, Loughborough, Leicestershire,LE11 3TU, United Kingdom; S. Kim, Department of AerospaceEngineering, Chungnam National University, 99 Daehak-ro, Yuseong-gu,Daejeon, 305-764, Republic of Korea (South), E-mail:([email protected]); A. Tsourdos, Institute of Aerospace Science,Cranfield University, Cranfield, MK43 0AL, United Kingdom.

0018-9251/15/$26.00 C© 2015 IEEE

I. INTRODUCTION

Surveillance, and subsequent tracking, of a stationaryor moving ground target of interest is one of the importantcapabilities of UAVs (Unmanned Aerial Vehicles) since itis essential to increase an overall knowledge of thesurrounding environment [1–3]. To produce appropriatesurveillance data to be used by UAVs, a ground movingtarget indicator (GMTI) is a well-suited sensor due to itswide coverage and all-weather, day/night, and real-timecapabilities [4]. From these sensor data, such as range,azimuth, or elevation of the target with respect to thesensor location (along with appropriate target dynamics), acertain level of accurate estimation could be obtainedusing conventional filtering techniques. However, asground target tracking is a challenging problem due to theuncertainty of the target maneuvers, all availableinformation sources should be exploited: its own sensordata, data from other UAVs, and contextual knowledgeabout the sensor performance and the environment. Inother words, information fusion is required to improve theestimation accuracy.

In particular, in many applications for ground targettracking, the majority of ground vehicles are moving onroad networks for which topographical coordinates couldbe known with a certain accuracy. Such road-mapinformation can be used for improving the quality oftracking significantly by constraining the state of theground target of interest, especially in its position,velocity, and acceleration within the road geometry. Thisis known as a road-constrained target tracking problem,and there are largely three categories of techniques formaking use of the information about the roads. The firstone is the postprocessing correction technique, which runsa tracking algorithm first without the road information,and correction is then applied. Tang et al. [5] andKanchanavally et al. [6] proposed a Bayesian filteringmethod with the hospitability map, which provides alikelihood for each point proportional to the ability of atarget to move at that location. Along with this approach,Kassas et al. [7] added the concept of a syntheticinclination map, which describes how the target will besynthetically inclined to move in different directions witha certain velocity component. The second one is thepreprocessing of target state or sensor measurements.Road information is exploited by defining the target statein road coordinates and performing transformationsbetween the road and ground coordinate system toconsider the sensor measurements in the filter update step[8, 9]. Herrero et al. [10] proposed the preprocessing ofsensor measurements with map restriction. Moreover, theyintroduced a map-tuned interactive multiple model (IMM)structure, which consists of constant speed, a longitudinalacceleration model, and a curvilinear model incorporatingmap information. The third one is a constrained filteringframework. Dan et al. [11] proposed Kalman filtering withstate equality constraints and used road information asequality constraints. Zhang et al. [12] used a

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015 975

pseudomeasurement approach, which treats the roadconstraints as additional fictitious measurements based onthe work of Tahk and Speyer [13]. To deal with a roadnetwork that has road junctions and crossing of severalroads, the variable structure IMM filtering concept wasalso proposed by [14, 15]. Even though the particle filtermight result in better tracking performance depending onthe situation, particularly for a highly nonlinear systemand non-Gaussian noise as described in [16, 17], it wouldrequire a significant computational cost. Since this paperconsiders the use of small and low-cost UAVs rather than asingle UAV with high computation power, this papermainly uses Kalman filter–based algorithms.

Having estimated target information, UAVs should beable to keep a certain distance from the moving target withprescribed intervehicle angular spacing in order to track itwithout being noticed and at the same time to acquireaccurate target information. The certain relative distancefrom the target is called the standoff distance, andtherefore this approach is known as standoff targettracking. For this standoff tracking problem, Lawrence[18] first proposed the application of Lyapunov vectorfields for standoff coordination of multiple UAVs. ThisLyapunov vector field guidance (LVFG) was furtherinvestigated by Frew et al. [19, 20] and Summers et al.[21] to include phase keeping as well as standoff distancetracking. They invented a decoupled control structure inwhich the speed and rate of heading change are separatelycontrolled for the standoff distance and phase anglekeeping, respectively. Similarly, Kingston et al. [22] usedthe vector field approach; however, they introduced asliding mode control and orbit radius change withoutvelocity change for phase-keeping of multiple UAVs.Yoon et al. [3] applied the stabilization of a sphericalpendulum to the conical motion of the aircraft motion inorder to obtain the standoff tracking guidance commands.Oh et al. [23] used the solution of differential geometrybetween the UAV and the target, which provides rigorousstability along with its inherent simplicity. Oh et al. [24]also introduced cooperative standoff tracking of groups ofmultiple targets using Lyapunov vector fields and anonline local replanning strategy. Kim et al. [25] applied areceding horizon model–based predictive control bycombining heading and speed control in a decentralizedmanner. Wise and Rysdyk [26] surveyed and comparedthe different methodologies for standoff tracking: Thesewere the Helmsman behavior, Lyapunov vector field,controlled collective motion, and model predictive control.

This paper presents road-map–assisted standofftracking of a moving ground vehicle using nonlinearmodel predictive control (NMPC) based on our previouswork [25]. In the previous work, the NMPC method wasable to contribute toward acquiring optimal performancein terms of standoff tracking performance and fuelconsumption compared with using the existing decoupledguidance structure. However, in this sort of modelpredictive control, since the prediction of the targetmovement plays an important role in the tracking

performance, this paper focuses on utilizing road-mapinformation to enhance the target estimation accuracy.There are not many works on road-constrained estimationusing real road-map data in the literature, androad-constrained estimation has rarely been dealt orcombined with target tracking guidance, even though aground vehicle of interest is moving only on the road inmany cases. Having this in mind, this paper firstlyproposes a practical road approximation algorithm usingconstant curvature segments. Secondly, to exploit roadinformation for precise target estimation, nonlinearroad-constrained Kalman filtering is applied using apseudomeasurement approach. Furthermore, to addressnonlinearity of road constraints and provide goodestimation performance, both an extended Kalman filter(EKF) and unscented Kalman filter (UKF) areimplemented along with the state-vector fusion techniquefor cooperative UAVs. Lastly, nonlinear model predictivecontrol standoff tracking guidance is explained briefly, andnumerical simulations with a pair of UAVs are performedusing realistic car trajectory data in city traffic in theUnited Kingdom. In the simulation results, the effect ofimproved estimation accuracy on the tracking guidanceperformance is analyzed for both broadly used LVFG andthe proposed NMPC guidance.

The overall structure of this paper is given as follows.Section II contains a definition of the UAV dynamicmodel, the ground target, and the sensor model consideredin this study. Section III explains the road-constrainedtracking filter design and sensor fusion utilizing the roadapproximation technique. Section IV explains thedecentralized structure, definition of performance indexand constraints, and nonlinear model predictive controllerdesign for cooperative standoff tracking. Section Vpresents numerical simulation results of a standofftracking scenario using realistic ground vehicle trajectorydata. Lastly, conclusions and future works are given insection VI.

II. PROBLEM FORMULATION

A. UAV Dynamic Model

Assuming each UAV has a low-level flight controllersuch as a stability/controllability augmentation system forheading and velocity hold functions, this study aims todesign guidance inputs to this low-level controller forstandoff target tracking. Consider a two-dimensional UAVkinematic model [25] as:

⎛⎜⎜⎜⎜⎜⎝

x

y

ψ

v

ω

⎞⎟⎟⎟⎟⎟⎠ = f (x, u) =

⎛⎜⎜⎜⎜⎜⎜⎜⎜⎜⎝

v cos ψ

v sin ψ

ω

− 1

τv

v + 1

τv

uv

− 1

τω

ω + 1

τω

uω

⎞⎟⎟⎟⎟⎟⎟⎟⎟⎟⎠

(1)

976 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015

where x = (x, y, ψ , v, ω)T are the inertial position,heading, speed, and yaw rate of the UAV, respectively; τ v

and τω are time constants for considering actuator delay;and u = (uv , uω)T are the commanded speed and turningrate constrained by the following dynamic limits offixed-wing UAV:

|uv − v0| ≤ vmax (2)

|uω| ≤ ωmax (3)

where v0 is the nominal speed of the UAV. The continuousUAV model in (1) can be discretized by Euler integrationinto:

xk+1 = fd (xk, uk) = xk + Tsf (xk, uk) (4)

where xk = (xk, yk, ψk, vk, ωk)T, uk = (uvk , uωk)T, and Ts isa sampling time.

B. Ground Target and Sensor Model

General target tracking filters have traditionally beendeveloped for monitoring aerial targets such as airplanes,missiles, and so on. Although ground vehicles move withmuch lower speeds than aerial targets, they often performirregular stop-and-go maneuvers with a much smaller turnradius. The constant-velocity model usually used for radartarget tracking is thus unsuitable for tracking groundvehicles, and hence an acceleration or jerk model is amore suitable candidate. Considering general car behavior,because the jerk is not negligible, but the acceleration canbe best modelled using a piecewise constant profile over aspecific duration of time, a good model to apply to thetracking of ground targets is the acceleration dynamicsmodel [25]. This acceleration model defines the targetacceleration as a correlated process with a decayingexponential autocorrelation function, which means if thereis a certain acceleration at a time t, then it is likely to becorrelated via the exponential at a time instant t + τ . Adiscretized system equation for this acceleration model fora ground vehicle is thus expressed in the form:

xtk = Fkxt

k−1 + ηk (5)

where the state vector is xtk = (xt

k, xtk, x

tk, y

tk, y

tk, y

tk)T ,

and where ηk is a process noise, which represents theacceleration characteristics of the target. The statetransition matrix Fk is given by:

Fk =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

1 Ts � 0 0 0

0 1(1 − e−αTs )

α0 0 0

0 0 e−αTs 0 0 0

0 0 0 1 Ts �

0 0 0 0 1(1 − e−αTs )

α

0 0 0 0 0 e−αTs

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(6)

where � = (e−αTs + αTs – 1)/α2, and α is a correlationparameter that models different classes of targets: a small

α for targets with relatively slow maneuvers and a high α

for targets with fast and evasive maneuvers. The details ofthe covariance matrix Qk, the process noise ηk, and othercharacteristics of this model can be found in [27, 28].

In addition, this study assumes the UAVs are equippedwith a GMTI sensor to localize the position of the target.Because the measurement of GMTI is composed of therange and azimuth of the target with respect to the radarlocation, the actual measurements are the relative rangeand azimuth with respect to the position of the airborneUAV. Note that in the present paper, the range ratemeasurement is not considered. The radar measurementzk = (rk, φk)T can be defined as the following nonlinearrelation using the target position (xt

k, ytk)T and the UAV

position (xk, yk)T as:

zk =h(xtk) + νk =

⎛⎜⎜⎝

√(xt

k − xk)2 + (ytk − yk)2

tan−1 ytk − yk

xtk − xk

⎞⎟⎟⎠+νk (7)

where νk is a measurement noise vector, and its noisecovariance matrix is defined as:

V [νk] = Rk =[

σ 2r 0

0 σ 2φ

](8)

III. ROAD-CONSTRAINED TRACKING FILTER

To make use of road-map information for theestimation of a target traveling on a road, it is required toexpress the road map as a certain type of mathematicalequation. This section first presents a road approximationalgorithm using constant curvature segments and thenapplies it to one of the constrained estimations based onKalman filtering along with decentralized sensor fusionusing multiple UAVs.

A. Road Approximation Using Constant CurvatureSegments

To generate the road using onboard sensormeasurements or approximate the real road from a givenroad map, this study uses constant curvature segments. Inthis approach, assuming that some vertices on the road canbe obtained, those vertices are connected by arc segmentsof constant curvature by introducing an intermediate pointwith C1 contact (which represents that the first derivativeis continuous), as shown in Fig. 1. The curved line (arc)between the two vertices represents the curved nature ofthe real road. The mathematical details of the constructionof the curvature segments between vertices can be foundin [29].

The entire road map can then be modelled by a set ofroad segments ri, where i ∈{1, . . . , nr}, and for each roadsegment, the center position of the curve and its curvatureare given by the approximation algorithm. Fig. 2 illustratesthe road approximation using the UAV sensor and constantcurvature segments. As the UAV acquires some points onthe road from the visual image sensor (marked as a cross

OH ET AL.: ROADMAP-ASSISTED STANDOFF TRACKING OF MOVING GROUND VEHICLE USING NMPC 977

Fig. 1. Arc segments connecting two vertices with C1 contact atintermediate vertex. Two arc configurations are possible with same

(orientation 2) or opposite (orientation 1) sign of curvature.

Fig. 2. Illustration of road approximation process using UAV visualsensor.

in Fig. 2), the road is generated and extended successively.If a new point lies on or around the tangent line of aprevious point, the road can be approximated as a straightline. Especially in the case that road information is notgiven in some area, the efficiency of this approach can beof interest, since only some of the points on the road andcorresponding segment curvature by the algorithm arerequired to approximate roads quite close to real roads.This can be readily exploited for the precise estimation ofthe succeeding ground target on the road.

Fig. 3 shows a sample road network of Devizes,Wiltshire, United Kingdom, together with geographic

Fig. 3. Sample road network with GIS satellite data overlaid(Google Map).

Fig. 4. Road approximation using constant curvature segments.

information system (GIS) satellite data. Information forthe road of interest, represented as the blue line, isassumed to be known in this study. Figs. 4–5 show theapproximated road and curvature for each road segmentusing some of the known points on the road. Apparently,the more vertices that are used, the better is the fit to theroad. However, since too many road segments might causeperformance degradation in the constrained estimation, theappropriate number of vertices on the road needs to bedetermined to get a reasonable fit considering theroad-network structure.

978 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015

Fig. 5. Curvature of each road segment.

B. Road-Constrained Estimation

Now, assuming that the ground vehicle moves along agiven road map consisting of nr road segments, thetwo-dimensional (2-D) position of the vehicle should lieon one of the segments. This can be expressed as thefollowing constraint:

ri(xtk, y

tk) = 0 (9)

where ri(·) denotes the ith road segment, which can bemodeled as a straight line, arc, or polynomials. Forexample, if the road is straight, the above road constraintcan be expressed as:

ri(xtk, y

tk) = tan θ · xt

k − ytk = 0 (10)

where θ is a given road direction. In this study, since theroad is approximated using constant curvature segments,as explained earlier, the road constraint is obtained as:

ri(xtk, y

tk) = (xt

k−xi,ct )2 + (yt

k−yi,ct )2−

(1

κi

)2

= 0 (11)

where (xi,ct, yi,ct) and κ i are the center position and thecurvature of the ith road segment, respectively.

Typically, there are two ways to deal with the roadconstraint in a constrained filtering framework. One is to

Fig. 6. Different ways to handle road constraints in constrained filteringframework.

use the road as equality constraints [12], and the other is touse the concept of a directional process noise [15], whichrepresents uncertainty components along and orthogonalto the road, as illustrated in Fig. 6. In Fig. 6a, the errorbound of a position estimate using conventional filteringwith Gaussian noise is represented as a circle, andunconstrained estimate xunconst is projected onto the road,resulting in a better estimate xconst . In Fig. 6b, the processnoise uncertainty Qconst is represented as an ellipsoidconsidering the higher motion uncertainty along the roadand the smaller uncertainty orthogonal to the road,compared to Qunconst, which is represented as a circle. Thisstudy uses a pseudomeasurement method, one of theconstrained Kalman filtering algorithms, which treats theequality constraints as additional fictitious orpseudomeasurements [13]. Unlike other approaches, suchas the maximum probability method and the projectionmethod [11], this approach has the advantage of enablingconsideration of the degree of constraint adherence, bymonitoring the magnitude of the additionalpseudomeasurement noise variance. Thepseudomeasurement model using road constraints can bewritten as:

zri

k = hri(xt

k) + νri

k (12)

OH ET AL.: ROADMAP-ASSISTED STANDOFF TRACKING OF MOVING GROUND VEHICLE USING NMPC 979

where zri

k = 0, hri(xt

k) = ri(xtk), and ν

ri

k is assumed to be azero mean white Gaussian noise with covarianceR

ri

k = (σ roadr )2, which accounts for the uncertainty of road

constraints. Then, the previous real measurement model(7) is augmented by adding the pseudomeasurement togive:

zak = ha(xt

k) + νak (13)

where zak = [zk z

ri

k ]T , ha(xtk) = [h(xt

k) hri(xt

k)]T , andνa

k = [νk νri

k ]T . The measurement noise covariance is alsoaugmented to be Ra

k = diag(Rk, Rri

k ). Considering thatha(xt

k) is nonlinear, the localization of a target can be doneby using the EKF with the augmented measurementequation, which will be called themeasurement-augmented EKF (MAEKF), in theform:time update:

xtk|k−1 = Fkxt

k−1|k−1 (14)

P tk|k−1 = FkP

tk−1|k−1F

Tk + Qk (15)

measurement update:

υk = zak − ha(xt

k|k−1) (16)

Sk = HkPtk|k−1H

Tk + Ra

k (17)

xtk|k = xt

k|k−1 + P tk|k−1H

Tk S−1

k υk (18)

P tk|k = (I − P t

k|k−1HTk S−1

k Hk)P tk|k−1 (19)

where the notation k|k − 1 and k|k represent a predictedstate (and covariance) at time step k from the previous stepk − 1 and an updated state at time step k using thepredicted state k|k − 1, respectively. The output matrix Hk

is a Jacobian of ha with respect to the time-update statext

k|k−1. As a target is moving from one road segment toanother, an appropriate segment on which the target istraveling is selected, based on its estimated or a prioritarget position, its error covariance, and the road-networkinformation as:[

xrie − xt

k|k−1

yrie − yt

k|k−1

]T [P

t,pos

k|k−1

]−1[

xrie − xt

k|k−1

yrie − yt

k|k−1

]< ε (20)

where (xrie , yri

e ) is the end position of the ith road segment,ε is the gate threshold parameter, and P

t,pos

k|k−1 is the positionsubmatrix of the prediction covariance P t

k|k−1. Here, acurrent road segment is updated to the next segment oncethe above condition is satisfied.

In the case that nonlinearity of the road segment issevere, since the EKF based on linearization can result inpoor performance, this study also designed the UKF, andwe compared the results between those two filteringmethods. Road constraints can be incorporated into theUKF by treating it as a pseudomeasurement in a similarway as in the EKF, but without linearization of constraints,which provides better accuracy. The UKF is a filter for

nonlinear systems that uses sigma points approximating agiven probability density function [30]. Among variousUKF methods dealing with pseudomeasurements, anequality constrained UKF (ECUKF) is adopted in thisstudy, considering its reasonable performance andcomputation time [31]. In the ECUKF, at each update step,the stated estimate of the unconstrained UKF is combinedwith the constraints, which are treated aspseudomeasurements, to obtain a constrained a posterioriUKF estimate. This constrained estimate is then used asthe initial condition for the next time step.

C. Data Fusion for a Network of UAV Sensors

Since this study assumes that two UAVs carry out thecooperative standoff tracking of a ground moving target,each UAV’s GMTI sensor can get its own measurementand execute the tracking filter algorithm separately. Aftereach UAV receives the other’s estimation viacommunication link, it can run a decentralized sensorfusion to enhance the tracking accuracy. This study simplyadopts the following simple convex combinationstate-vector fusion [32], which is one of the simplest andeasiest to implement state-vector fusion algorithms andassumes that the cross covariance between two trackestimates can be ignored, and each individual track isindependent [33, 34].

xtk = xt

k|k + P tk|k(P t

k|k + Pp

k|k)−1(xp

k|k − xtk|k) (21)

P tk = P t

k|k − P tk|k(P t

k|k + Pp

k|k)−1P tTk|k (22)

where xp

k|k and Pp

k|k represent the state and errorcovariance estimations of the pair of UAVs. It is assumedthat the communication bandwidth is wide enough totransmit the state (6 × 1) and covariance matrices (6 × 6)in both directions between the pair of UAVs, and theclocks of the UAVs are synchronized for track-to-trackfusion. Subsequently, xt

k in (21) will be used as the initialvalue of the state for the model prediction of a target fornonlinear model predictive control at each sampling point.

IV. MODEL PREDICTIVE COORDINATED STANDOFFTRACKING

The nonlinear model predictive coordinated standofftracking (NMPCST) [25] decides a control input sequencefor N sampling times:

Uk = {u0, u1, · · · , uN−1} (23)

that minimizes the following performance index formaintaining the distance between a UAV and a groundtarget as well as a relative phase angle between UAVs.

J = φ(rN , dN ) +N−1∑k=0

L(rk, dk, uk) (24)

φ(rN , dN ) = 1

2(pr r

2N + pdd

2N ) (25)

980 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015

L(rk, dk, uk) = 1

2

{qrrk + qdd2

k + rv

(uvk − v0

vmax

)2

+ rω

(uωk − v0

rd

ωmax

)2}(26)

where

rk = r2d − |rk|2

r2d

(27)

dk = rTk r

p

k + |rk‖rp

k |r2d

(28)

with rk and rp

k representing the relative vectors from thetarget position to the positions of the current UAV and itspaired UAV, respectively, and where rd is the desiredstandoff distance from the UAVs to the target position, v0

is the nominal speed of the UAVs, and v0rd

is the nominalangular velocity. Here, pr, pd, qr, qd, rv , and rω areconstant weighting scalars. The relative geometry betweenthe UAV, the paired UAV, and the ground target is shownin Fig. 7.

In (28), dk is derived from the inner product of rk andrp

k as 〈rk, rp

k 〉 = rTk rp

k = |rk‖rp

k | cos �θk , where�θk = |θp

k − θk| with the phase angles of the UAVpositions with respect to the current target location. If thephase difference �θ k is ideally maintained as π radian,the above equation is rearranged since cos π = −1 asrT

k rp

k + |rk‖rp

k | = 0. Therefore, if the left-hand side of theabove equation is minimized, the maintenance of thephase angle can be achieved between a pair of UAVs [25].To apply this technique to a different phase angle betweenUAVs other than 180 degrees, 1

2qd�θ2k can be used instead

of 12qd d

2k . For example, to compensate for the

measurement error in the azimuth of one UAV by thesmall range measurement error of the other UAV, a usercan select 90 degrees as a target phase difference betweenthe UAVs.

By incorporating the dynamics of the UAVs in (4) andadmissible control input ranges described in (2)–(3) asequality and inequality constraints, an augmentedperformance index can be derived as:

Ja = φ(rN , dN ) +N−1∑k=0

[L(rk, dk, uk)

+ λTk+1{fd (xk, uk) − xk+1}

+ 1

2μvlvkS

2v (uk) + 1

2μωlωkS

2ω(uk)

](29)

where Sv(uk) = |u1k−v0|−vmax

vmax≤ 0, Sω(uk) = |u2k |−ωmax

ωmax≤ 0,

λk is a Lagrange multiplier, and μv and μω are penaltyfunction parameters. Here, lvk , and lωk are defined to avoidunnecessary computation for satisfying inequalityconstraints:

l∗k ={

0, S∗ ≤ 0

1, S∗ > 0(30)

Fig. 7. Relative geometry between UAV, paired UAV, and ground target.

Let us define a Hamiltonian as:

Mk�= L(rk, dk, uk) + λT

k+1fd (xk, uk) + 1

2μvlvkS

2v (uk)

+ 1

2μωlωkS

2ω(uk) + 1

2μclckS

2c (xk) (31)

The variation of the augmented performance index isrepresented as:

dJa =(

∂φ(rN , dN )

∂xN

− λTN

)dxN +

N−1∑k=1

[(∂Mk

∂xk

− λTk

)dxk

+ ∂Mk

∂uk

duk

]+ ∂M0

∂x0dx0 + ∂M0

∂u0du0 (32)

By selecting the Lagrange multipliers as:

λTN = ∂φ(rN , dN )

∂xN

(33)

λTk = ∂Mk

∂xk

for k = N − 1, . . . , 0 (34)

the variation of Ja is changed to:

dJa =N−1∑k=0

∂Mk

∂uk

duk + λT0 dx0 (35)

Substituting duk to minimize Mk into (35) as

duk = −�k

∂Mk

∂uk

T

(36)

gives the following decreasing variation of Ja.

dJa = −N−1∑k=0

�k

∂Mk

∂uk

∂Mk

∂uk

T

+ λT0 dx0 (37)

Therefore, the control input can be updated using (36)as:

ui+1k = ui

k − �k

∂Mk

∂uk

T

for k = 0, . . . , N − 1 (38)

where i is the index of iteration, and �k is the step size.Derivation of the required Jacobians and definitions suchas ∂φ(rN ,dN )

∂xNand ∂Mk

∂ukcan be found in detail in [25].

Each UAV runs the above optimization routine in flightin a decentralized way at each sampling. When themeasurement on the target comes in, each UAV performsthe target localization and then shares the control/state of

OH ET AL.: ROADMAP-ASSISTED STANDOFF TRACKING OF MOVING GROUND VEHICLE USING NMPC 981

TABLE ISimulation Parameters

Parameter Value Unit

α 0.6 N/Aσ a 0.66 m/s2

θd π radv0 40 m/srd 500 mrc 30 m

vmax 10 m/sωmax 0.2 rad/sτ v , τω 1/3 s

N 4 (equivalent to 2 s) N/A(pr, pd, qr, qd, rv , rω) (2e5, 1e6, pr/N, pd/N, 1e2, 5e1) N/A

μv , μω 1e3 N/Aε 0.8 N/A

Fig. 8. State-vector fusion results based on EKF.

the UAVs and their state/covariance estimationinformation of the target via communications.

V. NUMERICAL SIMULATIONS

This section carries out numerical simulations usingthe proposed road-map–assisted NMPCST for a movingground vehicle. The vehicle trajectory data, acquired at2 Hz from an S-Paramics [35] traffic model using the

Fig. 9. State-vector fusion results based on road-constrained ECUKF.

Devizes map as previously shown in Fig. 3, are used togenerate the GMTI measurements, composed of relativerange and azimuth angle with respect to the position of theUAV. Generated GMTI measurements of a pair of UAVswere mixed with the white noise having the followingstandard deviations: UAV1 (σ r, σφ) = (20 m, 7 degrees)and UAV2 (σ r, σφ) = (30 m, 5 degrees). Note that falsealarms and missed detection in low Doppler or clutteredareas for the GMTI sensor are considered in this study. Forperformance analysis, we used Monte Carlo simulationswith a hundred independent runs and then averaged theresults, unless otherwise stated. The other parametersneeded for NMPCST can be found in Table I.

First, Figs. 8–9 display the estimated position andvelocity of a ground target using the state-vector fusionbased on the EKF and the road-constrained ECUKF,respectively, from single run results. Table II shows themean tracking errors in position and velocity amongdifferent filtering methods. Apparently, the EKF and theUKF using the decentralized sensor fusion based on thestate-vector fusion of two UAVs shows better performancethan that using only a single UAV, and theroad-constrained ECUKF with data fusion provides thebest estimation accuracy. In addition, Figs. 10–12 displayNMPCST simulation results, including the relative

982 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015

TABLE IIPerformance Comparison with Different Estimation Methods Averaged Over a Hundred Monte Carlo Simulations

Single UAV Multiple UAVs (State-Vector Fusion)

Unconstrained Unconstrained Road-Constrained

Mean error EKFUAV,1 EKFUAV,2 EKFmulti UKFmulti MAEKFmulti,c ECUKFmulti,c

Position (m) 20.7141 17.5897 14.4868 14.2238 7.7811 7.3599Velocity (m/s) 3.7757 3.4465 3.2418 3.3099 2.1498 2.0651

Fig. 10. Nonlinear model predictive standoff tracking simulationsresults.

trajectories of the UAVs with respect to the groundvehicle, standoff tracking error, phase angle differencebetween UAVs, and control input histories. Note thathigh-frequency control inputs are required to be followedby the UAVs for both velocity and turning rate, which arehard to achieve in practice. Even though actuator timedelay (τ v , τω) in a UAV kinematic model is used in orderto simulate this effect, more detailed control requirementswill be investigated in future work.

To verify the feasibility and benefits of the proposedapproach, the same scenarios explained above were testedfor broadly used LVFG [19] as well as NMPCST. Here,the LVFG uses a decoupled one-step feedback controlstructure [19]: the heading control for maintenance ofstandoff distance, which guides the UAV onto the

Fig. 11. Nonlinear model predictive standoff tracking simulationsresults.

generated stable orbit around a target, and the speedcontrol for phase angle maintenance on the same orbit.Meanwhile, the NMPC guidance utilizes the coupledoptimal control commands computed over the recedinghorizon time steps, thus relying more on the targetestimation accuracy. Table III compares tracking guidanceperformance for standoff distance and phase keepingbetween LVFG and NMPCST using either the EKF or theroad-constrained ECUKF. It is worthwhile to note that theperformance improvement of NMPCST with changingestimation method from the EKF to the ECUKF is moreremarkable than that of LVFG, since NMPCST usespredicted target information to a certain future timeexplicitly.

OH ET AL.: ROADMAP-ASSISTED STANDOFF TRACKING OF MOVING GROUND VEHICLE USING NMPC 983

Fig. 12. Nonlinear model predictive standoff tracking simulationsresults.

TABLE IIITracking Performance with Different Estimation Methods Averaged

Over a Hundred Monte Carlo Simulations

LVFG NMPCST

Mean error EKFmulti ECUKFmulti,c EKFmulti ECUKFmulti,c

Standoff distance (m) 16.0767 12.9999 13.4243 9.5025Phase keeping (deg) 13.0413 12.8627 12.5695 11.1718

VI. CONCLUSIONS AND FUTURE WORK

This paper has presented the road-map–assistedstandoff tracking of a moving ground vehicle usingnonlinear model predictive control, and particularlyfocused on using road-map information to enhance targetestimation accuracy. First, a practical road approximationalgorithm was proposed using constant curvaturesegments, and to exploit road information for precisetracking of a target, nonlinear road-constrained Kalmanfiltering using a pseudomeasurement approach wasapplied. To address nonlinearity of road constraints andprovide good estimation performance, both the EKF andthe UKF were implemented along with the state-vectorfusion technique. In the numerical simulation results as tostandoff target tracking, the effect of improved estimation

accuracy on the tracking guidance performance wasanalyzed for both LVFG and the proposed NMPCguidance. In brief, this study verified the impact of theproposed approach on the standoff tracking guidanceperformance, and this can be a foundation technique forintelligence, surveillance, target acquisition, andreconnaissance (ISTAR) applications in military andpolice enforcement domains.

Extension of the proposed road-constrained filtering tothe variable structure IMM filter concept [14, 15] will befollowed as future work, which can consider multipleroads at junctions and different vehicle models to assignthe target to the corresponding road segment correctlywhen there are ambiguities or unreliable sensormeasurements. Additionally, more options for the phaseangle difference between UAVs, including 90 degrees, willbe tested for obtaining better coordinated estimationperformance.

ACKNOWLEDGMENTS

This study was supported by 1) the UK Engineeringand Physical Science Research Council (EPSRC) underthe Grant EP/J011525/1 and 2) “Guidance/Control Studyfor Take-off and Landing on a Ship” program through theAgency for Defense Development (ADD) of KOREA(UD1130053JD).

REFERENCES

[1] Kim, M., and Kim, Y.Multiple UAVs nonlinear guidance laws for stationary targetobservation with waypoint incidence angle constraint.International Journal of Aeronautical and Space Sciences, 14,1 (2013), 67–74.

[2] Oh, H., Kim, S., Shin, H., Tsourdos, A., and White, B. A.Behaviour recognition of ground vehicle using airbornemonitoring of unmanned aerial vehicles.International Journal of Systems Science, 45, 12 (2013),2499–2514. DOI:10.1080/00207721.2013.772677.

[3] Yoon, S., Park, S., and Kim, Y.Circular motion guidance law for coordinated standofftracking of a moving target.IEEE Transactions on Aerospace and Electronic Systems, 49,4 (2013), 2440–2462.

[4] Koch, W., Koller, J., and Ulmke, M.Ground target tracking and road map extraction.ISPRS Journal of Photogrammetry and Remote Sensing, 61,3–4 (2006), 197–208.

[5] Tang, Z., and Ozguner, U.Sensor fusion for target tracking maintenance with multipleUAVs based on Bayesian filtering method and hospitabilitymap.In Proceedings of the 42nd IEEE Conference on Decision andControl, Maui, Hawaii, USA, Dec. 2003.

[6] Kanchanavally, S., Ordonez, R., and Layne, J.Mobile target tracking by networked uninhabited autonomousvehicles via hospitability maps.In Proceedings of the 2004 American Control Conference,Boston, MA, Jun.–Jul. 2004.

[7] Kassas, Z. M., and Ozguner, U.A nonlinear filter coupled with hospitability and syntheticinclination maps for in-surveillance and out-of-surveillancetracking.IEEE Transactions on Systems, Man, and Cybernetics–Part C:Application and Reviews, 40, 1 (2010), 87–97.

984 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015

[8] Ulmke, M., and Koch, W.Road-map assisted ground moving target tracking.IEEE Transactions on Aerospace and Electronic Systems, 42,4 (2006), 1264–1274.

[9] Streller, D.Road map assisted ground target tracking.In Proceedings of the 11th International Conference onInformation Fusion, Cologne, Germany, Jun.–Jul. 2008.

[10] Herrero, J. G., Portas, J. A. B., and Corredera, J. R. C.Use of map information for tracking targets on airport surface.IEEE Transactions on Aerospace and Electronic Systems, 39,2 (2003), 675–693.

[11] Simon, D., and Chia, T. L.Kalman filtering with state equality constraints.IEEE Transactions on Aerospace and Electronic Systems, 38,1 (2002), 128–136.

[12] Zhang, M., Knedik, S., and Loffeld, O.An adaptive road-constrained IMM estimator for groundtarget tracking in GSM networks.In Proceedings of the 10th International Conference onInformation Fusion, Quebec, Canada, Jul. 2007.

[13] Tahk, M., and Speyer, J. L.Target tracking problems subject to kinematic constraints.IEEE Transactions on Automatic Control, 35, 3 (1990),324–326.

[14] Kirubarajan, T., Bar-Shalom, Y., Pattipati, K. R., and Kadar, I.Ground target tracking with variable structure IMM estimator.IEEE Transactions on Aerospace and Electronic Systems, 36,1 (2000), 26–46.

[15] Arulampalam, M. S., Gordon, N., Orton, M., and Ristic, B.A variable structure multiple model particle filter for GMTItracking.In Proceedings of the 5th International Conference onInformation Fusion, Annapolis, MD, Jul. 2002.

[16] Arulampalam, M. S., Maskell, S., Gordon, N., and Clapp, T.A tutorial on particle filters for online nonlinear/non-GaussianBayesian tracking.IEEE Transactions on Signal Processing, 50, 2 (2002),174–188.

[17] Ristic, B., Arulampalam, S., and Gordon, N.Beyond the Kalman Filter: Particle Filters for TrackingApplications. London: Artech House Publishers, 2004.

[18] Lawrence, D. A.Lyapunov vector fields for UAV flock coordination.In 2nd AIAA Unmanned Unlimited Conference, Workshop,and Exhibit, Reston, VA, Sep. 2003.

[19] Frew, E. W., Lawrence, D. A., and Morris, S.Coordinated standoff tracking of moving targets usingLyapunov guidance vector fields.Journal of Guidance, Control, and Dynamics, 31, 2 (2008),290–306.

[20] Morris, S., and Frew, E. W.Cooperative tracking of moving targets by teams ofautonomous unmanned air vehicles.Arlington, VA: MLB Company, Technical ReportFA9550-04-C-0107, 2005.

[21] Summers, T. H., Akella, M. R., and Mears, M. J.Coordinated standoff tracking of moving targets: Control lawsand information architectures.Journal of Guidance, Control, and Dynamics, 32, 1 (2009),56–69.

[22] Kingston, D., and Beard, R.UAV splay state configuration for moving targets in wind.Lecture Notes in Control and Information, 369 (2007),109–128.

[23] Oh, H., Kim, S., Shin, H. S., White, B. A., Tsourdos, A., andRabbath, C. A.Rendezvous and standoff target tracking guidance usingdifferential geometry.Journal of Intelligent and Robotic Systems, 69, 1–4 (2013),389–405.

[24] Oh, H., Kim, S., Shin, H.-S., Tsourdos, A., and White, B.Coordinated standoff tracking of groups of moving targetsusing multiple UAVs.In 21st Mediterranean Conference on Control andAutomation, Crete, Greece, June 2013.

[25] Kim, S., Oh, H., and Tsourdos, A.Nonlinear model predictive coordinated standoff tracking of amoving ground vehicle.Journal of Guidance, Control, and Dynamics, 36, 2 (2013),557–566.

[26] Wise, R. A., and Rysdyk, R. T.UAV coordination for autonomous target tracking.In AIAA Guidance, Navigation and Control Conference,Keystone, CO, Aug. 2006.

[27] Mehrotra, K., and Mahapatra, P. R.A jerk model for tracking highly maneuvering targets.IEEE Transactions on Aerospace and Electronic Systems, 33,4 (1997), 1094–1105.

[28] Bar-Shalom, Y., Li, X. R., and Kirubarajan, T.Estimation with Applications to Tracking and Navigation:Theory, Algorithms, and Software. New York: John Wiley &Sons, 2001.

[29] White, B. A., Tsourdos, A., Ashokaraj, I., Subchan, S., andZbkowski, R.Contaminant cloud boundary monitoring using network ofUAV sensors.IEEE Sensors Journal, 8, 10 (2008), 1681–1692.

[30] Julier, S. J., and Uhlmann, J. K.A new extension of the Kalman filter to nonlinear systems.In Proceedings of AeroSense: 11th International Symposiumon Aerospace/Defense Sensing, Simulation and Controls,Orlando, FL, Apr. 1997, 182–193.

[31] Simon, D.Kalman filtering with state constraints: A survey of linear andnonlinear algorithms.IET Control Theory and Applications, 4, 8 (2010),1303–1318.

[32] Brooks, R. R., and Iyengar, S. S.Multi-sensor Fusion: Fundamentals and Applications withSoftware. Upper Saddle River, NJ: Prentice Hall, 1997.

[33] Ng, G. W., Tan, C. H., and Ng, T. P.Tracking ground targets using state vector fusion.In Proceedings of the 8th International Conference onInformation Fusion, Philadelphia, PA, Jul. 2005.

[34] Roecker, J. A., and McGillem, C. D.Comparison of two-sensor tracking methods based on statevector fusion and measurement fusion.IEEE Transactions on Aerospace and Electronic Systems, 24,4 (1988), 447–449.

[35] S-Paramics Software. SIAS Limited, http://www.sias.com, Jan2011.

OH ET AL.: ROADMAP-ASSISTED STANDOFF TRACKING OF MOVING GROUND VEHICLE USING NMPC 985

Hyondong Oh received B.Sc. and M.Sc. degrees in aerospace engineering from KoreaAdvanced Institute of Science and Technology (KAIST), South Korea, in 2004 and2010, respectively, and then he acquired a Ph.D. degree in autonomous surveillance andtarget tracking guidance using multiple UAVs from Cranfield University, UnitedKingdom, in 2013. He worked as a postdoctoral research associate on EC FP7 project“SWARM-ORGAN” at Nature Inspired Computing and Engineering (NICE) Group,University of Surrey, United Kingdom. He is currently a lecturer in autonomousunmanned vehicles at Loughborough University, United Kingdom.

Seungkeun Kim received a B.Sc. degree in mechanical and aerospace engineeringfrom Seoul National University (SNU), Seoul, Korea, in 2002, and then acquired aPh.D. degree from SNU in 2008. He is currently an assistant professor at theDepartment of Aerospace Engineering, Chungnam National University, Korea.Previously he was a research fellow and a lecturer at Cranfield University, UnitedKingdom, in 2008–2012. He has published book chapters, journal articles, andconference papers related to unmanned systems. His research interests cover nonlinearguidance and control, estimation, sensor and information fusion, fault diagnosis, faulttolerant control, and decision making for unmanned systems.

Antonios Tsourdos obtained an M.Eng. in electronic, control, and systems engineeringfrom the University of Sheffield (1995), an M.Sc. degree in systems engineering fromCardiff University (1996), and a Ph.D. degree in nonlinear robust missile autopilotdesign and analysis from Cranfield University (1999). He is a professor of controlengineering at Cranfield University, appointed as the head of the Centre forCyber-Physical Systems in 2013. Professor Tsourdos was a member of Team Stellar,the winning team for the United Kingdom Ministry of Defence (MoD) Grand Challenge(2008) and the Institution of Engineering and Technology (IET) Innovation Award(Category Team, 2009).

986 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 51, NO. 2 APRIL 2015