1

Estimation of the position of a moving target using the Extended Kalman Filter
Jorge Quijano
Cartesian coordinates would imply the use of a non-linear transformation to the polar estimates. For this reason, the problem was stated in Cartesian coordinates as shown in (1). Two approaches for estimating the position and velocity of the moving target have been explored. In the first approach, an Extended Kalman Filter (EKF) has been implemented to smooth the noisy observations of the horizontal range and the bearing angle. The final result is given in Cartesian coordinates. For the second approach, the same set of measurements was applied to a particle filter. Due to the large variation in the angle observation, a Cauchy distribution was assumed for the model of the observations. It was observed that the particle filter followed the noisy observations instead of providing a soother estimate. The objective of this paper is to analyze the performance of the EKF and the PF when used to smooth a set of noisy observations and when the noise level changes over time.

AbstractA set of noisy observations of the movement of a ship obtained with an active sonar system has been used to estimate the position of the ship with respect to a non-moving target. An extended Kalman filter (EKF) and a particle filter(PF) are utilized as smoothing filters to obtain an estimate of the Cartesian coordinates that describe the position of the ship. The goal of this paper is to demonstrate the capabilities of the EKF and the PF to smooth observations affected by noise and to compare the performance of the EKF with the PF, assuming a modeling error with a Gaussian distribution. Index TermsExtended Kalman smoothing, missing data, tracking. filter, Particle filter,

I. INTRODUCTION
CTIVE sonar can be used to track the position and velocity of moving objects under water. The analysis of active sonar signals is of great importance because it has application in many areas such as fishing, mapping of the ocean bottom and defense. Fig. 1 shows a map of the area where the experiment took place and a detail of the route followed by the ship. The speed of the ship is intended to be constant (with a random acceleration component), and there are two points where the direction of the ship changes. The horizontal range is the distance from the origin to each point of the ships trajectory. The bearing angle is measured with respect to the x axis. In real applications, the ship equipped with the active sonar system tracks the position of a moving target, but for this paper, the target is fixed and the ship is moving with respect to the origin, and the goal is to follow the trajectory. This problem is equivalent to having a non-moving ship tracking a moving target In a typical sonar system, the observation of the range is not directly available, as described in [1]. Instead of the horizontal range, a measurement of the traveling time for an acoustic wave is used to estimate the range with a non-linear transformation. In this paper, the observation of the range was readily available, and the problem could be solved just by using the ordinary Kalman Filter to get estimates of range/bearing angle in polar coordinates. Nevertheless, the transformation to Jorge Quijano is a graduate student at Portland State University, Portland Oregon.(e-mail: author@pdx.edu). This work has been submitted as a term paper for the class ECE 510 Statistical Signal Processing. Winter, 2006.

Initial position

Fig. 1. Location of the Malta Channel and diagram of the route followed by a ship towing an active sonar system. Measurements of the horizontal range and bearing angle between the ship and the target are available at a rate of 1 measurement/minute.

II. METHODOLOGY A. Data acquisition The data has been obtained at the North West Electromagnetics and Acoustics Research Lab (NEAR Lab), at Portland State University. It was generated by the Naval Undersea Warfare Center Division and an unclassified database has been released for research purposes.

2
TABLE I INFORMATION AVAILABLE IN THE SONAR DATABASE Data Range DESCRIPTION Active sonar measurement of the horizontal range between the ship and the target. The active sonar system measures the traveling time of an acoustic wave and assumes that the speed of the sound in the water is 1500 m/s. The sampling rate is 1 measurement/min. Angle in degrees with respect to the true east. This measurement can be combined with the range measurement to establish the position of the ship at each time. The sampling rate is 1 measurement/min. Location of the ship (latitude and longitude) measured with a GPS system. The sampling rate is 1 measurement/min. Location of the target (latitude and longitude) measured with a GPS system. Since the target is stationary, this is a single measurement.

R x ( n) R ( n) y x ( n) = V x ( n) V y (n)
where

(1)

Bearing angle

Data used with the EKF and the PF to estimate the position of the ship in Cartesian coordinates. This set of data includes outliers and measurement noise.

