x and y suer from two tyes of additive noise [], [3] Uncertainties e x, e y, where the only rior knowledge is their boundedness and zero mean Gaussian

Transcription

1 A New Estimator for Mixed Stochastic and Set Theoretic Uncertainty Models Alied to Mobile Robot Localization Uwe D. Hanebeck Joachim Horn Institute of Automatic Control Engineering Siemens AG, Cororate Technology Technische Universitat Munchen Information and Communications 8090 Munchen, Germany 8730 Munchen, Germany Abstract This work resents new results for state estimation based on noisy observations suering from two dierent tyes of uncertainties The rst uncertainty is a stochastic rocess given statistics. The second uncertainty is only known to be bounded, the exact underlying statistics are unknown. State estimation tasks of this kind tyically arise in target localization, navigation, and sensor data fusion. A new estimator has been develoed, that combines set theoretic and stochastic estimation in a rigorous manner. The estimator is ecient and, hence, well-suited for ractical alications. It rovides a continuous transition between the two classical estimation concets, because it converges to a set theoretic estimator, when the stochastic error goes to zero, and to a Kalman lter, when the bounded error vanishes. In the mixed noise case, the new estimator rovides solution sets that are uncertain in a statistical sense. Introduction In many alications, it is imortant todeduceasy- stem's state on the basis of uncertain observations of the system's outut. In addition, uncertain results of dierent estimators must be combined. Alications include vehicle or missile localization, target tracking, navigation, and sensor data fusion. The goal of these estimation rocedures is, of course, to reduce the uncertainty about the system's state as much as ossible. When an aroriate system model together noise statistics is given, the Kalman lter and its descendants [] have been successfully alied for more than 30 years. However, in the alications cited above, a detailed statistical noise model is often either not available or imractical. Secial caution is in order when neglecting strongly correlated noise or systematic errors. In that case, Kalman lter estimates tend to be overotimistic [8], i.e., the covariance estimate is unrealistically small. Several heuristics have been suggested for coing this roblem, ranging from articially increasing the covariance from time to time to emloying nonlinear re{lters. Of course, these techniques do not rovide otimal estimators. In some situations, although a statistical noise descrition cannot be given, bounds for the noise can be rovided. This may be the case for unmodeled dynamics, unmodeled nonlinearities, correlated noise, and systematic errors. In that case, set theoretic estimation can be alied [0], which often leads to good results [4]. However, when additional uncorrelated noise is resent, the error bounds become unnecessarily conservative. In [5], [6], a concet for estimation in the resence of both set theoretic and stochastic uncertainties has been introduced. The roosed algorithm is exact, but comutationally comlex. This aer resents a new, aroximate solution, that is comutationally attractive. Nevertheless, it combines both set theoretic and stochastic estimation in a rigorous manner. It bridges the ga between both estimation schemes, because a set theoretic estimator is attained, when the stochastic error goes to zero, and a Kalman lter, when the bounded error vanishes. When both tyes of uncertainty are resent, the new estimator rovides solution sets that are uncertain in a statistical sense. We restrict attention to the scalar case for the sake of simlicity. Estimation Concet Consider two uncertain measurements of an unknown state z given by x = z + e x + c x e x c x N0 x y = z + e y + c y e y c y N0 y

