FEL3330 Networked and Multi-Agent Control Systems. Lecture 11: Distributed Estimation

Transcription

1 FEL3330, Lecture 11 1 June 8, 2011 FEL3330 Networked and Multi-Agent Control Systems Lecture 11: Distributed Estimation Distributed Estimation Distributed Localization

2 where R X is the covariance of X. The proof is from the fact that P 1ˆx = H R 1 V y. FEL3330, Lecture 11 2 June 8, 2011 Distributed Estimation with a Central Node A random variable X being observed by n sensors: y i = H i x + v i where the noises v i are all uncorrelated with each other and with the variable X. Denote the estimate of x based on all the n measurements by ˆx and the estimate of x based only on the measurement y i by ˆx i. Then ˆx can be calculated using P 1ˆx = P 1 i ˆx i where P is the estimate error covariance corresponding to ˆx and P i is the error covariance corresponding to x i. Further P 1 = P 1 i (n 1)R 1 X

3 FEL3330, Lecture 11 3 June 8, 2011 Distributed Estimation Each node calculates the following: x i (k + 1) = x i (k) + hw 1 i j:jisconnectedtoi (x j (k) x i (k)) In our case, we let x i (0) to be the local estimate values and W i to be inverse of the local estimation error covariance, and obtain the required weighted sum. However, what if the measurements are not linear, and example is that they are distances.

4 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions

5 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions Other things needed:

6 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions Other things needed: In 2D, Positions of 3 Agents (non-collinear)

7 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions Other things needed: In 2D, Positions of 3 Agents (non-collinear) Agents with Known Positions: Anchors

8 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions Other things needed: In 2D, Positions of 3 Agents (non-collinear) Agents with Known Positions: Anchors A Formation Characteristic: Global Rigidity

9 FEL3330, Lecture 11 4 June 8, 2011 Localization Problem Known: Information between neighbour nodes, e.g. distances Goal: Nodes Positions Other things needed: In 2D, Positions of 3 Agents (non-collinear) Agents with Known Positions: Anchors A Formation Characteristic: Global Rigidity Problem to be addressed: Estimate the position of the nodes in a distributed way using only local measurements and information exchange.

10 P i R 2 P 0 FEL3330, Lecture 11 5 June 8, 2011 Barycentric Coordinate P 0 = π i P i π i 0 πi = 1

11 FEL3330, Lecture 11 6 June 8, 2011 Distributed Localization Algorithm A well-known barycentric coordinates for the points inside a convex polygon is called Wachspress coordinates, where A(P i 1, P i, P i+1 ) A(P l, P j, P j+1 ) π i = n A(P k 1, P k, P k+1 ) k=1 j i,i 1 j k,k 1 A(P l, P j, P j+1 ) (1) where A(P i, P j, P k ) is the signed area of the triangle P i P j P k.

12 FEL3330, Lecture 11 7 June 8, 2011 A(P i, P j, P k ) is the signed area of the triangle P i P j P k, i.e., A(P i, P j, P k ) = x i x j x k y i y j y k (2) An alternative method can be used to calculate A(P i, P j, P k ) using only the distances between the points: 0 dij 2 dik 2 1 A 2 (P i, P j, P k ) = 1 16 det dji 2 0 djk 2 1 dki 2 dkj (3)

13 FEL3330, Lecture 11 8 June 8, 2011 Distributed Localization Algorithm Assumption The neighborhood graph of sensor l, is complete and l lies in the convex hull of the neighbor nodes, V l. Proposition A network satisfying the abovementioned assumption with underlying graph G is globally rigid.

14 FEL3330, Lecture 11 9 June 8, 2011 Distributed Localization Algorithm The iterations are given by P l (k) l V P l (k + 1) = A π li Pi (k) l V (4) S i V l Let P(k)= [ P A (k) P S (k)] be the matrix obtained from stacking all the row vectors Then we have the following system P(t + 1) = ΥP(t) [ I 0 = B A ] P(t)

15 Distributed Localization Algorithm [ ] I 0 is stochastic and corresponds to an absorbing B A Markov chain. The estimate values P i converges to their real values if P S = (I A) 1 BP A. (P A denotes the position of the anchors, P S denotes the position of the sensors). Lemma The underlying Markov chain with the transition probability matrix given by the iteration matrix Υ is absorbing and [ lim P(k) = k I 0 (I A) 1 B 0 ] P(0). (5) Theorem The position estimate of each sensor l, P l (t), converges to the actual value Pl as t. FEL3330, Lecture June 8, 2011