R x (n), R y (n) = x,y coordinate of the ship1

V x (n),V y (n) = x,y-component of the velocity of the ship

The dynamics of the system can be described by (2) to (5).

R x (n + 1) = R x (n) + V x (n)T +

Position of the ship Position of the target

1 a x (n)T 2 2 1 R y (n + 1) = R y (n) + V y (n)T + + a y (n)T 2 2

(2) (3)

GPS data used as the theoretical position

V x (n + 1) = V x (n) + a x (n)T

(4) (5)

V y (n + 1) = V y (n) + a y (n)T
where ay(n) and ax(n) represent acceleration events. The state-space model is given by (6)

The database consists of measurements of the position of targets detected in shallow waters in the Malta Channel. Table 1 describes the information available for the target selected.

x(n + 1) = (n + 1) x(n) + g (n) (n)

where

(6)

1 0 ( n) = 0 0

0 1 0 0

T 0 0 T 1 0 0 1

(7)

Fig. 2 Horizontal range of the moving ship with respect to the non-moving target. There are three sections with constant velocity, where the acceleration has been modeled as random. There are also two points where the ship turned (red marks) and 6 missing measurements.

T 2 22 T g ( n) = (8) 2 T T 2 and (n) = WGN (0, ) is the system error, which

represents random acceleration events. Equation (9) is the observation vector, which describes the position of the ship in polar coordinates. The relation between the observations and the state vector is non-linear.

B. Modeling of the system for the EKF In this paper, it is assumed that the ship moves with a fairly constant velocity and present random acceleration events. Roughly, the speed of the ship is 9 km/h and the trajectory is straight with just two turns (fig. 1). In order to estimate the position of the ship at each time t, the state vector (1) is defined as:

1 Assuming a Cartesian coordinate system with origin at the non-moving target.

3
R ( n) + v r ( n) y ( n) = = h( x(n)) + v(n) ( n) + v ( n) where R (n) = the horizontal range from ship to target

(9)

R ( n ) + r ( n) y ( n) = = h( x(n)) + (n) ( n) + ( n )
where

(15)

(n) =

the bearing angle measurement

r (n)

and

(n)

are the errors in the range and bearing

v(n) = measurement noise vector. h(.) = non linear function that relates observations and state
given by (10).
R 2 ( n) + R 2 ( n) y x h(.) = R y ( n) 1 tan R y ( n)

observation. A Cauchy distribution was used to model those errors. In general, for a particle filter, the goal is to obtain a discrete representation of the a posteriori pdf (16):
i i p(xk | z1:k ) wk (xk xk ) i Ns

(10)

(16)

where

p( x k | z1:k ) = the probability of the state xk given the

observations z1 to zk. i wk = the weight of each particle xki.

The function h(.) can be linearized about xo, giving the model defined in (11) for which the Kalman filter can be applied:

N s = number of particles picked to represent the pdf.

Given the pdf in (16), the a posteriori estimate of the state xk given the measurements z1 to zk are obtained from (17):
i i E[xk | z1:k ] = xk p(xk | z1:k )dk wk xk i Ns

y (n) y (n 1) = y (n) = H (n)x(n) + v(n)

in which H(n) is the Jacobian of h(n) with respect to x(n):
1 H (n) = sin( ) A 1 cos( ) B 0 0 0 0

(11)

(12)

(17)

where R x ( n) sin 1 2 2 R x ( n) + R y ( n ) A= R x (n)

R y ( n) cos 1 2 2 R x ( n) + R y ( n) B= R y (n)

(13)

The algorithm for a particle filter can be summarized as follows: 1) Draw Ns particles of the state xk at time t=k. 2) Estimate the desired state variable (xk, for example) at t=k and project the future state of the particles, using the state equation(14). 3) If there is a new observation, calculate the weight wik+1 for each of the new particles i:1..Ns. 4) Replace particles with small weight and go back to step N.2 The most critical part of the algorithm for the PF is obtaining a recursive equation to calculate the weight wik+1 based on wik. One way to do it is described in [4]-[5]:
i i i wk+1 = wk p(zk | xk )

