A SQUARE ROOT ALGORITHM FOR SET THEORETIC STATE ESTIMATION

Transcription

1

2

3 A SQUARE ROO ALGORIHM FOR SE HEOREIC SAE ESIMAION U D Hanebec Institute of Automatic Control Engineering echnische Universität München München, Germany fax: UweHanebec@ieeeorg Keywords: Estimation, Bounded Uncertainty and Errors in Variables, Observers, Robust Filtering, Set membership Estimation and Identification Abstract his paper presents a modified set theoretic framewor for estimating the state of a linear dynamic system based on uncertain measurements he measurement errors are assumed to be unnown but bounded by ellipsoidal sets Based on this assumption, a recursive state estimator is (re )derived in a tutorial fashion It comprises both the prediction step (time update), ie, propagation of a set of feasible states by means of the system model and the filter step (measurement update), ie, inclusion of a new measurement into the current estimate he main contribution is an efficient square root formulation of this estimator, which is well suited especially for practical applications 1 Introduction In many applications, it is necessary to estimate the state of a dynamic system based on a linear state space model and erroneous observations of the system output When (erroneous) measurements of the system input are available, they can be included in the estimation process However, often only bounds for the measurement errors can be given, since a more detailed error description is not available or too complicated In this case, set theoretic estimation methods have found to be useful and even superior to statistical estimation methods [10, 11, 12] Set theoretic estimation provides the set of all feasible system states that are compatible with the system model, the observations, and the error bounds his is in sharp contrast to statistical estimation concepts, where an optimal point estimate is supplied together with its associated distribution Different descriptions of the feasible sets are found in literature: Rectangular sets [1], polytopes [22], and ellipsoids [25] his paper focuses on ellipsoidal sets, since they can be manipulated by simple matrix operations and lead to computationally efficient estimation algorithms Set theoretic state estimation consists of 1 propagating a set of feasible states with the aid of the system model, 2 testing the propagated set of states and the set of states defined by a new observation for common points, and 3 fusing the two sets via set intersection in case they overlap he exact description of the resulting feasible sets has been investigated in [30] An approximation by parallelotopes is derived in [29] Some early results on using ellipsoidal sets and approximating the intersection of two ellipsoids is found in [21] he topic has then been extended in [28] Parameter set estimation based on the approximation by bounding ellipsoids is discussed in [6, 7, 24, 8, 9, 3] State estimation, ie, a method for state propagation and several fusion schemes, is discussed in [2] For the case of one dimensional observations, a simple and efficient fusion method is given in [25], which is based on the results in [6, 7] his paper first (re )derives a solution framewor for the above mentioned problems in the case of ellipsoids, ie, 1 the propagation of an ellipsoid and 2 the fusion of an ellipsoid and a possibly degenerated ellipsoid based on the wor in [28] and then recasts the resulting algorithms into a square root form he resulting algorithms are simple and compact, efficient, and easy to implement Furthermore, they can be applied to arbitrary dimensional state and observation vectors he estimation problem is formulated in Sec 3 Section 4 introduces the concept of time updating by propagating the last estimate through the system model An efficient algorithm for performing measurement updates by fusion of an ellipsoid and a possibly degenerated ellipsoid is given in Sec 5 2 Preliminaries In the following, we will represent symmetric, positive definite matrices by means of their square roots, ie, F = FF, U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 Page 1/6 ISASUni-Karlsruhede

