ISSN 0005-1179, Automation and Remote Control, 2016, Vol. 77, No. 1, pp. 106–129. © Pleiades Publishing, Ltd., 2016. Original Russian Text © K.V. Andreev, E.Ya. Rubinovich, 2016, published in Avtomatika i Telemekhanika, 2016, No. 1, pp. 134–162.

TOPICAL ISSUE

Moving Observer Trajectory Control by Angular Measurements in Tracking Problem K. V. Andreev∗ and E. Ya. Rubinovich∗∗ ∗

Institute for Information Transmission Problems (Kharkevich Institute), Russian Academy of Sciences, Moscow, Russia ∗∗ Trapeznikov Institute of Control Sciences, Russian Academy of Sciences, Moscow, Russia e-mail: [email protected], [email protected] Received March 30, 2015

Abstract—An optimal path synthesis problem for a moving observer that performs angular observations over a target moving uniformly along a straight line on a plane is solved. It is supposed that elevation and azimuth angles can be observed when the observer moves in space and only the azimuth angle can be observed when the observer moves on a plane. Observer’s trajectories are obtained with the help of Pontryagin’s maximum principle as numerical solutions of an optimal control problem. As a performance criterion the trace of covariance matrix of the target motion elements estimate is used. A possibility of solving the problem in real time on board for unmanned aerial vehicle is investigated. A comparison with the scenario of two unmanned aerial vehicles using is given. DOI: 10.1134/S0005117916010069

1. INTRODUCTION When unmanned aerial vehicles (UAV) or unmanned underwater vehicles (UUV) perform autonomous missions, they are often trying to solve a tracking problem by angular measurements made for a moving target. However, an accuracy of target motion elements (TME) estimation depends significantly on a path along which the observer moves during the tracking, which is related to the target’s observability problem [1]. Therefore, a problem naturally arises to construct, in real time, a rational (from the point of view of improving TME estimation quality) path for the motion of a UAV or UUV. In other words, the trajectory control problem over observations on board a moving observer must be solved. The purpose of the paper is to optimize the moving target tracking process described above. The tracking process itself consists of two related processes: data acquisition about the target and data processing. Control over the process of data acquisition is called the observation control; date processing is called filtering [2]. Constructing optimal procedures for data acquisition and date processing in real time is hard due to the nonlinearities in both the observer–target system motion equations and the equations describing angular observations (bearing of the target and its elevation angle). [3]. To find TME estimates, one can use extended (linearized in the neighborhood of the current estimate) Kalman filter, pseudo-measurement methods [4], or various Bayesian filtering methods: Monte Carlo approaches [5, 6] or the method of Gaussian sums [7]. The latter methods can also be easily extended to the cases of several sensors [8]. However, it has been shown in [9] that TME estimates obtained with the help of above filtering methods have approximately the same accuracy. Due to overall dimensions and energy constraints, on-board computers on UAVs and UUVs are relatively small-duty, which requires sufficiently simple (recurrent) filtering algorithms constructed, 106

MOVING OBSERVER TRAJECTORY CONTROL

107

for instance, based on the extended Kalman filter. The “simplicity” of such algorithms and, in particular, linearization errors are compensated by higher quality information about the target obtained by the observation process optimizing. Numerical experiments show that the linearized Kalman filter with a controllable trajectory observation process yields TME estimates no worse and sometimes even better than complex nonlinear filtering algorithms for an arbitrary (non-optimal) observer motion. In the case of localizing an immovable target, the problem of constructing an optimal UAV trajectory has been studied in [10, 11]. A suboptimal method of constructing UAV trajectories has been considered in [12], where UAV control during a fixed time interval in constructed as a linear combination of the first N basis functions in some orthonormal basis on this interval: the system of Chebyshev’s orthogonal polynomials. This approach lets us reduce the optimal control problem to the maximization problem for a function of several variables (coefficients in the basis decomposition), but the choice of number N has not been considered in [12], so it is impossible to estimate the losses in the performance functional’s value incurred by using a suboptimal method for solving the problem. In [13], the UAV optimal control problem is posed from the point of view of partially observable Markov decision processes. In the present paper, a path synthesis problem for UUV and UAV performing observations over a moving target by azimuth angles (UUV) (Section 2) or azimuth and elevation angles (UAV) during a given observation time T (Section 3) is investigated. The corresponding Meyer’s optimal control problems that can be solved numerically with the help of Pontryagin’s maximum principle [14, 15] by reducing them to the two-point system of differential equations (Section 4) is formulated. The question of solving an optimal control problem on board for UAV (similar to [12]) in Section 5 is considered. Having an exact form of the optimal control, one can find the necessary number N of the first basis functions and parameterize the control by decomposing it with respect to a chosen basis. The criterion for passing to a suboptimal solution is the threshold value of relative increase in the functional. After a parameterization procedure, the optimal control problem reduces to finding an extremum of function of many variables whose arguments are the coefficients in a decomposition with respect to the basis functions. The suboptimal control construction algorithm is validated with the help of computational experiments by time discretization (Section 5). Here √ a discretization process accomplishes by scaling the errors in angle measurements with coefficient Δt, where Δt is a time interval between sequential observations: √ σdiscrete = σcont Δt. In the paper, two scenarios are considered. In the first scenario, it is possible to measure the elevation angle [16], which gives us a possibility (knowing the altitude of the UAV’s flight) to indirectly evaluate a distance to the target. As alternative might be to use two UAVs performing azimuthal observations. Characteristic features of this scenario and sample flight trajectories are given in Section 6. Results of this work can be applied to problems of finding TME of a ground target by bearing only observations from UAV both in the radio frequency range [16] (in this case, due to geometric features of UAV, the azimuthal direction can be measured more accurately than the elevation angle) and with infrared sensors (where both angles can be measured with the same accuracy), and also in the case of a surface vessel tracking by on-board UUV acoustics sensors. 2. FLAT OBSERVER’S MOTION 2.1. Problem Setting A flat motion of the observer P and the target E that moves uniformly along a straight line is considered. P has a constraint on the maximal admissible velocity and can maneuver inside this AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

108

ANDREEV, RUBINOVICH

constraint, solving at the same time the primary and auxiliary problems. The primary problem of P is to get as close as possible to (or as far as possible from) the target during given time. The auxiliary problem is to get the most accurate TME estimate with noisy current bearing only measurements. The criterion is a certain terminal (“compromise”) payoff functional that reduces two-criterial problem to a single-criterial one. This problem setting is relevant for underwater vehicles.

2.2. Problem Formalization Consider a motion of both the observer P and the target E in a fixed Cartesian coordinate system Oxy whose origin O coincides with the observer’s location at the initial time moment t0 = 0, and the Oy axis is oriented towards the first bearing measurement (Fig. 1). TMEs are characterized by a vector of parameters θ = (θ1 , θ2 , θ3 ), where θ1 and θ2 are the components of target’s velocity vector along axes Ox and Oy respectively, and θ3 is a distance from P to E at the initial time moment. The observer P on each tick of the motion t = 0, 1, . . . , T produces a measurement ξt of   the bearing ϕt to the target E with some additive noise σ εt , where εt t=0,1,2,... is a sequence of independent standard Gaussian random values, σ = const > 0 (mean squared bearing error). In other words, bearing measurements have the form ξt = arctan 

θ1 t − x(t) + σεt = ϕt (θ, t ) + σεt , θ3 + θ2 t − y(t)

t = 0, 1, . . . , T.

(1)



Here t = x(t), y(t) are current coordinates of the observer subject to equation t = t−1 + ut−1 ,

t = 1, . . . , T,

(2)

where the observer’s control ut = (ux (t), uy (t)) satisfies constraint |ut |  1.

(3)

For correctness of the problem setting, assume that ϕt ∈ (−π/2, π/2). V

Et y

υ

θ2

θ1

E0 σε0 θ3

ϕt σεt Pt

αt ut

ρt = (x(t), y(t)) P0 O

x

Fig. 1. Tracking geometry on a plane. E0 , P0 and Et , Pt are initial and current locations of target and observer. AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL  

109