C. Modeling of the system for the PF The PF used in this paper was modeled as in [4]. The state and observation models for the PF were defined as:

(18)

x p ( n ) + x ( n) x ( n) + ( n) = y p ( n) + y ( n )
where

(14)

where
i wk +1 = the weights of the particles i:1..Ns at time k+1.
i wk = the weights of the particles i:1..Ns at time k.

(n) =

system error, which was assumed to be Gaussian.

x p (n), y p (n) = x/y cartesian coordinate of the position of

the ship at time n.

i p ( zk | xk ) = the probability of having the observation zk

given the particle xik. This pdf is known and it is defined according to the model of the observation noise.

4 III. RESULTS The EKF was applied to the set of data and the observation and system error variance was adjusted to obtain a smooth estimate. When the EKF detected missing data, the update step is skipped, using the a priori estimate as the best estimate for those specific points. In fig. 3, the system and measurement noise were assumed to be time-invariant. It is observed in fig. 3(a) that the EKF followed the true trajectory only during the first leg of the path. To generate fig. 3(b), both the system and the measurement noise were decreased, and the EKF improved its performance for the first two legs. The deviation for the third leg of the path was large in both cases depicted in fig. 3. The magnitude of the correction for the x/y coordinates is shown in fig. 4, and it is observed that the EKF is trying the correct the error in the estimation of the Ry, without success.

Fig. 4 Magnitude of the state update as a function of the y-coordinate. The EKF was set with measurement noise variance Rn=0.01 and system error variance Qn=0.005.

Inspection of the observations (green dots in fig. 3) reveals that during the third leg, the error in the bearing angle is large as compared to the other two legs. For this reason, a timevariant observation noise was incorporated in the EKF, and the bearing angle noise was increased as in (19).

0.001 0 Rn = 1 0

(19)

(a)

With the time-variant observation error, the performance of the EKF for the third leg improved. In fig.(6), the error of the estimation of Rx and Ry with respect to the GPS true position was calculated. The error is less that 15% during the fisrt two legs and it increased to 50% for Ry for the third leg.

(b) Fig. 3 Use of the EKF to smooth noisy observations of the horizontal range and the bearing angle. The observations located at (0,0) correspond to missing measurements. (a) The EKF was set with a measurement noise variance Rn=0.01 and system error variance Qn=0.005. (b) The EKF was set with a measurement noise variance Rn=0.001 and system error variance Qn=0.00005.

Fig. 5 Use of the EKF to smooth noisy observations of the horizontal range, and the bearing angle. The filter was set with a measurement noise variance Rn=0.001 and system error variance Qn=0.00005 during the first two legs, and Rn given by (18) during the third leg.

5 IV. DISCUSSION In fig. 3, it is observed that the performance of the EKF in smoothing the noisy observations is good for the initial observations, when there is no missing data and the ship follows a linear trajectory. For the set of data utilized in this paper, the outliers were present when the ship changed direction (see fig. 2), which made difficult the task of the EKF at those points. The most critical interval occurs for the third leg of the trajectory, where the ship is changing direction and there are 4 outliers in the range. In fig. 3, it is observed that the EKF is effectively increasing the magnitude of the update for the estimation of Ry during the range 3.5<y<8 km. In this case, it would require more samples to stabilize the filter and decrease the error in the estimate. It was observed from fig.3 that the variability in the observation for the bearing angle during the third leg is increased. This reasoning is supported by a simulation presented in appendix 1, where synthetic data with Gaussian distribution and no missing samples was generated. In this case (fig. A1), the EKF effectively approximate the theoretical trajectory. In order to decrease the effect of the change in the observation variance, a time-varying observation noise correlation matrix Rn was used where the variance in the observation of the bearing angle was increased. The result of this time-varying matrix is observed in fig.5, where the EKF now relies more on the system dynamics rather than in the observations, which provides a better estimate. Fig. 6 reveals that the main source of error in the estimates during the interval 0<y<10 km is the estimate of the y coordinate, which intuitively is evident because the variation of the observations within this interval is mainly in the y coordinate. The PF approach was explored because it allows to make different choices for distribution for the observation noise. In this case, a Cauchy distribution was picked, since it present heavy tails as compared to the Gaussian distribution assumed for the EKF. Fig. 7 shows the filter is very sensitive to the weight of the tails, and for this dataset, the parameter =0.09 (small tails) was appropriate to follow the observations, but it did not smooth the track. V. CONCLUSIONS The EKF can be used as a smoother of noisy observations as long as the nonlinear function that relates the observations to the system state is smooth. For the ships trajectory presented in this paper, the performance of the EKF decreases when a sharp edge in the trajectory is reached. This could be related to the approximation of the nonlinear function at this point, because the first derivative is not defined at this point. One way to solve this problem is to monitor the observations and change the measurement noise variance as a function of time. The approach to get a better performance using a PF with a Cauchy distribution to model the noise was not successful. Nevertheless, the use of the PF was motivated for the material

