Hybrid Constrained Estimation For Linear Time-Varying Systems

Transcription

1 2018 IEEE Conference on Decision and Control (CDC) Miami Beach, FL, USA, Dec , 2018 Hybrid Constrained Estimation For Linear Time-Varying Systems Soulaimane Berkane, Abdelhamid Tayebi and Andrew R. Teel Abstract For linear time-varying systems with possibly constrained states, we propose a hybrid observer that guarantees the containment of the estimated state variables in a prescribed domain of interest. The hybrid observer employs a Kalmantype continuous estimator during the flows while, during the jumps, projects the state estimates onto the set described by the constraint equation. A suitable choice of the flow and jump sets allows to conclude uniform global asymptotic stability of the zero estimation error set. I. INTRODUCTION State estimation for general linear systems has been considered in the literature since early works by Kalman and Bucy [1]. In practice, however, additional information on the system in the form of constraints can be incorporated for different reasons [2]. For instance, meaningful state estimation for chemical reactions requires the concentrations and pressures to be nonnegative [3]. In an attitude estimation problem, the attitude quaternion must have unit norm [4] and the rotation matrix must be orthogonal [5] to obtain a meaningful attitude estimate. In a compartmental model with zero net inflow, mass is conserved [6]. In navigation systems, when the target moves with constant speed, the acceleration vector is orthogonal to the velocity vector (kinematic constraint) [7]. Kinematic constraints are used as additional measurements in [8]. Different Kalman-type filters have been proposed for timeinvariant linear systems with linear equality constraints. The approaches in [8], [7], [9] consider linear equality constraints with an uncertainty in the constraint (termed soft constraint). In this case, the constraint equation is used as a pseudo-measurement which is incorporated in a Kalman filter. In certain applications, the constraint is hard in the sense that it is known exactly. To deal with this type of constraints, a projection method is proposed in [10] in which the constrained estimate is obtained from the unconstrained estimate of the conventional Kalman filter by projecting onto the subspace or the surface described by the constraint equation. In [11], a constrained Kalman filter is designed for the projected system representation of the original constrained system leading to an improvement This research work was supported by NSERC-DG RGPIN, and is supported in part by NSF grant no. ECCS and AFOSR grants no. AFOSR FA and AFOSR FA S. Berkane (berkane@kth.se) is with the Department of Automatic Control, KTH Royal Institute of Technology, Stockholm, Sweden. A. Tayebi (atayebi@lakeheadu.ca) is with the Department of Electrical and Computer Engineering, University of Western Ontario, London, Ontario, Canada and also with the Department of Electrical Engineering, Lakehead University, Thunder Bay, Ontario, Canada. A. R. Teel (teel@ece.ucsb.edu) is with the Department of Electrical and Computer Engineering, University of California, Santa Barbara, USA. in the estimation covariance compared to the unconstrained Kalman filter. In this work, we propose a hybrid observer that is able to estimate the trajectories of a uniformly observable linear time-varying system while guaranteeing that the state estimates remain inside a prescribed domain of interest. The domain of interest is assumed to contain the surface set described by a given constraint equation. The proposed hybrid observer employs a Kalman-type estimator during the flows and projects the estimated state onto a constraint set (included in the prescribed domain) during the jumps. We show that the proposed hybrid observer leads to uniform global asymptotic stability of the set corresponding to zero observation error. The performance of the proposed hybrid observer is illustrated through a simple example. II. BACKGROUND AND PRELIMINARIES Throughout the paper, we use R and R 0 to denote the sets of real and nonnegative real numbers, respectively. We denote by R n the n-dimensional Euclidean space. The Euclidean norm of x R n is defined as x = x x and the induced (spectral) norm on R m n is given by A = λ max (A A) for any A R m n. Given a vector x R n and a non-empty set A R n, the distance of x to A is defined as x A := inf y A x y. The distance between two non-empty sets A, B R n is defined as dist(a, B) := inf x B x A. The closure of a set A is denoted as cl(a). A general model of a hybrid dynamical system takes the form [12]: { Ẋ F(X), X F, X + (1) J(X), X J, where the flow map, F : M TM governs the continuous flow of X on the manifold M R n, the flow set F M dictates where the continuous flow could occur. The jump map, J : M M, governs discrete jumps of the state X, and the jump set J M defines where the discrete jumps are permitted. The hybrid system (1) is defined by its data and is often denoted as H = (F, F, J, J) for compactness. A subset T R 0 N is a hybrid time domain, if it is a union of finitely or infinitely many intervals of the form [t j, t j+1 ] {j} where 0 = t 0 t 1 t 2..., with the last interval being written of the form [t j, t j+1 ] {j} or [t j, ) {j}. The ordering of points on each hybrid time domain is such that (t, j) (t, j ) if t t and j j. A hybrid arc is a function h : dom h M, where dom h is a hybrid time domain and, for each fixed j, t h(t, j) is a locally absolutely continuous function on the interval I j = {t : (t, j) dom h}. The time projection of h is defined as the function h : [0, T ) M such that h (t) = /18/$ IEEE 4643

2 h(t, j(t)) where T = sup{t : (t, j) dom h, j N} and j(t) = max{j : (t, j) dom h}. III. PROBLEM FORMULATION Consider the following linear time-varying (LTV) system: ẋ = A(t)x + B(t)u(t), x(0) R n, (2) y = C(t)x (3) where x R n is the state vector, y R p is the measured output vector and u : [0, + ) R m is a locally integrable function of time. The matrices A(t), B(t), C(t) are known, finite-dimensional matrix-valued (of appropriate dimensions) functions depending on time. They are continuously differentiable and uniformly bounded with bounded derivatives. Assumption 1: The pair (A( ), C( )) is uniformly observable 1. Under the above observability assumption, system (2)-(3) admits the following Kalman-type observer: ˆx = A(t)ˆx + B(t)u(t) + P (t)c(t) Q(t)(y C(t)ˆx) (4) where P (t) is solution to the following continuous Riccati equation (CRE): P (t) = A(t)P (t) + P (t)a(t) P (t)c(t) Q(t)C(t)P (t) + V (t) (5) where the following assumption holds: Assumption 2: P (0) is positive definite. Q(t) and V (t) are continuously differentiable. There exist c q, c v > 0 such that Q(t) c q and V (t) c v for all t 0. There exist ɛ q, ɛ v > 0 such that Q(t) ɛ q I n and V (t) ɛ v I n for all t 0. Note that in a stochastic setting, the above estimator corresponds to the optimal Kalman filter where matrices V (t) and Q(t) 1 are interpreted as covariance matrices of additive noise on the system state and output [1]. The above observer falls also under the framework of Riccati observers and can be shown to guarantee uniform global exponential stability of the estimation error [13], [14]. In some applications, it is desirable to guarantee that the estimated state ˆx remains in a prescribed region of the state space while converging to the true state x. To solve this type of estimation problem, we formulate the following objective: Objective 1: For system (2)-(3), design an estimation algorithm such that the state estimate ˆx converges asymptotically to x while (t, y, ˆx) lies in the prescribed set ˆΩo R 0 R p R n for all times. The following are the assumptions needed to establish our results: Assumption 3: There exist a known full row rank matrix D(t, y) R s n and a known vector d(t, y) R s, with 0 < s n, such that every solution x(t) for (2)-(3) satisfies: D(t, y(t))x(t) = d(t, y(t)), t 0. (6) 1 The definition of uniform observability can be found in [13]. Assumption 4: The set ˆD := {(t, y, ˆx) : D(t, y)ˆx = d(t, y)} satisfies ˆD ˆΩ o. The constraint equation (6) provides additional information about the system (2)-(3). This constraint can be derived from physical constraints that restrict the motion of the system for some practical purposes, or from virtual constraints obtained from the output equation C(t)x = y. In fact, if there are no physical constraints for the system, we can simply pick D(t, y) = C(t) and d(t, y) = y or any combination of the outputs to derive a constraint of type (6). Therefore, the results presented in this paper are quite general in the sense that they can be applied to constrained and unconstrained systems. The motivation behind introducing Assumption 4 is related to the estimation scheme described in the next section. The set ˆD represents the target set where we project the observer state whenever it leaves the prescribed domain. More precisely, we force the observer state ˆx to jump to the set ˆD whenever (t, y, ˆx) leaves (or is close to leaving) the objective set ˆΩo. The motivation behind introducing constraints of type (6) is the availability of a projection mechanism on the constraint set ˆD. In fact, the constraint equation (6) is linear if we consider (t, y) as exogenous measured signals, and projection tools on linear constraint surfaces already exist [10]. Interestingly, as will be detailed in the proof of our main result (Theorem 1), projecting the state estimate ˆx on the constraint set ˆD guarantees a decrease in a quadratic cost function. Assumptions 3 and 4 are design assumptions that need to be fulfilled before applying the estimation scheme proposed in the next section. In fact, an appropriate choice of the constraint equation (6) has to be made given an objective set ˆΩ o as it is going to be illustrated in the example provided in Section VI. IV. MAIN RESULTS In this section, we propose a hybrid observer for the LTV system (2)-(3) under a constraint of type (6) achieving Objective 1. Consider the following hybrid state observer: ˆx = A(t)ˆx + B(t)u(t) + P (t)c(t) Q(t)(y C(t)ˆx) (t, y, ˆx) ˆF (7) ˆx + = ˆx P (t)d(t, y) (D(t, y)p (t)d(t, y) ) 1 (D(t, y)ˆx d(t, y)) (t, y, ˆx) ˆ J (8) where the flow set ˆF ˆM := R 0 R p R n and jump set Jˆ ˆM are designed to satisfy the following conditions: ˆF ˆΩ o (9) dist( J ˆ, ˆD) > 0 (10) ˆF J ˆ = ˆM (11) for some given objective set ˆΩ o ˆM, where ˆD is defined in Assumption 4. The matrix P (t) is solution of the CRE 4644

3 equation (5) and is independent from the system state. It depends only on time t and the chosen initial condition P (0) and parameters Q(t) and V (t). Note that the matrix D(t, y)p (t)d(t, y) is non-singular thanks to the assumption that D(t, y) is always full row rank and the fact that P (t) is positive definite for all times. Condition (9) allows for the hybrid observer to flow only when (t, y, ˆx) ˆΩ o. Condition 10 states that there exists a minimum uniform distance between each point in the jump set J ˆ and the target set ˆD where solutions are projected after jump. The third condition (10) is necessary to guarantee that the observer s trajectory exists starting from any initial condition. Remark 1: Under the framework of [12], to ensure that the hybrid closed loop system is well-posed (which guarantees robustness to small measurement noise and some other nice properties), one has to further restrict the sets ˆF and J ˆ to be closed. However, this assumption is not needed to derive our main result. Remark 2: The update rule (8) is inspired from the projection method [10] used in constrained Kalman filtering. This technique allows to project the estimated state ˆx on the constraint surface such that ˆx + satisfies the constraint, i.e., D(t, y)ˆx + = d(t, y), and minimizes the cost function: min ˆx (ˆx+ ˆx) P (t) 1 (ˆx + ˆx). (12) To analyze the + behavior of the closed loop system, we define the state X := (t, x, ˆx) M := R 0 R n R n. Now, we can state our main result. Theorem 1: Consider system (2)-(3), under Assumptions 1-4, interconnected with the hybrid observer (7)-(8) where P (t) is solution to (5) with P (0), Q(t), V (t) satisfying Assumption 2. Assume that ˆF and J ˆ are designed such that conditions (9)-(11) are satisfied. Then, the set A := {(t, x, ˆx) : x ˆx = 0} is uniformly globally asymptotically stable and (t, y(t), ˆx (t)) ˆΩ o for all t 0. Furthermore, there exist k, λ > 0 and a strictly increasing continuous function κ : R 0 R >0 such that for any initial condition X(0, 0) = ζ 0 M and for all (t, j) domx one has 2 : i) X(t, j) A k exp( λt) ζ 0 A ; and ii) X(t, j) A κ( ζ 0 A ) exp( λ(t + j)) ζ 0 A. Moreover, the following proposition (proved in Appendix II) provides sufficient conditions, that imply (9)-(11), which are more convenient to verify in certain situations: Proposition 1: Consider a matrix-valued function L : R 0 R p R l s, l N. Let f : ˆM R s be such that f(t, y, ˆx) := L(t, y)(d(t, y)ˆx d(t, y)) 2. Assume that there exist scalars ɛ, c d, c l > 0 such that D(t, y) c d, L(t, y) c l for all (t, y) R 0 R p and f 1 ([0, ɛ]) ˆΩ o. If ˆF and ˆ J are chosen such that f 1 ([0, ɛ ]) ˆF ˆΩ o (13) ˆM \ ˆF J ˆ f 1 ([ɛ, + )) (14) 2 The global exponential stability in item i) is known as global exponential stability in the t-direction while in item ii) it is best described as a type of semi-global exponential stability. The later is weaker than the notion of global exponential stability as defined in [15]. for any 0 < ɛ ɛ then Assumption 4 holds and conditions (9)-(11) are satisfied. Proposition 1 provides some conditions which, when verified, allow to construct the flow set ˆF and jump set J ˆ guaranteeing (9)-(11). In certain applications, it is desirable to have the flow and jump sets closed (see Remark 1). If we further restrict f to be continuous in Proposition 1, then there exist closed sets ˆF and J ˆ satisfying (13)-(14) since f 1 ([0, ɛ ]) and f 1 ([ɛ, + )) are closed. V. PROOF OF THEOREM 1 In view of (2)-(3) and (7)-(8), the state X = (t, x, ˆx) evolves according to the hybrid system H = (F, F, J, J) with the following data: F(X) := 1 A(t)x + B(t)u(t) A(t)ˆx + B(t)u(t) + P (t)c(t) Q(t)C(t)(x ˆx) (15) t J(X) := x (16) ˆx + M(P (t), D(t, C(t)x))(x ˆx) F := {(t, x, ˆx) : (t, C(t)x, ˆx) ˆF} (17) J := {(t, x, ˆx) : (t, C(t)x, ˆx) J ˆ } (18) where we have used the fact that D(t, y)x = d(t, y) and defined M(A, B) := AB (BAB ) 1 B to write the jump map J(X). The matrix M(A, B), with A and BAB nonsingular, satisfies the following property which can be easily checked: (I M(A, B)) A 1 (I M(A, B)) = A 1 (I M(A, B)). (19) Now, consider the following Lyapunov function candidate: V(X) := (x ˆx) P 1 (t)(x ˆx) (20) which is continuously differentiable on an open set containing F and positive definite with respect to the set A defined in Theorem 1. Note that it is not difficult to show that the set A is closed and that for any X = (t, x, ˆx) M one has X A = 1 2 x ˆx. Moreover, thanks to the uniform observability assumption of the pair (A( ), C( )), uniform boundedness of A(t), and the fact that Q(t) and V (t) are uniformly positive definite and bounded matrices, one can show the existence of β 1, β 2 > 0 such that (see [16]) β 1 I P (t) β 2 I, t 0. (21) Therefore, we can derive the following bounds on V(X): 2β 1 2 X 2 A V(X) 2β 1 1 X 2 A. (22) 4645