To get the current estimate θˆt = θˆ1 (t), θˆ2 (t), θˆ3 (t) for the column vector of parameters θ, construct the linearized Kalman filter [2] (where * denotes transposition):   Pt−1 Ht∗ (θˆt−1 , t ) ξt − ϕt (θˆt−1 , t ) ˆ ˆ θt = θt−1 + , σ 2 + Ht (θˆt−1 , t )Pt Ht∗ (θˆt−1 , t ) Pt−1 Ht∗ (θˆt−1 , t )Ht (θˆt−1 , t )Pt−1 , Pt = Pt−1 − σ 2 + Ht (θˆt−1 , t )Pt Ht∗ (θˆt−1 , t )

where ϕt (θˆt−1 , t ) = arctan

(4) (5)

tθˆ1 (t − 1) − x(t) θˆ3 (t − 1) + tθˆ2 (t − 1) − y(t)

is the observation function, and Ht (θˆt−1 , t )  ∇ϕt (θˆt−1 , t ) is the observation function’s gradient with respect to the first argument; Pt is a covariance matrix of filtering errors. As a performance criterion we consider the terminal functional J = (1 − c)tr PT + cF (T ),

(6)

where tr is the trace operator for a matrix, and the weight coefficient c ∈ [0, 1] characterizes the sensitivity of criterion J either to the accuracy of finding the TME (c → 0) or to fulfilling the collision (pursuit) problem (c → 1) under low accuracy of TME estimates. Function F (T ) (whose specific form we will give below) characterizes the desired location of the observer at the final time moment T . We need to construct the observer’s trajectory in such a way as to minimize the functional J. We call the control that realizes such a trajectory optimal. To compute the optimal control, we need to vary the point (PT , T ). The first coordinate PT of the varying point is determined by the filter (5), and the second is determined by the observer’s motion. This problem, however, meets the following conceptual obstacle. The matter is that being in motion at an arbitrary point (Pt , t ), it is impossible to compute the point (PT , T ). Indeed, Eqs. (5) show that elements of matrix PT are random values depending on the entire realization of the observations ξ0T = {ξ0 , ξ1 , . . . , ξT } from the time moment t0 = 0 to the time moment T . At the current moment t, values of future observations are obviously unknown. One way to resolve this problem is to pass to the so-called conditionally program observer controls. The idea is as follows. Taking the current time moment as initial and the current parameter estimation as initial, we look for the optimal control in the class of program controls, i.e., in the class of controls that are functions of time, initial estimate, and initial time moment. Further, over some time interval τ the observer moves along the resulting program, and then it refines its estimates of the parameters and corrects the program, i.e., computes a new control as a function of time, new initial position, and new initial time moment, and so on. The control that results from this procedure is called conditionally program. To construct conditionally program controls, we fix in Eq. (5) the current estimate θˆt−1 . By fixing θˆt−1 , we get a possibility to integrate Eq. (5) together with the observer’s motion Eq. (2) “ahead” up until time moment T and find (PT , T ). In particular, the explicit dependence of PT on the value of Pt at an arbitrary known time moment t is given by 

−1 1 T H ∗ Hs PT = I + Pt 2 σ s=t s

−1

Pt ,

where I is the unit matrix. Since numerical solution of the variational optimal control problem by an observer requires several times more computational resources than solving the auxiliary problem AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

110

ANDREEV, RUBINOVICH

of estimating TMEs, it makes sense to solve the optimization problem not on every time tick (i.e., not after every bearing measurement) but only at certain moments of time which we further call correction moments. This is also useful because for a small tick duration TME estimates obtained on two subsequent ticks will be very similar, and the observer’s optimal trajectories corresponding to these estimates will be virtually the same. In our computer modeling (see results below) the filter operated on every time tick, and correction moments followed after every 6 ticks of the filter. Between correction moments, the observer moves along the program obtained (by solving the variational problem) at the previous correction moment. Now define a form of function F (T ) in the criterion (6). We let 



F (T )  a ν1 x(T ) + ν2 y(T ) ,

(7)

where ν1 , ν2  0, ν1 + ν2 = 1 and a = −1 for the problem of collision with the target (pursuit of the target). If a = 1, the problem at hand becomes an evasion problem from the object E moving uniformly along a straight line. For a = 0 we have the pure problem of estimating TMEs. Weight coefficients ν1 and ν2 characterize a sensitivity of function F (·) to a change with respect to coordinates, or, in other words, “how important” observer’s motion along a certain coordinate is from the point of view of fulfilling the primary problem (i.e., actually pursuing the object E or evading it). These coefficients can be determined, for instance, with the following reasoning. For P  position tk = x(tk ), y(tk ) that it occupies movement with constant speed u = |ut |  1 from the  at the kth correction moment, to the position T = x(T ), y(T ) at the terminal instant T along a trajectory corresponding the course αk (counting it from the Oy axis), it needs to implement the following shifts along the coordinate axes: x(T ) − x(tk ) = u(T − tk ) sin αk ,

(8)

y(T ) − y(tk ) = u(T − tk ) cos αk .

(9)

The ratio of these shifts determines preference in the motion along a certain coordinate, i.e., ν1 sin αk x(T ) − x(tk ) = = . y(T ) − y(tk ) ν2 cos αk From this ratio one can find ν1 , ν2 (taking into account ν1 + ν2 = 1). The observer’s course αk is chosen from the meaning of the original problem (pursuit, evasion, or only estimating TMEs). For instance, in the evasion problem for P in the case of the target E moving along a straight line faster than the observer P, the course αk at time moment tk is chosen by conditions 



αk = ϑˆk − 0.5π − α0 ,





α0 = arcsin |utk |/ˆ vk ,

where ϑˆk and vˆk are estimates of the path and velocity of the target at time moment tk . To illustrate this, consider numerical solutions of pursuit (a = −1) and evasion (a = 1) problems for UUV with initial data listed in Table 1. Table 1. Modeling parameters in the pursuit–evasion problem Parameter Value Measurement unit UUV speed 6 knots Target speed 18 knots Initial distance to target D0 50 nautical cable 30 degrees Target course ϑ Total tracking time T 900 s 0.5 degrees Bearing error Interval between observations Δt 10 s Interval between correction moments τ 60 s AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

111

Fig. 2. Pursuit (left) and evasion (right) trajectories with TME estimate.

Figure 2 (left) shows the influence of coefficient c (c = 0; 0.1; 0.2; . . .; 0.9; 1) on the pursuer’s trajectory. We performed computations with the same path of noise process in measurements. Figure 2 (right) reflects a similar dependence in case of solving the evasion problem. The graphs show a common trend, namely, as the c coefficient increases the motion trajectory tends to become more like a straight line. For c = 1 the trajectory is close to a straight line and, it might seen, is the best in terms of pursuit (evasion). However, it is not so. The problem is that as the c coefficient decreases the criterion’s sensitivity to the accuracy of finding TMEs increases, and the error in the TMEs may turn out to be unacceptable. Note also that the above algorithm has proven to be rather robust with respect to the error in estimating the prior distance to target. An error by a factor of 1.5 in the initial distance (either increasing or decreasing) has not led to divergence of the estimation procedure and has been processed over approximately 10 minutes out of the total target tracking time of 15 min. Remark 1. In the example above, the target’s uniform motion along a straight line is described with a three-component (rather than four-component!) vector of parameters θ. The threecomponent description assumes that the first (and only) bearing measurement is made absolutely precisely. Computer modeling has shown that error caused by this assumption does not exceed 2–3 percent but reduces computation time by a factor of more than 1.5. Remark 2. The methodology of constructing an optimal trajectory for an observer on a plane is considered in detail in Section 3 with the example of a more complicated three-dimensional problem considered in continuous time. To use this methodology, we made a standard continuous approximation of the discrete model considered here. 3. OBSERVER’S MOTION IN 3D 3.1. Problem Setting In this section, by an observer P we mean UAV that can perform 3D space maneuvers for trajectory control of observations under given constraints, controlling pitch and yaw angles. The AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

112

ANDREEV, RUBINOVICH

