State Estimation of Continuous-Time Systems with Implicit Outputs from Discrete Noisy Time-Delayed Measurements

Transcription

1 State Estimation of Continuous-Time Sstems with Implicit Outputs from Discrete Nois Time-Delaed Measurements A. Pedro Aguiar and João P. Hespanha Abstract We address the state estimation of a class of continuous-time sstems with implicit outputs, whose measurements arrive at discrete-time instants, are time-delaed, nois, and ma not be complete. The estimation problem is formulated in the deterministic H filtering setting b computing the value of the state that minimies the induced L 2-gain from disturbances and noise to estimation error, while remaining compatible with the past observations. To avoid weighting the distant past as much as the present, a forgetting factor is also introduced. We show that, under appropriate observabilit assumptions, the optimal estimate converges globall asmptoticall to the true value of the state in the absence of noise and disturbance. In the presence of noise, the estimate converges to a neighborhood of the true value of the state. The estimation of position and attitude of an autonomous vehicle using measurements from an inertial measurement unit (IMU) and a monocular charged-coupled-device (CCD) camera attached to the vehicle illustrates these results. In the contet of this application, the estimator can deal directl with the usual problems associated with vision sstems such as noise, latenc and intermittenc of observations. I. INTRODUCTION Consider a continuous-time sstem described b ẋ = A(, u)+g(u)w, () E j (, v j ) j = C j (, u)+v j, j I:= {, 2,...N} (2) where R n denotes the state of the sstem, u R m its control input, j R qj its jth measured output, w R r an input disturbance that cannot be measured, and v j R pj measurement noise affecting the jth output. The functions A(, u), C j (, u), and E j (, v j ) are affine in. The initial condition () of () and the signals w and v j are assumed deterministic but unknown. Each measured output j is onl defined implicitl through (2) and the map E j (, ) is such that Image E j (, ) = { l j E j ()+ α ij E ij : α ij R } (3) i= where E ij R pj qj and E j () are affine in. Note that although the implicit representation (2) is affine in, an eplicit representation would generall be nonlinear. We call () (3) a state-affine sstem with implicit outputs, or for short simpl a sstem with implicit outputs. These This material is based upon work supported b the Institute for Collaborative Biotechnologies through grant DAAD9-3-D-4 from the U.S. Arm Research Office and b the National Science Foundation under Grant No. CCR-384. The work of A. Aguiar was supported in part b project MAYA-Sub of the AdI and b the FCT-ISR/IST plurianual funding program through the POS-Conhecimento Program, in cooperation with FEDER. A. Pedro Aguiar is with the ISR/IST Institute for Sstems and Robotics, Instituto Superior Técnico, Torre Norte 8, Av. Rovisco Pais, 49- Lisboa, Portugal. pedro@isr.ist.utl.pt João P. Hespanha is with Department of Electrical and Computer Engineering, Universit of California, Santa Barbara, CA , USA. hespanha@ece.ucsb.edu tpe of sstems were introduced in [ and are motivated b applications in dnamic vision such as the estimation of the motion of a camera from a sequence of images. In particular, the sstem () (3) arises when one needs to estimate the pose (position and attitude) of autonomous vehicles using measurements from an inertial measurement unit (IMU) and a monocular charged-coupled-device (CCD) camera attached to the vehicle. It can also be seen as a generaliation of perspective sstems introduced b Ghosh et al. [2. The reader is referred to [3, [4 for several other eamples of perspective sstems in the contet of motion and shape estimation. The sstem with implicitl defined outputs described in [5 and the state-affine sstems with multiple perspective outputs considered in [6 are also special cases of () (3). In this paper, we address the state-estimation for the sstem () (2) supposing that the measurements are acquired onl at discrete times t i, i =,,...,k, with t <t <...<t k, and that we onl have access to them after a timedela τ i. Our sequence of measurements from t t t is therefore given b { } t i, ȳ j (t i ),j I i to time i=,...,k (4) where k is the number of measurements received from t to time t (i.e., t k t), ȳ j (t i ):= j (t i )= j(t i τ i ) denotes the time-dela observed variable, and t i = t i + τ i. In (4), we also assume that the measurements ma not be complete, that is, at time t i onl the outputs j R qj with j I i are available, where I i I, and the inclusion ma be strict when some measurements are missing. The problem under consideration is to design an observer which estimates the continuous-time state vector (t) governed b (), given the discrete time-dela measurements ȳ j (t i ) epressed b the output equation ( E j (ti τ i ),v(t i τ i ) ) ȳ j (t i ) ( = C j (ti τ i ),u(t i τ i ) ) + v j (t i τ i ), (5) Using a H deterministic approach, we propose an observer that estimates the state vector (t) given an initial estimate ˆ, the past controls {u(σ) : σ t} and the observations (4), and minimie the induced L 2 -gain from disturbances to estimation error. In particular, for the simpler case of no dela (τ i =), given a gain level γ >, the estimate ˆ should satisf (σ) ˆ(σ) 2 dσ γ 2( (() ˆ ) P (() ˆ ) + w(σ) 2 dσ + k ) v j (t i ) 2 j I i i=

2 where P > and ˆ encode a a-priori information about the state. We also consider the possibilit of introducing an eponential forgetting factor that decreases the weight of, w and v j from a distant past. Over the last two decades the H criterion has been applied to filtering problems, cf., e.g., [, [7 [2. In [, a state-estimator for () (3) was designed using a deterministic H approach, assuming that measurements were continuousl available. Given an initial estimate and the past controls and continuous observations collected up to time t, the optimal state estimate ˆ at time t was defined to be the value that minimies the induced L 2 -gain from disturbances to estimation error. Closel related to H filtering are the minimum-energ estimators, which were first proposed b Mortensen [3 and further refined b Hijab [4. Game theoretical versions of these estimators were proposed b McEneane [5. In [6, minimum-energ estimators were derived for sstems with perspective outputs and input-tostate stabilit like properties of the estimation error with respect to disturbances were presented. In general, both minimum-energ and H state estimators for nonlinear sstems lead to infinite-dimensional observers whose state evolves according to a first-order nonlinear PDE of Hamilton-Jacobi tpe, driven b the observations. The main contribution of this paper is a closed-form solution that is filtering-like and iterative, which continuousl improves the estimates as more measurements become available. More precisel, under appropriate observabilit assumptions, we prove that the optimal state estimate converges asmptoticall to the true value of the state in the absence of noise and disturbance. In the presence of noise, the estimate converges to a neighborhood of the true value of the state. We can therefore use this state-estimator to design outputfeedback controllers b using the estimated state to drive state-feedback controllers. These results etend and complement the ones in [ in that now the measured outputs are transmitted through a network, that is, the measurements arrive at discrete-time instants, are time-delaed, nois, and ma not be complete. We appl these results to the estimation of position and attitude of an autonomous vehicle using measurements from an IMU and a monocular CCD camera attached to the vehicle. In the contet of this application, the estimator can deal directl with the usual problems associated with vision sstems such as noise, latenc and intermittenc of observations. In Section II we formulate the state estimation problem using a H deterministic approach. Section III and Section I present the main results of the paper. In Section III we derive, using dnamic programming, the equations for the optimal observer. In Section I we determine under what conditions the state estimate ˆ converges to the true state. An eample in Section illustrates the results. Concluding remarks are given in Section I. Due to space limitations, some proofs are omitted. These can be found in [6. II. PROBLEM STATEMENT This section formulates the state estimation problem using a H deterministic approach. Consider the sstem with implicit outputs (), (5). From (), (t i ) satisfies (t i )=Φ(t i,t i τ i )(t i τ i ) (6) + Φ(t i,σ) [ A(,u(σ)) + G(u(σ))w(σ) dσ, where Φ(t, t ) is the transition matri of sstem () satisfing the linear time-varing differential equation Φ = A(u)Φ. (7) In (7), A(u) denote the gradient of A(, u) with respect to. Since A(, u) is affine in, it follows that A( ) onl depend on u. From (6) we obtain (t i τ i )=Φ (t i,t i τ i )(t i ) Φ (t i,t i τ i ) Φ(t i,σ) [ A(,u(σ)) + G(u(σ))w(σ) dσ. Substituting this equation in (5) and eploring the fact that E j (, v) and C j (, u) are affine functions in, we obtain Ē ji ((t i ),v j (t i τ i ))ȳ j (t i )= C j ((t i ),u)+ v j (t i ), j I i, (8) where Ē ji ((t i ), ) :=E j (Φ(t i τ i,t i )(t i ) ) Φ(t i τ i,t i ) Φ(t i,σ)a(,u(σ)) dσ,, C j ((t i ),u):= C j (u)(t i )+C j (,u(t i τ i )), C j (u) := C j (u(t i τ i ))Φ(t i τ i,t i ), C j (,u):= C j (u) Φ(t i,σ)a(,u(σ)) dσ + C j (,u(t i τ i )), v j (t i ):=µ j (t i )+v j (t i τ i ), µ j (t i ):=[ C j (u)+je j (ȳ j,v j (t i τ i )) Φ(t i,σ)g(u(σ))w(σ) dσ. The estimation problem can now be stated as follows: Problem : Consider the continuous-time state equation () together with the discrete-time implicit output equation (8). For a given gain level γ>, initial estimate ˆ, input u defined on an interval [,t), and measured outputs ȳ j (t i ), j I i with i =,,...k, t := t t k t t k+, compute the estimate ˆ(t) of the state at time t defined as ˆ(t) := arg min J(,t), (9) Rn where J(,t) is defined in (), at the top of the net page, P >, and λ is a forgetting factor. In a broad sense, for a given gain level γ>, the optimal state ˆ at time t is defined to be the value for the state that which implies that E j (, v) = JE j (, v) + E j (,v) and C j (, u) = C j (u) + C j (,u), where JE j (, v) is the Jacobian of E j (, v) with respect to

3 J(; t) := min w:[,t), v j(t i) i=,,...k { k γ 2 e 2λt (() ˆ ) P (() ˆ )+γ 2 e 2λ(t σ) w(σ) 2 dσ+γ 2 e 2λ(tk ti) v j (t i ) 2 i= j I i } e 2λ(t σ) (σ) ˆ(σ) 2 dσ : (t) =,ẋ = A(, u)+g(u)w, Ējiȳ j (t i )= C j ((t i ),u)+ v j (t i ) () is compatible with the initial estimate ˆ, the past controls and observations collected up to time t, and the dnamics of the sstem which ensures the prescribed bound γ on the discounted induced L 2 -gain from disturbances and noise to estimation error. The smmetric negative of J(,t) can also be viewed as the information state introduced in [7, [8 and interpreted as a measure of the likelihood of state = at time t. III. THE OBSERER EQUATIONS In this section we present the observer equations, which are derived using dnamic programming. In what follows, given a signal with a jump at time t, we denote b (t ) the limit of (τ) as τ t from below, i.e., (t ) := lim τ t (τ). Without loss of generalit we take all signals to be continuous from above at ever point, i.e., (t) = lim τ t (τ). We propose the following observer and will shortl show that it solves Problem. a) Initial condition t =, P(t )=P, ˆ(t )=ˆ () b) Dnamic equations for t [t i,t i+ ), i =,,...,k P (t) = P (t)( A(u)+λI) ( A(u)+λI) P (t) γ 2 (P (t)g(u)g(u) P (t)+γ 2 I), (2) ˆ(t) =A(ˆ, u), (3) with P (t i )=P i, and ˆ(t i )=ˆ i. c) Impulse equations at t = t i+, i =,,...,k P (t i+ )=P(t i+ )+γ2 Ψ j (t i+ ), (4) j I i+ ˆ(t i+ )=ˆ(t i+ ) (5) P (t i+ ) γ 2 [ Ψj (t i+ )ˆ(t i+ )+ψ j(t i+ ) where j I i+ Ψ j (t i ):= ( ) ( JE j (ȳ j (t i ))Φ(t i τ i,t i ) C j I Yji Yji ( I Y ji Yji )( ) JEj (ȳ j (t i ))Φ(t i τ i,t i ) C j, ψ j (t i ):= ( ) ( JE j (ȳ j (t i ))Φ(t i τ i,t i ) C j I Yji Yji ) ( I Y ji Yji ) ( E j ()ȳ j (t i ) JE j (ȳ j (t i )) Φ(t i τ i,t i ) Φ(t i,σ)a(,u(σ)) dσ C ) j (,u), Y ji := [ E j ȳ j (t i ) E 2j ȳ j (t i ) E ljjȳ j (t i ), (6) ) denotes the pseudo-inverse of Y ji, A(u) and JE j () are respectivel the gradient of A(, u) and the Jacobian of E j () both with respect to. The following result solves Problem. Theorem : The H state estimate ˆ(t) defined b (9) () can be obtained from the impulse sstem () (5). Furthermore, the cost function J(; t) defined in () is quadratic and can be written as Yji J(; t) = ( ˆ(t) ) ( ) P (t) ˆ(t) + c(t), (7) where c(t) satisfies an appropriate impulse equation (cf. (22), (26) below). Proof: [Outline The function J(; t), R n, t defined in () can be viewed as a cost-to-go and computed using dnamic programming. Take some t (t i,t i+ ). After some algebraic manipulation we obtain { J t (; t) = min γ 2( w 2 ˆ 2) w γ2 } J (; t)(a(,u)+g(u)w) 2λJ(; t) = 4γ 2 G (u)j (; t) 2 ˆ 2 J (; t)a(,u) 2λJ(; t), (8) where J t and J denote the partial derivatives of J with respect to t and, respectivel. For i =, the value of J(; t), t [t,t ), is determined from the linear partial differential equation (8), with initial condition J(;)=( ˆ ) P ( ˆ ), R n. (9) and can be written as (7) for appropriatel defined signals ˆ(t) and c(t). The signal ˆ thus minimie J(; t) and is therefore the estimate for the state of the implicit sstem (), (8). Moreover, matching (9) with (7) we conclude that P () = P, ˆ() = ˆ, c() =. To verif that the solution to (8) (9) can indeed be written as (7), we substitute (7) into (8) and obtain [ P + γ 2 PGG P + P A + A P +2λP + I +2 [ P ˆ P ˆ γ 2 PGG P ˆ A P ˆ + PA(,u) 2λP ˆ ˆ +ċ +2ˆ P ˆ +ˆ P ˆ + γ 2 ˆ PGG P ˆ 2ˆ PA(,u) +2λˆ P ˆ +2λc + ˆ 2 = where we have used the fact that A(,u) = A(u) + A(,u). Since this equation must hold for all R n we

4 conclude that P + γ 2 PGG P + P A + A P +2λP + I = (2) P ˆ P ˆ γ 2 PGG P ˆ A P ˆ +PA(,u) 2λP ˆ ˆ = (2) ċ +2ˆ P ˆ +ˆ P ˆ + γ 2 ˆ PGG P ˆ 2ˆ PA(,u)+2λˆ P ˆ +2λc + ˆ 2 = (22) Substituting (2) in (2), we conclude (2), (3), and (7) for t [,t ). Consider now the case t = t k, k>. From the definition of Ē ji and since E j (, v j (t k )+µ j (t k )) satisfies (3), the minimiation in () over v j (t k ) can be transformed into a minimiation over α jk := (α j,α 2j,...,α lj j). Thus, J(; t k )=J(; t k ) (23) + γ 2 min Ēj()ȳ j (t k )+Y jk α jk C j (,u) 2 α jk where Y jk is defined in (6) and Ē j ()ȳ j (t k )=JE j (ȳ j (t k ))Φ(t i τ i,t i ) JE j (ȳ j (t k ))Φ(t i τ i,t i ) Φ(t i,σ)a(,u(σ)) dσ + E j ()ȳ j (t k ) For k =we alread saw that J(,t ) is given b (7). Assuming that it has the same form at time t, substituting it in the left and right-hand-side of (23), we obtain [ P (t k ) P (t k ) γ2 Ψ j (t k ) +2 [ P (t k )ˆ(t k )+P(t k )ˆ(t k ) γ2 ψ(t k ) + c(t k )+ˆ(t k ) P (t k )(t k ) ˆ(t k ) P (t k )(t k ) c(t k ) γ 2 (Ēj ()ȳ j (t k ) C j (,u) ) ( I Yjk Y jk) ( I Yjk Yjk)(Ēj ()ȳ j (t k ) C j (,u) ) =. This equation holds for k =, provided that P (t k ) P (t k ) γ2 Ψ j (t k )=, (24) P (t k )ˆ(t k )+P(t k )ˆ(t k ) γ2 ψ(t k )=, (25) c(t k )+ˆ(t k ) P (t k )(t k ) =. (26) Thus, substituting (24) in (25) we conclude that (4) and (5) hold. Notice that P := P (t )=P(t )+γ2 j I Ψ j (t ) is positive definite because P (t ) > as it was proved above, and Ψ j (t i ), i =,...,k. Therefore, substituting the initial condition (9) b J(; t )=( ˆ ) P ( ˆ ), R n with ˆ =ˆ(t ), and solving the linear partial differential equation (8), it follows that (2) (5) hold for t [,t 2 ). Appling this line of reasoning successivel until i = k we conclude that (7) holds and that ˆ(t) given b (2) (5) is indeed the solution to Problem. To guarantee that the H state estimate has a global solution (T = ), the value of γ should be sufficientl large. In particular, a sufficient condition for this is given b the following observabilit condition. Lemma : The H estimator () (5) has a global solution and P (t) δi >, t, (27) for some δ>, if there eists a sufficientl large γ> such that the following condition γ 2 W (t) holds, where W (t) := { Φ(τ,σ):= Φ(τ,t) Φ(τ,t)dτ + δi t (28) k i= j I i Φ(ti,t) Ψ j (t i ) Φ(t i,t), (29) Φ i(τ,σ), Φ i(τ,t i+) Φ i+(t i+,t i+2) Φ j(t j,σ), i=j i<j τ [t i,t i+ ) and σ [t j,t j+ ), and Φ i (t, τ) denotes the state transition matri of ż =( A + γ 2 GG P + λi), τ [t i,t i+ ). I. ESTIMATOR CONERGENCE We are now interesting in determining under what conditions the state estimate ˆ converges to the true state. As in [6, the following technical assumption is needed: Assumption : Let Num(t, σ), σ < t denote the number of time instants at which measurement arrive in the open interval (σ, t). There eist finite positive constants τ D and N, for which the following condition holds: Num(t, σ) N + t σ. τ D The constant τ D is called the average dwell-time and N the chatter bound. This assumption roughl speaking guarantees that the average interval between consecutive arrival of measurements is no less than τ D. In this wa, the summation in () will not grow unbounded due to too frequent measurements. The following result establishes the convergence of the state estimate. Theorem 2: Assuming that the solutions to the sstem with implicit outputs (), (5) eists on [,T), T [,, the solution to the impulse state estimator () (5) also eists on [,T). Moreover, if P (t) δi, t [,T), δ>, then there eist positive constants c>,r <,γ w,γ,...,γ N such that (t k ) cr k () + γ w sup w(σ) + N j= γ j σ (,t k ) sup v j (σ), t k > (3) σ (,t k ) where (t) :=ˆ(t) (t) denotes the state estimation error.

5 . ILLUSTRATIE EXAMPLE To illustrate the results of the paper, we consider the problem of estimate the position and attitude of an autonomous vehicle with respect to an inertial coordinate frame defined b visual landmarks. In this setup we assume that the measurements are provided b an inertial measurement unit (IMU) and a monocular charged-coupled-device (CCD) camera mounted on-board. More precisel, let {} be an inertial coordinate frame defined b visual landmarks and {B} a bod-fied coordinate frame whose origin is located e.g., at the center of mass of the vehicle. The IMU provides the vehicle s linear velocit v R 3, angular velocit ω R 3, and pose (position and attitude) (p, R) SE(3) of {B} with respect to some inertial coordinate frame {I}. The camera attached to the vehicle sees N points Q i R 3, i {, 2,..., N} with known coordinates in the visual coordinate sstem {}. The objective is to determine the position P B R 3 and orientation B R SO(3) of the vehicle with respect to the visual coordinate sstem {}. It is assumed that the position and orientation of {I} with respect to the visual coordinate frame {} are unknown. In [, we have formulated this problem in the framework of state estimation of a sstem with implicit outputs of the form () (3). Denoting the measurements b ζ i, i {,...,4+N}, where the first four are obtained from the IMU, that is, ζ := v, ζ 2 := ω, ζ 3 := p, ζ 4 := R, and the rest of them are obtained from the CCD camera and satisf µ i+4 ζ i+4 = F C Q i, (3) [ ζ i+4 =, i {, 2,...,N} (32) where C Q i is the position of Q i epressed in the camera s frame {C}, µ i+4 R captures the depth of the point C Q i (which is unknown), and F is an upper triangular matri with the camera s intrinsic parameters, the sstem with implicit outputs is given b {}} { B R P I = S(ω) B R P I, stack( I Ṙ )= 9 B Q = S(ω) B Q v + ( Q ) I 3 3 stack( B R ), stack( BṘ )= ( I 3 3 S(ω) ) stack( B R ), = ( ) Q I 3 3 stack( B R ) B Q P I, ( ) I R I = stack( B R ), µ i+4 2+i = F C P B + [( Q i ) Q F C BR stack( B R )+F C BR B Q (see [ for details). In the above equations, the smbol denotes the Kronecker product and stack( ) is the stack operator that stacks the columns of the argument one on top of each other, with the first column on top. Given two frames {A}, {B}, the smbol B AR is the rotation matri from {A} to {B}, B Q is the position of the vector Q epressed in {B}, and B P A the position of the origin of frame {A} epressed in {B}. Defining the vectors R 24, R 2+N, and u R 6 as := B R P I stack( I R ) B Q stack( B R ), we obtain () (3) with A(, u) := := ζ 4ζ 3 [ 2 := stack(ζ 4 ) u := ζ ζ 2 2+i := ζ 4+i,i=,...,N [ S(ω) S(ω) Q I3 3 I 3 3 S(ω) + [ v, C (, u) :=[ I I Q I3 3, C 2 (, u) :=[I C 2+i (, u) :=[F C B R ( Q i Q ) F C B R + F C P B, i {,...,N}. B introducing additive noise to (3) we conclude that E (, v ):=I E 2 (, v 2 ):= I R I 3 3 E 2+i (, v 2+i ):=[ F [ C P B + C BR B R ( Q i ) Q + C BR B Q + v2+i, i {, 2,...,N} (see [ for details). The image of E j (, v j ) satisfies (3) with E () :=I, l =, E 2 () := I R I 3 3, l 2 =, E,2+i () :=, l 2+i =, E,2+i := i {,...,N}. We can now use the results given in the previous sections to compute ˆ. From B ˆQ and B ˆR, the position P B can also be estimated using ˆP B = Q B ˆR B ˆQ. We now illustrate the performance of the proposed estimator through computer simulation. The autonomous vehicle starts at the origin P B =with orientation B R = I and follows a circular path with a camera looking up at four noncoplanar points. The linear velocit is v =[.3,, m/s and the angular velocit is ω =[,,.2 rad/s. The vision sampling interval is T CCD =.4 s and the time-dela is τ CCD =.5 s. The IMU sampling interval is T IMU =.s and there is no time-dela. The estimator was initialied with ˆP B =[,, m/s and B ˆR [ = The measurements were corrupted with additive Gaussian noise with standard deviation equal to roughl 5% of the measurements. Fig. displas the time evolution of the estimation errors. It can be seen that the estimated pose without noise converges to ero (see Fig. (a)) and in the presence of noise tend to a small neighborhood of the true value (see Fig. (b)). To illustrate the benefit of having measurements from the IMU, Fig. (c) shows the time evolution of the estimation errors when there is onl measurements from the CCD camera. As epected, although the errors converge to a small neighborhood of the origin, the transients are worst than the ones displaed in Fig. (b).

6 I. CONCLUSIONS We considered the problem of estimating the state of a sstem with implicit outputs, whose measurements arrive at discrete-time instants, are time-delaed, nois, and ma not be complete. We designed a estimator using a deterministic H approach that is globall convergent under appropriate observabilit assumptions and can therefore, be used to design output-feedback controllers. These results were applied to the estimation of position and attitude of an autonomous vehicle using measurements from an inertial measurement unit and a monocular charged-coupled-device camera attached to the vehicle. Future work will address the derivation of conditions to test observabilit that are easier to test than the ones provided b Lemma. REFERENCES [ A. P. Aguiar and J. P. Hespanha, State estimation for sstems with implicit outputs for the integration of vision and inertial sensors, in Proc. of the 44th Conf. on Decision and Contr., Dec. 25. [2 B. K. Ghosh, M. Jankovic, and Y. T. Wu, Perspective problems in sstem theor and its application in machine vision, J. Math. Sst. Estimation Contr., vol. 4, no., pp. 3 38, 994. [3 B. K. Ghosh and E. P. Loucks, A perspective theor for motion and shape estimation in machine vision, SIAM J. Contr. Optimiation, vol. 33, no. 5, pp , 995. [4 S. Takahashi and B. K. Ghosh, Motion and shape parameters identification with vision and range, in Proc. of the 2 Amer. Contr. Conf., vol. 6, June 2, pp [5 A. Matveev, X. Hu, R. Frea, and H. Rehbinder, Observers for sstems with implicit output, IEEE Trans. on Automat. Contr., vol. 45, no., pp , Jan. 2. [6 A. P. Aguiar and J. P. Hespanha, Minimum-energ state estimation for sstems with perspective outputs, IEEE Trans. on Automat. Contr., vol. 5, no. 2, pp , Feb. 26. [7 T. Başar and P. Bernhard, H Optimal Control and Related Minima Design Problems: A Dnamic Game Approach, 2nd ed. Boston, MA: Birkhäuser, 995. [8 K. M. Nagpal and P. P. Khargonekar, Filtering and smoothing in an H setting, IEEE Trans. on Automat. Contr., vol. 36, no. 2, pp , Feb. 99. [9 L. Xie, Y. C. Soh, and C. E. de Soua, Robust Kalman filtering for uncertain discrete-time sstems, IEEE Trans. on Automat. Contr., vol. 29, no. 6, pp. 3 34, 994. [ A. J. Krener, Necessar and sufficient conditions for nonlinear worst case (H-infinit) control and estimation, J. Math. Sst. Estimation Contr., no. 7, p. 86, 997. [ A. H. Saed, A framework for state-space estimation with uncertain models, IEEE Trans. on Automat. Contr., vol. 46, no. 7, pp , Jul 2. [2 R. K. Boel, M. R. James, and I. R. Petersen, Robustness and risksensitive filtering, IEEE Trans. on Automat. Contr., vol. 47, no. 3, pp , Mar. 22. [3 R. E. Mortensen, Maimum likelihood recursive nonlinear filtering, J. Opt. Theor and Applications, vol. 2, pp , 968. [4 O. J. Hijab, Minimum energ estimation, Ph.D. dissertation, Universit of California, Berkele, 98. [5 W. M. McEneane, Robust/H-infinit filtering for nonlinear sstems, Sst. & Contr. Lett., no. 33, pp , 998. [6 A. P. Aguiar and J. P. Hespanha, State estimation of continuoustime sstems with implicit outputs from discrete nois time-delaed measurements, Institute for Sstems and Robotics, Tech. Rep., Mar. 26. [7 M. James, J. Baras, and R. Elliott, Output feedback risk-sensitive control and differential games for continuous-time nonlinear sstems, in Proc. of the 32nd Conf. on Decision and Contr., San Antonio, TX, USA, 993, pp [8, Risk-sensitive control and dnamic games for partiall observed discrete-time nonlinear sstems, IEEE Trans. on Automat. Contr., vol. 39, no. 4, p , Position error Orientation error R Position error.5.5 Orientation error R (a) Without noise and disturbances Orientation error R Position error Orientation error R 2 Orientation error R Orientation error R (b) With noise and disturbances Orientation error R Orientation error R Orientation error R (c) With noise, disturbances, and onl measurements from the CCD camera Fig.. Time evolution of the estimation errors in position and orientation. The orientation errors labeled R, R 2, and R 3 correspond to the estimation errors for the first, second, and third columns of B R, respectivel.