Target Tracking in 3-D Using Estimation Based Nonlinear Control Laws for UAVs †

This paper presents an estimation based backstepping like control law design for an Unmanned Aerial Vehicle (UAV) to track a moving target in 3-D space. A ground-based sensor or an onboard seeker antenna provides range, azimuth angle, and elevation angle measurements to a chaser UAV that implements an extended Kalman filter (EKF) to estimate the full state of the target. A nonlinear controller then utilizes this estimated target state and the chaser’s state to provide speed, flight path, and course/heading angle commands to the chaser UAV. Tracking performance with respect to measurement uncertainty is evaluated for three cases: (1) stationary white noise; (2) stationary colored noise and (3) non-stationary (range correlated) white noise. Furthermore, in an effort to improve tracking performance, the measurement model is made more realistic by taking into consideration range-dependent uncertainties in the measurements, i.e., as the chaser closes in on the target, measurement uncertainties are reduced in the EKF, thus providing the UAV with more accurate control commands. Simulation results for these cases are shown to illustrate target state estimation and trajectory tracking performance.


Introduction
Target tracking by Unmanned Aerial Vehicles (UAVs) is an important area of study, especially when it applies to tracking a target in 3-D space.Tracking capabilities of a UAV depend on many factors but we focus on how accurately it can obtain the target state information using onboard sensors, and how effectively it can generate its control commands so as to achieve tracking.For control laws that require full state information of a target, it is thus necessary that the target state information be reconstructed based on limited measurements observed by the chaser UAV.These measurements are often corrupted by external disturbances (wind gust, turbulence).In a target state estimation problem, the choice of the appropriate model to represent the target motion is also not clear.The classical approaches either use a constant velocity model leading to an α-β tracking filter [1,2], or a constant acceleration model that leads to the α-β-γ tracking filter [1,2].Mehrotra et al. [3] have proposed the use of a constant jerk model that allows for changes in acceleration of the target thereby accommodating a richer class of target maneuvers.This research focuses on an extended state kinematic model for the target with the objective to determine the location, flight path angle and course/heading of the target vehicle when full state information is not available or achievable due to limitation of sensors and their capabilities.In this study, the framework of an estimation based control architecture is presented, and careful considerations are made to achieve desired tracking performance.
The design of guidance and control algorithms for unmanned vehicles has received considerable attention in recent years.Many guidance and control techniques have been investigated by researchers: vector field approach [4][5][6][7][8][9][10], line of sight algorithms [11], Serret-Frenet representation based methods [6,12], feedback linearization [13][14][15] and thebackstepping approach [16][17][18].This paper explores the implementation of a nonlinear guidance and control design technique for a UAV to track a target in 3D environment.The nonlinear guidance controller requires full-state information of the target for implementation.We assume only the range, azimuth and elevation angle to be available.Thus the implementation requires a state estimator and it is the reconstructed target state that is employed in the guidance and control laws.We use a backstepping like approach for the controller design, the mathematical development of which is reported in [19,20].The backstepping control technique pioneered by Kokotovic [21] et al. has been studied by researchers in areas of trajectory tracking [16], bank-to-turn control [17], and missile guidance [18].Ren et.al. [16] have used the backstepping based control design in a 2D trajectory tracking case.In comparison, the main contribution of this paper is the implementation of a reconstructed state based nonlinear controller for 3D case where a chaser UAV tracks an arbitrary moving target in 3D space.The implementation for the full target state information case was shown in [19,20,22].
For target state estimation, an extended Kalman filter (EKF) is chosen.The Kalman filter approach is a commonly used estimation technique in many applications that include vision based estimation and target tracking [23], multi-agent consensus [2,[24][25][26], aerodynamic parameter estimation [27], relative position and attitude estimation for docking of spacecraft [28], vibration mitigation associated with sensors placed on vibrating structures [29], and orbital rendezvous [30].This is integrated with the controller so that it can use the target measurements (range, azimuth and elevation) received from sensors and estimate the full state of the target (three components of the position vector, speed, flight path angle and the heading/course angle).The target state estimate is then fed into the controller so as to achieve tracking successfully.Our initial study in [22] presented an EKF based target state estimation for the case where the measurements are corrupted by Gaussian white noise.The EKF linearizes the non-linear state dynamics and measurements around the last predicted and filtered state estimates at each cycle, which then eventually provides an approximation of the optimal state estimates.The Kalman filter for linear systems is known to be optimal (minimizes variance of the estimation errors) under the assumption that the process and measurement noise are Gaussian white noise and mutually uncorrelated.
However, white noise is not a realistic assumption in real world applications and therefore, an extra effort is required to handle the uncertainties when they are correlated in observations.The sample to sample measurement noises are correlated for high frequency measurement systems.One such example of correlated errors associated with GPS measurements is the multipath effect [31].Recently, a maneuvering target tracking problem has been investigated with observations subject to colored noise [32].In this paper, we also design an EKF where measurement uncertainties are highly correlated in the form of colored noise.The colored-noise filters are designed by incorporating first order shaping filters driven by Gaussian white noise [2].The system state is augmented to include the filter state, and the EKF implementation follows the same procedure as with the white noise case.We investigate the controller performance when the sensor measurements for range, azimuth angle, and elevation angle are corrupted by colored noise.The simulation results presented in the paper cover three interesting scenarios such as: (A) target moving horizontally at constant altitude; (B) moving vertically upwards at constant speed; and (C) moving in a helical path.Note all three cases are handled within the same framework and one consistent target model as opposed to using multiple models.It is worth mentioning that scenario (B) corresponds to an out of the plane tracking maneuver wherein the chaser needs to maneuver out of the horizontal plane to intercept the target.Simulation results are also summarized for a case when the measurement covariance associated with the target measurement changes as a function of the line of sight range between the chaser and the target.The main contribution of our work is the aspect of implementation of a nonlinear control law within a 3-D target tracking framework that makes use of reconstructed states as opposed to full state feedback.
The rest of the paper is organized as follows: Section 2 describes the problem being solved and presents the development of estimation based control architecture; Section 3 provides simulation results of the proposed development for several scenarios; and Section 4 provides a summary and conclusions based on our work in this paper.

Problem Description
The mathematical formulation of the estimation based controller is presented where a chaser UAV tracks a target autonomously based on the information available from onboard sensors about the target and the chaser.The chaser UAV is assumed to be equipped with on board sensors which provide the range (r), azimuth angle (θ), and elevation angle (φ) measurements of the target UAV.Target measurements are obtained by propagating the following kinematic equation: where, (x r , y r , z r ), v gr , γ r and χ r denote the reference position, ground speed, flight path angle and course (ground track) angle respectively.v gr , γ r and χ r are specified functions of time ∈ C ∞ .Note, the ground speed of the target can be also be expressed as For a rich set of trajectories, v gr , γ r and χ r could be arbitrary functions of time.
The measurement model for the target UAV is considered as follows: where ỹrk (X rk ) = [r rk , φrk , θrk ] T are the discrete-time measurements which are assumed to be available from a ground based sensor or from on-board sensors, h rk (X rk ) = [r rk , φ rk , θ rk ] T is the discrete function vector and the expressions for r rk , φ rk , and θ rk are of the following form: is the noise vector where v rrk , v φrk , v θrk are the measurement noises in range, azimuth, and elevation angle measurements respectively at the k time step.
The following kinematic model is considered for chaser UAV: where (x, y, z) is the vehicle position in inertial reference frame, v g is the magnitude of the velocity vector, χ is the course or ground track/heading angle and γ is the flight path angle.c 1 , c 2 , and c 3 (> 0) are the time constants associated with the dynamics.The magnitude of the velocity is: v g = ẋ2 + ẏ2 + ż2 .The three control inputs in the model: v c g , γ c , χ c are the commanded speed, flight path angle and course angle respectively that are computed by the controller.
Given the above governing equations for the target and the chaser as well as the measurement model we seek to implement a nonlinear guidance and control law that was previously shown to provide asymptotic tracking performance for arbitrary target maneuvers provided complete target state information was available for feedback.In the present problem, the control commands are synthesised using a reconstructed target state estimated from the target measurements and the performance of the estimation based controller is illustrated in simulation.
The nonlinear controller to be employed in this paper is summarized below.

Backstepping Based Controller
In [19,20], a backstepping based controller was proposed for a UAV that can track a dynamic moving target in 3D for the case when the target full state information is perfectly known.For this problem, firstly stable position error dynamics are specified and the desired virtual control commands are derived for ground speed , and ground heading/course angle, χ d so as to make the position error to zero exponentially.In the next step, the same technique is applied again for synthesizing a ground speed command v c g , to ensure that the actual ground-speed v g tracks the desired speed v d g .Similarly a flight path angle command γ c , and ground heading angle command χ c are synthesized.The mathematical derivation of the controller and Lyapunov based stability analysis can be found in [19,20].Thus, the proposed controller generates three commands: speed, flight path angle, and ground heading/course angle, respectively, as follows: We can calculate v1 d , v2 d and χd from the following: and therefore, the derivatives are as follows: where v 1 = v g cos γ, v 2 = v g sin γ, the position errors are: e x = x − x r , e y = y − y r , e z = z − z r , and the off-manifold errors are: ṽ1 user specified control gains associated with the control laws.
As mentioned before, if the presence of full target state information, the above control law guarantees asymptotic stability of the state tracking errors.In the next subsection, we assume that all the states of the target are not available for measurement and hence an estimation based controller is implemented, wherein an EKF is integrated that provides estimates of the target state to be utilized in the controller.

Case 1: Target measurements corrupted by stationary white noise
The measurements for range, azimuth angle, and elevation angle are available from on-board or ground based sensors.We utilize these measurements in the EKF to estimate the target's six states that include the three position states x r , y r , z r , flight path angle γ r , and course angle χ r .The EKF provides the estimates of the system states using available measurements and a suitable target model while assuming a priori known statistical models for the system and measurement noises.The control algorithms developed as described above use these state estimates to generate the control commands for target tracking.
We consider the discrete-time measurement model as in Equation ( 2) which is affected by the measurement uncertainties, assumed to be zero-mean Gaussian white noise with known covariance.We also consider that the dynamic model for target UAV is corrupted by white noise with known covariance.Therefore, the continuous-time state model and the discrete-time measurements for target UAV can be written as follows: where X r = [x r , y r , z r , v gr , γ r , χ r ] T is the state vector; u r (t) = v c gr , γ c r , χ c r T is the control input vector, where v c gr is the commanded ground speed, γ c r is the commanded flight path angle, and χ c r is the commanded heading angle; ỹrk is the measurement vector; w r = [0, 0, 0, ω vgr , ω γr , ω χr ] T ∼ N(0, Q r (t)) where, Q r (t) = E{w r (t)w T r (t)} and ω vgr , ω γr , and ω χr are Gaussian white noise components that represent the dynamic uncertainties affecting the system; We further assume that the process noise and the measurement noise are uncorrelated.
The expression for the nonlinear continuous-time function vector f r (X r (t), u r (t), t) used in Equation ( 9) is: and the matrix G r (t) = [0 3×3 I 3×3 ] T where I is the Identity matrix.The standard EKF equations used for target state estimation based on reference [2] are listed in Table 1.The system state X in the EKF for this case is replaced by X r .Kalman gain: Error covariance is propagated by: Ṗ(t) = F( X(t), t)P(t) Assume, f r (.), and h r (.) are locally differentiable.We can determine the state Jacobian matrix F r ( Xr (t), t) = ∂f r ∂X r | Xr (t) as: where cos γr cos χr − vgr sin γr cos χr − vgr cos γr sin χr cos γr sin χr − vgr sin γr sin χr vgr cos γr cos χr sin γr vgr cos γr 0 and the measurement Jacobian matrix H k ( Xr (k)) = ∂h r ∂X r evaluated at Xrk as follows: where The EKF estimator above will synthesize, xr , ŷr , ẑr , vgr , γr , χr .Additional derivatives needed by the controller in Equations ( 5)-( 7) are obtained by using a 'derviative estimation filter' that synthesizes the numerical derivatives [33].To synthesize the derivative of any state, say, z f , the following derivative state estimation filter equations are augmented, where żd f and zd f are the filter states for the derivative state estimation.The values of the constants a f , b f , c f , d f are determined numerically using simulations to improve the signal to noise ratio of the derivative estimates.The above-mentioned "derivative estimation filter" is applied to the EKF synthesized state vector to obtain the necessary state derivatives that are then used in the nonlinear controller shown in Equations ( 5)- (7).

Case 2: Target measurements corrupted by stationary colored (non-white) noise
A continuous-discrete EKF is designed when partial target information is available to the chaser to estimate the target full states.Assume that the target measurements are corrupted by colored noise.Colored noise is propagated by solving a first order differential equation with band limited white noise as an input into it.We can write the augmented system dynamics which includes the target kinematic model and colored noise dynamics.
Assume the measurements are corrupted by Gaussian non-white uncertainties.The following three measurements about the target, i.e., (range (r), azimuth angle (φ), and elevation angle (θ)) are available to the chaser and the associated noise vector v rk (Equation ( 9)) is non-white.This non-white measurement noise vector v rk is modeled by the following first-order shaping filter equation which is driven by zero-mean band limited white noise, ν r f k : where, Z r = [z rr z φr z θr ] T is the state vector of the shaping filter (z rr for the range measurement, z φr for the azimuth angle measurement and z θr for the elevation angle measurement).
w r f ∈ R 3 is the Gaussian white noise vector in the shaping filter dynamics, and v r f k ∈ R 3 is the Gaussian white noise vector in the measurement noise equation above.The terms f r f = F r f Z r and h r f = H r f Z r are defined for convenience to integrate the above into the model to be used to set up the EKF.Thus the state and measurement equations for the augmented system can be re-written as follows: where, w ra (t) Assume that w r , and w r f are uncorrelated, i.e., the process noise covariance matrix Q ra (t) for the augmented system can be written by: The measurement covariance matrix associated with the augmented measurement model is: We can then essentially apply the same set of filter equations which are listed in Table 1 for the augmented system.In this case, for the augmented system, X is replaced by X ra .The corresponding Jacobian matrices are given below: State Jacobian matrix: F ra ( Xra (t), t) = ∂f ra ∂X ra | Xra (t) is written as follows: The measurement Jacobian matrix, H rak ( Xrak ) = ∂h ra ∂X rak | Xrak and can be expressed as:

Case 3: Target measurements corrupted by non-stationary white noise with range dependent covariance
In practice, the measurement covariance is not necessarily constant.To implement a more realistic scenario for target tracking, we include the following variance function associated with the target range measurement: where a 0 > 0, a 1 > 0 a 2 > 0, and r is the distance between the target and chaser.The above function in Equation ( 15) is utilized in [34] to represent the variance of range measurement noise.Note, the error variance is getting smaller as the chaser closes onto the target and when it reaches to the target it has minimum error variance a 0 .The variances for azimuth angle and elevation angle measurements are also affected as the angular resolution of the target improves as the chaser comes closer to the target.Hence, the variances for azimuth and elevation angle can be written as follows: where α φ > 0, and α θ > 0. The covariance matrix for target measurement becomes ).If the parameters are chosen as a 0 = 1 m 2 , a 1 = 1 m 2 , a 2 = 0.0024 m 2 , α φ = 0.01(rad/m) 2 , α φ = 0.01(rad/m) 2 , and r varies from 0 to 100 m, we can easily see from Figure 1 that the variances for range, azimuth angle, and elevation angle become minimum at r = 1 m.

