DESIGN OF A CONFIDENCE INTERVAL OBSERVER -APPLICATION TO VEHICLE POSITIONING

DESIGN OF A CONFIDENCE INTERVAL OBSERVER APPLICATION TO VEHICLE POSITIONING G. Goffaux ∗,1 A. Vande Wouwer ∗ M. Remy ∗ ∗ Service

d’Automatique, Facult´e Polytechnique de Mons, Boulevard Dolez 31, B-7000 Mons, Belgium

Abstract: State estimation methods allow the vehicle position and velocity to be reconstructed by combining information from sensors and vehicle model. From a security point of view, position and velocity have to be known with a high level of confidence in order, for example, to avoid vehicle collision. In this paper, a confidence interval observer is developed to enclose positioning variables with some confidence degree (or integrity level). Based on a vehicle model and intervals bounding, with some associated probability, the uncertain initial conditions, inputs, model parameters and measurements, a predictor provides intervals for the state estimate trajectory between two measurement times. At each measurement time, confidence intervals from the sensors and the predictor are combined with union and intersection operations so as to satisfy the specified integrity level. Finally, the shortest non-empty intervals are chosen among the safe intervals. This method is illustrated with simulation tests based on an autonomous underwater vehicle described by a nonlinear model. Copyright © 2007 IFAC Keywords: nonlinear state estimation, interval observers, confidence interval, vehicle positioning, marine vehicle

1. INTRODUCTION State estimation is commonly used in vehicle positioning applications where velocity and position are reconstructed based on a vehicle model and on-line measurements. Popular methods include Kalman filtering combining a kinematic model and measurements from GPS (Global Positioning System) and INS (Inertial Navigation System) (Gordon et al., 1993), particle filtering for positioning, navigation and tracking applications (Ristic et al., 2004) or nonlinear filtering in marine applications (Fossen, 2002). Besides precision objectives, security is the main concern in some positioning applications. In these applications, positioning information is usually given in the form of intervals bounding variables and parameters 1

Author to whom correspondence should be addressed: e-mail: [email protected] phone: +32 (0)65374132 fax: +32 (0)65374136

with some degree of confidence. In this context, recent work has been carried out using interval algebra and the concept of guaranteed intervals (Jaulin et al., 2001). However, guaranteed intervals are a too idealistic concept in some applications with critical security (e.g. SIL4 2 ). In these situations, position and velocity intervals have to be known with a high confidence level. In this context, recent work has been devoted to the manipulation of confidence intervals in a static configuration (Druet et al., 2006; Zhu and Li, 2006). The objective of this study is to develop a confidence interval observer, including a prediction step based on a nonlinear dynamic model (between measurement times) and a correction step (at the measurement in2

SIL (Safety Integrity Level) is a statistical representation of the reliability of safety-related process (IEC61508, 2000). SIL4 means 10−9 tolerated error per hour.

stants). Assuming intervals bounding with some associated probability, the uncertain initial conditions, inputs, model parameters and measurements, the predictor provides intervals for the state estimate trajectory. Then, at each measurement time, intervals from sensors and the predictor are combined by union and intersection operations. Every possible combinations are investigated such that the derived integrity satisfies the integrity objective.The chosen application is an autonomous underwater vehicle (AUV, Marine Systems Simulator, 2005). This paper is organized as follows. Section 2 presents the notations, the concept of a confidence interval and the basic operations - intersection and union - that can be carried out on these intervals. Section 3 describes the proposed confidence interval observer which is divided into a prediction step between two measurement times and a correction step at each sampling time. Section 4 is devoted to the algorithm illustration. Finally, conclusions are drawn in Section 5. 2. PRELIMINARIES We consider continuous-time nonlinear models associated with discrete-time measurements: x˙ (t) = f(x(t), u(t), θ )

x(0) = x0

yk = C(tk )x(tk )

(1) (2)