target E, similar to Section 2, moves on a plane uniformly along a straight line. We can observe (with noise) its azimuth and elevation angles. To find out how the accuracy of TME estimates is influenced by UAV’s spatial maneuvering, in Section 3 we consider the TME estimation problem only. 3.2. Formalization and Solution Let us consider the Cartesian coordinate system OXY Z (Fig. 3) whose OX, OY , and OZ axes point to east, north, and up respectively. We assume the Earth surface to be locally flat and define it with equation Z = 0. Suppose that along this plane, the target moves uniformly along a straight line with speed v. Consider the relative coordinate system oxyz with axes parallel to the OXY Z system whose origin is at UAV’s center of mass. Initial coordinates of the target (at the time moment t = 0) in this system equal (x0 , y0 , z0 ), where z0 is the initial altitude of UAV’s flight taken with an opposite sign. Let vx and vy be the components of target’s velocity vector. Since coordinate z of the target is always known, TMEs are given by the vector t = (x(t), y(t), vx , vy ). Motion equations for UAV–target system have the following form: ⎧ ⎪ ˙ = vx −V cos β(t) cos γ(t) ⎨ x(t)

y(t) ˙ = vy −V cos β(t) sin γ(t) ⎪ ⎩ z(t) ˙ = −V sin β(t),

(10)

where β = β(t) and γ = γ(t) are UAV controls with respect to pitch and yaw angles respectively, and V = const is a given speed of UAV’s flight. We will assume that at the initial time moment t0 = 0 we have a prior estimate ˆ0 for the TMEs, and we know the 4 × 4 covariance matrix of the estimation errors for P0 . Evolution of matrix Pt is defined in the Appendix.   The UAV can observe the column vector ξt = ξA (t), ξE (t) (here ξA (t) and ξE (t) are noisy azimuth and elevation angles respectively) with passive bearing means in either radio [17] or infrared range (Fig. 3). Such an observation scheme has been presented, for instance, in [16]. Suppose that at every time moment coordinates of UAV itself, the horizon line, and the direction to the north are known exactly, and a detecting device does not change orientation in the UAV maneuvers, i.e., there are no systematic errors in observations. Under these assumptions, the observation equation has the form



 ϕA (t) σA w1 (t) + wt , where wt = , ξt = ϕE (t) σE w2 (t) w1 (t) and w2 (t) are independent standard Wiener processes, σA and σE are given constants. We will assume that the accuracy of finding the coordinates does not depend on the signal-noise ratio

Fig. 3. The problem geometry. On the left: control includes pitch β(t) and yaw γ(t) angles. On the right: observed azimuth ϕA and elevation ϕE angles. AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

113

and mutual location of sensor and target. Similar to Section 2, we look for the control β(t), γ(t) of UAV in the class of conditionally program with terminal performance criterion (payoff functional) in the form of (6) (for c = 0) J = tr PT .

(11)

In numerical computations, we take σA ≈ 26 mrad and σE ≈ 78 mrad. Due to higher sensitivity of criterion (11) to the accuracy of velocity component estimation, in computing the trace of the matrix PT as the unit of length we use a kilometer, and as the unit of speed we use a meter per second. An alternative to functional (11) might be the determinant of the Fischer information matrix [18]. We choose a trace of covariance matrix due to its simple physical interpretation: a square root of the trace of covariance matrix characterizes a linear size of σ-confidence region in the resulting TME estimates. We will solve the resulting UAV control problem with Pontryagin’s maximum principle [14], and for TME estimates we will apply the extended Kalman filter [2, 3]. To do this, we construct a linearized (in a neighborhood of the current TME estimates) function   of observations (in the relative coordinate system), introducing notation rf = x2 + y 2 , rs = x2 + y 2 + z 2 . Then ϕE = arctan

z , rf

y ϕA = arctan . x

(12)

The observation function in question ⎛

H=

⎜ ∂(ϕE , ϕA ) ⎜ =⎜ ∂(x, y, vX , vY ) ⎝

y rf2

− −

x rf2

0 0

yz xz − 2 0 0 2 rs rf rs rf

⎞ ⎟ ⎟ ⎟. ⎠

(13)

To solve the problem above, one need to write differential equations for the Kalman filter to estimate ˆt , TME, and the covariance matrix Pt of this estimate explicitly. Equation for Pt has the following form (we omit subscript t in what follows): P˙ = F P + P F ∗ − P H ∗ R−1 HP ∗ ,

(14)

where F is the matrix of TME’s linear dynamics (i.e., ˙ = F )

F =

0 I 0 0



,

H is the linearized observation function (13). The observation error is assumed to be normal with zero mean and a covariance matrix whose elements are by assumption constant, R=

2 σ



0

A

2 0 σE

.

Evolution of TME estimates satisfies the equation ˆ˙ = F ˆ + P H ∗ R−1 (ξ − H ˆ) . Since P is a symmetric 4 × 4 matrix, Eq. (14) is given by ten variables: elements of matrix P . Let us simplify the Eq. (14), introducing a symmetric matrix Hxx of size 2 × 2. The last two columns of matrix H (see Eq. (13)) are zero, so component H ∗ R−1 H from Eqs. (14) can be rewritten as

∗

H R

−1

H =

Hxx 0 0 0

AUTOMATION AND REMOTE CONTROL



Vol. 77

,

Hxx = No. 1

2016

h1 h2 h2 h3



,

(15)

114

ANDREEV, RUBINOVICH

where h1 =

x2 z 2 y2 + 2 2 , rs4 rf2 σE rf4 σA

h2 =

xyz 2 xy − 4 2, 2 2 4 rs rf σE rf σA

h3 =

y2z2 x2 + 2 2 . rs4 rf2 σE rf4 σA

We write matrix P in block form:

P =

∗ Pxx Pxv



,

Pxv Pvv

(16)

and then for each of the four 2 × 2 blocks we introduce notation corresponding to its elements:

Pxx =

pxx pxy pxy pyy



,

Pxv =

pxvx pvx y pxvy pvy y



,

Pvv =

pvx vx pvx vy pvx vy pvy vy



.

(17)

In this notation, the trace of the covariance matrix is given by tr P = pxx + pyy + pvx vx + pvy vy . 3.3. The Optimal Control Problem Our problem reduces to Meyer’s optimal control problem and can be formulated as follows. For a system of Eqs. (14) and (10) with initial conditions (x, y, z)|t=0 = (x, y, z)0 , P |t=0 = P0

(18)

that define the initial estimate for relative coordinates of the target and covariance matrix, find such controls β(t) and γ(t) on a fixed time interval t ∈ [0; T ] that the functional (a trace of covariance matrix)   J = pxx + pyy + pvx vx + pvy vy t=T at the final time moment t = T when the mission is accomplished would have minimal value. The matrix Eq. (14) is represented in elementwise form in the Appendix, and its final form is given by Eq. (A.2). 4. CONSTRUCTING PROGRAM TRAJECTORIES FOR UAV FLIGHT Let us construct the conjugate system whose 10 equations correspond to the dynamics of covariance matrix elements (Eq. (14), and its elementwise representation as the system (A.2)), and the next three correspond to relative coordinates of the target (10). Denote by ψ = (ψ1 , . . . , ψ10 ) the conjugate variables for elements of covariance matrix corresponding to the order pxx , pxy , pyy , pvx vx , pvx vy , pvy vy , pxvx , pxvy , pvx y , pvy y . The system’s Hamiltonian H = HψP − ψ11 (V cos β cos γ − vx ) − ψ12 (V cos β sin γ − vy ) − ψ13 V sin β, where HψP is defined by the first ten phase variables and conjugate variables (it is considered in detail in the Appendix, see Eq. (A.3)) and is a linear function with respect to the variables h1 , h2 , and h3 : HψP = −Ah1 − Bh2 − Ch3 + D, where A, B, C are defined in (A.3). Dynamics of conjugate variables {ψ1 , . . . , ψ10 } is defined by HψP (it is represented elementwise in Eq. (A.4) in the Appendix). Now {ψ11 , ψ12 , ψ13 } satisfy AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

115

the following differential equations (here and below the argument t is omitted for brevity): ⎧ ∂h1 ∂h2 ∂h3 ⎪ ⎪ ⎪ +B +C ψ˙ 11 = A ⎪ ⎪ ∂x ∂x ∂x ⎪ ⎪ ⎪ ⎨

ψ˙

=A

∂h1

+B

∂h2

+C

