2013 Aymeric Bethencourt et al., licensee Versita Sp. z o. o.

Μέγεθος: px

Εμφάνιση ξεκινά από τη σελίδα:

Download "2013 Aymeric Bethencourt et al., licensee Versita Sp. z o. o."

Ἀναξαγόρας Λευί Σκλαβούνος
6 χρόνια πριν
Προβολές:

1 Aymeric Bethencourt et al., licensee Versita Sp. z o. o. This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs license, which means that the text may be used for non-commercial purposes, provided credit is given to the author. Autonomous Underwater Vehicles Aymeric.Bethencourt@ensta-bretagne.fr Luc.Jaulin@ensta-bretagne.fr Kalman Filter Extended Kalman Filter Sequential Monte Carlo R R n tubes 233

2 i ẋ i = f(x i, u i ) + n x y i = g(x i ) (evolution equation) (observation equation) (1) x i i u i y i f g n x y i Formulation intertemporal relation ping p = (a, b, i, j) a R b R i {1,..., m} j {1,..., m} b > a (a, b) t p (k) k th t τ = h i (t) t τ i h i i {1,..., m} t R k {1,..., k max } (i) ẋ i (t) = f (x i (t), u i (t)) + n x (t) (ii) g ( x i(k) (a (k)), x j(k) (b (k)), a (k), b (k) ) = 0 (iii) ã (k) = h i(k) (a (k)) (iv) b (k) = h j(k) (b (k)) (v) ḣ i (t) = 1 + n h (t) (2) b(k) a(k) a(k) b(k) Simultaneous Localization and Mapping k t = a(k) t = b(k) c (b(k) a(k)) c a(k) b(k) h i n x (t) g : R n R n R R g (x 1, x 2, a, b) = x 1 x 2 c (b a) (3) c (x i(k) (a (k)) x j(k) (b (k))) 2 c (b(k) a(k)) = +(y i(k) (a (k)) y j(k) (b (k))) 2 (4) ã (k) i (k) k th b (k) j (k) k th n h (t) k ã (k), b (k), i (k), j (k). Z z Z z Z z x h lattice (E, ) x y join x y supremum meet x y 234