4 U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 where F is a lower triangular matrix, eg the cholesy factor of F Furthermore, the following lemma is used extensively [26]: Lemma 21 For two matrices F, G, there exists an orthogonal matrix Θ, ΘΘ = I, such that F Θ=G if, and only if, FF = GG holds F is called the pre array, G is called the post array [23] 3 Problem Formulation Consider a discrete time linear dynamic system given by x +1 = A x + B u (1) z = H x + e, (2) with N dimensional state vector x and M dimensional observation vector z he initial state x 0 is assumed to be confined to an ellipsoidal set given by X 0 = { x 0 :(x 0 ˆx 0 X 1 0 (x 0 ˆx 0 ) 1} Observations suffer from output noise e, which is modeled via amplitude bounds according to e E,whereE is an ellipsoid given by E = { e : e E 1 e 1} (3) In addition, the system input u is uncertain and also modeled by amplitude bounds u U where U is an ellipsoid given by U = { u : u U 1 u 1} (4) When an uncertain measurement û of the system input is available, the uncertainty set (4) will be centered around û according to U = { u :(u û U 1 (u û ) 1 } (5) he uncertainty models (3), (5) are more general than component wise error bounds, since they also include correlation between variables he sizes of the uncertainty ellipsoids are assumed to be a priori nown for the purpose of this paper he state estimation or filtering tas consists of calculating the set of feasible system states at time denoted by S basedonobservationsz 0, z 1,, z Rather than performing batch processing, an estimate should be made available at every time he following notation 1 is used: 1 his notation is preferred to X P X M, X S to avoid excessive sub /superscripting P denotes the set of predicted system states at time with { ) P = x : (x ( ) } ˆp (P ) 1 x ˆp 1 M denotes the set of system states defined by an observation at time, ands denotes the set of estimated system states at time according to { } S = x :(x ŝ (S ) 1 (x ŝ ) 1 he system equation (1) is used to predict all feasible system states P at time based on an estimated system state S 1 at time 1 and the system input U 1 A new observation z at time defines a set M of states that could possibly cause the observation hus, at time, two state estimates are available: he set of predicted states P and the set of states M compatible with the observation Set theoretic estimation now calculates the set of feasible states via set intersection [5], ie, the exact set of estimated system states S at time is given by S = P M Neither the set of predicted states P nor the fusion result S are in general ellipsoids Hence, the remainder of this paper will be concerned with the efficient calculation of outer bounding ellipsoids for P, S to arrive at a simple recursive estimation scheme similar to the Kalman filter 4 Prediction Step (ime Update) Given a set of estimated system states S 1 at time 1, the system equation (1) is used to predict all feasible system states P at time (time update) Since ellipsoids are closed under affine transformations, the right hand side of (1) can be interpreted as the summation of two ellipsoids with centers A 1 ŝ 1, B 1 û 1 and definition matrices A 1 S 1 A 1, B 1U 1 B 1, respectively wo ellipsoidal sets are added up by Minowsi summation, which is defined by adding every point contained in the first ellipsoid to all the points contained in the second ellipsoid However, the Minowsi sum of two ellipsoids is not in general an ellipsoid Nevertheless, a family of bounding ellipsoids can be found that contain the Minowsi sum [28] he center is always given by the sum of centers ˆp = A 1 ŝ 1 + B 1 û 1 (6) and the matrix P is given by P =(05 κ ) 1 A 1 S 1 A 1 +(05+κ ) 1 B 1 U 1 B 1 with parameter κ for κ ( 05, 05) (7) Page 2/6 ISASUni-Karlsruhede

5 Proof: Given a direction vector t of unit length, ie, t = 1, and tangential planes for both the Minowsi sum and the bounding ellipsoid, for which t is the normal vector hen, the normal distances from the center (6) to the planes are given by p Minowsi = t A 1 S 1 A 1 t + t B 1 U 1 B 1 t for the Minowsi sum and by p bound = t P t = (05 κ ) 1 t A 1 S 1 A 1 t +(05+κ ) 1 t B 1 U 1 B 1 t for the bounding ellipsoid P is in fact the definition matrix of a bounding ellipsoid, since p Minowsi p bound (8) for every t, t = 1 his is easily verified by squaring both sides of (8), which leads to ) 2 (t A 1 S 1 A t 1 t + B 1 U 1 B 1 t (05 κ ) 1 t A 1 S 1 A 1 t +(05+κ ) 1 t B 1 U 1 B 1t Hölder s inequality is given by ( N N a i b i a i p ) 1 p ( N b i q ) 1 q for p 1 + q 1 =1,p>1, q>1 Squaring both sides and setting p =2,q = 2, Cauchy s inequality is obtained as ( N 2 ( N )( N ) a i b i ) a i 2 b i 2 (9) We define lower triangular square root factors according to and S 1 = S 1 (S 1 P = P (P heorem 41 A square root algorithm for the prediction step (6), (7) is given by (05 κ ) 1 2 A 1 S 1 (05+κ ) 1 2 B 1 U 1 (05 κ ) 1 2 ŝ 1 (S 1 1) (05+κ ) 1 2 û 1 P 0 = ), ˆp (P 1 (U 1 1 Θ 1 (10) where Θ 1 is an arbitrary orthogonal rotation matrix with Θ 1 Θ 1 = I, that performs the desired triangularization denotes elements, that are not of interest for the prediction Proof: According to Lemma 21, squaring the left hand side of (10) leads to (05 κ ) 1 2 A 1 S 1 (05+κ ) 1 2 B 1 U 1 (05 κ ) 1 2 ŝ 1 (S 1) (05+κ ) 1 2 û 1 (U 1 ) Θ 1 1 [ (05 κ ) 1 2 S 1 A 1 (05 κ ) 1 2 S 1 ] 1ŝ 1 with Θ 1 (05+κ ) 1 2 U 1 B 1 = K11 1 K 1 12 K 1 21 K 1 22 (05+κ ) 1 2 U 1 1û 1 K 11 1 =(05 κ ) 1 A 1 S 1 A 1 U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 he special case of a i = α 1 2 i, b i = α 1 2 i β i gives ( N 2 ( N )( N ) β i ) α i α 1 i βi 2 Setting N = 2, β 1 = t A 1 S 1 A 1 t, β 2 = t B 1 U 1 B 1 t, α 1 =05 κ, α 2 =05+κ yields (9) and concludes the proof Remar 41 κ may be selected in some optimum way, for example in such a way as to minimize the volume of P +(05+κ ) 1 B 1 U 1 B 1 K 12 1 =A 1 ŝ 1 + B 1 û 1 K 21 1 =ŝ 1 A 1 +û 1 B 1 K 22 1 =(05 κ )ŝ 1S 1 1ŝ 1 +(05+κ )û 1U 1 1û 1 Squaring the right hand side of (10) gives P 0 [P P 1 ) ˆp ] [ P ˆp = 0 ˆp (P 1 ˆp ˆp P 1 ˆp + ] Page 3/6 ISASUni-Karlsruhede

6 Comparing the elements completes the proof with U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 he elements of the resulting post array (10) are then used for calculating the desired prediction ellipsoid P given by the center ˆp and the definition matrix P For that purpose, we use boxes to denote the bloc matrices P and ˆp (P 1 that are directly obtained from the post array in (10) he center and the definition matrix are then calculated according to ˆp = P ˆp P = P P (P 1 ) 5 Fusion Step (Measurement Update) An observation z at time defines a set M of states given by M = {m : z H m E } according to (2) or equivalently by { } M = m :(z H m E 1 (z H m ) 1 Calculation of the minimum volume bounding ellipsoid for the intersection of P and M requires numerical optimization Here, a suboptimal method is pursued, that 1 provides a whole family of bounding ellipsoids and 2 yields recursion equations similar to the Kalman filter his method has first been reported in [28] and consists of bounding the intersection set by the convex combination of P and M according to S = { s :(05 λ )(z H s E 1 (z H s ) } +(05+λ )(s ˆp ) P 1 (s ˆp ), (11) After some manipulations and bilinear transformation λ = 05+λ 05 λ,whereλ [0, ] andλ [ 05, 05], S can be written in the following feedbac form Lemma 51 A bounding ellipsoid for the intersection of P and M is given by ŝ =ˆp + λ P H R 1 ˆɛ S = d C for λ [0, ] d =1+λ λ ˆɛ R 1 ˆɛ C = P λ P H R 1 H P R = E + λ H P H ˆɛ = z H ˆp Remar 51 Obviously, we have (P M ) S (P M ), ie, the bounding ellipsoid S not only contains the intersection of P and M, but is also itself contained in their union heorem 51 A square root algorithm for the fusion step (11) is given by: E z = λ 1 2 P H ˆɛ λ 1 2 H P 0 P (E 1 ˆp (P 1 R 0 (R 1 C (R 1 ŝ (C 1 Θ 2 with R = R R, C = C C,ansE = E E (12) Proof: Again, according to Lemma 21, the left hand side of (12) is squared E λ 1 2 H P 0 P Θ 2 z (E 1 ˆp (P 1 E 0 E 1 z = λ 1 2 P H P P 1 ˆp Θ 2 ( ) E + λ H P H λ 1 2 H P z H ˆp λ 1 2 P H P ˆp ) (z ˆp H ˆp P 1 ˆp λ z E 1 z ˆp Page 4/6 ISASUni-Karlsruhede

7 and compared with the squared right hand side of (12) R 0 λ 1 2 P H (R 1 C ˆɛ (R 1 ŝ (C 1 R λ 1 2 R 1 H P R 1 ˆɛ = 0 C C 1 ŝ R λ 1 2 H P ˆɛ λ 1 2 P H C + λ P H R 1 H P ŝ λ P H R 1 ˆɛ ˆɛ ŝ λ ˆɛ R 1 H P ŝ C 1 ŝ + λ ˆɛ R 1 ˆɛ to prove (12) Again, we use boxes to denote the bloc matrices C, ŝ (C 1, and λ 1 2 ˆɛ (R 1 that are directly obtained from the post array in (12) he center and the definition matrix of the fusion ellipsoid S are then calculated on the basis of these bloc matrices according to ŝ = C ŝ (C 1 ) S = d C C, d =1+λ ˆɛ, (R 1 λ 1 2 ˆɛ 6 Recursive State Estimation (R 1 At every time, the set of feasible states is predicted using (10) In addition, when a new observation z is available, a fusion step according to (12) is subsequently performed he resulting recursion equations are similar in appearance to the Kalman filter equations However, the resulting state estimates are different because of the different uncertainty model At first glance, four inversions of square root factors U 1, E 1, P 1, S 1 must be performed for setting up the left hand side bloc matrices for prediction and fusion, respectively However, in fact only U 1 and E 1 must be calculated, since ˆp (P 1 is available as a by product from the prediction post array Furthermore, the lower left bloc matrix of the prediction pre array is calculated on the basis of the elements of the fusion post array according to (05 κ ) 1 2 ŝ 1 =(05 κ ) 1 2 (S ŝ 1 (C d 1) Some technical details lie the determination of overlap of P and M and the calculation of κ, λ for obtaining minimum volume bounding ellipsoids at every time step have been omitted for the sae of brevity It is important to note, that the calculation of a minimum volume ellipsoid at each time step allows for recursive computation Hence, it is useful from a practical point of view However, doing so does not mean that the final ellipsoid recursively obtained after several time steps is of minimum volume For calculating a minimum volume ellipsoid after several time steps, all previous measurements have to be reconsidered 7 Conclusions Efficient approximate set theoretic state estimators have been derived in square root form by generalizing the results for Kalman filtering reported in [23] he estimators apply to linear state space models for the case, that both system and observation uncertainties are modeled as unnown but bounded by ellipsoidal sets Besides having numerical advantages, the square root form allows a parallel or even decentralized implementation of set theoretic estimators similar to the treatment of Kalman filtering in [4] he proposed square root algorithms are also useful in the context of the unification of stochastic and set theoretic estimators proposed in [13] [20] References [1] S Atiya and G D Hager, Real ime Vision Based Localization, IEEE ransactions on Robotics and Automation, Vol 9, No 6, pp , 1993 [2] F L Chernouso, State Estimation for Dynamic Systems CRC Press, 1994 [3] M-F Cheung, S Yurovich, and K M Passino, An Optimal Volume Ellipsoid Algorithm for Parameter Set Identification, IEEE ransactions on Automatic Control, Vol38, No 8, pp , 1993 [4] M Chin, W C Karl, and A S Willsy, A Distributed and Iterative Method for Square Root Filtering in Space time Estimation, automatica, Vol 31, No 1, pp 67 82, 1995 [5] P L Combettes, he Foundations of Set heoretic Estimation, Proceedings of the IEEE, Vol 81, No 2, pp , 1993 [6] J R Deller, Set Membership Identification in Digital Signal Processing, IEEE ASSP Magazine, Vol6, pp 4 20, 1989 [7] J R Deller and C Lu, Linear Prediction Analysis of Speech Based on Set Membership heory, U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 Page 5/6 ISASUni-Karlsruhede

8 U D Hanebec, "A Square-Root Algorithm for Set heoretic State Estimation", Proceedings of the European Control Conference (ECC'2001), Porto, Portugal, 2001 Computer Speech and Language, Vol 3, pp , 1989 [8] J R Deller, M Nayeri, and S F Odeh, Least Squares Identification with Error Bounds for Real ime Signal Processing and Control, Proceedings of the IEEE, Vol 81, No 6, pp , 1993 [9] J R Deller and S F Odeh, Adaptive Set Membership Identification in O(mime for Linear in Parameters Models, IEEE ransactions on Signal Processing, Vol 41, No 5, pp , 1993 [10] G D Hager, S P Engelson, and S Atiya, On Comparing Statistical and Set Based Methods in SensorDataFusion,Proceedings of the 1993 IEEE International Conference on Robotics and Automation, Atlanta, GA, pp , 1993 [11] U D Hanebec and G Schmidt, Set theoretic Localization of Fast Mobile Robots Using an Angle Measurement echnique, Proceedings of the 1996 IEEE International Conference on Robotics and Automation, Minneapolis, Minnesota, pp , 1996 [12] U D Hanebec and G Schmidt, Mobile Robot Localization Based on Efficient Processing of Sensor Data and Set theoretic State Estimation, in A Casals and A de Almeida, eds, Experimental Robotics V, he Fifth International Symposium, Barcelona, Catalonia, 1997, Lecture Notes in Control and Information Sciences 223, pp , Springer, 1998 [13] U D Hanebec and J Horn, A New State Estimator for a Mixed Stochastic and Set heoretic Uncertainty Model, Proceedings of SPIE Vol 3720, AeroSense Symposium, Orlando, Florida, USA, 1999 [14] U D Hanebec and J Horn, A New Estimator for Mixed Stochastic and Set heoretic Uncertainty Models Applied to Mobile Robot Localization, Proceedings of the 1999 IEEE International Conference on Robotics and Automation (ICRA 99), Detroit, Michigan, USA, 1999 [15] U D Hanebec, J Horn, and G Schmidt, On Combining Statistical and Set heoretic Estimation, Automatica, Vol 35, No 6, pp , 1999 [16] U D Hanebec and J Horn, New Estimators for Mixed Stochastic and Set heoretic Uncertainty Models: he Vector Case, Proceedings of the Fifth European Control Conference (ECC 99), Karlsruhe, Germany, 1999 [17] U D Hanebec and J Horn, New Results for State Estimation in the Presence of Mixed Stochastic and Set heoretic Uncertainties, Proceedings of the 1999 IEEE Systems, Man, and Cybernetics Conference (SMC 99), oyo, Japan, 1999 [18] U D Hanebec, J Horn, A New Concept for State Estimation in the Presence of Stochastic and Set heoretic Uncertainties, Proceedings of the 1999 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 99), Kjongju, Korea, 1999 [19] U D Hanebec, J Horn, New Estimators for Mixed Stochastic and Set heoretic Uncertainty Models: he Scalar Measurement Case, Proceedings of the 1999 IEEE Conference on Decision and Control (CDC 99), Phoenix, Arizona, 1999 [20] U D Hanebec, J Horn, Fusing Information Simultaneously Corrupted by Uncertainties with Known Bounds and Random Noise with Known Distribution, Information Fusion, Elsevier Science, Vol 1, No 1, pp 55 63, 2000 [21] W Kahan, Circumscribing An Ellipsoid About the Intersection of wo Ellipsoids, Canad Math Bull, Vol 11, No 3, pp , 1968 [22] M Milanese and G Belaforte, Estimation heory and Uncertainty Intervals Evaluation in the Presence of Unnown but Bounded Errors: Linear Families of Models and Estimates, IEEE ransactions on Automatic Control, Vol 27, pp , 1982 [23] P Par and Kailath, New Square Root Algorithms for Kalman Filtering, IEEE ransactions on Automatic Control, Vol 40, No 5, pp , 1995 [24] A K Rao, Y-F Huang, and S Dasgupta, ARMA Parameter Estimation Using a Novel Recursive Estimation Algorithm with Selective Updating, IEEE ransactions on Signal Processing, Vol 38, No 3, pp , 1990 [25] A Sabater and F homas, Set Membership Approach to the Propagation of Uncertain Geometric Information Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, pp , 1991 [26] A H Sayed and Kailath, A State Space Approach to Adaptive RLS Filtering, IEEE Signal Processing Magazine, Vol 11, No 3, pp 18 70, 1994 [27] F C Schweppe, Recursive State Estimation: Unnown but Bounded Errors and System Inputs, IEEE ransactions on Automatic Control, Vol 13, pp 22 28, 1968 [28] F C Schweppe Uncertain Dynamic Systems Prentice Hall, 1973 [29] A Vicino and G Zappa, Sequential Approximation of Feasible Parameter Sets for Identification with Set Membership Uncertainty, IEEE ransactions on Automatic Control, Vol 41, No 6, pp , 1996 [30] E Walter and H Piet-Lahanier, Exact Recursive Polyhedral Description of the Feasible Parameter Set for Bounded Error Models, IEEE ransactions on Automatic Control, Vol 34, No 8, pp , 1989 Page 6/6 ISASUni-Karlsruhede