Optimal Linear Unbiased Filtering with Polar Measurements for Target Tracking Λ

Transcription

1 Optimal Linear Unbiased Filtering with Polar Measurements for Target Tracking Λ Zhanlue Zhao X. Rong Li Vesselin P. Jilkov Department of Electrical Engineering, University of New Orleans, New Orleans, LA 018 Phone: , Yunmin Zhu Department of Mathematics, Sichuan University, Chengdu, Sichuan 100, P. R. China Abstract In tracking applications, target dynamics is usually modeled in the Cartesian coordinates, while target measurements are directly available in the original sensor coordinates. Measurement conversion is widely used such that the Kalman filter in the Cartesian coordinates can be applied. A number of improved measurementconversion techniques have been proposed recently. However, they have fundamental limitations, resulting in performance degradation, as pointed out in Part III [] of a recent survey. This paper proposes a recursive filter that is theoretically optimal in the sense of minimizing the meansquare error among all linear unbiased filters in the Cartesian coordinates. The proposed filter is free of the fundamental limitations of the measurement-conversion approach. Results of an approximate implementation are compared with those obtained by two state-of-the-art conversion techniques. Simulation results are provided. Keywords: Target tracking, measurement conversion, Kalman filter, optimal linear filtering, filter credibility 1 Introduction In tracking applications, target dynamics is usually modeled in Cartesian coordinates, while the measurements are directly available in the original sensor coordinates, most often in spherical coordinates. Tracking in Cartesian coordinates using spherical measurements is mostly handled by measurement conversion method []. The basic idea is to transform the nonlinear measurements into a pseudolinear form in the Cartesian coordinates, and estimate the bias and covariance of the error of the converted measurement, and then use the Kalman filter framework to do tracking. A number of improved techniques have been proposed [,, 8,,, 1, 9, 10] by different ways of obtaining the bias and covariance of the measurement noise. Measurement Λ Research Supported in part by ONR grant N , NSF grant ECS-98, and NASA/LEQSF grant (001-)-01. models used in target tracking, including measurement conversion methods, are surveyed in []. The pros and cons of various measurement-conversion techniques have been revealed therein with succinct explanation. In particular, it was pointed out that these conversion methods have fundamental flaws, as elaborated in Section of this paper. The idea of developing an optimal linear filter for tracking with linear dynamics and nonlinear measurements was briefly discussed in []. In this paper, such an optimal linear filter is developed based on this idea. This development stems from the recognition that the Kalman filter is nothing else but a recursive BLUE (Best Linear Unbiased Estimator) for a linear system; furthermore, although the Kalman filter cannot optimally handle any nonlinear measurement directly, BLUE filter is still optimal for nonlinear measurements. This paper presents such a recursive BLUE filter for tracking with linear dynamics and polar (nonlinear) measurements. For simplicity, this paper deals only with the -dimensional case. Extensions to other cases will be reported in a forthcoming. The rest of the paper is organized as follows. In Section, we summarize the measurement-conversion method and explain its fundamental flaws. In Section we derive formulas of the recursive BLUE filter for nonlinear observation. In Section, the error analysis is given for the implementation of the optimal recursive BLUE filter. In Section, We compare the implementation of this new method with two state-of-the-art measurement-conversion techniques by simulation. Our conclusions are given in Section. Measurement-Conversion Approach The measured range and bearing are defined with respect to the true range r and bearing r m = r +~r; m = + ~ 1