Simulation Case Studies
The estimation based controller is implemented for the three cases as before.The estimation errors are shown together with the 3 − σ bounds obtained from the estimation error covariance matrix.

Case 1: Target measurements corrupted by stationary white noise
For this case, we simulate the tracking scenario for three different types of trajectories

A
Straight line trajectory at constant altitude, B Straight line trajectory along the z-axis(out-of-plane maneuver), and C Helical trajectory The parameters used in simulation: For each case, the simulation was run for 60 s and the data was updated at ∆t = 0.05 s interval, i.e., the update rate is 20 Hz.The controller continuously updates three control commands based on the estimated target and chaser UAV states.The transfer function used to approximate the estimated target state derivatives is chosen to be: The parameters used for the target and chaser UAV are as follows: For the target UAV, the initial position for all these cases are: x r (0) = 100 m, y r (0) = 100 m, z r (0) = 100 m; the initial speed is: v gr (0) = 10 m/s.We choose the following parameters to generate 3 different target trajectories: The variance and standard deviation of noise associated with the sensors for target measurements are listed in Table 2.The chaser UAV is initialized as follows: v g (0) = 8 m/s, γ(0) = 0 • , χ(0) = 0 • , x(0) = 50 m, y(0) = 50 m, z(0) = 50 m.For Case 1, the target state estimation filter is implemented using a constant measurement and process covariance.The initial error covariance for target: P 0 = I 6×6 where I is an identity matrix.The covariance matrix of process noise for the target is: Q r = 2 × diag([1, (0.57π/180) 2 , (0.57π/180) 2 ]), and the covariance matrix of measurement noise for target is: R rk = 0.75diag 1, (0.57π/180) 2 , (0.57π/180) 2 .
The simulation results shown in Figures 2-6,  show that tracking is achieved with reasonable accuracy.The EKF provides the target state estimate closer to the target true value, and all errors are within 3 − σ error bounds, i.e., the filter shows satisfactory performance with reasonable accuracy.