∂h3

(19)

12 ⎪ ∂y ∂y ∂y ⎪ ⎪ ⎪ ⎪ ⎪ ∂h ∂h ∂h ⎪ ⎪ ⎩ ψ˙ 13 = A 1 + B 2 + C 3 ,

∂z

∂z

∂z

where partial derivatives of the elements h1 , h2 , h3 are given by



2xz 2 x2 2x2 4y 2 x ∂h1 = 2 4 2 1− 2 − 2 − 6 2 , ∂x rf rs σE rf rs rf σA ∂h1 2x2 yz 2 = 2 4 2 ∂y rf rs σE







2 2y x2 − y 2 1 + + , 2 rf2 rs2 rf6 σA



∂h1 2x2 z 2z 2 = 2 4 2 1− 2 , ∂z rf rs σE rs













yz 2 2x2 4x2 y 3x2 − y 2 ∂h2 = 2 4 2 1− 2 − 2 + , 2 ∂x rf rs σE rf rs rf6 σA

xz 2

2y 2

4y 2

∂h2 = 2 4 2 1− 2 − 2 ∂y rf rs σE rf rs ∂h3 2xy 2 z 2 = 2 4 2 ∂x rf rs σE



+

x

3y 2 − x2 2 rf6 σA



,

2z 2

∂h2 2xyz = 2 4 2 1− 2 ∂z rf rs σE rs



(20)

,



2 2x y 2 − x2 1 + + , 2 rf2 rs2 rf6 σA



2yz 2 y 2 2y 2 4x2 y ∂h3 = 2 4 2 1− 2 − 2 − 6 2 , ∂y rs rf rs σE rf rf σA



∂h3 2y 2 z 2z 2 = 2 4 2 1− 2 . ∂z rs rf rs σE

To construct the control program we have to maximize the Hamiltonian with respect to the control, which is equivalent to min {ψ11 cos β cos γ + ψ12 cos β sin γ + ψ13 sin β} . β,γ

(21)

It is easy to see that the Hamiltonian is maximized when it holds that 

ψ13

, sin β = −  2 + ψ2 + ψ2 ψ11 12 13 ψ12 , sin γ = −  2 + ψ2 ψ11 12

2 + ψ2 ψ11 12 cos β =  , 2 2 2 ψ11 + ψ12 + ψ13

ψ11 cos γ = −  . 2 + ψ2 ψ11 12

(22)

(23)

Transversality conditions can be conveniently represented in the same form as they are presented in [15]: [δJ − Hδt + ψ × δz]fi = 0, where the subscript i corresponds to the set of boundary conditions at the initial point of UAV trajectory; the superscript f , at the final point. Since the initial position of UAV and total mission time are fixed, half of the initial conditions are given at the left end of the trajectory in the form AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

116

ANDREEV, RUBINOVICH Table 2. Modeling parameters in the problem of constructing the flight trajectory for an UAV observing a moving target with two angles Parameter

Value

Measurement unit

UAV flight speed 100 Accuracy of the azimuth angle σA 26 78 Accuracy of the elevation angle σE Observation frequency 10 20 Initial distance from UAV to target Mission time 205–215 10 Prior accuracy of estimating target coordinates σx Prior accuracy of estimating target velocities σv 15

m/s mrad mrad Hz km s km m/s

Table 3. Varying parameters in the experiments on constructing UAV flight trajectories when localizing a moving target with two angles Parameter UAV initial altitude Target motion speed Direction ϑ of the target’s motion with respect to the initial UAV–target line

Set of values 1, 5, 10

Measurement unit km

0, 15, 30 90, 45, 0

m/s degrees

of Eqs. (18). On the right end of the trajectory (at time moment t = T ), transversality conditions yield 13 more constraints on conjugate variables: ⎧ f ψ ⎪ ⎪ ⎨ 1,3,4,6

= −1

f

ψ2,5,7,8,9,10 = ⎪ ⎪ ⎩ f ψ11,12,13

=

(24)

0 0.

We solve the two-point problem by numerical methods [19]. The essence of the method from [19] is to update the control iteratively. At the initial time moment, one choose some initial control and construct the phase trajectory for the system with the chosen control and initial conditions (18). Then we solve the conjugate system of differential equations in reverse time, i.e., “from the right to the left”. The values of phase variables that are needed for that must be taken from the solutions of the system of differential equations on phase variables with initial control. To pass to the next iteration, we update the control: at every point of the trajectory the new control uk+1 = (β, γ) is constructed based on the “old” control uk and the optimal control uk+1 obtained with (22) and (23) from the Hamiltonian’s maximization condition (21) uk+1 = αuk + (1 − α) uk+1 , where α ∈ (0, 1) is the smoothing parameter. UAV trajectories were computed for constant parameters listed in Table 2. In all cases, the  prior covariance matrix is equal to P0 = diag σx2 , σx2 , σv2 , σv2 . Varying experimental parameters are listed in Table 3. It should be noted that the error in computing the azimuth is less than the error in computing the elevation angle due to constructive limitations for the placement of antenna grids in the vertical plane on board of UAV. The modeling time was chosen to be such that UAV would not have time to reach the final point of target’s motion; it was 205–215 seconds depending on target’s motion direction. Figures 4–6 show flight trajectories for UAV and a target with v = 0, and also for a target moving uniformly along a straight line with velocities v = 15 m/s (54 km/h) and v = 30 m/s AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

117

Fig. 4. Program flight trajectory for the UAV (3D-trajectory and its projection on the OXY plane) for an immovable target (at the origin) with initial UAV altitude of 5 km.

Fig. 5. Program flight trajectories for UAV (3D-trajectory and its projection on OXY plane) for target motion speed 15 m/s and three versions of the direction ϑ where the target is moving (line segments 1, 2, 3). The initial UAV altitude is 5 km.

Fig. 6. Program flight trajectories for UAV (3D-trajectory and its projection on z = 0 plane) for target motion speed 30 m/s and three versions of the direction ϑ where the target is moving (line segments 1, 2, 3). The initial UAV altitude is 5 km.

AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

118

ANDREEV, RUBINOVICH Table 4. Accuracy of TME estimation along program trajectories of the flight for initial UAV flight altitudes of 1, 5, and 10 km and distance to target of 20 km v=0

10 km 5 km 1 km

Estimate accuracy δx, m δv, m/s δx, m δv, m/s δx, m δv, m/s

29.49 0.49 24.66 0.42 24.46 0.39

v = 15 m/s ϑ = 90 23.80 0.40 23.22 0.38 23.06 0.36

◦

ϑ = 45 37.81 0.46 30.35 0.41 29.50 0.37

◦

v = 30 m/s ◦

ϑ=0 43.39 0.50 33.50 0.43 32.04 0.39

ϑ = 90 31.10 0.42 26.26 0.37 26.16 0.35

◦

ϑ = 45◦ 52.45 0.49 41.01 0.43 37.84 0.40

ϑ = 0◦ 64.48 0.57 48.44 0.48 43.54 0.43

(108 km/h) respectively. Three different trajectories correspond to different directions of target motion ( ϑ angle in Table 3) with respect to the projection of the line connecting the initial UAV position and initial estimate of the target coordinates on Z = 0 plane. On all illustrations, the initial altitude of UAV flight is 5 km. In all scenarios, the optimal path is a wavelike maneuver in the horizontal plane and some altitude gain. The maximal altitude gain rate corresponds to the scenario when the target get furthest away from the UAV. For target motion speed of 30 m/s, UAV has to have vertical speed of up to 78 km/h (the pitch angle reaches 14◦ for initial flight altitude of 5 km). It turns out that the higher is the initial flight altitude of UAV, the less is the required maneuver intensity in the vertical plane. The maximal pitch angle among all conducted experiments was observed in the scenario where the target moves away from the initial UAV location with speed 30 m/s and with initial UAV flight altitude of 1 km. The maximal value of pitch in this case was 16.6◦ . Accuracies of the resulting coordinate estimates are shown in Table 4. The value of δv is defined as a root of the trace of covariance matrix for velocity components, and δx is an arithmetic root of the trace of covariance matrix for coordinate estimates. These values characterize a linear size of σ-region in the space of coordinates and velocities, which defines 50 % confidence interval [3]. In all results above, the target is no further than 150 meters from its 99 % level estimate (3σ-confidence region). Note that as the initial UAV flight altitude grows, we can get more exact TME estimates, since as UAV flight altitude grows, the elevation angle provides more and more information (the linearized function of observations by elevation angle has the form rzf ). However, the most significant effect in improving accuracy of estimates occurs when we pass from the initial flight altitude of 1 km to the initial altitude 5 km rather than from 5 to 10 km. In what follows we will assume that for this initial distance to target, the initial flight altitude of 5 km is more preferable. 5. SOLVING THE PROBLEM ON BOARD OF UAV As it was noted above, conditionally program controls are updated at given correction moments as new angle measurement information arrives. However, it is hard to use the iterative process for solving the two-point problem on board of UAV in real time. Therefore, it makes sense to use simple methods to find a suboptimal flight trajectory with given properties. As we have already noted, in the considered class of scenarios, the optimal UAV trajectories in all cases represent some wave-like maneuver, so for control components it makes sense to look for suboptimal solutions in the form of a Fourier series decomposition on the interval [0; T ] along harmonic functions with some finite number N of harmonics, μ(t) ≈ a0 +

