Intervals for state estimation

Transcription

1 Intervals for state estimation Luc Jaulin, ENSTA Brest ECA Brest, 23 Mai, 2015

2 1 Interval analysis

3 1.1 Basic notions on set theory

4 Iff is defined as follows f(a) = {2,3,4} = Im(f). f 1 (B) = {a,b,c,e} = dom(f). f 1 (f(a)) = {a,b,c,e} A f 1 (f({b,c})) = {a,b,c}.

5 Iff(x) =x 2, then f([2,3]) = [4,9] f 1 ([4,9]) = [ 3, 2] [2,3]. This is consistent with the property f f 1 (Y) Y.

6 1.2 Interval arithmetic

7 If {+,,,/,max,min} For instance, [x] [y] = [{x y x [x],y [y]}]. [ 1,3]+[2,5] = [1,8] [ 1,3] [2,5] = [ 5,15] [ 1,3]/[2,5] = [ 1 2, 3 2 ].

8 [x,x + ]+[y,y + ] = [x +y,x + +y + ]. [x,x + ] [y,y + ] = [x y x + y x y + x + y +, x y x + y x y + x + y + ].

9 Iff {cos, sin,sqr, sqrt, log, exp,...} For instance, f ([x]) = [{f(x) x [x]}]. sin([0,π]) = [0,1], sqr([ 1,3]) = [ 1,3] 2 = [0,9], abs([ 7,1]) = [0,7], sqrt([ 10,4]) = [ 10,4] = [0,2], log([ 2, 1]) =.

10 1.3 Boxes

11 Abox, orintervalvector [x] ofr n is [x] = [x 1,x+ 1 ] [x n,x + n] = [x 1 ] [x n ]. The set of all boxes ofr n will be denoted byir n.

12 Theprincipalplane of [x] is the symmetric plane [x] perpendicular to its largest side.

13 1.4 Inclusion function

14 The interval function [f] fromir n toir m, is aninclusion function off if [x] IR n, f([x]) [f]([x]). Inclusion functions [f] and [f] ; here, [f] is minimal.

15 The natural inclusion function forf(x) =x 2 +2x+4 is [f]([x]) = [x] 2 +2[x]+4. If [x] = [ 3,4], we have [f]([ 3,4]) = [ 3,4] 2 +2[ 3,4]+4 = [0,16]+[ 6,8]+4 = [ 2,28]. Notethatf([ 3,4]) = [3,28] [f]([ 3,4]) = [ 2,28].

16 Iff is given by the algorithm Algorithmf(in: x = (x 1,x 2,x 3 ), out: y = (y 1,y 2 )) 1 z :=x 1 ; 2 fork:= 0 to z :=x 2 (z +kx 3 ); 4 next; 5 y 1 :=z; 6 y 2 := sin(z x 1 );

17 Its natural inclusion function is Algorithm [f](in: [x], out: [y]) 1 [z] := [x 1 ]; 2 fork:= 0 to [z] := [x 2 ] ([z]+k [x 3 ]); 4 next; 5 [y 1 ] := [z]; 6 [y 2 ] := sin([z] [x 1 ]); Here, [f] is a convergent, thin and monotonic inclusion function forf.

18 1.5 Subpavings

19 Asubpaving ofr n is a set ofnon-overlapping boxes ofr n. Compact sets X can be bracketed between inner and outer subpavings: X X X +.

20 Example. X ={(x 1,x 2 ) x 2 1 +x2 2 [1,2]}. Set operations such asz:=x+y, X :=f 1 (Y),Z := X Y... can be approximated by subpaving operations.

21 1.6 Set inversion

22 Let f : R n R m and let Y be a subset of R m. Set inversion is the characterization of X ={x R n f(x) Y} =f 1 (Y).

23 We shall use the following tests. (i) [f]([x]) Y [x] X (ii) [f]([x]) Y = [x] X =. Boxes for which these tests failed, will be bisected, except if they are too small.

24 . Algorithm Sivia(in: [x](0),f,y) 1 L :={[x](0)}; 2 pull [x] froml; 3 if [f]([x]) Y, draw([x], red ); 4 elseif [f]([x]) Y =, draw([x], blue ); 5 elseifw([x])<ε, {draw ([x], yellow )}; 6 else bisect [x] and push intol; 7 ifl=, go to 2