Case 2: Target measurements corrupted by stationary colored (non-white) noise
Range and line-of-sight measurements (e.g., on-board seeker antenna) are typically corrupted due to uncertainties typically arising from eclipsing, radar cross section variation, and variation in the line-of-sight rate at high chaser-target ranges.It is common practice to model these uncertainties using colored (non-white) noise.The simulation results for target tracking with the target measurements corrupted by colored noise are shown in Figures 17-21.We only show the results for trajectory C (helical trajectory).The power spectral density associated with the colored noise uncertainties are exponentially correlated functions as we can see in Figure 19.The filter performance is quite good, i.e., all state errors (true-estimated) are within 3 − σ bounds as shown in Figures 17-18.The controller gains and actuator constants used in simulation are the same as in the previous case.The variances associated with measurement noises are: σ 2 r = 4 m 2 , σ 2 φ = (0.65π/180) 2 rad 2 , and σ 2 θ = (0.65π/180) 2 rad 2 .The initial error covariance for target is P r0 = 10 × I 9×9 .The process noise covariance matrix Q ra (t) for target is calculated using Equation (13).The following matrices are needed to calculate Q ra (t), and the simulation is executed for parameters listed in Table 3: The measurement noise covariance matrix used in EKF for target: R rk = 0.75diag 4, (0.65π/180) 2 , (0.65π/180) 2