N  



ak sin(kω0 t) + bk cos(kω0 t) , for ω0 =

k=1

2π , T

where μ(t) is any of the components β(t) or γ(t). AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

119

Note that since optimal UAV flight trajectories do not have segments with intensive maneuvering, one can expect the absolute values of Fourier coefficients of control functions to decrease quickly. The relative increment δJ/J0 of the payoff functional when we pass from the optimal (with the functional value J0 ) to the trajectory parameterized by Fourier coefficients will be considered as the criterion of their proximity. Let us compare the relative increment of functional for all our computational experiments. For any low-frequency filtering of control functions the value of the functional increases, which provides an additional way to test that our solution is correct. Results of relative losses in the value of the functional depending on the number of Fourier coefficients which the optimal control Fourier decomposition is bounded with is given in Table 5 in the form of average relative losses in the functional values over all variables. Note that the higher is the initial UAV flight altitude, the more information we can extract from the resulting observations, and the less is the sensitivity of the functional’s deterioration to low-frequency filtering of control functions. This statement is additionally illustrated in Tables 6, 7, and 8, which show the mean deterioration of the payoff functional for a group of scenarios with initial UAV flight altitudes 1, 5, and 10 km respectively. As a result, in all scenarios one can achieve no more than 2 % relative losses in the payoff functional value by using the first two harmonics of yaw angle and a single harmonic of pitch angle. Besides, it turns out that the value of the functional is much more sensitive to changes in yaw angle than to changes in pitch angle. Indeed, if we look along the rows in any one of the Tables 6, 7, or 8, we see that the functional changes much more than when moving along the columns of these tables. Parameterizing the control with a finite Fourier series with two harmonics of the yaw angle and one harmonic of the pitch angle reduces the variational problem to the problem of finding the minimum of a function of eight variables. As the method for minimizing a function of several variables, it makes sense to use the simplex method (Nelder–Mead method) since it does not require us to compute the derivatives of function being optimized. To use this method, one need to perform an additional procedure related to the detalization of initial values for control parameters to start the iterative process. The procedure is as follows. • We begin by minimizing the function of the smallest possible number of Fourier coefficients. The physical sense of the resulting trajectory is that UAV moves with constant values of pitch and yaw angles. • On the next step, we increase the number of Fourier coefficients over which the functional is minimized. As the initial estimate for the optimized function’s arguments on this iteration we choose coefficients obtained on the previous step. As the initial estimate for newly added Fourier coefficients we choose zeros. • Sequentially increasing the number of coefficients first for yaw angle and then for pitch angle, we find Fourier coefficients that provide a local minimum of the optimized functional. Our choice to sequentially increase the coefficients starting from yaw angle is caused by the fact that the largest losses in the values of the functional occur when we restrict the number of harmonics for yaw angle. After we have constructed the initial control, the resulting trajectory is used to move for some small time interval τ , and then the optimal control estimate is updated for newly updated TME. On each following step, as the initial trajectory parametrization in the Nelder–Mead algorithm we use Fourier coefficients obtained before. We have studied the operation of this suboptimal method for control construction with simulation modeling. The role of quality parameters for the trajectories resulting with the method shown above is fulfilled by the following values. • Measure of proximity for trajectories obtained in different replicas with respect to the optimal trajectory (as such a proximity measure we have chosen the mean squared deviation of pitch and yaw angles from the average over all replicas of these angles depending on time). AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

120

ANDREEV, RUBINOVICH Table 5. Averages over all experiments for relative losses in the values of the functional (in %) for different number of Fourier harmonics which the control is bounded with in both pitch and yaw

Pitch angle

Number of harmonics 0 1 2 3 4 5

Yaw angle 0 56.53 52.46 51.55 51.19 51.00 50.89

1 4.35 3.84 3.71 3.66 3.63 3.62

2 1.30 0.97 0.88 0.85 0.83 0.82

3 0.74 0.45 0.37 0.34 0.32 0.31

4 0.59 0.31 0.23 0.20 0.19 0.18

5 0.54 0.26 0.18 0.15 0.14 0.13

Table 6. Averages over experiments for relative losses in the values of the functional (in %) for different number of Fourier harmonics which the control is bounded with in both pitch and yaw. The initial UAV flight altitude is 1 km

Pitch angle

Number of harmonics 0 1 2 3 4 5

Yaw angle 0 109.70 100.08 97.72 96.76 96.26 95.95

1 6.35 5.65 5.40 5.29 5.24 5.21

2 1.76 1.40 1.26 1.19 1.16 1.14

3 0.97 0.69 0.57 0.51 0.48 0.46

4 0.78 0.52 0.39 0.34 0.31 0.29

5 0.71 0.46 0.34 0.28 0.25 0.24

Table 7. Averages over experiments for relative losses in the values of the functional (in %) for different number of Fourier harmonics which the control is bounded with in both pitch and yaw. The initial UAV flight altitude is 5 km

Pitch angle

Number of harmonics 0 1 2 3 4 5

Yaw angle 0 37.24 34.82 34.48 34.36 34.30 34.26

1 3.89 3.15 3.03 2.99 2.97 2.95

2 1.33 0.76 0.66 0.63 0.61 0.60

3 0.87 0.35 0.25 0.22 0.20 0.19

4 0.77 0.25 0.16 0.13 0.11 0.10

5 0.74 0.23 0.13 0.10 0.09 0.08

Table 8. Averages over experiments for relative losses in the values of the functional (in %) for different number of Fourier harmonics which the control is bounded with in both pitch and yaw. The initial UAV flight altitude is 10km

Pitch angle

Number of harmonics 0 1 2 3 4 5

Yaw angle 0 22.64 22.47 22.46 22.46 22.45 22.45

1 2.81 2.72 2.71 0.71 2.71 2.71

2 0.82 0.75 0.74 0.74 0.73 0.73

3 0.37 0.30 0.29 0.29 0.29 0.29

4 0.22 0.15 0.14 0.14 0.14 0.14

AUTOMATION AND REMOTE CONTROL

Vol. 77

5 0.16 0.09 0.08 0.08 0.08 0.08

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

121

Fig. 7. Program flight trajectory UAV (solid line 1—the 3D-trajectory and its projection on the z = 0 plane) and a synthetic UAV flight trajectory obtained with the suboptimal method for control construction (thin line 2). The top graph shows view from above, the bottom graph shows a three-dimensional view of the trajectory. Line 3 shows the target’s motion trajectory.

• Average value (over all replicas) of the covariance matrix and its trace. • Ratio of the resulting trace of the average covariance matrix to the value of the payoff functional obtained by solving the optimal control problem. • Trace of the covariance matrix for the factual estimation errors x ˆ − x, i.e., ⎛

⎞

(ˆ x1 − x)∗ ⎜ ⎟ ··· Z =⎝ ⎠, ∗ (ˆ xN − x)

P = ZZ ∗ .