25 If X denotes the union of yellow boxes and ifx is the union of red boxes then : X X X X.

26 2 Contractors

27 To characterize X R n, bisection algorithms bisect all boxes in all directions and become inefficient. Interval methods can still be useful if the solution set X is small(optimization problem, solving equations), contraction procedures are used as much as possible, bisections are used only as a last resort.

28 2.1 Definition

29 The operator C X : IR n IR n is a contractor for X R n if [x] IR n CX ([x]) [x] (contractance),, C X ([x]) X = [x] X (completeness).

30

31

32 C X is said to beconvergent if [x](k) x C X ([x](k)) {x} X.

33 2.2 Projection of constraints

34 Letx,y,z be 3 variables such that x [,5], y [,4], z [6, ], z = x+y. Which values forx,y,z are consistent.

35 Since x [,5],y [,4],z [6, ] and z = x+y, we have z =x+y z [6, ] ([,5]+[,4]) = [6, ] [,9] = [6,9]. x =z y x [,5] ([6, ] [,4]) = [,5] [2, ] = [2,5]. y =z x y [,4] ([6, ] [,5]) = [,4] [1, ] = [1,4].

36 The contractor associated withz=x+y is. Algorithm pplus(inout: [z],[x],[y]) 1 [z] := [z] ([x]+[y]); 2 [x] := [x] ([z] [y]); 3 [y] := [y] ([z] [x]).

37 The projection procedure developed for plus can be extended to other ternary constraints such as mult:z =x y, or equivalently mult (x,y,z) R 3 z=x y. The resulting projection procedure becomes Algorithm pmult(inout: [z],[x],[y]) 1 [z] := [z] ([x] [y]); 2 [x] := [x] ([z] 1/[y]); 3 [y] := [y] ([z] 1/[x]).

38 Consider the binary constraint exp{(x,y) R n y = exp(x)}. The associated contractor is Algorithm pexp(inout: [y],[x]) 1 [y] := [y] exp([x]); 2 [x] := [x] log([y]).

39 Any constraint for which such a projection procedure is available will be called a primitive constraint.

40 Projection of the sine constraint

41 2.3 Solvers

42 A CSP (Constraint Satisfaction Problem) is composed of 1) a set of variablesv ={x 1,...,x n }, 2) a set of constraintsc={c 1,...,c m } and 3) a set of interval domains{[x 1 ],...,[x n ]}.

43 Principle of propagation techniques: contract [x] = [x 1 ] [x n ] as follows: (((((([x] c 1 ) c 2 )...) c m ) c 1 ) c 2 )..., until a steady box is reached.

44 Example. Consider the system. y = x 2 y = x.

45 We build two contractors [y] = [y] [x] 2 C 1 : [x] = [x] [y] C 2 : [y] = [y] [x] [x] = [x] [y] 2 associated toy=x 2 associated toy= x

46

47

48

49

50

51

52

53

54

55 Exemple. Consider the system y = 3sin(x) y = x x R,y R.

56

57

58

59

60

61

62

63

64

65

66 2.4 Decomposition into primitive constraints

67 can be decomposed into a =xy b = sin(a) c =x+b x+sin(xy) 0, x [ 1,1],y [ 1,1], x [ 1,1] a [, ] y [ 1,1] b [, ] c [,0]

68 3 Redermor

69 The Redermor, GESMA

70 The Redermor at the surface

71 Why choosing an interval constraint approach for SLAM? 1) A reliable method is needed. 2) The model is nonlinear. 3) The pdf of the noises are unknown. 4) Reliable error bounds are provided by the sensors. 5) A huge number of redundant data are available.

72 3.1 Sensors

73 A GPS (Global positioning system) at the surface only. t 0 = 6000 s, l 0 =( o, o )±2.5m t f = s, l f =( o, o )±2.5m

74 A sonar (KLEIN 5400 side scan sonar). Gives the distance r between the robot to the detected object.

75 Screenshot of SonarPro

76 Detection of a mine using SonarPro

77 A Loch-Doppler. Returns the speed of the robotv r and the altitudeaof the robot±10cm.

78 A Gyrocompass(Octans III from IXSEA). Returns the roll φ, the pitchθ and the headψ. φ θ ψ φ θ ψ [ 1,1] [ 1,1] [ 1,1].

