PRIN project MARIS: Marine Autonomous Robotics for InterventionS, call of year 2010-2011, prot. 2010FBLHRJ.  Research Unit UNISALENTO: MARIS Technical Report version of August, 31st 2014: updated version (with minor typing corrections) of the first version of July 29th, 2013. Further results on the observability analysis and observer design for single range localization in 3D Giovanni Indiveri and Gianfranco Parlangeli, Dipartimento Ingegneria dell’Innovazione University of Salento (ISME node), Lecce, Italy  arXiv:1308.0517v3 [cs.RO] 1 Sep 2014
Further results on the observability analysis and observer design for single range localization in 3D  Giovanni Indiveri   a , Gianfranco Parlangeli   a  a Dipartimento Ingegneria Innovazione - ISME node Universit` a del Salento, via Monteroni, 73100 Lecce - Italy  Abstract  Single range localization in 3D is an open challenge with important implications for service robotics applications. The issue of single range observability analysis and observer design for the kinematics model of a 3D agent subject to a constant unknown drift velocity is addressed. The proposed approach departs from alternative ones and leads to the definition of a linear time invariant state equation with a linear time varying output that can be used to globally solve the original nonlinear state estimation problem with a standard linear estimator. Simple necessary and sufficient observability conditions are derived. Numerical simulation examples are reported to illustrate the performance of the method.  Key words:   Observability; Localization; Navigation; Marine Systems; Autonomous Vehicles.  1   Introduction  The problem of single range based localization is relevant in several land [13], [20], aerial [11] and marine robotics [18] [1] [12] applications. In essence, the problem consists in estimating an agent’s position exploiting knowledge about its motion model (typically its kinematics model where the velocity is a known input), a range measure- ment from a source and eventually other sensor readings related to the vehicle’s attitude. The challenge of using single range information for localization is related to the fact that traditional trilateration algorithms used in sys- tems as the Global Positioning System (GPS), long base line (LBL) or ultra short base line (USBL) underwater navigation systems are ill posed when only range from a single source is known. Yet fusing information from a motion model of the agent (including velocity and atti- tude) and a single range measurement can be sufficient to estimate the position of the agent. Finding the con- ditions on the agent’s motion state that allow to esti- mate its position from a single range measurement is an observability problem that needs to be tackled in order  ?   This paper was not presented at any IFAC meeting. Cor- responding author G. Indiveri. Tel. +39-0832-297220. Fax 39-0832-297733.  Email addresses:   giovanni.indiveri@unisalento.it  (Giovanni Indiveri),  gianfranco.parlangeli@unisalento.it  (Gianfranco Parlangeli).  to eventually design an observer. Given that range is a nonlinear function of the position, even if the motion model of the vehicle should be linear, the observability issue is inherently nonlinear. A major contribution on observability for nonlinear systems is [8] where the fun- damental ideas and results about local and weakly lo- cal observability are described: single range localization studies building on differential geometric tools need to tackle the difficulties related to local and weakly local observability as opposed to the global observability con- cept known for linear systems. Such issues are clearly addressed, by example, in references [10], [15] and [7]. Single range aided localization is particularly relevant in cooperative navigation applications where a team of vehicles needs to perform collaborative motion control tasks while possibly performing relative localization ex- ploiting intra-vehicle communication to measure rela- tive ranges. Such problems arise in several underwater robotics scenarios [6] [19] [17] as well as in more general settings [4]. Moreover, the problem of range based lo- calization is technically similar to the problem of source localization where a vehicle knowing its own position is asked to estimate the position of a target from which it acquires range measurements [5] [9]. Indeed the local- ization solution described in this paper was partially in- spired by the work in [9] and it can be used to address the problem outlined in [5] as illustrated in section 5. A milestone contribution in the area of single range local-  arXiv
ization is given by the work of Batista et al. [2] [3]: they propose to study the single range localization problem of an agent subject to a constant, but unknown, drift ve- locity through an augmented state approach. The origi- nal nonlinear system (where the state belongs to   R 6   and is made of the agent’s position and unknown drift veloc- ity components) is transformed in a linear time varying (LTV) system in   R 9   through an augmented state tech- nique. This leads to the remarkable result of allowing to study the   global   observability properties of the system with well known Gramian based tools of LTV systems theory [16] and of designing a Kalman filter for global state estimation. The LTV system derived in [2] [3] is of the form  ̇ z   =   A ( t,   u ( t ) , y ( t ))   z   +   B   u ( t )   (1)  y ( t ) =   C   z   (2) namely,   the   system   matrix   A ( t )   explicitly   depends, among the rest, on the output   y ( t ) that is the range measurement. More precisely, the   A ( t ) matrix is a func- tion of terms proportional to 1 /y ( t ): this poses both fundamental as well as implementation issues. From a theoretical perspective, assuming that the output   y ( t ) should be affected by additive noise, the dependency of   A ( t ) from   y ( t ) implies that some of its entries are stochastic and that the model uncertainty on the state equation (1) could not be possibly assumed to be only additive as is usually done within the theory of Kalman filtering. As a consequence assuming additive gaussian noise on the state and output equations (1) - (2), the associated Kalman state estimator is not guaranteed to be optimal in the usual sense. Indeed the numerical examples provided in [3] confirm that the Kalman filter estimates converge to the true state variables, but there is no   a priori   guarantee that the estimate is optimal in terms of estimate covariance. A second potential diffi- culty arising from the structure of equations (1) - (2) is related to the eventual stability analysis of the Kalman filter on equations (1) - (2) coupled with a motion con- troller. Indeed the presence of the output   y ( t ) in the matrix   A ( t ) does not allow to exploit in a straightfor- ward fashion the standard separation principle used within linear systems theory to study the convergence and stability of state estimation filters coupled with feedback controllers. As for the implementation of the Kalman filter described in [3], given the dependency of some entries of   A ( t ) from 1 /y ( t ), it is necessary to assume that   y ( t )   6   = 0 at all times. Indeed in [3] it is assumed that   y ( t ) has strictly positive, finite, lower and upper bounds: yet in real ap- plication scenarios with unpredictable sensor noise and outliers one would need to pre-filter the output in order to guarantee the absence of numerical issue related to exceedingly small or null   y   readings. In the light of the above observations and inspired by the work in [9] and [3], this paper describes an alter- native approach to address the single range localization problem. As a result, the same problem addressed in [3] is globally solved by introducing an LTV system of the form  ̇ z   =   A   z   +   B   u ( t )   (3)  y ( t ) =   C ( u ( t ))   z   (4) namely where the state equation is completely linear time invariant (LTI) and has dimension 8 rather than 9. The output equation is still LTV, but has a very sim- ple structure. The proposed method does not build on state augmentation techniques, but rather exploits the structure of the original state equations expressed in an inertial frame as opposed to the body frame formulation used in [3]. Within the proposed solution, given the LTI nature of the state equation (3), the difficulties related to the dependency of   A ( t ) in equation (1) from 1 /y ( t ) are completely removed. Also notice that the output ma- trix   C ( t ) in equation (4) within the solution presented in this paper depends on the input   u ( t ), but not on the output   y ( t ): as a consequence an additive measurement noise would not affect the entries of   C ( t ) nor of   A   and   B  hence preserving the optimality of a Kalman filter as a state estimator as long as noise is gaussian (and the in- put   u ( t ) is perfectly known, i.e. noise free). Indeed, as in [3], the localization problem can be globally solved with a standard Kalman filter with dimension 8 instead of 9. Moreover, given the extremely simple structure of equa- tions (3) - (4), the observability analysis is extremely simple and it allows to derive necessary and sufficient observability conditions on the agent input (i.e. its ve- locity). The main ideas and methods used to solve the problem are described in section 2 on a simplified version of the problem, namely in the absence of drift velocity terms as eventually present in underwater applications due to constant and unknown currents. The resulting strategy for single range localization in the absence of currents is addressed in section 3. The complete case of single range localization in the presence of constant and unknown currents is solved in section 4. Simulation examples are presented and discussed in section 5 while conclusions are summarized in section 6.  2   Main ideas and methods  Consider a 3D kinematics vehicle model by the following equations:  ̇ x   =   u   (5)  y ( t ) =   x > x   =   ‖ x ( t ) ‖ 2   (6) being   x ( t )   ∈   R 3 × 1   the unknown vehicle position (state),  u ( t )   ∈   R 3 × 1   its velocity (known input) and   y ( t )   ∈   [0 ,   ∞ ) the measured output at time   t . 2
The issue of estimating the state   x ( t ) is ultimately re- lated to identifying the initial state  x 0   :=   x ( t ) | t =0   .   (7) In particular, with reference to the model in equations (5) - (6),   x 0   is related to the systems input and output by the following  x ( t ) =   x 0   +  ∫   t  0  u ( τ   ) dτ   (8)  y ( t ) =   x > x   =   ‖ x 0 ‖ 2   + 2  (∫   t  0  u ( τ   ) dτ  ) >  x 0   + +  ∥ ∥ ∥ ∥ ∫   t  0  u ( τ   ) dτ  ∥ ∥ ∥ ∥  2  .   (9) Denoting with  I u ( t ) :=  ∫   t  0  u ( τ   ) dτ   = ( I x ( t ) , I y   ( t ) , I z   ( t )) >   ∈   R 3 × 1  (10) equation (9) results in  ̄ y ( t ) := 1 2  ( y ( t )   −   y (0)   − ‖ I u ( t ) ‖ 2 )   (11)  ̄ y ( t ) =   I u ( t ) > x 0   (12) where the left hand side of equation (12) is known as well as the time varying vector   I u ( t ) on the right hand side. Notice that knowledge of   x 0   corresponds to observability (by definition) as well as to reconstructability as   x ( t ) can be directly calculated through equation (8). Equation (12) allows to directly identify the necessary and sufficient conditions on   u   for observability (and re- constructability) on a finite time interval. The analysis is performed both in the discrete and continuous time cases.  2.1   Discrete time case  The discrete time version of equation (12) leads to a Least Squares (LS) problem as follows: assume that time is sampled at a rate   T s   such that  t k   =   k T s   :   k   = 1 ,   2 , . . . , n   (13) and  ̄ y   := ( ̄ y ( t 1 ) ,    ̄ y ( t 2 ) , . . . ,    ̄ y ( t n )) >   ∈   R n × 1   (14)  H   :=          I x ( t 1 )   I y   ( t 1 )   I z   ( t 1 )  I x ( t 2 )   I y   ( t 2 )   I z   ( t 2 ) . . .   . . .   . . .  I x ( t n )   I y   ( t n )   I z   ( t n )          ∈   R n × 3   (15) then equation (12) evaluated at each sampling time leads to the LS problem  H x 0   =  ̄ y   (16) that admits a unique solution ˆ x 0   iff rank( H ) = 3, i.e. rank( H ) = 3   ⇐⇒   ˆ x 0   = ( H > H ) − 1 H >    ̄ y .   (17) The discrete time case sufficient and necessary observ- ability condition rank( H ) = 3 requires that   n   ≥   3 (nec- essary, but not sufficient) and, most important, that the velocity input   u ( t ) generates columns of   H   in equation (15) that are linearly independent. Stated formally, the above lead to the following result:  Statement 1   -   Observability conditions for the discrete time case.  Given a discretized version of model (5) - (6) by sam- pling time as in (13) with   n   ≥   3, the state   x 0   is ob- servable (and   x ( t ) is reconstructable) if and only if the velocity signal   u   guarantees that   H   ∈   R n × 3   in equation (15) has rank 3.  Proof of Statement 1  It follows by standard LS theory. Contrary to linear systems, the observability depends on the input. A similar observability condition can be derived for the problem of localizing the position of a fixed target on behalf of a moving vehicle from single range measurements. Details are discussed in [9].  2.2   Continuous time case  The line of thought for the continuous time case is the same of the discrete time case. With reference to equa- tion (12) consider multiplying both sides by   I u ( t ) and integrating over a finite interval, namely:  ∫   t  0  I u ( τ   ) I u ( τ   ) >   x 0   dτ   =  ∫   t  0  I u ( τ   ) ̄ y ( τ   ) dτ.   (18) The left hand side of equation (18) can be written in terms of the the 3   ×   3 real Gramian matrix  G ( t ) :=  ∫   t  0  I u ( τ   ) I u ( τ   ) > dτ   ∈   R 3 × 3   (19) and the right hand side in terms of a vector   μ   ∈   R 3 × 1  μ ( t ) :=  ∫   t  0  I u ( τ   ) ̄ y ( τ   ) dτ   (20) leading to writing equation (18) as  G ( t ) x 0   =   μ ( t ) .   (21) 3
Statement 2   -   Observability conditions for the contin- uous time case.  Given model (5) - (6) the state   x 0   is observable (and  x ( t ) is reconstructable) if and only if the velocity signal  u   guarantees that the Gramian   G ( t )   ∈   R 3 × 3   in equation (19) has rank 3.  Proof of Statement 2  Sufficiency  If   G ( t )   ∈   R 3 × 3   in equation (19) has rank 3 then equation (21) implies  x 0   = ( G ( t )) − 1   μ ( t ) .   (22)  Necessity  It is proven by absurd. Suppose that   x 0   is observable and rank( G ( t ))   <   3 in the time interval [0 , t ]. The hypothesis that rank( G ( t ))   <   3 on [0 , t ] implies that the kernel of  G ( t ) is nonempty. Consider  ν   ∈   R 3 × 1   :   ν   6   =   0 ,    ̇ ν   =   0   and   G ( t ) ν   =   0   (23) in [0 , t ]. Then by definition of   G ( t ) and of the constant  ν   it follows 0 =   ν > G ( t ) ν   =  ∫   t  0  ν > I u ( τ   ) I u ( τ   ) > ν dτ   = =  ∫   t  0  ‖ I u ( τ   ) > ν ‖ 2 dτ   = ⇒   by norm properties  I u ( τ   ) > ν   = 0   ∀   τ   ∈   [0 , t ] .   (24) The above result implies that   x 0   +   ν   and   x 0   are two distinct (because   ν   6   =   0 ) solutions of equation (12) (and hence of equation (9)) on the finite time interval [0 , t ], i.e.   x 0   + ν   and   x 0   are undistinguishable on the finite time interval [0 , t ] as they produce the same output   y ( t ). This violates the hypothesis that   x 0   was observable, hence it is proven that if   x 0   is observable in [0 , t ] the Gramian  G ( t ) must be full rank in [0 , t ]. The above results allow to identify all the motions (i.e. velocity inputs) generating globally observable states. In particular, in the discrete time case the persistently exit- ing input velocity   u   assuring that   H   in equation (15) has full rank will guarantee position observability. Similarly, in the continuous time case, the persistently exiting in- put velocity   u   assuring position observability is given by the values making the Gramian (19) invertible. These re- sults can be exploited to design optimal movements (i.e. velocity profiles) with reference to criteria such as, for example, the condition number of the regression matrix  H   eventually subject to constraints as the infinity norm of   u . Other measures for optimal estimation inputs are of course also possible in the line of the system identifi- cation literature on optimal experimental design. Most important and most remarkably, the proposed framework allows to derive a linear system with a time invariant (LTI) state equation and a time varying (LTV) output equivalent to the one in equations (5) - (6) on which to design a Kalman filter for solving the local- ization problem in case that the proposed observability conditions are met. Contrary to the state augmentation approaches discussed in [3] and exploited also in [14], the proposed approach does not require state augmen- tation and leads to a linear system with a LTI state equation and a LTV output. Notice also that the Gramian full rank condition in state- ment 2 closely resembles the integral condition of theo- rem 2.1 in [5]; indeed,   I u ( t ) can be viewed as a filtered version of   x ( t ) in the assumption that   x t =0   =   0 .  3   Single range localization  With reference to equation (8), consider the following  x 0   =   x ( t )   −  ∫   t  0  u ( τ   ) dτ   =   x ( t )   −   I u ( t )   (25)  x >  0   x 0   =   y (0) = =   y ( t ) +   ‖ I u ( t ) ‖ 2   −   2 I u ( t ) > x ( t )   (26) that can be re-written as follows  ̄ y ( t ) =   I u ( t ) > x ( t )   (27) where the output signal  ̄ y ( t ) is the known quantity  ̄ y ( t ) = 1 2  [ y ( t )   −   y (0) +   ‖ I u ( t ) ‖ 2 ]   .   (28) Having introduced the time varying output equation (28) the original system in equations (5) - (6) can be equivalently represented by the LTV system  ̇ x   =   u   (29)  ̄ y ( t ) =   I u ( t ) > x ( t )   (30) that is (globally) observable as long as the conditions in Statement 2 are met. The localization problem of estimating   x ( t ) in the model (29) - (30) can thus be solved resorting to a Kalman-Bucy filter. Notice that contrary to the standard LTV setting, the output ma- trix   C ( t ) =   I u ( t ) >   in equation (30) is a function of the very input, hence observability (as already discussed) will depend on the velocity input   u ( t ). The derived continuous LTV system (29) - (30) can be discretized through sampling at times   t k   =   kT s   as in equation (13). Assuming that   u ( t ) is constant in any time interval [ kT s ,   ( k   + 1) T s ], the discrete time version of the LTV system (29) - (30) results in  x k +1   =   x k   +   v k   (31)  ̄ y k   =   I >  k   x k   (32) 4
where   v k   :=   u ( kT s )   T s ,   I k   :=   I u ( kT s ) ,   x k   :=  x ( kT s ) ,    ̄ y k   :=  ̄ y ( kT s ) , y k   :=   y ( kT s ) and  ̄ y k   = 1 2  [ y k   −   y (0) +   ‖ I k ‖ 2 ]   (33)  I k +1   =   I k   +   v k +1   :   I 0   =   0 .   (34) Given the model structure in equations (31) - (32), a linear state estimation filter can be designed to estimate the position   x k   as long as the observability conditions in Statement 1 are satisfied. An example of such application is outlined and tested in section 5.  3.1   Persistently exiting velocity input: an example  Following the design presented in [9], consider the fol- lowing input velocity   u   = ( u 1 , u 2 , u 3 ) >  u i ( t ) =   A i   n i   ω   cos( n i ωt )   (35) where   n i   is a positive integer such that   n i   6   =   n j   for any   i, j   ∈ { 1 ,   2 ,   3 } ,   A i   is a design parameter such that  | A i |   n i   ω   is the maximum speed in direction   i   and   ω   is a design parameter chosen as  ω   = 2 π/T 0   :   T 0   =   n 0 T s   (36) for some positive integer   n 0   being   T s   the sampling time. With the above designed velocity input, the re- gression matrix   H   in equation (15) over a time interval [0 ,   ( n t n 0 T s )] for some positive integer   n t   results [9] to be diagonal greatly simplifying the issue of designing optimal maneuvers for single range localization.  4   Single range localization in the presence of constant and unknown currents  The described approach can be extended to the case where the kinematic point mass vehicle is subject to a constant and unknown ocean current, namely the same model used in [3]. For the sake of comparing the present solution to the one in [3], a similar notation will be used. In particular consider an inertial frame   { I }   and a body fixed frame   { B }   such that the SO(3) rotation matrix from frame   { B }   to frame   { I }   is denoted as   I R B   . Such rotation matrix, as in [3], is assumed to be known. The components of a vector   a   ∈   R 3   in frames   { I }   or   { B }  will be denoted as   I   a   and   B   a   respectively. Following standard kinematics results, the angular velocity of the vehicle satisfies  I    ̇ R B   =   S ( I ω B/I   )   I R B   =   I R B   S ( B ω B/I   )   (37) where   S ( a )   ∈   R 3 × 3   is the skew symmetric matrix asso- ciated to the vector product   a   ×   and   I ω B/I   ,   B ω B/I   are the angular velocities of frame   { B }   with respect to frame  { I }   expressed in frames   { I }   and   { B }   respectively. The term   B ω B/I   is assumed to be measured by the agent itself thanks to the on board navigation system (Atti- tude and Heading Reference System - AHRS). The term  I ω B/I   is also assumed to be accessible as it is given by  I ω B/I   =   I R B B ω B/I   .   (38) The position of the vehicle is given by   x   ∈   R 3 × 1   and the fixed landmark in frame   { I }   from which the agent can measure its euclidean distance has fixed position  s   ∈   R 3 × 1   ( I    ̇ s   =   0 ). The velocity  ̇ x   of the agent is given by the superposition of an ocean current (assumed constant in frame   { I } ) denoted with   v f   and a relative (w.r.t. the water) velocity   v r   such that  I    ̇ x   =   I   v f   +   I   v r   =   I   v f   +   I R B B   v r   (39) where the relative velocity in the agents frame   B   v r   is assumed to be actively controlled and measured by the agents on board navigation system (typically thanks to a DVL - Doppler velocity logger - sensor). The agent measures the euclidean distance to the target   s , namely the norm (or, equivalently, the square norm) of   r   given by  I   r   :=   I   s   −   I   x   (40)  B   r   =   B R I  ( I   s   −   I   x )   (41)  B    ̇ r   =   − B   ω B/I   ( t )   ×   B   r ( t )   −   B   v f   ( t )   −   B   v r   ( t )   (42) where the angular velocity property   B   ω I/B   =   − B   ω B/I  has been used. Equation (42) together with  B    ̇ v f   =   − B   ω B/I   ( t )   ×   B   v f   ( t )   (43)  r ( t ) =   ‖ B   r ( t ) ‖   (44) are exactly the same in equation (1) of [3] where the lo- calization problem is formulated as a state estimation issue where the state variables are ( B   r > , B   v >  f   ) >   having dynamics given by equations (42) - (43) and the output map is equation (44). In particular, by estimating the term   B   r , the absolute position of the vehicle in the iner- tial frame can be recovered through equation (40) as  I   x   =   I   s   −   I R B B   r .   (45) This formulation of the single range localization prob- lem has the advantage of making explicit use of the agent velocities   v r   and   ω B/I   in the body frame where they are actually measured (i.e.   { B } ). The drawback is that in body frame   { B }   the current isn’t constant. Given that the rotation matrix from the body frame to the inertial frame is nevertheless necessary to recover the absolute position of the agent and that it is thus assumed to be known, one might just as well formulate the localization 5
problem in the inertial frame where the dynamics equa- tions are simpler. Notice, moreover, that in the very ap- proach of [3] a Lyapunov transformation based on   I R B  is used to transform model (42) - (44) in a different dy- namical system for observability analysis and observer design. In order to exploit the same approach derived in section 3 for the current free case, the system model will be derived in the inertial frame   { I } . In particular from equations (39) and (40) it follows that  ̇ r   =   − v f   −   v r   (46)  ̇ v f   =   0   (47)  y   =   ‖ r ‖ 2   (48) where all vectors are expressed in frame   { I }   and the left hand side superscript   { I }   has been omitted for the sake of notation compactness. Indeed, from this point on, all vectors expressed in the inertial frame   { I }   will be de- noted without left hand side superscript   { I } . The term  v r   in equation (46) is assumed known as it may be com- puted as   v r   =   I R B B   v r   given that   B   v r   is generated by the agents propulsion system and is measured on board as already mentioned. Consider the integral of equation (46)  r ( t )   −   r 0   =   − v f   t   −  ∫   t  0  v r   ( τ   ) dτ   = =   − v f   t   −   I v r   ( t )   (49) having defined   I v r   ( t )   ∈   R 3 × 1   as  I v r   ( t ) :=  ∫   t  0  v r   ( τ   )   dτ   (50) and  r 0   :=   r ( t ) | t =0   .  Equation (49) allows to compute ( r ( t ) +   I v r   ( t )) >   ( r ( t ) +   I v r   ( t )) = ( r 0   −   v f   t ) >   ( r 0   −   v f   t ) implying  ‖ r ( t ) ‖ 2   +   ‖ I v r   ( t ) ‖ 2   + 2   I >  v r   ( t ) r ( t ) = =   ‖ r 0 ‖ 2   +   ‖ v f   ‖ 2   t 2   −   2 ( r >  0   v f   ) t   (51) namely  ‖ r ( t ) ‖ 2   − ‖ r 0 ‖ 2   +   ‖ I v r   ( t ) ‖ 2   = =   − 2   I >  v r   ( t ) r ( t )   −   2 ( r >  0   v f   ) t   +   ‖ v f   ‖ 2   t 2 .   (52) Notice that the left hand side of equation (52) is made of all known terms and it can be used as a new output map  ̄ y ( t ) =   ‖ r ( t ) ‖ 2   − ‖ r 0 ‖ 2   +   ‖ I v r   ( t ) ‖ 2  =   y ( t )   −   y 0   +   ‖ I v r   ( t ) ‖ 2   (53) and the right hand side of equation (52) can be expressed as a linear time varying (LTV) term in the new state variable   z   ∈   R 8 × 1  z   = ( r > ,   ( r >  0   v f   ) ,   ‖ v f   ‖ 2 ,   v >  f   ) > ,   (54) i.e.  ̄ y ( t ) =   C ( t )   z   = =  [  − 2   I >  v r   ( t )   −   2   t   t 2   0 1 × 3  ]  z .   (55) Given the definition of   z   in equation (54) and the model (46) - (47), its dynamic equation is linear time invariant (LTI):  ̇ z   =   A   z   +   B   v r   (56) namely  ̇ z   =   d dt          r  ( r >  0   v f   )  ‖ v f   ‖ 2  v f          = =          0 3 × 3   0 3 × 1   0 3 × 1   − I 3 × 3  0 1 × 3   0   0   0 1 × 3  0 1 × 3   0   0   0 1 × 3  0 3 × 3   0 3 × 1   0 3 × 1   0 3 × 3                 r  ( r >  0   v f   )  ‖ v f   ‖ 2  v f          + +          − I 3 × 3  0 1 × 3  0 1 × 3  0 3 × 3          v r   .   (57) The range-only localization problem of estimating   r   and the current velocity   v f   from a measurement of   ‖ r ‖ 2   in equations (46) - (48) is hence reduced to a state estima- tion problem on a linear time invariant state equation (56) - (57) with an LTV output map (55), namely  {    ̇ z   =   A   z   +   B   v r   ̄ y ( t ) =   C ( t )   z .   (58) The LTI state equations, moreover, have a very simple structure. As anticipated in section 1, notice that this re- sults is similar to the one described in [3], but with a few 6
significant differences: the state matrix   A   does not de- pend on the output and is actually LTI rather than LTV. Only the output map is time varying and depends on the vehicle’s velocity. Moreover the state vector has dimen- sion 8 rather than 9. As a consequence, the Gramian ob- servability matrix to be used for observability analysis has a simpler structure as well as the resulting observer that can be chosen to have a Kalman filter structure. Estimating   z   will result in estimating both   r   and the current velocity   v f   . From   r   the absolute position of the vehicle can be computed as   x   =   s   −   r   (refer to equation (40)).  4.1   Observability analysis in the presence of currents  The observability properties of system (58) can be stud- ied through the observability Gramian  G ( t ) =  ∫   t  0  e A > τ   C > ( τ   )   C ( τ   )   e Aτ   dτ.   (59) Given the structure of the   A   matrix in equation (57), notice that   A 2   = 0 8 × 8   implying that the exponential matrix exp( At ) is simply  e At   =   I 8 × 8   +   At   (60) such that   C ( t ) exp( At ) results in  C ( t )   e At   =  [  − 2   I >  v r   ( t )   −   2   t   t 2   2 t   I >  v r   ( t )  ]  (61) and exp( A > τ   )   C > ( τ   )   C ( τ   ) exp( Aτ   ) = =          4   I v r   I >  v r   4 τ   I v r   − 2 τ   2   I v r   − 4 τ   I v r   I >  v r  4 τ   I >  v r   4   τ   2   − 2 τ   3   − 4 τ   2   I >  v r  − 2 τ   2   I >  v r   − 2   τ   3   τ   4   2 τ   3   I >  v r  − 4 τ   I v r   I >  v r   − 4 τ   2 I v r   2 τ   3 I v r   4 τ   2 I v r   I >  v r          (62) where the dependency of   I v r   from   t   has been omitted for the sake of notation compactness. As for the observability conditions, following standard results for LTV systems [16], the model in equation (58) will be completely observable in the time interval [0 , t ] if and only if the Gramian given by equations (59) and (62) has full rank. Moreover, the structure of equation (62) implies that a necessary condition for the complete observability of (58) in the time interval [0 , t ] is that  G 11 ( t ) := 4  ∫   t  0  I v r   ( τ   )   I >  v r   ( τ   ) dτ   (63) has full rank, i.e. three. Notice that this latter necessary condition for the observability in the presence of con- stant and unknown currents was shown to be both nec- essary and sufficient in the current free case (Statement 2). Overall, the observability properties in the presence of constant currents can be summarized as follows.  Statement 3   -   Observability conditions for the contin- uous time case with constant current.  The model in equations (55) - (57) is observable on [0 , t ] if and only if the velocity signal   v r   guarantees that the Gramian in equations (59) and (62) has full rank. Moreover a necessary condition for full observability on [0 , t ] is that the matrix   G 11 ( t )   ∈   R 3 × 3   in equation (63) has rank 3.  Proof of Statement 3  The necessary and sufficient conditions on the Gramian in equations (59) and (62) follow from standard LTV sys- tems theory [16]. As for the necessary condition on the rank of the matrix   G 11 ( t )   ∈   R 3 × 3   in equation (63), fol- lowing the same method used to prove Statement 2 it re- sults that if   G 11 ( t ) should   not   be full rank on [0 , t ], there would exist a constant vector   ν   ∈   R 3 × 1 ,   ν   6   =   0   such that   I v r   ( τ   ) > ν   = 0   ∀   τ   ∈   [0 , t ]: this implies that any vector parallel to   z ∗   = ( α   ν > ,   0 ,   0 , β   ν > ) >   ∈   R 8 × 1   for any constant   α, β   ∈   R   would belong to the kernel of the Gramian (59) - (62) that, hence, would not be full rank. This proves that rank( G 11 ( t )) = 3   :   G 11 ( t )   ∈   R 3 × 3   is defined in equation (63) is a necessary condition for the observability in [0 , t ] of the model in equations (55) - (57). Notice that if the   v r   components should be defined as in equations (35) - (36), the observability conditions of Statement 3 would indeed be satisfied. Also notice that the results in Statement 3 allow to consider optimal de- sign issues of the vehicle’s input   v r   : in Kalman filtering theory, in fact, the observability Gramian is related to the estimate covariance and to the Fisher information matrix. Building on the results in Statement 3, one could formulate optimal design problems for the input   v r   aim- ing at maximizing, by example, metrics as the norm or the determinant, or the condition number of the result- ing Fisher information matrix.  5   Kalman filter design and simulation examples  With reference to the model in equations (29) - (30) de- rived from the original system (5) - (6), assume to explic- itly account for noise in the standard Kalman filtering framework, i.e. consider the system  x k +1   =   x k   +   v k   +   ω k   (64)  ̄ y k   =   I >  k   x k   +   ε k   (65) 7
where   ω k   and   ε k   are zero mean state and output mutu- ally independent disturbances with covariances cov( ω k ) =   E   [ ω k ω >  k  ]   =   Q k   (66) cov( ε k ) =   E   [ ε 2  k  ]   =   R k .   (67) Denoting with   ˆ x k | k   the Kalman estimate at step   k  and with ˆ x k +1 | k   the model prediction, the localization Kalman filter results in ˆ x k +1 | k   = ˆ x k | k   +   v k   (68)  P k +1 | k   =   P k | k   +   Q k   (69)  I k +1   =   I k   +   v k +1   :   I 0   =   0   (70)  K   =  (  P   − 1  k +1 | k   +   I k +1   R − 1  k +1   I >  k +1  ) − 1  I k +1   R − 1  k +1   (71) ˆ x k +1 | k +1   = ˆ x k +1 | k   +   K ( ̄ y k +1   −   I >  k +1   ˆ x k +1 | k )   (72) where   P k +1 | k   is the covariance of the prediction ˆ x k +1 | k  while cov(ˆ x k +1 | k +1 ) =  (  P   − 1  k +1 | k   +   I k +1   R − 1  k +1   I >  k +1  ) − 1  .   (73) The proposed Kalman filter described in equations (68) - (73) for the current free case has been implemented and tested using inputs of the form described in equa- tions (35) - (36). The simulation parameters relative to equations (35) - (36) and (68) - (73) are summarized in the following table: Parameter   Value   Dimension  x 0   (25 ,   25 ,   25) >   m ˆ x 0   (125 ,   125 ,   125) >   m ( n 1 , n 2 , n 3 )   (1 ,   2 ,   3)   − − −  ω   10 − 2 π   rad/s  A i n i ω   (for   i   = 1 ,   2 ,   3)   0 . 5   m/s Sampling Time   10 − 2   s  Q   10 − 4 I 3 × 3   m 2  R   1   m 2 . The resulting trajectories are depicted ad different zoom levels in figure 1: the initial position of the vehicle is marked with a green   ∗   symbol (visible in the bottom plot). The vehicle trajectory is plotted in green while the Kalman estimate is plotted in red. The blue line is the one-step Kalman predicted position. As for the case presented in section 4 in the presence of a constant and unknown current   v f   , the state   z   can be es- timated with a Kalman filter. In particular, a numerical 0 50 100 150 0 50 100 150 0 20 40 60 80 100 120 140 x [m] y [m] z [m] 0 10 20 30 40 50 15 20 25 30 35 18 20 22 24 26 28 30 32  x [m] y [m]  z [m]  Fig. 1. Kalman filter estimated position in the current free case. Refer to the text for details. 10   20   30   40   50   60  − 30  − 20  − 10 0  Position and Position Estimate x [m] 10   20   30   40   50   60 0 10 20  y [m] 10   20   30   40   50   60  − 20 0 20  Time [s] z [m]  Fig. 2. Kalman filter estimation including currents: real (in green) and estimated (in red) components of   x .  experiment is performed using the same agent velocity profile   v r   used in the examples presented in [5] and [3] namely   v r   = (2 cos( t ) ,   − 4 sin(2 t ) ,   cos( t/ 2)) >   (m/s). The target   s   is   s   = (2 ,   3 ,   1) >   (m), the current is assumed null, 8
10   20   30   40   50   60 0 2 4 6  v f   and v f   estimate v f x   [m/s] 10   20   30   40   50   60  − 2 0 2  v f y   [m/s] 10   20   30   40   50   60  − 10  − 5 0  Time [s] v f z   [m/s] Fig. 3. Kalman filter estimation including currents: real (in green) and estimated (in red) components of the current velocity   v f   .  v f   =   0   (m/s), and the initial position of the agent is  x 0   = (2 ,   2 ,   0) >   (m) such that the inertial position of the agent by   x ( t ) = (2 + 2 sin( t ) ,   2 cos(2 t ) ,   2 sin(0 . 5 t )) >   (m). Notice that, by direct calculation, the above   v r   input satisfies the observability condition given in Statement 3. The covariances on the state   z   and output  ̄ y ( t ) employed in the Kalman filter are   Q   = (1 e   −   2)diag([1 ,   1 ,   1 ,   1 e   −  4 ,   1 e   −   6 ,   (1 e   −   2) ,   (1 e   −   2) ,   (1 e   −   2)) and   R   = 1 respec- tively with proper units (i.e. [ m 2 ] for position variables and [( m/s ) 2 ] for velocity variables). The filter is initial- ized with a position ˆ x 0   = ( − 30 ,   20 ,   30) > [m] as opposed to the real initial position   x 0   = (2 ,   2 ,   0) > [m] and a cur- rent estimate ˆ v f   = (0 . 1 ,   − 0 . 1 ,   0 . 1) > [m/s] as opposed to the real null current. Denoting with   T s   the sampling time (that was (1 / 750)[s] in the described example), the Kalman filter equations result in:  A d   = ( I 8 × 8   +   T s   A )   (74)  B d   =   T s   B   (75) ˆ z k +1 | k   =   A d   ˆ z k | k   +   B d   v r k   (76)  P k +1 | k   =   A d   P k | k   A >  d   +   Q k   (77)  K   =  (  P   − 1  k +1 | k   +   C >  k +1   R − 1  k +1   C k +1  ) − 1  C >  k +1   R − 1  k +1   (78) ˆ z k +1 | k +1   = ˆ z k +1 | k   +   K ( ̄ y k +1   −   C k +1   ˆ z k +1 | k )   (79) where   Q k   and   R k   were constant and equal to the values reported above. The resulting time evolution of the agent position   x   =  s   −   r   and its estimate ˆ x   =   s   −   ˆ r   are plotted in figure 2 while the current estimate is plotted in figure 3.  5.1   Discussion  As already noticed, the proposed solution allows to de- sign a Kalman filter for state estimation on a system where all the system matrices ( A ,   B   and   C ( t )) are not affected by measurement noise. This preserves the opti- mality of the Kalman filter as a state estimator in case of additive gaussian noise on the output and state equa- tions. Yet the new output  ̄ y ( t ) in equations (28) and (53) (and in their discrete time counterparts) always de- pends on the very first measurement   y (0). This depen- dency can impact on the robustness of the solution as a single bad measurement (as an outlier) at   t   = 0 will af- fect the output for ever. A remedy to this issue can be found by periodically re-setting the initial measurement  y (0) with   y ( t ). In the discrete time case this would cor- respond to periodically mapping   y 0   −→   y k ∗   as if the measurement had started at step   k ∗   while the state es- timate ˆ x k | k   continues its update dynamics. A detailed analysis of this implementation detail goes beyond the scope of this paper and will not be addressed further, but it will be subject to future investigation.  6   Conclusions  The problem of single range based localization for the kinematics model of a 3 D   vehicle was addressed in this paper. The problem is relevant in several filed robotics applications, particularly in underwater scenarios where ranges are measured acoustically and alternative radio frequency based localization devices as GPS are not available. Single range based localization techniques al- low to avoid using trilateration based devices such as long base line (LBL) transponders that are very demand- ing in terms of cost and deployment effort. The vehicle is assumed to be equipped with standard on board nav- igation sensors as a doppler velocity logger DVL and an attitude heading reference system AHRS allowing to ac- cess the linear and angular vehicle velocities as well as the system’s attitude, i.e. the rotation matrix   I   R B   from body frame   { B }   to the inertial frame   { I } . The localiza- tion problem addressed is equivalent to the one presented in [3] and it explicitly accounts for the effects of a con- stant, but unknown, ocean current that is estimated to- gether with the vehicle position. The proposed solution allows to address the observability analysis and the state estimation filter design on a linear time invariant state equation defined on   R 8   with a time varying scalar output equation. In this respect the proposed solution resem- bles the one in [3] where the original problem was trans- formed in a linear time varying state equation defined on  R 9   with a linear time invariant scalar output equation. Yet contrary to this previous solution, the state equa- tion matrix   A   does not depend on the inverse of the out- put   y ( t ) hence preserving the optimality of the Kalman filter in case of additive gaussian noise on the state and output equations. Moreover, the simple structure of the derived linear system for observability analysis allows to define straightforward necessary and sufficient observ- ability conditions. The proposed solution can be applied in underwater cooperative navigation applications, sen- sor networks and source localization problems. Exam- 9
ples of Kalman estimation filters for single range local- ization are provided in the absence and in the presence of constant ocean currents.  Acknowledgements  This work was partially supported by MIUR (Italian Ministry of Education, Universities and Research) un- der the PRIN project MARIS: Marine Autonomous Robotics for InterventionS, call of year 2010-2011, prot. 2010FBLHRJ.  References  [1]   Filippo   Arrichiello,   Gianluca   Antonelli,   Anto- nio   Pedro   Aguiar,   and   Antonio   Pascoal.   Ob- servabiliy metrics for the relative localization of AUVs based on range and depth measurements: theory and experiments.   In   Proceedings of the 2011 IEEE/RSJ International Conference on In- telligent Robots and Systems, IEEE-IROS 2011 , Hilton   San   Francisco   Union   Square,   San   Fran- cisco, CA, USA, September 2011.   doi: 10.1109/ IROS.2011.6094466.   URL   http://dx.doi.org/ 10.1109/IROS.2011.6094466 . [2]   Pedro Batista, Carlos Silvestre, and Paulo Oliveira. Single beacon navigation: Observability analysis and filter design. In   Proceedings of the 2010 Amer- ican Control Conference, ACC2010 , pages 6191– 6196, Marriott Waterfront, Baltimore, MD, USA, June 30 – July 02 2010. IEEE.   ISBN 978-1-4244- 7426-4. [3]   Pedro Batista, Carlos Silvestre, and Paulo Oliveira. Single range aided navigation and source localiza- tion: Observability and filter design.   Systems & Control Letters , 60:665–673, 2011.   doi: 10.1016/j. sysconle.2011.05.004.   URL   http://dx.doi.org/ 10.1016/j.sysconle.2011.05.004 . [4]   Ming Cao, Changbin Yu, and Brian D.O. Ander- son. Formation control using range-only measure- ments.   Automatica , 47:776–781, 2011. doi: 10.1016/ j.automatica.2011.01.067.   URL   http://dx.doi. org/10.1016/j.automatica.2011.01.067 . [5]   Sandra H. Dandach, Bari ̧ s Fidan, Soura Dasgupta, and Brian D.O. Anderson. A continuous time lin- ear adaptive source localization algorithm, robust to persistent drift.   Systems & Control Letters , 59:7–16, 2009. doi: 10.1016/j.sysconle.2008.07.008. URL   http://dx.doi.org/10.1016/j.sysconle. 2008.07.008 . [6]   Maurice F. Fallon, Georgios Papadopoulos, John J. Leonard, and Nicholas M. Patrikalakis.   Cooper- ative AUV navigation using a single maneuvering surface craft.   I. J. Robotic Res. , 29(12):1461–1474, 2010. doi: 10.1177/0278364910380760. URL   http: //dx.doi.org/10.1177/0278364910380760 . [7]   A. S. Gadre and D. J. Stilwell.   Toward under- water navigation based on range measurements from a single location.   In   Proceedings of IEEE International   Conference   on   Robotics   and   Au- tomation, 2004 (ICRA 2004) , New Orleans, LA, USA,   26   April   –   1   May   2004   2004.   doi:   10. 1109/ROBOT.2004.1302422.   URL   http://dx. doi.org/10.1109/ROBOT.2004.1302422 . [8]   Robert Hermann and Arthur J. Krener.   Non- linear   controllability   and   observability.   IEEE Transactions   on   Automatic   Control ,   22(5):728 –   740,   October   1977.   doi:   10.1109/TAC.1977. 1101601.   URL   http://dx.doi.org/10.1109/ TAC.1977.1101601 . [9]   Giovanni Indiveri, Paola Pedone, and Michele Cuc- covillo.   Fixed target 3D localization based on range   data   only:   A   recursive   least   squares   ap- proach. In   Proceedings of the 2012 IFAC Workshop on Navigation, Guidance and Control of Underwa- ter Vehicles, IFAC-NGCUV2012 , pages 140 – 145, Porto, Portugal, 10 - 12 April 2012. doi: 10.3182/ 20120410-3-PT-4028.00024.   URL   http://dx. doi.org/10.3182/20120410-3-PT-4028.00024 . [10] J ́ erˆ ome Jouffroy and Johann Reger.   An algebraic perspective to single-transponder underwater nav- igation.   In   Computer Aided Control System De- sign, 2006 IEEE International Conference on Con- trol Applications, 2006 IEEE International Sym- posium on Intelligent Control , pages 1789–1794. IEEE, 2006. doi: 10.1109/CACSD-CCA-ISIC.2006. 4776912.   URL   http://dx.doi.org/10.1109/ CACSD-CCA-ISIC.2006.4776912 . [11] Zaher M Kassas and Todd E Humphreys.   Ob- servability analysis of opportunistic navigation with pseudorange measurements.   In   AIAA Guidance, Navigation, and Control Conference, AIAA GNC , 2012. doi: 10.2514/6.2012-4760. URL   http://dx. doi.org/10.2514/6.2012-4760 . [12] Toshihiro Maki, Takumi Matsuda, Takashi Saka- maki, Tamaki Ura, and Junichi Kojima.   Naviga- tion method for underwater vehicles based on mu- tual acoustical positioning with a single seafloor station.   IEEE Journal Of Oceanic Engineering , 38(1):167 – 177, 2013.   doi: 10.1109/JOE.2012. 2210799.   URL   http://dx.doi.org/10.1109/ JOE.2012.2210799 . [13] A. Martinelli and R. Siegwart. Observability anal- ysis for mobile robot localization.   In   Intelligent Robots   and   Systems,   2005.   (IROS   2005).   2005 IEEE/RSJ   International   Conference   on ,   pages 1471 – 1476, aug. 2005.   doi: 10.1109/IROS.2005. 1545153.   URL   http://dx.doi.org/10.1109/ IROS.2005.1545153 . [14] Gianfranco Parlangeli, Paola Pedone, and Giovanni Indiveri.   Relative pose observability analysis for 3D nonholonomic vehicles based on range measure- ments only. In   Proceedings of the 9th IFAC Confer- ence on Manoeuvring and Control of Marine Craft, MCMC 2012 , Arenzano (GE), Italy, 19 - 21 Septem- ber 2012. [15] Andrew Ross and J ́ erˆ ome Jouffroy. Remarks on the 10
observability of single beacon underwater naviga- tion. In   Int. Symp. on Unmanned Untethered Sub- mersible Technology (UUST 05) , Durham, NH, Au- gust 2005. [16] Wilson J. Rugh.   Linear System Theory . Prentice- Hall, 1996. ISBN 0134412052. [17] Jorge M. Soares, A. Pedro Aguiar, Antonio M. Pascoal, and Alcherio Martinoli. Joint ASV/AUV range-based formation control: Theory and exper- imental results. In   2013 IEEE International Con- ference on Robotics and Automation ICRA 2013 , Karlsruhe, Germany, 6-10 May 2013. [18] Taek Lyul Song.   Observability of target tracking with range-only measurements.   Oceanic Engineer- ing, IEEE Journal of , 24(3):383 – 387, jul 1999. doi: 10.1109/48.775299. URL   http://dx.doi.org/10. 1109/48.775299 . [19] Sarah E. Webster, Jeffrey M. Walls, Louis L. Whit- comb, and Ryan M. Eustice.   Decentralized ex- tended information filter for single-beacon cooper- ative acoustic navigation: Theory and experiments.  IEEE Transactions on Robotics , (99):1–18, 2013. doi: 10.1109/TRO.2013.2252857. URL   http://dx. doi.org/10.1109/TRO.2013.2252857 . [20] Xun S Zhou and Stergios I Roumeliotis.   Robot- to-robot relative pose estimation from range mea- surements.   IEEE Transactions on Robotics , 24 (6):1379–1393,   2008.   doi:   10.1109/TRO.2008. 2006251.   URL   http://dx.doi.org/10.1109/ TRO.2008.2006251 . 11