Computational experiments have been performed for target speed 15 m/s. The angle between the line connecting initial positions of UAV and target and UAV velocity vector was 45◦ . The UAV’s initial altitude was 5 km; flight speed, 360 km/h. Parameters of the experiments are shown in Table 9. The experiment was repeated 50 times with independent sequences of random numbers. The initial estimate of coordinates was constructed by the first 20 observations with maximal likelihood approach. For all 50 replicas, we found the average value of the control with which we constructed UAV flight synthetic trajectory. The result is shown on Fig. 7. Along the OZ axis, we show for clarity the change of UAV altitude from its start moment rather than absolute flight altitude. All the values are in kilometer. The synthetic trajectory constructed with the average control sufficiently precisely corresponds to the trajectory obtained with a numerical solution for the optimal control problem. As a measure of “variance” for the trajectory we considered how a AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

122

ANDREEV, RUBINOVICH Table 9. Parameters of the computational experiment on constructing UAV flight trajectory Parameter Value and dimension UAV flight speed m/s 100 15 m/s Target velocity Initial distance from UAV to target 20 km 5 km UAV initial altitude Mission completion time 210 s Accuracy of the azimuth σA 1.5 degrees 4.5 degrees Accuracy of the elevation angle σE Observation frequency 10 Hz Prior uncertainty in the motion speed σv 15 m/s 10 s Flight trajectory parameters update period τ Table 10. Results of the computational experiment on constructing suboptimal UAV flight trajectory Parameter Result Maximal mean squared deviation of the yaw angle 10 degrees Maximal mean squared deviation of the pitch angle 0.5 degrees 0.16553 Value of J in solving the optimal control problem Mean value of the estimate covariance matrix trace 0.16633 0.17031 Trace of the covariance matrix of deviations of target coordinate estimates from their true positions

mean squared deviation of control depends on the mean value for different replicas. As a result, a variation of yaw angle among different replicas did not exceed 10◦ (see Table 10), and in constructing the suboptimal UAV flight trajectory losses in the values of the functional were no more than 3 %. For τ = 10 s, the algorithm completed in one thread in at most 15 s (MATLAB interpreted code), without any optimizations whatsoever (like code compilation or parallel execution), so we conclude that the algorithm can work in real time. 6. SCENARIO WITH TWO UAV AND BEARING ONLY OBSERVATIONS Here we show the solution of the optimal control problem for two UAVs that perform localization of a moving target by bearing only observations and compare the accuracy of the resulting estimates with the accuracy of estimates obtained when measuring azimuth and elevation angles on board a single UAV. Note that in bearing measurements only control with respect to yaw angle makes sense. 6.1. Problem Setting The problem setting is shown on Fig. 8. Two UAVs starting from the same point and flying in the horizontal plane with constant speed have strictly synchronized clocks. Along the surface of the Earth, the target is moving uniformly along a straight line. Both UAVs know noisy bearing only observations of the target. In our notation above, motion equations for the UAV–target system have the following form: ⎧ ⎪ x˙ 1 (t) = V1 cos γ1 (t) − vx ⎪ ⎪ ⎪ ⎨ y˙ (t) = V cos γ (t) − v 1

1

1

y

(25)

⎪ x˙ 2 (t) = V2 cos γ2 (t) − vx ⎪ ⎪ ⎪ ⎩

y˙ 2 (t) = V2 cos γ2 (t) − vy ,

AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

123

Fig. 8. Control problem setting for two UAVs performing bearing only observations.

where (xi , yi ) are relative (with respect to the target) coordinates of the ith UAV, and Vi is its velocity, i = 1, 2. The target velocity vector (vx , vy ) is constant on the mission interval [t0 ; t0 + T ]. Since by assumption observations arrive synchronously, at every time moment we have two independent observation channels, and the linearized observation function has the form ⎛

⎞ y1 x1 0 0 2 2 2 + y 1 x1 + y 1 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ x2 y2 ⎜− ⎟ 0 0 ⎜ x2 + y 2 x2 + y 2 ⎟ H =⎜ ⎟. 1 1 1 1 ⎜ ⎟ ⎜ 0 0 0 0⎟ ⎜ ⎟

−

x21

⎝

⎠

0

0

0 0

This setting satisfies all assumptions made in the Appendix, so the system dynamics (coordinates of the two UAVs and elements of the TME estimates covariance matrix) are described by Eqs. (25) and (A.2). The Hamiltonian of system (HψP is defined by Eq. (A.3)) H = HψP + ψ11 (V1 cos γ1 − vx ) + ψ12 (V1 sin γ1 − vy ) +ψ13 (V2 cos γ2 − vx ) + ψ14 (V2 sin γ2 − vy ). To obtain the conjugate system, we write elements h1,2,3 of matrix Hxx that has a form similar to Eq. (15). As the covariance matrix of observations we have R=

2  σ1 0

0 σ22

,

where σi is the mean squared measurement error for the bearing direction to the target for the ith UAV, and y2 y2 +  22 , h1 =  12 r12 σ12 r22 σ22

x1 y 1 x2 y 2 h2 = −  2 −  2 , 2 2 r1 σ1 r22 σ22



x2 x2 h3 =  21 +  22 , r12 σ12 r22 σ22

where ri = x2i + yi2 . Now the conjugate system is described by the following differential equations. The first ten conjugate variables {ψ1 , . . . , ψ10 } that correspond to the dynamics of covariance matrix elements are defined by Eqs. (A.4), and the dynamics of conjugate variables {ψ11 , . . . , ψ14 } is defined AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

124

ANDREEV, RUBINOVICH

by system ⎧ ∂h1 ⎪ ⎪ ψ˙ 11 = −A ⎪ ⎪ ⎪ ∂x 1 ⎪ ⎪ ⎪ ⎪ ∂h ⎪ 1 ⎪ ⎪ ψ˙ 12 = −A ⎨ ∂y1 ⎪ ∂h 1 ⎪ ⎪ ψ˙ 13 = −A ⎪ ⎪ ⎪ ∂x2 ⎪ ⎪ ⎪ ⎪ ∂h ⎪ ⎪ ⎩ ψ˙ 14 = −A 1

∂h2 ∂x1 ∂h2 −B ∂y1 ∂h2 −B ∂x2 ∂h2 −B ∂y2 ∂y2 −B

∂h3 ∂x1 ∂h3 −C ∂y1 ∂h3 −C ∂x2 ∂h3 −C , ∂y2 −C

(26)

where coefficients A, B, and C are defined in Eq. (A.3). Partial derivatives to the case of a single UAV: 4y 2 xi ∂h1 = − i6 , ∂xi ri 







∂h2 yi 3x2i − yi2 = , ∂xi ri6 

are computed similar 

∂h3 2xi yi2 − x2i = , ∂xi ri6



2yi x2i − yi2 ∂h3 = , ∂yi ri6

∂hi ∂xj



∂h2 xi 3yi2 − x2i = , ∂yi ri6

∂h3 4x2 yi = − i6 . ∂yi ri

6.2. Numerical Results of Solving the Optimal Control Problem Similar to the previous problem, we will construct a flight trajectory for two UAVs such that the trace of the covariance matrix of TME estimates is minimal. According to Pontryagin’s maximum principle, a solution of the corresponding optimal control problem reduces to solving a two-point problem where half of the conditions is specified at the left end and is defined by the initial position of two UAVs with respect to the initial target estimate and elements of the covariance matrix at the initial time moment. On the right end, the boundary conditions follow from transversality conditions for Meyer’s problem (similar to (24)) ⎧ f ⎪ ψ ⎪ ⎨ 1,3,4,6

ψf ⎪ 2,5,7,8,9,10 ⎪ ⎩ f ψ11,12,13,14

= −1 =

(27)

0

= 0,

where superscript f denotes the set of conditions at the end point of UAV trajectory. When constructing flight trajectories for two UAVs, we use the computational procedure proposed in Section 4. Parameters of the experiment are given in Table 11. Trajectories of UAV flight are computed for the cases when the target speed is 15 m/s. The results are shown on Fig. 9. The axes show distances in kilometers. Table 11. Modeling parameters in the problem of constructing flight trajectories for two UAVs localizing a moving target with bearing only observations Parameter Flight speed for each UAV Accuracy of the azimuth (σA ) Observation frequency Initial distance from UAV to target Mission time Prior accuracy of estimating target coordinates (σx ) Prior accuracy of estimating target velocities (σv )