79 3.2 Data Foreachtimet {6000.0,6000.1,6000.2,..., }, we get intervals for φ(t),θ(t),ψ(t),v x r(t),v y r(t),v z r(t),a(t).

80 Six mines have been detected by the sonar: i τ(i) σ(i) r(i)

81 3.3 Constraints satisfaction problem t {6000.0,6000.1,6000.2,..., }, i {0,1,...,11}, px (t) p y (t) = cos l y (t) 180 π lx (t) l 0 x 0 l y (t) l 0 y, p(t) = (p x (t),p y (t),p z (t)), R ψ (t) = R θ (t) = cosψ(t) sinψ(t) 0 sin ψ(t) cos ψ(t) cosθ(t) 0 sinθ(t) sinθ(t) 0 cosθ(t),,

82 R ϕ (t) = cosϕ(t) sinϕ(t) 0 sinϕ(t) cosϕ(t) R(t) =R ψ (t).r θ (t).r ϕ (t), ṗ(t) =R(t).v r (t) m(σ(i)) p(τ(i)) =r(i),, R T (τ(i))(m(σ(i)) p(τ(i))) [0] [0, ] 2, m z (σ(i)) p z (τ(i)) a(τ(i)) [ 0.5,0.5].

83 3.4 GESMI

84 GESMI (Guaranteed Estimation of Sea Mines with Intervals)

85

86

87

88 Trajectory reconstructed by GESMI

89 4 SAUC ISSE

90 Robot SAUC ISSE

91 Portsmouth, July 12-15, 2007.

92

93

94

95

96

97 4.1 Localization with sonar

98

99 4.2 Set-membership approach

100 x(k+1) = fk (x(k),n(k)) y(k) = g k (x(k)), withn(k) N(k) andy(k) Y(k).

101 Without outliers X(k+1) =f k X(k) g 1 k (Y(k)), N(k).

102 4.3 Relaxed intersection

103

104 4.4 Robust localization

105 Define def f k:k (X) = X f k1 :k 2 +1(X) def = f k2 (f k1 :k 2 (X),N(k 2 )), k 1 k 2. The setf k1 :k 2 (X) represents the set of allx(k 2 ), consistent withx(k 1 ) X.

106 Consider the set state estimator X(k) = f 0:k (X(0)) ifk<m, (initialization step) X(k) = f k m:k (X(k m)) {q} f k i:k g 1 k i (Y(k i)) ifk m i {1,...,m}

107

108 4.5 Application to localization

109 Sauc isse robot inside a swimming pool

110 The robot evolution is described by ẋ 1 = x 4 cosx 3 ẋ 2 = x 4 sinx 3 ẋ 3 = u 2 u 1 ẋ 4 = u 1 +u 2 x 4, wherex 1,x 2 are the coordinates of the robot center,x 3 is its orientation and x 4 is its speed. The inputs u 1 and u 2 are the accelerations provided by the propellers.

111 The system can be discretized byx k+1 =f k (x k ), where, f k x 1 x 2 x 3 x 4 = x 1 +δ.x 4.cos(x 3 ) x 2 +δ.x 4.sin(x 3 ) x 3 +δ.(u 2 (k) u 1 (k)) x 4 +δ.(u 1 (k)+u 2 (k) x 4 )

112 Underwater robot moving inside a pool

113

114

115

116 5 Scout project Goal : (i) coordination of underwater robots ; (ii) collaborative behavior.

117 Supervisors: L. Jaulin, C. Aubry, S. Rohou, B. Zerr, J. Nicola, F. Le bars Compagny: RTsys (P. Raude) Students: G. Ricciardelli, L. Devigne, C. Guillemot, S. Pommier, T. Viravau, T. Le Mezo, B. Sultan, B. Moura, M. Fadlane, A. Bellaiche, T. Blanchard, U. Da rocha, G. Pinto, K. Machado.

118

119

120

121

122 5.1 Simulator

123 MOOS, MORSE, BLENDER, IBEX, Vibes, GIT

124 MOOS architecture

125 5.2 Controller

126

127 5.3 Localization Range only Based on interval analysis Robust with respect to outliers Distributed computation Low rate communication

128

129 Presentation of the scout project

130 5.4 Tests

131

132