where x(t) ∈ ℜnx is the vector of state variables, u(t) ∈ ℜnu is the vector of inputs, yk ∈ ℜny is the vector of measurements at the discrete time tk , θ is a parameter vector. The subvectors x1 and x2 represent the measured and unmeasured state variables, respectively, i.e. h iT xk = xT1,k , xT2,k . Intervals are assumed for the initial state x0 , the parameters θ and the inputs u(t). Their confidence (or integrity) level is defined in the following way:  
+ p x0 ∈ x− ≥ β0 0 , x0  − + 
≥ βθ p θ ∈ θ ,θ  − + 
p u(t) ∈ uk , uk ≥ βuk

Moreover, a Boolean random variable B• is related to each computed interval (parameters and variables), following the definition (Bilenne, 2006):   if a ∈ a− , a+

Ba = true Ba = false

otherwise

(6)

From this definition, one can deduce :   p(Ba = true) = p(a ∈ a− , a+ ) = βa

(7)

Intuitively, it means that, for infinitely repeated random events computing an interval on a, the bounding will occur in the mean at the rate βa . Basic operations on these random variables can now be defined (Bilenne, 2006). Let Ba1 and Ba2 denote two Boolean random variables deduced  −from  two + sensors measuring a with the intervals a , a 1 1 and  − + a2 , a2 , and let βa1 and βa2 denote the lower bounds on the integrity levels, respectively.   + p(Ba1 = true) = p(a ∈ a− 1 , a1 ) ≥ βa1   + p(Ba2 = true) = p(a ∈ a− 2 , a2 ) ≥ βa2   + p(Ba1 = false) = p(a ∈ / a− 1 , a1 ) ≤ 1 − βa1   + p(Ba2 = false) = p(a ∈ / a− 2 , a2 ) ≤ 1 − βa2 Intersection : • If we consider independent random variables    − + 
+ p a ∈ a− = 1 , a1 ∩ a2 , a2 p (Ba1 = true and

Ba2 = true) ≥ βa1 βa2 (8)

• If we consider dependent random variables p (Ba1 = true and

Ba2 = true) ≥

(9)

p (Ba1 = true) − p (Ba2 = false) ≥ βa1 + βa2 − 1 Union :

tk ≤ t ≤ tk+1

(3)

Let ymeas,k ∈ ℜny denote the sensor measurements. In a similar way, intervals are built such that the integrity level is given by:  h i + p x1,k ∈ y− ≥ βyk meas,k , ymeas,k

(4)

   − + 
+ p a ∈ a− = 1 , a1 ∪ a2 , a2

p (Ba1 = true

or

1 − p (Ba1 = false

Ba2 = true) =

and

Ba2 = false) ≥

1 − (1 − βa1 )(1 − βa2 ) = βa1 + βa2 − βa1 βa2 (10) • If we consider dependent random variables

Considering these notations, the confidence interval observer aims at computing upper x+ (t) and lower x− (t) bounds of the state vector x(t) so that the related integrity satisfies the objective :  
p x(t) ∈ x− (t), x+ (t) ≥ βob j

• If we consider independent random variables

(5)

p (Ba1 = true or

Ba2 = true) ≥ max (βa1 , βa2 ) (11)

These notions are now used in the confidence interval observer design.

3. CONFIDENCE INTERVAL OBSERVER Based on the general model (1)-(2) and intervals for the initial state, the parameters, the inputs and the measurements (3)-(4), the confidence interval observer proceeds in two separate steps with respect to the measurement times: 3.1 Prediction step Between two measurement times, the predictor propagates the intervals bounds on the state variables. If we assume bounded intervals at time tk (x(tk ) ∈ − + + [x− P (tk ), xP (tk )]), then the predictor (xP , xP ) has to be designed in such a way that it guarantees x(t) ∈ + [x− P (t), xP (t)] for t ≥ tk . Equivalently, the error vector e(t) ∈ ℜ2nx is defined such that 

  +  e+ (t) xP (t) − x(t) e(t) = − = ≥ 0 (∀t ≥ tk )(12) e (t) x(t) − x− P (t) To prove this, it is sufficient to check that, assuming e(t0 ) ≥ 0, if ∃t ∗ ≥ t0 s.t. ei (t ∗ ) = 0 and e j (t ∗ ) ≥ 0 (∀ j 6= i ∈ {1, . . . , 2nx }), then e˙i (t ∗ ) ≥ 0 (13) This means that every time a component ei of the positive error vector vanishes, its derivative must be non negative in order to prevent the corresponding error from becoming negative. The predictor design is based on the system model (1). The derivatives of the lower and upper bounds ˙+ (˙x− P ,x P ) are computed by replacing parameters and variables in (1) by the bounds allowing to satisfy criteria (13). If a confidence level is related to the intervals at time tk , a lower bound of the integrity level can be derived for the intervals on the predicted state variables (Fruchard et al., 2002) and is given by :  