Value 100 26 10 20 205–215 10 15

Measurement unit m/s mrad Hz km s km m/s

AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

125

Fig. 9. Program flight trajectories for two UAVs and three different directions ϑ of the target’s motion.

Fig. 10. Dependence of the expected value of the functional of time in case of one (lines 1 and 3) and two (line 2) UAVs for the same configurations of modeling scenario. For a scenario with a single UAV, we show graphs for σE = 3σ0 , σA = σ0 (line 1) and for σA = σE = σ0 (line 3). We set σ0 = 1.5◦ .

Figure 10 shows how the payoff functional depends on time in scenarios that use a single UAV. Using two UAVs, we can significantly reduce the value of covariance matrix trace. The graph shows how the functional depends on time from the start of mission in the scenario where UAVs are at distance 20 km from the initial target location, the target speed is 15 m/s, and the target is moving away from UAVs with angle 45◦ to the initial line between UAV starting point and the target. Using two UAVs, we can get significantly more accurate estimates of the coordinates. Moreover, these estimates strongly depend on time for t → T . Figure 10 shows how the payoff functional depends on time for two UAVs (line 2) and for a single UAV observing the azimuth and altitude for 3σA = σE (line 1) and σA = σE (line 3). In case of two UAVs, when the difference of observation angles is close to π2 , the observation function is virtually linear, and observation error is given by the value σA × r, where r is the distance between UAV and target. Note that in case of two UAVs at the initial time moment (30th–60th seconds of flight), a value of functional virtually does not decrease (as compared to the case of a single UAV with equal observation accuracies for two angles). This is due to the fact that both flying vehicles get close values of observations, and it is hard to extract information on the distance to target from these observations. AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

126

ANDREEV, RUBINOVICH

Besides, centralized processing of observations is not always possible (this is usually due to restrictions on using communication lines between UAVs since such communication may unmask them), so we have to use additional algorithms to unite observation results (Track-to-Track fusion, see [8]). It is also important to note that for two UAVs the localization problem for many targets becomes significantly harder: the intersection of two bearing lines from each UAV form four intersection points, i.e., two possibilities for the target location. If the elevation angle is available for observation, this problem will not arise: in the absence of observation errors no two targets will be able to generate identical observations. 7. CONCLUSION For UUVs, we have considered the scenario of trajectory control for bearing only observations over a target moving uniformly along a straight line while simultaneously solving the main (pursuing the target or evading it) and auxiliary (estimating TMEs) problems. For UAVs, we have considered different ways to find the coordinates of moving target with both a single UAV observing azimuth and elevation angles and two UAVs observing azimuth angles. Program trajectories have been obtained with numerical solution of the optimal control problem in continuous time. It appears best to use a single UAV that observes two angles. The initial flight altitude of 5 km with initial distance to target of 20 km has led to the best results. We have shown that the optimal trajectory of a single UAV performing angular observations of the azimuth and elevation angles can be sufficiently accurately approximated with a finite harmonic Fourier series with a small number of coefficients. This has let us replace the search for solutions in the variational problem with minimizing a function of several variables (Fourier coefficients). It has turned out that using only two harmonics in the yaw angle and a single harmonic in the pitch angle leads to at most 2% losses in the values of the payoff functional. The proposed approach to constructing suboptimal trajectories can be used on board a UAV in real time. Using two UAVs starting from the same point restricts the possibilities of getting exact coordinate estimates for the target in the first seconds since the target cannot be observed. For the same accuracy of azimuth and elevation angle measurements, using a single UAV appears to be better than two since for sufficient initial altitude the target observability problem does not appear and, besides, the value of the payoff functional in case of a single UAV decreases faster with time. ACKNOWLEDGMENTS The work of E.Ya. Rubinovich was supported in part by the Russian Foundation for Basic Research, project no. 13-08-01096-a. APPENDIX KALMAN FILTER FOR A TARGET MOVING UNIFORMLY ALONG A STRAIGHT LINE 1. System of differential equations for the covariance matrix. According to Eqs. (15), (16), and (17), dynamics equations for the elements of the covariance matrix (Eq. (14)) can be represented as

∗

FP + PF =

0 I



0 0

∗

PH R

−1

∗

HP =

∗ Pxx Pxv



Pxv Pvv ∗ Pxx Pxv

Pxv Pvv



+

∗ Pxx Pxv

Pxv Pvv

Hxx 0 0

0





0 0



,

I 0

∗ Pxx Pxv



Pxv Pvv

AUTOMATION AND REMOTE CONTROL

. Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

127

Finally:

P˙ =

∗ −P H P ∗ Pxv + Pxv xx xx xx Pvv − Pxx Hxx Pxv ∗ −Pxv Hxx Pxv

Pvv − Pxv Hxx Pxx



.

(A.1)

Next we rewrite the dynamics of covariance matrix elements as a system of differential equations for each matrix element. Multiplying for this individual matrix blocks in Eq. (A.1), we get ⎧ p˙ xx = 2pxvx −p2xx h1 −2pxx pxy h2 −p2xy h3 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ p˙ xy = pxvy +pvx y −pxx pxy h1 −(p2xy +pxx pyy )h2 −pxx pyy h3 ⎪ ⎪ ⎪ ⎪ ⎪ p˙ yy = 2pvy y −p2xy h1 −2pxy pyy h2 −p2yy h3 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪p˙ vx vx = −p2xvx h1 −2pvx y pxvx h2 −p2vx y h3 ⎪ ⎪ ⎪ ⎪ ⎨p˙ v v = −pxvy pxvx h1 −(pvy y pxvx +pxvy pvx y )h2 −pvy y pvx y h3 x x ⎪ −p2xvy h1 −2pvy y pxvy h2 −p2vy y h3 ⎪p˙ vy vy = ⎪ ⎪ ⎪ ⎪ ⎪ p˙ xvx = pvx vx −pxvx pxx h1 −(pvx y pxx +pxvx pxy )h2 −pvx y pxy h3 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ −pxvy pxx h1 −(pvy y pxx +pxvy pxy )h2 −pvy y pxy h3 ⎪p˙ xvy = pvx vy ⎪ ⎪ ⎪ ⎪ p˙ vx y = pvx vy −pxvx pxy h1 −(pvx y pxy +pxvx pyy )h2 −pvx y pyy h3 ⎪ ⎪ ⎪ ⎩

p˙ vy y =

pvy vy

−pxvy pxy h1

(A.2)

−(pvy y pxy +pxvy pyy )h2 −pvy y pyy h3 .

2. The conjugate system of differential equations for the Kalman filter. In solving the optimal control problem, we have to construct the Hamiltonian of the system and find the system of differential equations for the conjugate variables. When tracking a target moving uniformly along a straight line in the OXY plane, ten variables correspond to elements of the covariance matrix (Eq. (A.2)). Let us write the part of the Hamiltonian corresponding to the dynamics of elements of the covariance matrix (Eq. (A.2)) and conjugate variables (ψ1 , . . . , ψ10 ), as HψP = −Ah1 − Bh2 − Ch3 + D, where

(A.3)

⎧ A = p2xx ψ1 + pxx pxy ψ2 + p2xy ψ3 + p2xvx ψ4 + pxvy pxvx ψ5 + p2xvy ψ6 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ +pxvx pxx ψ7 + pxvy pxx ψ8 + pxvx pxy ψ9 + pxvy pxy ψ10 ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ 2 +p p ⎪ B = 2p p ψ + p ψ2 + 2pxy pyy ψ3 ⎪ xx xy 1 xx yy xy ⎪ ⎪   ⎪ ⎪ ⎪ +2p p ψ + p p + p p ψ5 + 2pvy y pxvy ψ6 v y xv 4 v y xv xv v y ⎪ x x y x y x ⎪ ⎪   ⎪ ⎨ + (pvx y pxx + pxvx pxy ) ψ7 + pvy y pxx + pxvy pxy ψ8   ⎪ + (pvx y pxy + pxvx pyy ) ψ9 + pvy y pxy + pxvy pyy ψ10 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ C = p2xy ψ1 + pxy pyy ψ2 + p2yy ψ3 + p2vx y ψ4 + pvy y pvx y ψ5 + p2vy y ψ6 ⎪ ⎪ ⎪ ⎪ ⎪ +pvx y pxy ψ7 + pvy y pxy ψ8 + pvx y pyy ψ9 + pvy y pyy ψ10 ⎪ ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ D = 2pxvx ψ1 + pxvy + pvx y ψ2 + 2pvy y ψ3 ⎪ ⎪ ⎪ ⎩