Fig. 6 Error of the estimates shown in fig. 5 (b) taking the GPS measurement as the theoretical value. The error in the x coordinate is always less than 13%(except at the beginning due to the settling of the EKF). The error in the y axis is 40% after 110 samples, which corresponds to the interval 0<y<10 km in fig. 5(b). Fig. 7 shows the result of the smoothing using a particle filter. In this case, the Cartesian position was modeled as a random walk with Gaussian noise, and the observation noise was modeled as a Cauchy distribution given by (20):

P ( x, y ) =

2 ( x + y 2 + 2 )3 / 2
2

(20)

(a)

(b) Fig. 7 Use of a particle filter to smooth noisy observations of the horizontal range and the bearing angle. (a) 2000 particles with a Cauchy distribution with =0.9; (b) 2000 particles with a Cauchy distribution with =0.09.

6 presented in [2], where the variance of the observations is one of the entries in the state vector. The PF provides an estimate of the joint pdf of the state, and therefore, it could be used to estimate the second moment of Rx and Ry, which would provide an estimate of the variance of the observations. Using this approach, the parameter can be adjusted online. VI. APPENDIX: USE
OF THE EKF AND THE PF TO SMOOTH NOISY OBSERVATIONS OF BEARING ANGLE AND RANGE WITH SYNTHETIC DATA

REFERENCES
[1] F. El-Hawary, F. Aminzadeh and G. Mbamalu, The generalized Kalman filter approach to adaptive underwater target tracking IEEE J. Oceanic Engineering, vol. 17, Jan. 1992, pp. 129137. N. Ichimura, Stochastic filtering for motion trajectory in image sequences using a Monte Carlo filter with estimation of hyperparameters in Proc. 16th International Conference on Pattern Recognition, vol. 4, 2002, pp. 68-73. D.G. Monolakis, V.K. Ingle, and S.M. Kogon, Statistical an Adaptive Signal Processing. Norwood, MA: Artech House, 2005, pp.381-389. M. Sanjeev, S. Maskell, N. Gordon, and T. Clapp, A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking IEEE Trans. Signal Processing, vol. 50, pp. 174-188, Feb. 2002. P. Djuric, J. Kotecha, J. Zhang, Y. Huang, T. Ghirmai, M. Bugallo, J. Miguez, Paticle Filtering, IEEE Signal Processing Magazine, Sept. 2003, pp. 19-38.

Synthetic data was applied to the EKF and the PF. In this case, a set of observations of the horizontal range and bearing angle were used to test the performance of the EKF and the PF under ideal conditions (non missing data, smooth and nonlinear trajectory, Gaussian distribution for the observations and system noise). Fig.A1 shows that the EKF improves the tracking of the moving target, but the PF just follows the noisy observations.

[2]

[3] [4]

[5]

Fig. A1 Use of the EKF to smooth noisy observations of the horizontal range and the bearing angle. The EKM was set with a measurement noise variance Rn=0.05 and system error variance Qn=0.0005. In this example, the measurement noise and the system error have a Gaussian distribution with no missing samples and the true position is smooth as compared to the trajectory of the ship shown in fig. 2.

Fig. A2 Use of the PF to smooth noisy observations of the horizontal range and the bearing angle. The PF follows the observations instead of smooth them.