+ p x(t) ∈ x− P (t), xP (t) = p Bx(t) = true ≥ βxk βθ βuk

(14)

≥ βob j with βxk , βθ and βuk the integrity related, respectively, to the state vector at tk , the parameters and the inputs. Indeed, the predictor is designed in such a way that it is sufficient for all the variables and parameters to be bounded at the present time to guarantee bounded intervals on state variables at future instants. 3.2 Correction step At a measurement time, measurement intervals and predictor intervals are combined by union and intersection operations. The resulting integrity is based on the Boolean random variable (6) related to each

interval and their corresponding operations (8)-(11). Every possible combinations are considered such that the derived integrity satisfies the integrity objective. The combination set is summarized in the following. Considering n intervals denoted [a1 ], . . . , [an ], let C k define the set of k-combinations from the set of n elements {1, . . . , n}. The number of elements in this set is given by : nc =

  n! n = k k!(n − k)!

The resulting intervals are provided by the following formulas Ikh (i) =

ii (h)

\

Uk ( j) k ∈ {1, . . . , n}

(15)

j=ii (1)

C k = {c1 , . . . , cnc }, describes all possible unions of k intervals among n, and consequently we have Uk ( j) = [ac j (1) ] ∪ . . . ∪ [ac j (k) ];   h ∈ {1, . . . , hmax } and hmax = nk is the maximum number of possible intersections between the intervals derived from the union operations; Ihk = {i1 , . . . , ini }, describes all possible intersections of h intervals contained in Uk preventing that the original intervals appear twice. Example : combination set for 4 intervals [a1 ], [a2 ], [a3 ], [a4 ]. • k = 1, h = 1 : [a1 ], [a2 ], [a3 ], [a4 ] ; • k = 1, h = 2 : [a1 ] ∩ [a2 ], [a1 ] ∩ [a3 ], [a1 ] ∩ [a4 ], [a2 ] ∩ [a3 ], [a2 ] ∩ [a4 ], [a3 ] ∩ [a4 ] ; • k = 1, h = 3 : [a1 ] ∩ [a2 ] ∩ [a3 ], [a1 ] ∩ [a2 ] ∩ [a4 ], [a1 ] ∩ [a3 ] ∩ [a4 ], [a2 ] ∩ [a3 ] ∩ [a4 ] ; • k = 1, h = 4 : [a1 ] ∩ [a2 ] ∩ [a3 ] ∩ [a4 ] ; • k = 2, h = 1 : [a1 ] ∪ [a2 ], [a1 ] ∪ [a3 ], [a1 ] ∪ [a4 ], [a2 ] ∪ [a3 ], [a2 ] ∪ [a4 ], [a3 ] ∪ [a4 ] ; • k = 2, h = 2 : ([a1 ] ∪ [a2 ]) ∩ ([a3 ] ∪ [a4 ]), ([a1 ]∪ [a3 ]) ∩ ([a2 ] ∪ [a4 ]), ([a1 ] ∪ [a4 ]) ∩ ([a2 ] ∪ [a3 ]); • k = 3, h = 1 : [a1 ] ∪ [a2 ] ∪ [a3 ], [a1 ] ∪ [a2 ] ∪ [a4 ], [a1 ] ∪ [a3 ] ∪ [a4 ], [a2 ] ∪ [a3 ] ∪ [a4 ] ; • k = 4, h = 1 :[a1 ] ∪ [a2 ] ∪ [a3 ] ∪ [a4 ] . Then, integrity is computed for each combination thanks to intersection and union formulas (8)-(11). Finally, the shortest non-empty interval satisfying the integrity objective is chosen. 4. VEHICLE POSITIONING : AN AUTONOMOUS UNDERWATER VEHICLE In this section, the confidence interval observer is applied to an AUV positioning problem. To avoid handling a too complex model and without loss of generality, the vehicle is assumed moving in a horizontal plane and is described by a 3 degrees of freedom (DOF) model. The 3 DOF model considers 6 state