2 x and y suer from two tyes of additive noise [], [3] Uncertainties e x, e y, where the only rior knowledge is their boundedness and zero mean Gaussian noise rocesses c x, c y, which are assumed to be uncorrelated. First, assume that x, y can be observed out stochastic uncertainty. Then, since there is no rior information about e x, e y besides their boundedness, we make the worst case assumtion that e x, e y are fully correlated. In that case, a set theoretic estimator is aroriate for fusing the information sources. An ecient form of a set theoretic estimator, which is an aroximation based on the convex combination of the original sets, is given by the set [0] = fz z ; ^z E z g which is an interval in the scalar case. The interval midoint is given by weighting factors ^z = w x x + w y y 3 w x =05 ; P z w y =05+ P 4 z 05 ; P z = ; 5 where w x + w y =. The aroriate selection of the arameter [;05 05] will be discussed later. The set theoretic uncertainty is given by E z = d P z 6 d =; ; 05 ; x ; y P z 7 It is imortant to note that the set theoretic uncertainty E z deends on the actual observations x, y. From now on,we will bound E z from above bysetting d =,whichgives 05 ; E z = ; 8 The so obtained interval always contains the true interval. Most imortantly, E z does not deend on the actual observations, which simlies some of the following derivations. Since x, y cannot be observed directly, but are corruted by Gaussian noise, ^z is a random variable statistics that can be obtained from 3. Note that E z in 8 is not a random variable, because it does not deend on the actual observation. We rovide three solutions The exact density of^z Sec. 3, an aroximation of the densityby a sum Sec. 4, and an exact second{order descrition, i.e., mean and variance Sec. 5. The rst solution is mainly useful for validation uroses, the other two solutions are useful for data{ recursive estimation. 3 The Exact Density For w x 6= 0, the density f z z of^z is given by f z z = z ; wy y f xy y dy 9 w x w x ; From we deduce the constraint jx ; yj B B = + 0 which leads to fx x f y y for jx ; yj B f xy x y = 0 elsewhere The constraint can be rewritten as ;B z ; w yy w x B z ; w xb w x + w y y z + w xb w x + w y Hence, 9 gives f z z = w x z+wxb z;wxb and after some maniulations f z z = w x x y Iz = z+wxb z;wxb z ; wy y f x f y y dy w x ; z ; w x m x + w y m y wx S x + wy S y w x S x + w y S y w x S xs y y ; w x m ys x ; w x w y m x S y + w y S y z w x S x + w y S y Iz 3 dy

3 Iz simlies to Iz = wx x y q wx S x + wy S y 0 erf@ Num q x y w x + w y wx S x + wy S y 0 +erf@ Num q x y w x + w y wx S x + wy S y Num =Bw x S x + w y S y+zw x S x ; w y S y A A + w x w y m x S y ; m y S x +w y m xs y ; w x m ys x Num =Bw x S x + w y S y+zw y S y ; w x S x + w x w y m y S x ; m x S y +w x m ys x ; w y m xs y where erf x is dened as erf x = x y=0 ; y dy 4 according to [9]. The exact density 3 can easily be calculated, but is not useful for recursive alications, because the derivation has been erformed for the case that the rimary stochastic uncertainties are Gaussian. Hence, the rst of the two aroximation aroaches of the exact density f z z roosed in this aer is given by a sum of Gaussian densities. 4 The Aroximate Density The key idea to nding an aroximate solution for the robability density ; function is to look at the constraint 0 as rect x;y B and to aroximate this function by aweighted sum of Gaussian densities x ; y LX rect B c ; x ; y ; mgi g m gi = i B L, g = c B L, so that the integral from ; to over the sum gives B. The free arameter c 0 maybeobtainedby a one{dimensional search to yield the best function aroximation according to a given norm. A lot of maniulation gives f z z g i ; z;z i z g i g i = ; m x ; m y ; m gi S g + 5 z i = S g [w x m x + w y m y ] + S x [w x m gi + m y +w y m y ] + S y [w y ;m gi + m x +w x m x ] Sg + 6 = +wx + w y S x S y ; S g w x S x + wy S y Sg + 7 where the shorthand notation S g = g has been used. Using this aroximate df, we can easily calculate the aroximate mean and variance of z. Furthermore, the exact mean and variance can be derived for L!. 5 Exact Mean and Variance Given 5, the aroximate mean is calculated as m z g i z i g i 8 The aroximation of the variance is given by z = E z ; m z E z ; g i +z i g i 9 Analytic ressions for the exact mean and variance are calculated from 8, 9 for L!, which imlies S g! 0. The mean is then given by B m x ; m y ; v m z = ; S x [w x v + m y +w y m y ] + S y [w y ;v + m x +w x m x ], B which results in ; dv m x ; m y ; v dv m z = w x m x + w y m y ; w x S x ; w y S y G 0

4 G = ; ; B ; m x + m y ; B + m x ; m y, Sx + Sy erf +erf The variance is given by!!!! B + m x ; m y Sx + S y! B ; m x + m y Sx + S y B S z = ; m x ; m y ; v " w x + w y S x S y +S x [w x v + m y +w y m y ] Sx + S y + S y [w y ;v + m x +w x m x ] # dv, B which results in S z ; m x ; m y ; v dv ; m z S x + S y = w x Sx + w y Sy ; wxsx ; wysy G ; B ; m x + m y +B + m x ; m y erf ; ;! B ; m x + m y + erf B ; m x + m y B + m x ; m y 6 The New Estimator wxsx ; wysy 3=!! B + m x ; m y Given two uncertain information sources according to, it has been shown in Sec. 4, that the fusion result is given as the sum of a bounded uncertainty and a sum of Gaussian densities. When the number of terms included in the Gaussian sum tends towards in- nity, the exact density derived in Sec. 3 is aroached. This imortant result can now be alied to derive two dierent estimators for solving ractical estimation roblems. The rst estimator is obtained by keeing a nite number of, say M, terms in the Gaussian sum. Of course, when using this estimator recursively, there will be M terms after the rst recursion ste. Hence, for recursive alication, the number of terms must be ket xed by selecting the M most imortant terms after each ste. An even simler estimator is obtained, when using the additional results derived in Sec. 5, i.e., the exact second{order descrition of the density of the interval midoint. The estimator is then given by the interval = fz z ; ^z E z g midoint ^z, that is uncertain in a stochastic sense. Mean m z and variance S z of ^z are given by 0 and. The set theoretic uncertainty E z is not a random variable and given by 05 ; E z = ; for [;05 05]. The new estimator contains the classical estimation concets, i.e., the set theoretic estimator and the Kalman lter, as border cases. We rst investigate the case of a vanishing set theoretic uncertainty. 6. Border Case Stochastic Uncertainty Only This case is equivalent to B = 0. Hence, we have to examine the ression for m z from 0 and S z from for the limit B! 0. By alying the rule of l`hosital, we obtain for the mean m z B! 0=w x + w y S xm y + S y m x which can be simlied using w x + w y = to yield m z B! 0 = m y S y + mx S x S x + S y which is exactly the mean of a Kalman lter estimate. When examining, we obtain S z B! 0=w x + w y S xs y This is simlied by using w x + w y = again to yield S z B! 0 = + ; S x S y which is the variance of a Kalman lter estimate.

5 6. Border Case Set Theoretic Uncertainty Only The second border case is equivalent tos x =0,S y = 0. Nowwehave to examine the ression for m z from 0 and S z from for the limit S x! 0, S y! 0. For the mean, we obtain m z S x! 0S y! 0 = w x m x + w y m y The limiting variance of the new estimator vanishes, i.e., S z S x! 0S y! 0=0 as ected. Hence, for the border case of vanishing stochastic uncertainties, the new estimator converges to a set theoretic estimator according to 3 and 8. 7 Simulative Verication Consider a vehicle equied a range sensor that measures the distance to a number of boxes, Fig.. The box ositions are known in a given geometric tolerance, i.e., i x B = i ~x B + i x B i x B b i where i ~x B denotes the unknown true value and i x B is the unknown but bounded deviation of the nominal value i x B. The range sensor is corruted by additive white Gaussian noise zero mean and a variance i which deends on the surface characteristic of the box i. The measurement equation is thus given by i x B + D k i = x + i x B + c k i where c k i N 0 i, x denotes the vehicle osition, and Di k is the measured distance. Atruevehicle osition x = 00 is assumed. The remaining simulation arameters are given in Tab.. According to Fig., the vehicle moves along the four boxes. The boxes are samled sequentially 00 samles for each box. For k = 00, box is used, for k = 0 00, box is used, for k = 0 300, box 3 is used, and for k = , box 4isused. The recursive estimation scheme is initialized to m = x B + D k E = b S = This estimate is recursively udated using 4, 8, 0, and m x = m k ; = E k ; S x = S k ; and m y = i x B + D k i = b i S y = k Figure Exerimental setu for localization. Box 3 4 True value i ~x B Nominal value i x B Bound b i Standard deviation i Table Parameters of localization eriment. Thus, the estimate m k =m z E k =E z S k =S z incororates all measurements available u to time k. The arameter in 4, 8 is chosen such that E k+3 S k is minimized. Fig. deicts the ected value and the condence interval using the roosed estimator. Samling a secic box reduces the stochastic uncertainty. The initial deterministic uncertainty given by the interval [80 60] is reduced each timetheboxischanged When changing from box to box, the resulting intersection set out stochastic noise would be [85 45],whichiseventually aroached when samling box for an innite numberoftimes.traversing from boxtobox3gives the underlying set [90 30], from box3tobox 4 it is given by[95 5]. The realistic quantication may be loited when attemting to navigate a mobile robot through narrow oenings. It is common ractice to aroximate systematic errors by stochastic noise. To demonstrate the negative eect of doing so, the deterministic error of the box ositions will now be modeled as additional uncorrelated noise the standard deviation set to b i. For this noise descrition, estimates can be obtained by Kalman ltering. Fig. 3 deicts the corresonding ected value and the condence interval. The resulting estimate is obviously overotimistic, because the

6 Robot Position k Figure Simulative results using the new estimator True value, ected value, and condence interval. Robot Position k Figure 3 Simulative results using the Kalman lter True value, ected value, and condence interval. condence interval does not always include the true vehicle osition. 8 Conclusions Avast class of estimation roblems can be attacked as a mixed noise roblem, i.e., the arising uncertainties can be modeled as being additively comosed of both noise known bounds and noise known statistics. For this class, a new estimator has been derived, which combines set theoretic and stochastic estimation in a rigorous manner. Hence, it rovides solution sets that are uncertain in a statistical sense. The roosed estimator is ecient and, hence, well{suited for ractical alications. Our research also includes the vector case. However, this resentation has been restricted to the scalar case for ease of lanation. References [] B. D. O. Anderson and J. B. Moore, Otimal Filtering, Prentice{Hall, 979. [] V.. Filiovic, \Identication in H in the Presence of Deterministic and Stochastic Noise", Proceedings of the Third Euroean Control Conference, Rome, Italy, Setember 995,. 3{35. [3] K. Forsman and L. Ljung, \On the Dead{one in System Identication", Identication and System Parameter Estimation 99. Selected Paers from the Ninth IFAC/IFORS Symosium on System Identication, Budaest, Hungary, July 99, ed. C. Banyasz, L. Keviczky, Pergamon Press, Oxford, UK, 99,. 959{963. [4] U. D. Hanebeck and G. Schmidt, \Set{theoretic Localization of Fast Mobile Robots Using an Angle Measurement Technique", Proceedings of the 996 IEEE International Conference on Robotics and Automation, Minneaolis, Minnesota, USA, Aril 996,. 387{394. [5] U. D. Hanebeck, J. Horn, and G. Schmidt, \On Combining Set Theoretic and Bayesian Estimation", Proceedings of the 996 IEEE International Conference on Robotics and Automation, Minneaolis, Minnesota, USA, Aril 996,. 308{3086. [6] U. D. Hanebeck, J. Horn, and G. Schmidt, \On Combining Statistical and Set Theoretic Estimation", Automatica, Vol. 35, No. 6, 999, to aear. [7] J. Horn and G. Schmidt, \Continuous Localization for Long{Range Indoor Navigation of Mobile Robots", Proceedings of the 995 IEEE International Conference on Robotics and Automation, Nagoya, Jaan, May 995,. 387{394. [8] S. J. Julier and J. K. Uhlmann, \A non-divergent estimation algorithm in the resence of unknown correlations", Proceedings of the 997 American Control Conference, Albuquerque, New Mexico, USA, June 997, [9] A. Paoulis, Probability, Random Variables, and Stochastic Processes, Second Edition, McGraw{ Hill Book Comany, 984. [0] F. C. Schwee, Uncertain Dynamic Systems, Prentice{Hall, 973.