4 For all X F, and in view of (2)-(3), (7) and (5), one has V(X) = (ẋ ˆx) P 1 (t)(x ˆx) + (x ˆx) P 1 (t)(ẋ ˆx) (x ˆx) P 1 (t) P (t)p 1 (t)(x ˆx) = (x ˆx) ( C (t)q(t)c(t) +P 1 (t)v (t)p 1 (t) ) (x ˆx) ɛ vβ 1 β2 2 V(X) (23) where we used V (t) ɛ v I n > 0. Moreover, for all X J and in view of (16), one has x + ˆx + = x ˆx M(P (t), D(t, C(t)x))(x ˆx) = (I M(P (t), D(t, C(t)x)))(x ˆx) (24) which, using property (19), leads to V(X + ) = (x + ˆx + ) P 1 (t)(x + ˆx + ) = (x ˆx) (I M(P (t), D(t, C(t)x))) P 1 (t) (I M(P (t), D(t, C(t)x)))(x ˆx) = (x ˆx) P 1 (t)(i M(P (t), D(t, C(t)x)))(x ˆx) = V(X) (D(t, C(t)x)(x ˆx)) (D(t, C(t)x)P (t)d(t, C(t)x) ) 1 (D(t, C(t)x)(x ˆx)) V(X) β2 1 (D(t, C(t)x)(x ˆx)) (D(t, C(t)x)D(t, C(t)x) ) 1 (D(t, C(t)x)(x ˆx)) (25) where we have used the fact that D(t, y) is full row rank and, hence, D(t, y)p (t)d(t, y) is nonsingular and upper bounded by the matrix β 2 D(t, y)d(t, y) which is also nonsingular. To proceed, we need the following lemma whose proof is given in Appendix I. Lemma 1: Consider the set ˆD defined in Assumption 4. Then, for any ξ = (t, y, ˆx) R 0 R p R n, one has ξ ˆD = (D(t, y)ˆx d(t, y)) (D(t, y)d(t, y) ) 1 (D(t, y)ˆx d(t, y)). (26) Using condition (10), there exists ɛ > 0 such that for all ξ = (t, y, ˆx) J ˆ one has ξ ˆD ɛ. It follows that for all X = (t, x, ˆx) J one has (t, C(t)x, ˆx) ˆD ɛ. Therefore, in view of the result of Lemma 1 and (25), one has for all X J V(X + ) V(X) β2 1 (t, C(t)x, ˆx) ˆD β 1 ɛ. (27) In view of (22), (23) and (27), it follows that for all (t, j) domx one has which leads to V(X(t, j)) V(X(0, 0)) β2 1 ɛj 2β1 1 X(0, 0) 2 A β2 1 ɛj. j 2β 2 β 1 ɛ X(0, 0) 2 A. (28) It follows, by [12, Proposition 3.30], that the set A is uniformly globally pre-asymptotically stable. Now, we argue that every solution of H = (F, F, J, J) is complete. In fact, 2 in view of (11), one has F J = M which covers the whole state space. Moreover, since u : [0, + ) R m is locally integrable, solutions to (2) are unique and complete. Thus, it is clear that starting from any initial condition in cl(f) \ J there exists a solution to the differential inclusion Ż F(Z) that flows for some positive time. Therefore, by [12, Proposition 2.10], there exists a nontrivial solution to H from any initial condition in M. Moreover, since solutions to (2) are complete (no escape time) and the error x ˆx is bounded by the Lyapunov argument above, it follows that solutions to H cannot escape to infinity in finite time. It follows, from [12, Proposition 2.10] that every solution is complete. Finally, uniform global pre-asymptotic stability for A and completeness of solutions imply uniform global asymptotic stability. Now let X(0, 0) = ζ 0 M. In view of (23) and (27), it is clear that for all (t, j) domx one has V(X(t, j)) exp( 2λt)V(ζ 0 ) (29) with λ = ɛ v β 1 /2β 2 2. This leads, using (22), to the result of item i) with k = β 2 /β 1. Moreover, using (28), one has X(t, j) A k exp( λt) ζ 0 A k exp(2β 2 λ ζ 0 2 A/β 1 ɛ) exp( λ(t + j)) ζ 0 A := κ( ζ 0 A ) exp( λ(t + j)) ζ 0 A. VI. EXAMPLE In this section we provide a simple example that illustrates the concept presented in this paper. Consider a vehicle moving in the two dimensional plane R 2 with position denoted by (x 1, x 2 ) and velocity denoted by (x 3, x 4 ). The vehicle under consideration is equipped with a position sensor. The dynamics of the vehicle are given by the linear system: ẋ = Ax + Bu(t), (30) y = Cx, (31) where x = [x 1, x 2, x 3, x 4 ] R 4 is the state of the vehicle, y = [y 1, y 2 ] R 2 is the output of the position sensor, u(t) R 2 is the known acceleration input and ] A = , B = [ , C = (32) The acceleration input of the vehicle is further constrained to make an angle θ with the horizontal, i.e., [ ] cos(θ) u(t) = v(t) (33) sin(θ) where v(t) R is the amount of acceleration applied (v(t) 0 corresponds to deceleration and v(t) 0 corresponds to acceleration). We assume that the vehicle was initially (times before t 0 = 0) moving along the road that makes an angle of θ with the horizontal and passing by the origin. Hence, it 4646

5 can be checked that the state of the vehicle under the input (33) satisfies the following constraint for all times: [ ] sin(θ) cos(θ) 0 0 x(t) = 0. (34) 0 0 sin(θ) cos(θ) Assume that we want to solve the estimation problem for system (30)-(31) while guaranteeing that the state estimate lies in the objective set ˆΩ o = {(t, y, ˆx) : sin(θ)ˆx 1 cos(θ)ˆx 2 2 µ} (35) for some µ > 0 or, in other words, the estimated position (ˆx 1, ˆx 2 ) is sufficiently close to the line making an angle θ with the horizontal and passing through the origin. The possible available constraints for system (30)-(31) under input (33) are: D 1 x := [ ] x = y 1, (36) D 2 x := [ ] x = y 2, (37) D 3 x := [ sin(θ) cos(θ) 0 0 ] x = 0, (38) D 4 x := [ 0 0 sin(θ) cos(θ) ] x = 0. (39) We can pick any subset of constraints from the above set of constraints to define the matrices D(t, y) and d(t, y) in Assumption 3. However, we need to make sure that the obtained D(t, y) matrix is full row rank and that Assumption 4 is fulfilled. For instance, the choice D(t, y) = [D 1 ; D 2 ; D 3 ] is not suitable since the resulting D(t, y) is not full row rank. Also, D(t, y) = [D 1 ; D 4 ] is not a suitable choice either since Assumption 4 does not hold. In the following, we will discuss and compare the following two valid choices for the constraint of type 6: D(t, y) = [D 1 ; D 2 ] = C, d(t, y) = y, (40) D(t, y) = D 3, d(t, y) = 0. (41) In both cases above, we can verify that Assumptions 3-4 are satisfied. Now, we turn to the design of the flow and jump sets using the approach of Proposition 1. We pick L(t, y) = I 2, i.e., f(t, y, ˆx) = D(t, y)ˆx d(t, y) 2. Let us show that for both choices above, one has f 1 ([0, µ]) ˆΩ o. In fact, for the choice of the constraint in (40), one has sin(θ)ˆx 1 cos(θ)ˆx 2 2 = D 3 (ˆx x) 2 (42) = D 3 C C(ˆx x) 2 (43) D 3 C 2 C(ˆx x) 2 (44) = C ˆx y 2 = f(t, y, ˆx) (45) where we used the facts that D 3 x = 0, D 3 C C = D 3 and D 3 C = 1. Therefore, if f(t, y, ˆx) µ then one has (t, y, ˆx) ˆΩ o. The second choice of the constraint in (41) also leads to the same conclusion. Consequently, according to Proposition 1, it is sufficient to pick the flow and jump sets as follows: ˆF = { (t, y, ˆx) : D(t, y)ˆx d(t, y) 2 ɛ } (46) J ˆ = { (t, y, ˆx) : D(t, y)ˆx d(t, y) 2 ɛ } (47) with 0 < ɛ µ. We consider a simulation scenario with θ = π/3, µ = ɛ = 1, x(0) = [0, 0, 0, 0], ˆx(0, 0) = x x 1 Fig. 1: Real position of the vehicle (black), unconstrained estimated position (green) and constrained estimated position (solid blue and dashed red). The constraint equation used is (40). x x 1 Fig. 2: Real position of the vehicle (black), unconstrained estimated position (green) and constrained estimated position (solid blue and dashed red). The constraint equation used is (41). [ 10, 30, 10, 10], P (0) = I 4, Q = 0.1I 2 and V = 0.1I 4. The considered acceleration input is v(t) = 1 for t [0, 5] [23, 30], v(t) = 0 for t [9, 18] and v(t) = 1 for t [5, 9] [18, 23], with all intervals in seconds. Figures 1 and 2 show the estimated position of the vehicle when considering a hybrid observer under constraint (40) and a hybrid observer under constraint (41), respectively. Both observers guarantee that the state estimates are within the region of interest ˆΩo as defined in (35). However, since D 3ˆx C ˆx y, one can show that the jump set in (47) under constraint (40) is larger that the jump set defined using constraint (41) which explains the larger number of jumps in the scenario of Figure 1 compared to Figure 2. VII. CONCLUSION We proposed a uniformly globally asymptotically stable hybrid observer, for time-varying (and possibly constrained) 4647

6 x ˆx t (seconds) Fig. 3: Estimation error plot versus time. Unconstrained estimator (green), constrained estimator with constraint equation (40) (solid blue and dashed red) and constrained estimator with constraint equation (41) (solid black and dashed magenta). linear systems, that guarantees the containment of the estimated state in a prescribed domain of interest. Uniform global exponential stability in the t-direction as well as non-uniform global exponential stability in the hybrid time domain are also established. The proposed hybrid observer flows only when the state estimates are within a subset of the domain of interest, and jumps to the constraint region otherwise. In this work, only time-varying (outputdependent) linear constraints are considered. An interesting direction for future work is to consider a more general class of dynamical systems subject to nonlinear equality and/or inequality constraints. APPENDIX I PROOF OF LEMMA 1 For a given (t, y) R 0 R p, define the set Π (t,y) ˆD := {ˆx R n : (t, y, ˆx) ˆD}. Let ξ = (t, y, ˆx) and ξ = (t, y, ˆx ) in R 0 R p R n. Using the definition of the distance one has ξ 2ˆD : = inf ξ ξ 2 = inf ˆx ˆx 2 ξ ˆD ˆx Π (t,y) ˆD = inf ˆx ˆx ˆx 2 subject to D(t, y)ˆx = d(t, y). (48) The above minimization can be solved as in [10] using Lagrange multipliers and yield the solution ˆx = ˆx D(t, y) (D(t, y)d(t, y) ) 1 (D(t, y)ˆx d(t, y)) which, when substituted in (48), yields the expression (26). APPENDIX II PROOF OF PROPOSITION 1 Let ξ = (t, y, ˆx) R 0 R p R n. Assume that there exist ɛ, c d, c l > 0 and a matrix function L(, ) such that D(t, y) c d and L(t, y) c l for all (t, y) R 0 R p and f 1 ([0, ɛ]) ˆΩ o. Thus, there exists ˆF and J ˆ such that (13)-(14) hold for any 0 < ɛ ɛ. Condition (9) holds in view of (13). Moreover, by (14) one has ˆM = ( ˆM\ ˆF) ˆF Jˆ ˆF. However, J ˆ ˆF ˆM which leads to J ˆ ˆF = ˆM and hence (11) holds. Now, assume that ξ J ˆ which implies by (14) that ξ f 1 ([ɛ, + )). Therefore, ɛ f(t, y, ˆx) = L(t, y)(d(t, y)ˆx d(t, y)) 2 c 2 l (D(t, y)ˆx d(t, y) 2. (49) On the other hand, since D(t, y) is full row rank and uniformly bounded one has 0 < D(t, y)d(t, y) c 2 d I 3, which implies that c 2 d I 3 (D(t, y)d(t, y) ) 1 <. Consequently, in view of Lemma 1, one has dist( ˆ J, ˆD) : = inf ξ Ĵ ξ ˆD c 2 d inf D(t, y)ˆx d(t, y) 2 ξ Ĵ c 2 d c 2 l ɛ > 0. (50) It follows that condition 10 is fulfilled. REFERENCES [1] R. E. Kalman and R. S. Bucy, New results in linear filtering and prediction theory, Journal of basic engineering, vol. 83, no. 3, pp , [2] E. L. Haseltine and J. B. Rawlings, Critical evaluation of extended kalman filtering and moving-horizon estimation, Industrial & engineering chemistry research, vol. 44, no. 8, pp , [3] P. Vachhani, S. Narasimhan, and R. Rengaswamy, Robust and reliable estimation via unscented recursive nonlinear dynamic data reconciliation, Journal of process control, vol. 16, no. 10, pp , [4] J. L. Crassidis and F. L. Markley, Unscented filtering for spacecraft attitude estimation, Journal of guidance, control, and dynamics, vol. 26, no. 4, pp , [5] R. Mahony, T. Hamel, and J.-M. Pflimlin, Nonlinear complementary filters on the special orthogonal group, IEEE Transactions on Automatic Control, vol. 53, no. 5, pp , [6] B. O. S. Teixeira, J. Chandrasekar, L. A. Torres, L. Aguirre, and D. S. Bernstein, State estimation for equality-constrained linear systems, in Conference on Decision and Control, 2007, pp [7] A. T. Alouani and W. D. Blair, Use of a kinematic constraint in tracking constant speed, maneuvering targets, IEEE Transactions on Automatic Control, vol. 38, no. 7, pp , [8] M. Tahk and J. L. Speyer, Target tracking problems subject to kinematic constraints, IEEE transactions on automatic control, vol. 35, no. 3, pp , [9] Y.-H. Chen and C.-T. Chiang, Adaptive beamforming using the constrained kalman filter, IEEE Transactions on Antennas and Propagation, vol. 41, no. 11, pp , [10] D. Simon and T. L. Chia, Kalman filtering with state equality constraints, IEEE transactions on Aerospace and Electronic Systems, vol. 38, no. 1, pp , [11] S. Ko and R. R. Bitmead, State estimation for linear systems with state equality constraints, Automatica, vol. 43, no. 8, pp , [12] R. Goebel, R. G. Sanfelice, and A. R. Teel, Hybrid Dynamical Systems: modeling, stability, and robustness. Princeton University Press, [13] R. S. Bucy, Global theory of the riccati equation, Journal of computer and system sciences, vol. 1, no. 4, pp , [14] T. Hamel and C. Samson, Position estimation from direction or range measurements, Automatica, vol. 82, pp , [15] A. R. Teel, F. Forni, and L. Zaccarian, Lyapunov-based sufficient conditions for exponential stability in hybrid systems, IEEE Transactions on Automatic Control, vol. 58, no. 6, pp , [16] R. S. Bucy, The riccati equation and its bounds, Journal of computer and system sciences, vol. 6, no. 4, pp ,