variables grouped in two vectors, η and ν and two reference frames, the NED-frame and the BODY-frame (figure 1).

η = [x, y, ψ ]T

ν = [u, v, r]T

(16)

where x and y are the vehicle origin location in the NED-frame, ψ is its heading, u and v are its linear velocity according to the BODY-frame and r is the angular velocity with respect to the z-axis. • NED-frame. The North-East-Down system xn , yn , zn , is usually defined as the tangent plane on the surface of the Earth and can be considered as fixed in a local area. In this system, the xaxis points towards the true North, the y-axis points towards the East while the z-axis points downwards, in the normal direction to the Earth surface. • BODY-frame. This frame xb , yb , zb is fixed to the vehicle and is thus a moving coordinate frame.

Fig. 1. Frames and 3 DOF vehicle state variables. 4.1 Vehicle modelling The vessel dynamics can be divided in two parts : kinematics which describes the velocity conversion from the BODY-frame to the NED-frame and kinetics which is the analysis of forces and momentums equilibrium. The chosen evolution equations are given by

η˙ = J(η )ν Mν˙ +C(ν )ν + D(ν )ν + g(η ) = τ c

(17) (18)

J(η ) is the rotational matrix computing coordinates from the BODY-frame to the NED-frame.   cos ψ − sin ψ 0 J(η ) =  sin ψ cos ψ 0  0 0 1 M ∈ ℜ3×3 represents the system inertia matrix which includes added mass. C(ν ) ∈ ℜ3×3 is the Corioliscentripetal matrix (including added mass too). D(ν ) ∈ ℜ3×3 is the damping matrix containing hydrodynamic damping. Generally, it contains linear and quadratic terms which are assumed uncoupled : D(ν ) = DL + DQ (ν ). In this study, without loss of generality, only

the linear term is considered. This means that the vehicle moves at a low speed. g(η ) ∈ ℜ3 is the vector of gravitational and buoyancy forces and moments. Moreover, this vector is assumed equal to zero by considering that the AUV is neutrally buoyant and that the centre of gravity and the centre of buoyancy are placed on the same vertical axis. Finally, τ c ∈ ℜ3 is the vector of control inputs. The nonlinear 3 DOF dynamic model becomes :

ν˙ = M −1 (τ c −C(ν )ν − Dν )

(19)

Nonlinear terms come from the Coriolis effect. More information about marine vehicle modelling can be found in (Fossen, 2002). 4.2 Sensor description Several sensors can be used for AUV positioning, for instance: • Acoustic Positioning Systems. Using beacons placed on the sea bed, they provide position (x, y) and heading (ψ ) measurements. Because the acoustic signals are sent from and received by the AUV, noise sources come from the environment and a loss of signal can occur if obstacles are in the acoustic trajectory. • Inertial Navigation Systems. Using accelerometers and gyros, these sensors measure the acceleration vector (u, ˙ v) ˙ and the velocity angle r with respect to the z-axis. • Radars. They measure the frequency drift between an emitted signal and its reflection. The Doppler principle allows to derive velocity vector (u, v). Remark 1. In this study, the main assumption is the availability of sensors providing intervals bounding the true state with some probability. So, the presented results can be extended to other vehicles (e.g. other marine vessels or cars) described by similar dynamic equations and to other kinds of sensors (e.g. GPS). Moreover, statistical information is assumed to be known such that confidence intervals can be derived. Finally, parameters and inputs are assumed perfectly known. 4.3 Predictor description First consider the evolution of the position vector η (17) x˙ = u cos ψ − v sin ψ y˙ = u sin ψ + v cos ψ ψ˙ = r

(20)

The prediction equations for the evolution of the lower and the upper bounds of x, y and ψ are given by :

x˙+ = (ucosψ )+ − (vsinψ )− y˙+ = (usinψ )+ + (vcosψ )+ ψ˙ + = r+