2 the noise ~r and ~ are assumed to be independent with zero mean and standard deviations ff r and ff, respectively. The converted-measurement method converts the polar coordinates measurement into the Cartesian coordinates by x m = r m cos m =(r +~r) cos( + ~ ) y m = r m sin m =(r +~r) sin( + ~ ) and then transform the above nonlinear form into pseudolinear form with respect to (x; y), x c = x +~x c ; y c = y +~y c (1) where (x c ;y c ) is the converted (most often) debiased measurement, (x; y) = (r cos ; r sin ) is the true measurements and (~x c ; ~y c ) is the converted measurement noise. Observe that (1) just divides the transformed debiased measurement into true measurement (x; y) and measurement noise (~x c ; ~y c ). Note, however, that (~x c ; ~y c ) is actually dependent on the true measurement. The converted measurement method uses the Kalman filter based on (1) to do tracking. All converted measurement techniques differ from each other in the ways that the bias and the covariance of the measurement noise are calculated. Specific comparisons can be found in []. Basically, one way is to compute the mean and covariance conditioned on the measurement as E [(~x c ; ~y c ) jr m ; m ] and cov[(~x c ; ~y c ) jr m ; m ]. Another way is to compute them firstly conditioned on the true state and then conditioned on measurement as E [E [(~x c ; ~y c ) jr; ] jr m ; m ] and E [cov ((~x c ; ~y c ) jr; ) jr m ; m ]. They are referred to as fixed measurement and fixed truth approaches in [], respectively. As argued in [], however, we will call them measurement-conditioned () approach and nested conditioning () approach. From the above analysis, all converted measurement techniques have the following fundamental flaws. First, (~x c ; ~y c ) is state dependent; second, its covariance is estimated conditioned on the current measurement or state/measurement; third, the measurement noise sequence f(~x c ; ~y c ) k g is not white anymore. However, in the assumptions of Kalman filter, the measurement noise is independent of the state, its covariance is not conditioned on the current measurement, and it is white. These fundamental flaws were ignored or overlooked in these techniques. As a result, these techniques are by no means optimal. For example, when the measurement noise is dependent on state, the measurement prediction covariance formula of the Kalman filter should be modified to take into account the crosscorrelation between the state and the measurement noise. All these techniques fail to do such modification. Recursive BLUE Filter Notice that the Kalman filter is simply a recursive BLUE filter for a linear system. Nevertheless, BLUE does not require the observation model to be linear. A BLUE filter can be outlined below. Let z k and xk be the measurement and state at time k; z k1 stand for the sequence of all past measurements, and E Λ [yjz] stand BLUE estimator of y using z. Suppose at time k 1 the state estimator and covariance is ^xk1and P k1. The prediction part of a recursive state estimator is as follows. μxk = E Λ [xkjz k1 ] ~xk = xk μxk μz k = E Λ [z k jz k1 ] ~z k = z k μz k μp k = cov(~xk) S k = cov(~z k ) K k = cov(~xk; ~z k )S 1 k then the update is ^xk = μxk + cov(~xk; ~z k )S 1 k (z k μz k ) P k = P μ k cov(~xk; ~z k )S 1 k cov(~z k; ~xk) This recursive BLUE filter is optimal provided the process and measurement noises are white and uncorrelated, which is the case for the problem under consideration. This recursive BLUE filter requires the knowledge of the predicted state μxk, its error covariance P μ k, the predicted measurement μz k, its covariance cov(~z k ) and crosscovariance cov(~xk; ~z k ). In view of the above, a recursive BLUE filter is derived as follows. The key is to express the state in the Cartesian coordinates while keeping the measurement error in the polar coordinates. Because the target model is still linear, μxk and P μ k are the same as in Kalman filter. We still adopt the nonlinear observation form and calculate E Λ [z k jz k1 ]; cov(~z k ) and cov(~xk; ~z k ) analytically with only ^xk1 and P k1 given. First, transform the measurement in polar coordinates into the Cartesian coordinates: x m = (r +~r)cos( + ) ~ = x cos ~ y sin ~ x + px + y ~r cos ~ y px + y ~r sin ~ () y m = (r +~r) sin( + ) ~ = y cos ~ + x sin ~ y + px + y ~r cos ~ x + px + y ~r sin ~ () Note that since (r; ) and (~r; ~ ) are independent, (x; y) and (~r; ~ ) are also independent. The tracking system has the nonlinear observation: z k = h(xk;v k ) () where z k = [x m ;y m ] 0 k, x k = [x; _x; y; _y] 0 k, v k = [~r; ~ ] 0 k, and the white noise v k is independent of the state xk. 18