3 R n x y i {1,..., n}, x i y i. x y = (x 1 y 1,..., x n y n ) and x y = (x 1 y 1,..., x n y n ) x i y i = min (x i, y i ) x i y i = max (x i, y i ). E complete A E A A A E E E R R = R {, } = E = E (E 1, 1 ) (E 2, 2 ) (E, ) (a 1, a 2 ) E 1 E 2 (a 1, a 2 ) (b 1, b 2 ) ((a 1 1 b 1 ) and (a 2 2 b 2 )). interval [x] x x [x] = [x, x] = {x R, x x x} This representation has several advantages: It allows us to represent random variables with imprecise probability density functions, deal with uncertainties in a reliable way and last but foremost, it is possible to contract the interval around all feasible values given a set of constraints ( i.e. equations or inequalities). closed interval interval [x] E E [x] = {x E [x] x [x]}. E E E IE E [x] E [x] = [ [x], [x]] E. = [, ] R ; R = [, ] R ; [0, 1] R [0, ] R R {2, 3, 4, 5} = [2, 5] N N {4, 6, 8, 10} = [4, 10] 2N 2N intersection [x] [y] [x] [y] = { [max{x, y}, min{ x, ȳ}] if max{x, y} min{ x, ȳ} otherwise [1, 3] [2, 5] = [2, 3] union [x] [y] [x] [y] = [min{x, y}, max{ x, ȳ}] [1, 3] [5, 7] = [1, 7] We can apply arithmetic operators and functions to intervals to obtain all feasible values of the variable. For example, consider a real α [x] α[x] = { [αx, α x] [α x, αx ] if α 0 if α 0 [x] [y] {+,,, /} [x] [y] x y x [x] y [y] [x] [y] = [{x y R x [x], y [y]}] [x] + [y] = + y, x ȳ] [x [x] [y] = ȳ, x y] [x [x] [y] = [min{x y,x ȳ, xy, xȳ}, max{x y,x ȳ, xy, xȳ}] [1/ȳ, 1/y] 1/[y] = [1/ȳ, [ ], 1/ȳ] ], [ [x]/[y] = [x] (1/[y]) if [y] = [0, 0] if 0 / [y] if y = 0 and ȳ > 0 if y < 0 and ȳ = 0 if y < 0 and ȳ > 0 [ 1, 3] + [2, 7] = [1, 10] [ 1, 3] [2, 7] = [ 8, 1] [ 1, 3].[2, 7] = [ 7, 21] [ 1, 3]/[2, 7] = [ 1/2, 3/2] f([x]) f f([x]) = {f(x) x [x]} f f([x]) [f]([x]) = [{f(x) x [x]}] [x] [exp]([x]) = [exp x, exp x] [cos]([ π, π]) = [ 1, 1] [cos( π), cos(π)] = [ 1, 1] n x x i R, i {1,..., n f } n f f j (x 1, x 2,..., x nx ) = 0, j {1,..., n f } (5) 235 nauthent cated

4 f j x i X i [x i ] x = (x 1, x 2,..., x nx ) T x [x] = [x 1 ].[x 2 ]...[x nx ] f f j f(x) = 0 constraint satisfaction problem I I : (f(x) = 0, x [x]) S I S = {x [x] f(x) = 0} I [x] [x ] S [x ] [x] I [x] S I contractor constraint satisfaction problems I : (f(x) = 0, x [x]) f j (x 1, x 2,..., x nx ) = 0 f j +,,, /, sin, cos, exp,...etc d = (x i x j ) 2 + (y i y j ) 2 (6) i 1 = x j i 2 = x i + i 1 i 3 = i 2 2 i 4 = y j (7) i 5 = y i + i 4 i 6 = i 2 5 i 7 = i 3 + i 6 d = i 7 i k ] ; [ I d = i 7 d = i 7 i 7 = d 2 [d] = [d] [i 7 ] [i 7 ] = [i 7 ] [d 2 ] et s consider the constraint i 7 = i 3 + i 6 initial intervals [i 3 ] = [, 2], [i 6 ] = [, 3] and [i 7 ] = [4, ]. We can easily contract these intervals without removing any feasible value: i 7 = i 3 + i 6 i 7 [4, ] ([, 2] + [, 3]) = [4, ] [, 5] = [4, 5] i 3 = i 7 i 6 i 3 [, 2] ([4, ] [, 3]) = [, 2] [1, ] = [1, 2] i 6 = i 7 i 3 z [, 3] ([4, ] + [, 2]) = [, 3] [2, ] = [2, 3] We obtain much smaller intervals for [i 3 ] = [1, 2], [i 6 ] = [2, 3] and [i 7 ] = [4, 5]. tubes Tube F n R R n x y t R, x(t) y(t). tube [x] ( [x, x + ] F n x, x + t x (t) x + (t) F n IF n. x F n [x] t, x (t) [x] (t) x F 1 [x] x 236

5 Algorithm C FB (in : box, inout : [x], [ẋ]) // Forward steps 1 [i 1 ] := [x j ] 2 [i 2 ] := [x i ] + [i 1 ] 3 [i 3 ] := [i 2 2 ] 4 [i 4 ] := [ y j ] 5 [i 5 ] := [y i ] + [i 4 ] 6 [i 6 ] := [i 2 5 ] 7 [i 7 ] := [i 3 ] + [i 6 ] 8 [d] := [d] [i 7 ] // Bacward steps 9 [i 7 ] := [i 7 ] [d] 2 10 [i 3 ] := [i 3 ] ([i 7 ] [i 6 ]) 11 [i 6 ] := [i 6 ] ([i 7 ] [i 3 ]) 12 [i 5 ] := [i 5 ] (sqr 1 [i 6 ]) 13 [y i ] := [y i ] ([i 5 ] [i 4 ]) 14 [i 4 ] := [i 4 ] ([i 5 ] [y]) 15 [y j ] := [y j ] [i 4 ] 16 [i 2 ] := [i 2 ] (sqr 1 [i 3 ]) 17 [x i ] := [x i ] ([i 2 ] [i 1 ]) 18 [i 4 ] := [i 4 ] ([i 2 ] [x i ]) 19 [x j ] := [x j ] [i 1 ] [x] ([t]) [x] (t), t [t] x [x], t [t] x (t) [x] ([t]), [x] ([t]) Tube arithmetic F n t F n Integral., t 2 t 2 0 [x] [, t 2 ] t2 [x] (τ) dτ = { t2 } x (τ) dτ such that x [x]. t 2 t2 [ t2 t2 ] [x] (τ) dτ = x (τ) dτ, x + (τ) dτ. x [x] t2 x (τ) dτ t2 [x] (τ) dτ. interval primitive t [x] (τ) dτ 0 t = 0 Tube contractor L(x) L C L [x] [y] [x] [y] [x] R x Interval evaluation of a tube x R R n x F n x ([t]) = {x (t), t [t]}. [x] [x] ([t]) = t [t] [x] (t), ( t, [x](t) ) [y](t) (contraction) L(x) = x [y] (completeness) x [x] C minimal contractor L [y] L Minimal tube contractor L(x 1,..., x p ) x 1,..., x p IF n p x 1 = f 1 (x 2,..., x p ) x 2 = f 2 (x 1, x 3,..., x p )... Proposition 1. x y t [0, [, x(t) y(t) 237

6 x [x] y [y], C ( ) ( ) [x] [x, x + y + ] = [y] [x y, y + ] Proof. x y a y = x + a a 0 [y] = [y] ([x] + [a]) [y] = [y (x + a ), y + (x + + a + )] [y] = [y (x + 0), y + (x + + )] [y] = [y x, y + ] Proposition 3. t [0, [, x(t) = y(t T ) [a] = [0, [ x = y + a a 0 [x] = [x, x + y + ] Proposition 2. x = y t [0, [, x(t) = y(t) ( ) ( ) [x] [x y, x + y + ] C = = [y] [x y, x + y + ] Proof. x = y x y y [x] = [x] [y] [x] = [x y, x + y + ] C delay ( ) ( ) [x] [x (t) y (t τ), x + (t) y + (t τ)] = [y] [x (t + τ) y (t), x + (t + τ) y + (t)] Proof. [x (t) y (t τ), x + (t) y + (t τ)] j [x j ] [h j ] 238

] ([b (k))]) (after) [c (b(k) a (k))] [b(k)] ẋ i (t) = ( ) ui1 (t). cos(u i2 (t)) + n x (t) u i1 (t).

7 [x j ](t) = [x j ](t) [h j ] (t) = [h j ] (t) t 0 t 0 ([f]([u j (τ)],[x j (τ)]) + [n x (τ)])dτ (8) (1 + [n h ] (τ))dτ (9) t [x j ] [h j ] [x j ] [h j ] i j [x i ] [a(k)] i k j i j [a (k)] [b(k)] [x i(k) ](a (k)) [x j(k) ](b (k)) [y i(k) ](a (k)) [y j(k) ] (b (k)) x j(k) (b (k))) [x j(k) ] ([b (k))]) (before) [x i ] [x j(k) ] ([b (k))]) (after) [c (b(k) a (k))] [b(k)] ẋ i (t) = ( ) ui1 (t). cos(u i2 (t)) + n x (t) u i1 (t). sin(u i2 (t)) u i1 (t) u i2 (t) u i (t) i x i x i y i i n x (t) ( ) sin t x 1 (t) = 10 cos t ( ) sin(t + π) x 2 (t) = 10 cos(t + π) For AUV 1, n x (t) = 0.1t and n h (t) = 0.01t For AUV 2, n x (t) = t and n h (t) = 0.1t 239

8 [h](t) τ = h(t) (10, 0) T ( 10, 0) T c = 100 k = 0 t = 1 ã(0) = 1 a(0) [h 1 ] [a(0)] [x 1 ] [y 1 ] [x 1 ]([a(0)]) [y 1 ]([a(0)]) ã(0) [a(0)] [x 1 ]([a(0)]) [y 1 ]([a(0)]) 1.00 [0.99, 1.01] [5.21, 5.59] [8.26, 8.57] t = 1.37 b(0) = 1.37 b(0) [h 2 ] [b(0)] [x 2 ] [y 2 ] [x 2 ]([b(0)]) [y 2 ]([b(0)]) b(0) [b(0)] [x 2 ]([b(0)]) [y 2 ]([b(0)]) 1.37 [1.22, 1.51] [ 4.67, 0.90] [ 11.49, 8.16] b(0) [b(0)] [x 2 ]([b(0)]) [y 2 ]([b(0)]) 1.23 [1.22, 1.23] [ 4.67, 1.25] [ 11.49, 9.98] ( ) sin t 10 i {1, 2, 3}, x i (t) = 10 cos t ( ) sin t i {4, 5, 6}, x i (t) = 10 cos t 5m 10 i = 1, 2 3 x i (t) < 9m 4,

9 [h 4 ] [x 4 ] CISCREA u i

10 References IBEX Automatica Proc. CP, Constraint Programming Automatica IN PROC. 10TH INTERNATIONAL SYMPOSIUM ON EXPERIMENTAL ROBOTICS (ISER), RIO DE NOLCOS 2010 Proceedings of the International Conference on Logic Programming International Journal of Advanced Robotic Systems Méthodes ensemblistes pour le diagnostic, l estimation d état et la fusion de données temporelles. Artificial Intelligence and Symbolic Computation Artificial Intelligence Introduction to Lattices and Order ION GNSS Int. Symposium on Information Theory and its Applications IEEE Transaction on Robotics Springer-Verlag Ellipsoidal Calculus for Estimation and Control International Journal of Robotics Research Intelligent Robots and Systems, (IROS 2005)

11 IEEE/RSJ International Conference on IEEE Transactions on Automatic Systems Bounding Approaches to System Identification Methods and Applications of Interval Analysis LAP Academic Publishers, Kln, Germany Journal of the Association for Computing Machinery Probabilistic Robotics The Psychology of Computer Vision Journal of Marine Science and Application 243

$0SQ_\O [x1 ] [y1 ] b (0) [h2 ] a (0) [h2 ] [h1 ] [x2 ] [y2 ] [a(0)] [a(0)] [y1(0)]([a(0)]) [x1(0)]([a(0)]) [b(0)] [x2$

12 0SQ_\O [x1 ] [y1 ] b (0) [h2 ] a (0) [h2 ] [h1 ] [x2 ] [y2 ] [a(0)] [a(0)] [y1(0)]([a(0)]) [x1(0)]([a(0)]) [b(0)] [x2 ] [b(0)] [y2(0)]([b(0)]) 244 Unauthenticated [y2 ] [x2(0)]([b(0)]) :+6+.C8 4Y_\XKV YP,ORK`SY\KV <YLY^SM]

Σχετικά έγγραφα

Generating Set of the Complete Semigroups of Binary Relations

Applied Mathematics 06 7 98-07 Published Online January 06 in SciRes http://wwwscirporg/journal/am http://dxdoiorg/036/am067009 Generating Set of the Complete Semigroups of Binary Relations Yasha iasamidze