∗ + ∗ e˙+ ˙ ∗) v (t ) = v˙ (t ) − v(t

x˙− = (ucosψ )− − (vsinψ )+ y˙− = (usinψ )− + (vcosψ )− ψ˙ − = r−

Then consider the evolution equations of the velocity vector ν (18) written in the following form: u˙ = a + αu u + αvr vr + αrr r2 v˙ = b + βv v + βr r + βuv uv + βur ur r˙ = c + γv v + γr r + γuv uv + γur ur

(22)

Coefficients are assumed perfectly known. Consequently, predictor equations can be derived from the system model (22) by considering each term separately :
+

v˙+ = b + βv v+ + (βr r)+ + (βuv uv)+ + (βur ur)+ r˙+ = c + (γv v)+ + γr r+ + (γuv uv)+ + (γur ur)+ (23) u˙ = a + αu u + (αvr vr) + αrr r −

−

−

= (βr r)+ − βr r + (βur ur)+ − βur ur
+ (βuv uv)+ − βuv uv ≥ 0

(21)

where (.)− and (.)+ denote lower and upper bounds of the elements inside the brackets. For instance, with [ψ − ψ + ] ∈ [− π2 π2 ], (usinψ )+ is given by
max v+ sin(ψ + ),v− sin(ψ + ),v+ sin(ψ − ),v− sin(ψ − )

u˙+ = a + αu u+ + (αvr vr)+ + αrr r2

= b + βv v+ + (βur ur)+ + (βuv uv)+
+ (βr r)+ − (b + βv v + βr r + βuv uv + βur ur)


2 −

v˙− = b + βv v− + (βr r)− + (βuv uv)− + (βur ur)− r˙− = c + (γv v)− + γr r− + (γuv uv)− + (γur ur)− To prove boundedness, we assume that the initial intervals enclose the state variables so that the error vector e(t0 ) ≥ 0 (12). To sketch the demonstration, condition (13) is checked for a state variable from η and from ν .

Indeed, when v = v+ , βv v+ = βv v and the other components related to the predictor are, by construction, larger than the real unknown values. Remark 2. If v remains positive, (βuv uv)+ can be replaced by (βuv u)+ v+ ≤ (βuv uv)+ and the correspond
ing error component becomes (βuv u)+ − βuv u v ≥ 0. As v is unknown in the estimation process, (βuv u)+ v+ can only be used when v+ ≥ 0 (if v+ < 0, v < 0). Moreover, the predictor performance is related to the values and the sign of parameters. For example, a negative βuv can decrease e˙+ v (t) which can even become negative. Finally, similar expressions can be deduced for the other bounds.

4.4 Simulated example The confidence interval algorithm (section 3) using the predictor defined in section 4.3 is now illustrated with the AUV positioning. Each state variable is measured by three sensors : 3 APS sensors for (x, y) and ψ , 3 gyros for r and 3 radars for (u, v). A common sampling time of 20s is used. Confidence intervals are computed assuming Gaussian distribution. Information on the standard deviation can be found in Table 1. The integrity objective is (1 − 10−8 ) and the integrity of each measurement interval is defined as (1 − 10−4 ). In the correction step, the resulting integrity is computed for each state variable using (8) and (10) to reach the integrity objective. Concerning the prediction step, intervals generated by the predictor have to satisfy condition (14), which is, in this application,  
+ p x(t) ∈ x− P (t), xP (t) ≥ βxk

• Upper bound on x : x+ If ∃t ∗ s.t. x(t ∗ ) = x+ (t ∗ ) (other error components ≥ 0) ∗ + ∗ e˙+ ˙ ∗) x (t ) = x˙ (t ) − x(t


= (ucosψ )+ − (vsinψ )− − (ucosψ − vsinψ )

= (ucosψ )+ − ucosψ + vsinψ − (vsinψ )− | {z } | {z } ≥0

≥0

(24)

It only depends on the integrity of the state vector at time tk . βxk is computed thanks to (8) or (9) for independent or dependent random variables. x 30m

y 30m

ψ 0.6 rad

u 0.6 m/s

v 0.6 m/s

r 0.015 rad/s

Table 1. Standard deviations of measurement errors.

≥0

Note that the error derivatives related to η always remain positive with time and the errors increase if they are not corrected by measurements. • Upper bound on v : v+ If ∃t ∗ s.t. v(t ∗ ) = v+ (t ∗ ) (other error components ≥ 0)

Figure 2 shows interval state estimation for the described configuration. We first observe that the computed intervals enclose the state variables as expected. The algorithm ensures that unknown state variables are bounded with a probability above (1 − 10−8 ). To illustrate this, figure 3 shows the evolution of the error level.

It is also apparent that the position intervals given by the predictor increase as time evolves (due to the uncertainty on the model initial condition) and are then reduced at each measurement time by the interval combinations carried out in the correction step. Indeed, the correction step chooses the best combination among the intervals from sensors and from the predictor. Usually, the sensor intervals appear more accurate and reliable than the predictor intervals. 10 5

ψ (rad)

Y (m)

1500 1000 500 0 −1000

0 X (m)

0 −5 0

1000

200 400 Time (s)

ACKNOWLEDGEMENTS

2000 Y (m)

X (m)

1000 0

0 200 400 Time (s)

600

0

40

v (m/s)

u (m/s)

600

REFERENCES

6

20 10

4 2 0

200

Time (s)

400

−2 0

600

20

0.1

15

0.05 r (rad/s)

vtot (m/s)

200 400 Time (s)

8

30

10 5 0 0

This work is performed in the framework of the PIST project funded by the Walloon Region - DGTRE (Belgium).

1000

−1000

0 0

200

Time (s)

400

600

400

600

0 −0.05

200

Time (s)

400

600

−0.1 0

200

Time (s)

Fig. 2. Confidence interval state estimation : position x, y, heading ψ , linear (u, v) and angular r velocity. x

u

−8

−8

−10

−10

−12 0

−12 200

400

600

0

200

y −8

−8 −10

−12

600

400

600

400

600

−12 200

400

600

0

200

ψ

r

−8

−8

−10

−10

−12 0

400

v

−10

0

The developed method performs well and allows to satisfy security demand in critical applications. This method is general and can be applied to various problems characterized by uncertainties on the initial state, inputs and parameters. Furthermore, it can handle any kind of statistical distribution.

600

2000

−2000 0

variables. This algorithm is designed so as to always guarantee a specified integrity level by combining information (provided in the form of intervals) from a model and several sensors. The method proceeds in two steps, i.e. a predictor based on a vehicle description and a correction step at each sampling time combining measurement intervals by union and intersection operations.

−12 200

400

600

0

200

Fig. 3. Logarithm of the error level : log(1 − β• ) for each state variable. 5. CONCLUSIONS In this work, a state estimation algorithm is developed which provides confidence intervals on the state

Bilenne, O. (2006). Integrity-directed sensor fusion by robust risk assessment of fault-tolerant interval functions. In: Proceedings of the 9th International Conference on Information Fusion. Druet, E., O. Bilenne, M. Massar, F. Meers and F. Lef`evre (2006). Interval-based state estimation for safe train positioning. In: 7th World Congress on Railway Research. Fossen, T.I. (2002). Marine Control Systems: Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles. Fruchard, M., O. Bernard and J.-L. Gouz´e (2002). Interval observers with guaranteed confidence levels. application to the activated sludge process.. In: Proceedings of the IFAC World Congress. Barcelona, Spain. Gordon, N.J., D.J. Salmon and A.F.M. Smith (1993). Novel approach to nonlinear/non-gaussian Bayesian state estimation. IEE Proceedings, Part F 140(2), 107–113. IEC61508 (2000). Functional safety of electrical / electronic / programmable electronic safetyrelated systems. International Electrotechnical Commission, International Standard. Jaulin, L., M. Kieffer, O. Didrit and E. Walter (2001). Applied Interval Analysis. Springer, London, England. Marine Systems Simulator (2005). Norwegian University of Science and Technology. Trondheim, Norway. . Ristic, B., S. Arulampalam and N. Gordon (2004). Beyond the Kalman Filter – Particle Filters for Tracking Applications. Artech House, Boston. Zhu, Y. and B. Li (2006). Optimal interval estimation fusion based on sensor interval estimates with confidence degrees. Automatica 42, 101–108.