3 As pointed out above, μxk and μ P k are the same as in the Kalman filter. For brevity, we will drop the time index k and the conditioning on z k1. Now denote μxk and μ P k by μx =[μx μ _x μy μ _y] 0 μp = cov(~x) cov(~x; ~ _x) cov(~x; ~y) cov(~x; ~ _y) cov( ~ _x; ~x) cov( ~ _x) cov( ~ _x; ~y) cov (_x; ~ _y) cov(~y; ~x) cov(~y; ~ _x) cov(~y) cov(~y; ~ _y) cov( ~ _y; ~x) cov( ~ _y; ~ _x) cov( ~ _y; ~y) cov( ~ _y) What remains is to calculate E Λ [z k jz k1 ]; cov(~z k ) and cov(~x k ; ~z k ). Notice that E Λ [(~r k ; ~ k ) 0 jz k1 ]=E[(~r k ; ~ k ) 0 ] since the noise sequence [~r; ] ~ 0 k is white. Assuming ~r ο N (0;ffr ) and ~ ο N (0;ff ) are independent, we have Thus E[cos ~ ]=e ff = = 1 E[sin ~ ]=0 E[cos ~ ]= 1 (1 + eff )= E[sin ]= 1 (1 eff )= E[sin ~ cos ~ ]=0 μx m = E Λ [x m ] E [~r] =0 = E Λ [x cos ~ y sin ~ x + p x + y ~r cos ~ y p x + y ~r sin ] ~ = E Λ [x] E Λ [cos ~ ] = 1 μx () μy m = E Λ [y m ] = E Λ [y cos ~ + x sin ~ y + p x + y ~r cos ~ x + p x + y ~r sin ] ~ = E Λ [y] E Λ [cos ~ ] = 1 μy () Since the BLUE estimator is unbiased and orthogonal to its error, it can be easily shown that cov(~x) = E[(x μx)(x μx) 0 ] = E[xx 0 ] E[μxμx 0 ] cov(~x; ~z) = E[(x μx)(z μz) 0 ] = E[xz 0 ] E[μxμz 0 ] cov(~z~z 0 ) = E[(z μz)(z μz) 0 ] = E[zz 0 ] E[μz μz 0 ] NOTE μx and μz are the predicted state and measurement BLUE estimator here. The crosscovariance between measurement z and state x is calculated as follows. First, E [xz 0 ]=E xx m _xx m yx m _yx m m _ m yy m _yy m Substituting () and () into the above yields Then E [xz 0 ]= 1 E cov(~x; ~z) = E [xz 0 ] E[μxμz 0 ] = 1 E x _x _ y x _y _yy x cov(~x) = 1 cov( ~ _x; ~x) cov(~y; ~x) cov( ~ _y; ~x) x _x _ y x _y _yy x 1E cov(~x; ~y) cov( ~ _x; ~y) cov(~y) cov( ~ _y; ~y) The measurement prediction covariance is Since cov (~z) = E [zz 0 ] E[μz μz 0 ] cov(~xm ) cov(~x m ; ~y m ) = cov(~y m ; ~x m ) cov(~y m ) μx μxμy μx μ _x _xμy μxμy μy μx μ _y μ _y μy () E[x m ] = E[x cos ~ y sin ~ x + p x + y ~r cos ~ y p x + y ~r sin ] ~ and similarly = E[x cos ~ + y sin ~ + x x + y ~r cos ~ + y x + y ~r sin ] ~ = 1 (1 + eff )E x Λ + 1 (1 eff )E y Λ + 1 ff r + 1 ff r eff E x y x + y = E x Λ + E y Λ + 1 ff r + 1 ff r eff E x y x + y E[y m ] = E y Λ + E x Λ + 1 ff r + 1 ff r eff E y x x + y E[x m y m ] = e ff E [] +ff r eff E x + y 19