16 FEL3330, Lecture June 8, 2011 Distributed Localization Algorithm P(t+1) = P 1 (t + 1) P 2 (t + 1) P 3 (t + 1) P 4 (t + 1) P 5(t + 1) P 6 (t + 1) P 7(t + 1) = a a 45 0 a a 54 0 a 56 a 57 0 a a 65 0 a a 73 a 74 0 a 76 0 P 1 (t) P 2 (t) P 3 (t) P 4 (t) P 5(t) P 6 (t) P 7(t)

17 FEL3330, Lecture June 8, 2011 Distributed Localization Algorithm We propose a continuous algorithm where the steady-state solution of it is equal to the steady-state solution of the aforementioned discrete time algorithm, i.e. the actual positions of the the sensors. The continuous-time algorithm is Υ c = Υ I Ṗ = Υ c P Theorem The position estimate of each sensor l, P l (t), converges to the actual value Pl as t.

18 FEL3330, Lecture June 8, 2011 Distributed Localization Algorithm We modify the system so that the estimate of positions of the sensors converge to their actual values in finite-time. for positive α and β. Ṗ = αυ c P + βsgn (Υ c P) Theorem The position estimate of each sensor l, P l (t), converges to the actual value Pl in finite time. In particular, P(t) = P for any t T where { Pi (0) P } i T = max (6) i β

19 Simulation Results FEL3330, Lecture June 8, 2011

20 FEL3330, Lecture June 8, 2011 Sensor Deployment Lemma A sufficient condition to triangulate sensor l is to have at least one node in each of the four disjoint equal area sectors Q i, which partition the circle of radius r l centered at l.

21 FEL3330, Lecture June 8, 2011 Convex Hull Membership Problems P i R 2 Consider the graph G(V, E).

22 FEL3330, Lecture June 8, 2011 Convex Hull Membership Problems Problem Consider a vertex l V. How can one determine if P l is inside the convex hull defined by the set of points {P i, i V \ {l}} using only distances P i P j, for all i, j V? Problem How can one find the p points from the set {P i } i V\{l} that are the vertices of the largest convex polygon encircling the whole set?

23 FEL3330, Lecture June 8, 2011 Distance Only Algorithm of Square Order Choose an arbitrary i V; Form ( ( V 1) 2) triangles, j, with i as a vertex; ) do for j = 1 to ( ( V 1 2) if l C( j ) then return True; end if end for return False;

24 Distance Only Algorithm of Square Order FEL3330, Lecture June 8, 2011

25 Distance Only Algorithm of Square Order FEL3330, Lecture June 8, 2011

26 Distance Only Algorithm of Square Order FEL3330, Lecture June 8, 2011

27 FEL3330, Lecture June 8, 2011 Distance Only Algorithm of Cubic Order C V \ {l} for i V \ {l} do Form ( ( V 2) 2) triangles, j, with i as a vertex; for k = 1 to ( ( V 2) 2) do if l C( j ) then C \ {i} break; end if end for return C; end for

28 Distance Only Algorithm of Square Order FEL3330, Lecture June 8, 2011

29 Distance Only Algorithm of Square Order FEL3330, Lecture June 8, 2011

30 FEL3330, Lecture June 8, 2011 Algorithm of Linear-logarithmic Order with Rough Localization Localize(realize) G for some 3 fixed points; % O( V ) Find a triangulation graph for {P i }; % O( V log V ) Label f faces of the graph j (f 2 V 4); for j = 1 to f do if l C( j ) then return True; end if end for return False;

31 Algorithm of Linear-logarithmic Order with Rough Localization FEL3330, Lecture June 8, 2011

32 Algorithm of Linear-logarithmic Order with Rough Localization FEL3330, Lecture June 8, 2011

33 Algorithm of Linear-logarithmic Order with Rough Localization FEL3330, Lecture June 8, 2011

34 Algorithm of Linear-logarithmic Order with Rough Localization FEL3330, Lecture June 8, 2011

35 FEL3330, Lecture June 8, 2011 Algorithm of Square Order with Rough Localization We use Gift-wrapping algorithm to find the convex hull.

36 FEL3330, Lecture June 8, 2011 Conclusions The dynamical version of the estimation algorithm proposed here can be constructed, usually more complicated. A distributed algorithm to achieve localization is proposed. The algorithm converges to the real values for positions of the sensors in finite-time. Some tests (in polynomial time) are proposed to check if a point is inside the convex hull of a set of points where the distances between every pair of the points are available.