Case 3: Target measurements corrupted by non-stationary white noise with range dependent covariance
The simulation is performed for the case when the measurements are corrupted by non-stationary white noise uncertainties with range dependent covariance.The parameters chosen to calculate the variances for range, azimuth angle, and elevation angle as: a 0 = 1 m 2 , a 1 = 1 m 2 , a 2 = 0.0024 m 2 , α φ = 1e

Conclusions
This work presented an estimation based nonlinear controller for UAVs which can track 3D trajectories using imperfect state knowledge of the target.A continuous-discrete extended Kalman filter was designed to estimate the states of a target UAV and the controller generates the control signals for speed, flight path angle, and course angle to propagate the chaser motion for tracking.The controller performance is shown for three different target trajectories.The proposed architecture can be generalized to track arbitrary target trajectories.We can conclude that the proposed estimation based controller achieves the tracking of target UAV defined in 3D space and shows robust performance to stationary/non-stationary white and colored measurement noise.

Figure 1 .
Figure 1.Measurement error variances as a function of the range.

Figure 19 .
Figure 19.Power spectral density function over the frequency range (Case 2C).

Figures 20 and 21
Figures 20 and 21 clearly show satisfactory tracking performance of the controller.

Table 2 .
Minimum variances and standard deviations of measurement noises used in simulation (target).

Table 3 .
Variances of process noises used in simulation (target).