4 we have cov(~x m )=E[x m ] E[μx m μx 0 m ] (λ λ 1 )/λ λ /λ = E x Λ + E y Λ + 1 ff r + 1 ff r eff E x y x + y 1 E[μx ] = cov(~x) + cov(~y) + 1 ff r + μy +( 1 )μx + 1 x y ff r eff E x + y +( 1 )(E[μx ] μx )+ (E μy Λ μy ) (8) ß cov(~x) + cov(~y) + 1 ff r + μy +( 1 )μx + 1 x y ff r eff E x + y cov(~y m )=E[x m ] E[μx m μx 0 m ] = cov(~y) + cov(~x) + 1 ff r + μx +( 1 )μy + 1 y x ff r eff E x + y (9) +( 1 )(E μy Λ μy )+ (E[μx ] μx )(10) ß cov(~y) + cov(~x) + 1 ff r + μx +( 1 )μy + 1 y x ff r eff E x + y Let = e ff 1. Then cov(~x m ; ~y m )=E[x m y m ] E[^x m ^y m ] = e ff cov(~x; ~y) +ffr eff E x + y ß (11) + μxμy + (E[μxμy] μxμy) (1) e ff cov(~x; ~y) +ffr eff E x + y + μxμy (1) With (8), (10) and (1), the optimal recursive BLUE filter has the state update below ^x = μx + cov(~x; ~z)cov 1 (~z)(z μz) (1) P = P μ cov(~x; ~z)cov 1 (~z) cov(~z; ~x) (1) Error Analysis The two terms in (8) ( 1 )(E[μx ] μx )+ (E μy Λ μy ) are dropped in (9). We justify this approximation as follows. As we know, (μx E[μx ]) is usually much smaller σ θ Fig. 1: Ration of ( 1 ) and than μx in a tracking problem. Besides, the ratio of ( 1 ) and are as Fig. 1. Usually in a tracking problem, the bearing noise standard deviation ff is below 0:rad, so the contribution of (E[μx ] μx ) and (E μy Λ μy ) to cov(~x m ) will be greatly decreased by ( 1 ) and compared with the term cov(x). For example, when ff =0:rad, ( 1 ) = 0:0% and =%. Therefore, the dropped term is negligible compared with other terms. A similar justification can be applied to (11) and (1). So this approximation is highly accurate. It seems that the two terms E h y x x +y i and E h i x +y involved in cov(~z) have either no closed form or a complex expression. We approximate them by Taylor series expansion. The Taylor series expansion of a function f (x; y) at point (x 0 ;y 0 ) is f (x; y) ßf (x 0 ;y 0 )+(x x 0 (x 0;y 0 +(y y 0 (x 0;y 0 ρ + 1 (x x 0 f (x 0 ;y 0 +(x x 0 )(y y 0 f (x 0 ;y 0 ff +(y y 0 f (x 0 ;y 0 (1) Expanding y x x +y and x +y by Taylor series at (μx; μy) and then taking expectation yields y x E x + y ß μy μx μx +μy + 1! ( μy μy μx (μx +μy ) cov(x) + 8μxμy μx μy cov(x; (μx +μy ) y) ) μx μx μy (μx +μy ) cov(y) (1) 10

5 E x + y ß μxμy μx +μy + 1! ( μxμy μx μy (μx +μy ) cov(x) + μx μy μx μy cov(x; y) (μx +μy ) + μxμy μy μx (μx +μy ) cov(y) ) (18) It is reasonable to expect that this approximation is not crude at all since the denominator has higher order than the numerator in the differential terms and (μx +μy ) is usually much bigger than cov(~x),cov(~y) and cov(~x; ~y). Once (1) and (18) are calculated, our proposed recursive BLUE filter can be implemented straightforwardly. This implementation is a highly accurate approximation to the optimal BLUE filter. The above analysis will be verified in the next section. We can see that actual estimation errors are almost always perfectly consistent with the filter s computed covariance in all simulation results. Simulation and Comparison In this section, we compare two state-of-the-art conversion techniques with our proposed method. We choose the measurement-conditioned () approach and one of the nested conditioning () approaches, called fixed measurement and additive fixed truth approach II in [], respectively. See appendix for their formulas. We denote these two approaches by and, respectively. For convenience of comparison and computation, we use scenarios similar to that of [], but simplify it to two dimensions by eliminating elevation. The scenario is as follows. A two-dimensional Cartesian x-y space with a single sensor locates at the origin. Target sampling time is one sec. The coordinates (x; y) of the target object at time zero are determined by random draws from two independent, Gaussian distributions with means 0 km and 00 km, respectively, and common standard deviation km. The target moves at a constant high velocity (including direction and speed), whose components _x and _y; determined by a random draw from a Gaussian distribution, have means 1000m/s and 0m/s, respectively, and a common standard deviation 0:1 km/s. The sensor s independent measurement errors have standard deviations ff r = m and ff =10millirad. Following [], all the filters are initialized with an effectively infinite initial state error covariance matrix, and a highly inaccurate initial state estimate. The tracking period begins 100 sec after time zero and continues for 100 sec. We will compare the average normalized estimation error squared (ANEES) and root-mean-square error (RMSE) of position of these three techniques by increasing the process noise, and then increasing the measurement noise. The ANEES is defined by μψ = 1 Nn NX i=1 (xi^xi) 0 P 1 i (xi^xi) (19) where (xi^xi) is the -dimensional vector of state estimation errors in sample run i, and N is the total number of samples used in the test. If the estimation error and estimated covariance is consistent, the expectation of ANEES is 1 []. Because the target moves at a constant high velocity, the velocity RMSEs of those three filters have little difference in all simulations and will not be present. The performance of the three filters in different cases can be summarized as follows. Case 1 No process noise; measurement errors ff r = m and ff =10millirad; 100 runs Fig. : ANEES (Case 1) Fig. : Position RMSE (Case 1) 11

6 Fig. compares the ANEES of the and techniques with the proposed filter. The proposed one is much more consistent than the other two. Furthermore, the ANEES of the proposed filter is very close to the ideal unity, which indicates the estimator is almost perfectly consistent. Fig. compares the position RMSE of the three techniques. For convenience, we separate the transient part and steadystate part. It is not easy to tell which one is better in the transient part. However, our method is much more accurate than the other two after transient. Case Process noise Q = 10 I; measurement errors ff r =m and ff =10millirad; 100 runs filter has also smaller position RMSE than the other two after transient. Case Measurement errors ff r = 0m and ff = 100millirad; process noise Q =10 I; 100 runs Fig. : ANEES (Case ) 1. x Fig. : ANEES (Case ) Fig. : Position RMSE (Case ) In this case, the process noise is increased. As is clear from Figs. and, the ANEES of our proposed filter is very close to unity and is better than the other two. Our Fig. : Position RMSE (Case ) In this case, the measurement noise is increased. The simulation results are plotted in Figs. and. Our filter is much more accurate and credible. In particular, the ANEES of our filter is still very close to the ideal unity. Case Measurement errors ff r = 00m and ff = 00millirad; process noise Q =10 I; 100 runs In this case, the measurement noise is increased greatly. The simulation results in Figs. 8 and 9 show that ANEES of our filter is still very credible even though position RMSE 1

7 Fig. 8: ANEES (Case ) Fig. 10: ANEES (Case ) x x Fig. 9: Position RMSE (Case ) Fig. 11: Position RMSE (Case ) is rather large. For this problem, where bearing error is dominant, the tracking performance depends mainly on ff (in fact, changing ff r =0mtoff r =00m has virtually no impact). Case The coordinates (x; y) of the target object at time zero are determined by random draws from two independent, Gaussian distributions with means -0 m and 00 m, respectively, and common standard deviation m. The target moves at a constant velocity (including direction and speed), whose components _x and _y; determined by a random draw from a Gaussian distribution, have means m/s and 0m/s, respectively, and common standard deviation 0.1 m/s. The sensor s independent measurement errors have standard deviations ff r = m and ff = 0millirad. process noise Q =0:1 I. 100 runs In this case, our filter in this short range application still has a good performance as showed in Figs. 10 and 11.This indicates the approximation of the implementation is not sensitive to the scenario. Similar observations were made for many other cases not presented here. In the simulation conducted, the standard implementation of the and conversion techniques sometimes exhibited numerical problems; we had to recourse to the numerically more robust Joseph form for the covariance computation. On the contrary, we did not experience any numerical problem with the approximate implementation of our proposed filter. The three filters have essentially the same computational complexity. 1

8 Conclusion An optimal recursive filter in the sense of BLUE has been presented for linear dynamics with nonlinear (polar) measurements. This filter operates entirely in the Cartesian coordinates but is free of the fundamental flaws of the measurement-conversion method. The simulation-based comparison of an approximate implementation of the proposed filter with two state-of-the-art measurement-conversion techniques has demonstrated the following. In terms of estimation errors and filter credibility, the proposed filter outperforms significantly the two measurement-conversion techniques in all cases tested; in particular, the proposed filter is almost always perfectly credible in that the actual estimation errors are consistent with the filter s self-assessment. By abandoning the Kalman filter framework and using optimal BLUE filter in the implementation, the good performance is achieved without increasing the computational complexity. Moreover, the optimal filter framework can be extended to the D case and the RUV coordinate systems. Extensions to these cases will be reported in a forthcoming paper. Appendix The two measurement-conversion techniques provide the following converted measurement in the Cartesian coordinates: ffl Measurement-conditioned () (referred to as fixedmeasurement approach in []): x m = r m cos m A; y m = r m sin m A C ~x = 0:R 1 T 1 R 0 cos m A C ~y = 0:R 1 T R 0 sin m A C ~x~y = cos m sin m (R 1 A R 0 A ) ffl Nested-conditioning () (referred to as additive fixed-truth approach II in []): where x m = r m cos m S; y m = r m sin m S C ~x = R 0 cos m S(S A) +0:R 1 T 1 C ~y = R 0 sin m S(S A) +0:R 1 T C ~x~y = cos m sin m (R 0 S(S A) +R 1 A ) [] D. Lerro and Y. Bar-Shalom. Tracking with Debiased Consistent Converted Measurements vs. EKF. IEEE Trans. Aerospace and Electronic Systems, AES- 9():10110, July 199. [] X. R. Li and V. P. Jilkov. A Survey of Maneuvering Target Tracking Part III: Measurement Models. In Proc. 001 SPIE Conf. on Signal and Data Processing of Small Targets, vol., pages, San Diego, CA, USA, 001. [] X. R. Li, Z. Zhao, and V. P. Jilkov. Estimator s Credibility and Its Measures. In Proc. IFAC 1th World Congress, Barcelona, Spain, July 00. [] M. Miller and O. Drummond. Coordinate Transformation Bias in Target Tracking. In Proceedings of SPIE Conference on Signal and Data Processing of Small Targets 1999, pages 09, SPIE Vol [] M. D. Miller and O. E. Drummond. Comparison of Methodologies for Mitigating Coordinate Transformation Bias in Target Tracking. In Proc. 000 SPIE Conf. on Signal and Data Processing of Small Targets, vol. 08, pages 1, Orlando, Florida, USA, April 000. [] L. Mo, X. Song, Y. Zhou, and Z. Sun. An Alternative Unbiased Consistent Converted Measurements for Target Tracking. In Proceedings of SPIE: Acquisition, Tracking, and Pointing XI, Vol. 08, pages 08 10, 199. [8] L. Mo, X. Song, Y. Zhou, Z. Sun, and Y. Bar- Shalom. Unbiased Converted Measurements for Tracking. IEEE Trans. Aerospace and Electronic Systems, AES-():1010, [9] P. Suchomski. Explicit Expressins for Debiased Statistics of D Converted Measurements. IEEE Trans. Aerospace and Electronic Systems, AES- (1):80, [10] M. Toda and R. Patel. Performance Bounds for Continuous-Time Filters in the Presence of Modeling Errors. IEEE Trans. Aerospace and Electronic Systems, AES-1():9190, Nov A = e 0:ff ; R 0 = r m ; R 1 = r m + ff r R = r m +ff r ; S =1 eff + e 0:ff T 1 = 1+e ff cos m ; T =1 e ff cos m References [1] Y. Bar-Shalom and X. R. Li. Multitarget-Multisensor Tracking: Principles and Techniques. YBS Publishing, Storrs, CT,