+pvx vx ψ7 + pvx vy (ψ8 + ψ9 ) + pvy vy ψ10 .

The system of conjugate equations can be found by computing partial derivatives of the Hamiltonian 

ψ˙ 1 , . . . , ψ˙ 10

AUTOMATION AND REMOTE CONTROL

∗

Vol. 77

=− No. 1

∂H . ∂p 2016

128

ANDREEV, RUBINOVICH

As a result, we get the following system of differential equations for the conjugate variables: ⎧ ˙ ⎪ ⎪ ⎪ψ1 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ψ˙ 2 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ψ˙ 3 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ψ˙ 4 ⎪ ⎪ ⎪ ⎪ ⎨ψ˙ 5





= h1 2pxx ψ1 + pxy ψ2 + pxvx ψ7 + pxvy ψ8 + h2 2pxy ψ1 + pyy ψ2 + pvx y ψ7 + pvy y ψ8 







= h1 pxx ψ2 + 2pxy ψ3 + pxvx ψ9 + pxvy ψ10  + h2 2pxx ψ1 +2pxy ψ2 +2pyy ψ3 +pxvx ψ7 +p ψ +p ψ +p ψ  xvy 8 vx y 9 vy y 10 + h3 2pxy ψ1 + pyy ψ2 + pvx y ψ7 + pvy y ψ8 = h2 pxx ψ2 + 2pxy ψ3 + pxvx ψ9 + pxvy ψ10 + h3 pxy ψ2 + 2pyy ψ3 + pvx y ψ9 + pvy y ψ10 = −ψ7 = − (ψ8 + ψ9 )

(A.4)

⎪ ψ˙ 6 = −ψ10 ⎪ ⎪ ⎪ ⎪   ⎪ ⎪ ψ˙ 7 = h1 2pxvx ψ4 + pxvy ψ5 + pxx ψ7 + pxy ψ9 ⎪ ⎪ ⎪ ⎪ ⎪ + h2 2pvx y ψ4 + pvy y ψ5 + pxy ψ7 + pyy ψ9 − 2ψ1 ⎪ ⎪ ⎪   ⎪ ⎪ ˙ ⎪ ⎪ψ8 = h1 pxvx ψ5 + 2pxvy ψ6 + pxx ψ8 + pxy ψ10 ⎪ ⎪ ⎪ + h2 2pvx y ψ5 + pvy y ψ6 + pxy ψ8 + pyy ψ10 − ψ2 ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ ψ˙ 9 = h2 2pxvx ψ4 + pxvy ψ5 + pxx ψ7 + pxy ψ9 ⎪ ⎪ ⎪ ⎪ ⎪ + h3 2pvx y ψ4 + pvy y ψ5 + pxy ψ7 + pyy ψ9 − ψ2 ⎪ ⎪ ⎪   ⎪ ⎪ ψ˙ 10 = h2 pxvx ψ5 + 2pxvy ψ6 + pxx ψ8 + pxy ψ10 ⎪ ⎪ ⎪ ⎩

+ h3 2pvx y ψ5 + pvy y ψ6 + pxy ψ8 + pyy ψ10 − 2ψ3 . REFERENCES

1. Nardone, S.C. and Aidala, V.J., Optimization of Observer Trajectories for Bearings-Only Target Localization, IEEE Trans. Aerospace Electron. Syst., 1981, vol. AES-17, no. 2, pp. 162–166. 2. Grigor’ev, F.N., Kuznetsov, N.A., and Serebrovskii, A.P., Upravlenie nablyudeniyami v avtomaticheskikh sistemakh (Controlling Observations in Automated Systems), Moscow: Nauka, 1986. 3. Koks, D., Numerical Culations for Passive Geolocation Scenarios, in Defense Science and Technology Organization Research Report, DSTO-RR-0319, Australia, 2009, http://www.dsto.defence. gov.au/publications/scientific record.php?record=4559. 4. Miller, A. and Miller, B., Tracking the UAV Trajectory on the Basis of Bearing-Only Observations, Proc. 53rd IEEE Conf. Decision and Control (CDC 2014), Los Angeles, 2014, pp. 4178–4184. 5. Zajic, T. and Mahler, R.P.S., Particle-Systems Implementation of the PHD Multitarget Tracking Filter, Proc. SPIE Conf. Series, 2003, vol. 5096, pp. 291–299. 6. Stepanov, O.A. and Toropov, A.B., Applying Sequential Monte Carlo Methods with Analytic Integration Procedures in Processing Navigational Information, Proc. XII Russian Meeting on Control Problems, Moscow: 2014, pp. 3730–3740. 7. Lanneuville, D. and Houssineau, J., Passive Multi-Target Tracking with GM-PHD Filter, Proc. 13th Conf. Inform. Fusion, 2010, pp. 1–7. 8. Bar-Shalom, Y., Willett, P.K., and Tian, X., Tracking and Data Fusion: A Handbook of Algorithms, Storrs: YBS-Press, 2011, http://books.google.ru/books?id=2aOiuAAACAAJ. 9. Lin, X., Kirubarajan, T., Bar-Shalom, Y., et al., Comparison of EKF Pseudomeasurement and Particle Filters for a Bearing-Only Target Tracking Problem, Proc. SPIE Conf. Series Signal and Data Processing of Small Targets, Oliver E. Drummond, Ed., 2002, vol. 4728, pp. 240–250. 10. Liu, P.T., An Optimum Approach in Target Tracking with Bearing Measurements, J. Optimiz. Theory Appl., 1988, vol. 56, no. 2, pp. 205–214. 11. Hammel, S.E., Liu, P.T., Hilliard, E.J., et al., Optimal Observer Motion for Localization with Bearing Measurements, Comput. Math. Appl., 1989, vol. 18, no. 3, pp. 171–180. AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016

MOVING OBSERVER TRAJECTORY CONTROL

129

12. Oshman, Y. and Davidson, P., Optimization of Observer Trajectories for Bearings-Only Target Localization, IEEE Trans. Aerospace Electron. Syst, 1999, vol. 35, no. 3, pp. 892–902. 13. Miller, S.A., Harris, Z.A., and Chong, E.K.P., A POMDP Framework for Coordinated Guidance of Autonomous UAVs for Multitarget Tracking, EURASIP J. Adv. Signal Process., Special Issue on Signal Processing Advances in Robots and Autonomy, 2009, January, pp. 1–7. 14. Moiseev, N.N., Elementy teorii optimal’nykh sistem (Elements of Optimal System Theory), Moscow: Nauka, 1975. 15. Letov, A.M., Dinamika poleta i upravlenie (Flight Dynamics and Control), Moscow: Nauka, 1969. 16. Menegozzi, L.N., Harding, A.C., and van Alstine, E.F., Azimuth and Elevation Direction Finding System Based on Hybrid Amplitude/Phase Comparison, US Patent 6 061 022, 2000, http:// www.google.com/ patents/US6061022. 17. Adamy, D.L., EW 103: Tactical Battlefield Communications Electronic Warfare, Boston: Artech House, 2009, http://books.google.ru/books?id=7793SVcHOS8C. 18. Ponda, S.S., Kolacinski, R.M., and Frazzoli, E., Trajectory Optimization for Target Localization Using Small Unmanned Aerial Vehicles, AIAA Guidance, Navigation and Control Conf., Chicago, 2009. 19. Chernous’ko, F.L. and Banichuk, N.V., Variatsionnye zadachi mekhaniki i upravleniya. Chislennye metody (Variational Problems of Mechanics and Control. Numerical Methods), Moscow: Nauka, 1973.

This paper was recommended for publication by A.P. Kurdyukov, a member of the Editorial Board

AUTOMATION AND REMOTE CONTROL

Vol. 77

No. 1

2016