18.08.2013 Views

Show PDF File - robotics - Universidad de Zaragoza

Show PDF File - robotics - Universidad de Zaragoza

Show PDF File - robotics - Universidad de Zaragoza

SHOW MORE
SHOW LESS

You also want an ePaper? Increase the reach of your titles

YUMPU automatically turns print PDFs into web optimized ePapers that Google loves.

R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


R. Aragüés (1980, Zaragoza) received the M.S. degree in

system engineering and computer science from the

Universidad de Zaragoza, Zaragoza, Spain, in 2008, and

the Ph.D. degree with the Departamento de

Informática e Ingeniería de Sistemas in March 2012.

Her research interests include multirobot perception,

map merging, and distributed consensus in robotic

networks.

Distributed Algorithms on Robotic Networks for Coordination in Perception Tasks

Programa Oficial de Posgrado en Ingeniería Informática

Distributed Algorithms on Robotic Networks

for Coordination in Perception Tasks

PhD Thesis

Rosario Aragüés Muñoz

Departamento de Informática e Ingeniería de Sistemas (DIIS)

Instituto de Investigación en Ingeniería de Aragón

Escuela de Ingeniería y Arquitectura (EINA)

Universidad de Zaragoza


n

n






















Z p n M p n


n

n






















Z p n M p n


n

n






















Z p n M p n


n

n






















Z p n M p n


n

n






















Z p n M p n


y

1000

500

−500

2000

1000


0

z 0

−1000 −500 0 500 1000 1500

x

y

1000

500

2000

0

1000


z 0

−500

0

x

500

1000

y

1000

0

−1000

2000

1500

1000

500

z

0

−500

x

0

500

y

1000

0

−1000

3000

2000

1000

z 0

−1000

0

x

1000

1000

500

0

−500

−1000

−1000 −500 0 500 1000

x

















y

0 1000

2000

z






















k−






















k

k


k−






















k

k


k−






















k

k


























(x, y)













360◦






i ∈ N

j ∈ N xri = [xri , yri , θri ]T

i xri , yri ∈ R θri ∈ [−π, π]

i j

xj = [xj, yj] T xj = [xj, yj, θj, ρj] T

xj, yj ∈ R θj ∈ [−π, π] ρj ∈ R≥0 x

xr n

x = [xr, x1...xn] T ,


P i

j zij zj




x



(x, y)






xj = [xj, yj] T xr = [xr, yr, θr] T



yr − yj

zj = h(xr, xj) = arctan

− θr.

xr − xj

xj

(xj, yj) θj

1/ρj mj Rr


cos θj

mj = ,

sin θj


cos θr

Rr =

− sin θr

sin θr

cos θr

j

xj = [xj, yj, θj, ρj] T


[xj, yj] T + 1/ρj mj,

hj = atan2(h y

j , hx j ),

h xy

j = [hx j , h y

j ]T j xr =

[xr, yr, θr] T

h xy

j =


x hj h y

j


= Rr

xj

yj


+ 1/ρj mj −

xr

yr


.


.

ρi > 0


h xy

j =


x hj h y


xi xr

= Rr ρi − + mi .

j

yi yr


ρi = 0












xr1 xr2

xr1 z1j, z2j j xr1

xr2 α α = z1j − z2j dp dt xr1 xr2

z1j

dt dj xr1 j


dj = dp/ tan α.



(0/0) dp = 0 α = 0 + kπ

k ∈ Z

dt

α = 0 + kπ k ∈ Z dp


xr1 xr2

xr1 z1j, z2j j xr1

xr2 θr2

θr2 = z1i − z2i.


z1i − z2i dp < ∞






















dmin


j

xr zj

ρ0 xj

dmin

ρmin = 1

dmin

, ρ0 = ρmin

2 , σρ = ρmin

4 ,


ρmin ρ0

[0, ρmin] σρ ρ0

95% ρ [ρ0 − 2σρ, ρ0 + 2σρ] = [0, ρmin]


xj = gj(xr, zj, ρ0) = [xr, yr, θr + zj, ρ0] T .



z1j z2j j

xr1 xr2 xj

z2j ρ0

xj = g(xr1, xr2, z1j, z2j) = [xr2, yr2, θr2 + z2i, ρ0] T , ρ0 =

ci si i ∈ {1, . . . , 2}

s2 c1 − c2 s1

c1 (yr1 − yr2) − s1 (xr1 − xr2) ,

ci = cos(θi + zij), si = sin(θi + zij).





z1j, z2j j xr1, xr2

xj

xj = g x j (xr1, xr2, z1j, z2j) = xr1 s1 c2 − xr2 s2 c1 − (yr1 − yr2) c1 c2

,

yj = g y

s1 c2 − s2 c1

j (xr1, xr2, z1j, z2j) = yr2 s1 c2 − yr1 s2 c1 + (xr1 − xr2) s1 s2

,

s1 c2 − s2 c1

ci si i ∈ {1, . . . , 2}


xri i = 1, 2 zij

j

αij

αij = θri + zij,

i = 1, 2 Sαij αij


(α1j − α2j) 2

Sα1j + Sα2j

> χ 2 0.99,1d.o.f.








xri i = 1, 2 j

αij Sαij


θt

xr1 xr2


yr2 − yr1

θt = arctan

,

xr2 − xr1

Sθt


(α1j − α2j) 2

Sα1j + Sα2j

≤ χ 2 0.99,1d.o.f,


(θt − αij) 2

Sθt + Sαij

≤ χ 2 0.99,1d.o.f,

j = 1, 2


90 ◦


st






st

st



xr 0.01 st 0.03 st 0.03 st

yr 0.01 st 0.03 st 0.03 st

θr 2 ◦ 2.5 ◦ 2.5 ◦


0.125 0.250 0.5 1



(xr, yr, θr)


(a − a) 2

Pa

≤ χ 2 0.99,1,

a xr yr θr a Pa

xr yr θr


j

xj yj

|a − a|


+

Pa χ 2 0.99,1

≤ 1.5

a a Pa

xj yj


xr yr θr












dmin = 0.5m


% final divergence

number of steps to initialize features

100

80

60

40

20

0

4

3.5

3

2.5

2

1.5

1

Final divergence

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1


Feature initialization step

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1


% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

Trajectory divergence

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1

Map consistency per step

xy−d

xy−f

xy−l

0.1250.25 0.5

step size

1


x


% final divergence

number of steps to initialize features

100

80

60

40

20


0

4

3.5

3

2.5

2

1.5

1

Final divergence

id−d

id−f

id−l

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

id−d

id−f

id−l

0.1250.25 0.5

step size

1


Feature initialization step

id−d

id−f

id−l

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

0.1250.25 0.5

step size

1


id−d

id−f

id−l

% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

Trajectory divergence

id−d

id−f

id−l

0.1250.25 0.5

step size

1

Map consistency per step

id−d

id−f

id−l

0.1250.25 0.5

step size

1


x










dmin = 0.5m





0.125


14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (two observations)

F36

F37

F28

F42

F43

F27

F38

F13

F44

F7

F8 F9

F18

F10

F11

F39

F12

F1 F2

F26

F34

F33

F32 F31

F40

F35

F41

F3 F4

F29

F30

F22

F20 F23

F14

F5 F6

−5 0 5 10 15

F25

F24

F21

F19

F17

F16

F15

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed Feature not aligned

F28 F27 F26 F25 F24 F23

F34

F17

F33

F40

F32 F31 F30 F29

F22

F35

F36

F37

F38

F41

F42

F43

F44

F7 F8 F9 F10

F18

F11

F39

F12

F1 F2 F3 F4 F5 F6

−5 0 5 10 15

F21

F20

F19

F16

F15

F14

F13

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (Finite depth)

F36

F37

F42

F43

F38

F44

F8

F7

F39

F1 F2

F3

−5 0 5 10 15


F34

F35

F28

F27

F33

F40

F41

F32

F26

F31

F25

F4

F24

F17

F29

F30

F22

F16

F5

F21

F20

F19

F23

F15

F14

F13

F18

F9 F10

F11

F12



% final divergence

number of steps to initialize features

100

4

3

2

1

0

80

60

40

20

0

Final divergence

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


Feature initialization step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

0.1250.25 0.5

step size

1

F6

Trajectory divergence

id−l

id−u

xy−f

Map consistency per step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

x

dmin = 0.5m


14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (two observations)

F36

F37

F28

F42

F43

F27

F38

F13

F44

F7

F8 F9

F18

F10

F11

F39

F12

F1 F2

F26

F34

F33

F32 F31

F40

F35

F41

F3 F4

F29

F30

F22

F20 F23

F14

F5 F6

−5 0 5 10 15

F25

F24

F21

F19

F17

F16

F15

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed Feature not aligned

F28 F27 F26 F25 F24 F23

F34

F17

F33

F40

F32 F31 F30 F29

F22

F35

F36

F37

F38

F41

F42

F43

F44

F7 F8 F9 F10

F18

F11

F39

F12

F1 F2 F3 F4 F5 F6

−5 0 5 10 15

F21

F20

F19

F16

F15

F14

F13

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (Finite depth)

F36

F37

F42

F43

F38

F44

F8

F7

F39

F1 F2

F3

−5 0 5 10 15


F34

F35

F28

F27

F33

F40

F41

F32

F26

F31

F25

F4

F24

F17

F29

F30

F22

F16

F5

F21

F20

F19

F23

F15

F14

F13

F18

F9 F10

F11

F12



% final divergence

number of steps to initialize features

100

4

3

2

1

0

80

60

40

20

0

Final divergence

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


Feature initialization step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

0.1250.25 0.5

step size

1

F6

Trajectory divergence

id−l

id−u

xy−f

Map consistency per step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

x

dmin = 0.5m


14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (two observations)

F36

F37

F28

F42

F43

F27

F38

F13

F44

F7

F8 F9

F18

F10

F11

F39

F12

F1 F2

F26

F34

F33

F32 F31

F40

F35

F41

F3 F4

F29

F30

F22

F20 F23

F14

F5 F6

−5 0 5 10 15

F25

F24

F21

F19

F17

F16

F15

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed Feature not aligned

F28 F27 F26 F25 F24 F23

F34

F17

F33

F40

F32 F31 F30 F29

F22

F35

F36

F37

F38

F41

F42

F43

F44

F7 F8 F9 F10

F18

F11

F39

F12

F1 F2 F3 F4 F5 F6

−5 0 5 10 15

F21

F20

F19

F16

F15

F14

F13

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (Finite depth)

F36

F37

F42

F43

F38

F44

F8

F7

F39

F1 F2

F3

−5 0 5 10 15


F34

F35

F28

F27

F33

F40

F41

F32

F26

F31

F25

F4

F24

F17

F29

F30

F22

F16

F5

F21

F20

F19

F23

F15

F14

F13

F18

F9 F10

F11

F12



% final divergence

number of steps to initialize features

100

4

3

2

1

0

80

60

40

20

0

Final divergence

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


Feature initialization step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

0.1250.25 0.5

step size

1

F6

Trajectory divergence

id−l

id−u

xy−f

Map consistency per step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

x

dmin = 0.5m


14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (two observations)

F36

F37

F28

F42

F43

F27

F38

F13

F44

F7

F8 F9

F18

F10

F11

F39

F12

F1 F2

F26

F34

F33

F32 F31

F40

F35

F41

F3 F4

F29

F30

F22

F20 F23

F14

F5 F6

−5 0 5 10 15

F25

F24

F21

F19

F17

F16

F15

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed Feature not aligned

F28 F27 F26 F25 F24 F23

F34

F17

F33

F40

F32 F31 F30 F29

F22

F35

F36

F37

F38

F41

F42

F43

F44

F7 F8 F9 F10

F18

F11

F39

F12

F1 F2 F3 F4 F5 F6

−5 0 5 10 15

F21

F20

F19

F16

F15

F14

F13

14

12

10

8

6

4

2

0

−2

−4

FINAL MAP: INVERSE−DEPTH Delayed (Finite depth)

F36

F37

F42

F43

F38

F44

F8

F7

F39

F1 F2

F3

−5 0 5 10 15


F34

F35

F28

F27

F33

F40

F41

F32

F26

F31

F25

F4

F24

F17

F29

F30

F22

F16

F5

F21

F20

F19

F23

F15

F14

F13

F18

F9 F10

F11

F12



% final divergence

number of steps to initialize features

100

4

3

2

1

0

80

60

40

20

0

Final divergence

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% consistent features

100

80

60

40

20

0

Map consistency

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


Feature initialization step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

% features used per step

100

80

60

40

20

0

Feature usage

id−l

id−u

xy−f

0.1250.25 0.5

step size

1


% steps divergence

% consistent features per step

100

80

60

40

20

0

100

80

60

40

20

0

0.1250.25 0.5

step size

1

F6

Trajectory divergence

id−l

id−u

xy−f

Map consistency per step

id−l

id−u

xy−f

0.1250.25 0.5

step size

1

x

dmin = 0.5m


i, i ′ , j, j ′

r, r ′ , s, s ′

G

t t ∈ N

f i r

Ar,s

Aij

[Aij]r,s

Mi

MG

r i

(r, s) A

(i, j) A

(r, s) Aij

i


n ∈ N G =

(V, E) V = {1, . . . , n}

i j (i, j) ∈ E

Ni i

Ni = {j | (i, j) ∈ E, j = i}.


i mi ∈ N

i

Mi = + mi

ˆxi ∈ R Mi Σi ∈ R Mi×Mi

= 3

(x, y) θ = 2 = 3

xi ∈ R Mi

mi

ˆxi = xi + vi,

vi Σi


m ∈ N

i ∈ {1, . . . , n} mi ≤ m x ∈ R MG

n m MG = n +m

Hi ∈ {0, 1} Mi×MG x

i 0 1

1 xi = Hix

ˆxi = Hix + vi,

vi Σi


T ˆx 1 . . . ˆx T T T

n = H1 . . . H T T T

n x + v1 . . . v T T n .

vi

E[ vT 1 . . . vT T

T

n v1 . . . vT

n ] = diag (Σ1, . . . , Σn)

i Ii ∈

RMG×MG ii ∈ RMG Ii = H T i Σ −1

i Hi, ii = H T i Σ −1

i ˆxi.

n


IG =

n

Ii, iG =

i=1

n

ii,



i=1

ˆxG = (IG) −1 iG, ΣG = (IG) −1 .


i Îi G (t) ∈ RMG×MG

îi G (t) ∈ RMG

Î i G(0) = H T i Σ −1

i Hi, î i G(0) = H T i Σ −1

i ˆxi,

t ≥ 0

Î i G(t + 1) =

n

j=1

Wi,j Îj

G (t), îi G(t + 1) =

n

j=1

Wi,jîj G (t),

ˆxi Σi i Wi,j G


G


lim Î

t→∞

i G(t) = IG/n, lim î

t→∞

i G(t) = iG/n,

IG iG

ˆx i G (t) ∈ RMG

ˆ Σi G (t) ∈ RMG×MG i ∈ V t ≥ 0

ˆx i −1 −1 i i

G(t) = ÎG(t) îG(t), Σˆ i i

G(t) = ÎG(t) ,


lim

t→∞ ˆxi G(t) = ˆxG, lim ˆΣ

t→∞

i G(t) = nΣG,

ˆxG ΣG ˆx i G (t)

ˆx i G (t) i ∈ {1, . . . , n}


x E [ˆx i G (t)] = x t ≥ 0

t ≥ 0

Q i G (t) Qi G (t) ˆ Σ i G (t)


Wi,j = 0 j /∈ Ni


G(t) = (V, E(t))

i t → ∞


i Hi

Hi

i


i Li




i ∈ V

Li = {L i 1, . . . , L i mi } mi f i r f j s

i j L i r = (i⋆, r⋆) L j s = (i ′ ⋆, r ′ ⋆)


i⋆ = i ′ ⋆ and r⋆ = r ′ ⋆.


Lj Hi

Li

Li Li L i r = (i⋆, r⋆)

i⋆ r⋆ i

Li Li



|Li| Li

Li Lj Li ⊆ Lj Li Lj Γ

Li Lj Lij

Li Lj

Γ

(i) Γ(Li, Lj) = Γ(Lj, Li);

(ii) Γ(Li, Li) = Li;

(iii) Γ(Li, Γ(Lj, Lk)) = Γ(Γ(Li, Lj), Lk);

(iv) Li, Lj ⊆ Γ(Li, Lj);


Li, Lj, Lj ′

H

Li Lj Li ⊆ Lj H(Li, Lj) Hi j ∈ {0, 1} |Li| ×|Lj|

Li

Lj H Li ⊆ Lj ⊆ Lj ′ H(Li, Lj) = Hi j

H(Lj, Lj ′) = Hj

j ′ Hi jH j

j ′ = Hi j ′ = H(Li, Lj ′) H

H(Li, Li) = I

Lj j = 1, . . . , n LG

m L1, . . . , Ln

LG = Γ(Γ(. . . Γ(L1, L2), . . . ), Ln).

LG Γ

Li Li i ∈ {1, . . . , n}

Hi H(Li, LG)


LG Hi


LG i

Li

LG i

t














Hi


Lchild i Li

Lchild j j i

Lchild i = Li

i Lchild j1 , . . . , Lchild j

|childi |

childi Li

L child

i

= Γ(Γ(Γ(. . . Γ(L child

j1 , L child

j2 ), . . . ), L child

j |childi | ), Li) i


Lchild i

LG = Lchild root


LG


LG












LG i Hi Li

LG Hi = H(Li, LG) Hi


Îi G (t), îiG (t)


i Li

LG i ∈ {1, . . . , n}

Li(t) t

i i G (t) ∈ RMi(t) I i G (t) ∈ R Mi(t)×Mi(t)

Mi(t) t Mi(t) = |Li(t)| t = 0


i

Li(0) = Li, I i G(0) = Σ −1

i , i i G(0) = Σ −1

i ˆxi.

t Li(t + 1)

Lj(t) j ∈ Ni ∪ {i}

Li(t + 1) = Γ(Γ(. . . Γ(Lj1(t), Lj2(t)), . . . ), LjN (t)),

i +1

{j1, . . . , jNi+1} = Ni ∪ {i} Ii G (t) iiG (t)


I i G(t + 1) =

n

j=1

H j,t

i,t+1 = H(Lj(t), Li(t + 1))

Wi,j(H j,t

i,t+1 )T I j

G (t)Hj,t

i,t+1 , ii G(t + 1) =

n

j=1

Wi,j(H j,t

i,t+1 )T i j

G (t),


t ≥ 0 i ∈ {1, . . . , n}

Î i G(t) = (H i,t

G )T I i G(t)H i,t

G , îi G(t) = (H i,t

G )T i i G(t),

H i,t

G = H(Li(t), LG) diam(G)

t ≥ 0 i ∈ {1, . . . , n}

Î i G(diag(G) + t) = I i G(diag(G) + t), î i G(diag(G) + t) = i i G(diag(G) + t).

Ii G (t)

ii G (t) t = 0


Î i G(0) = H T i Σ −1

i Hi = (H i,0

G )T I i G(0)H i,0

,

i ∈ {1, . . . , n} t i ∈ {1, . . . , n}

Î i G(t + 1) =

n

j=1

= (H i,t+1

G ) T

Wi,jÎj G (t) =

n

j=1

n

j=1

Wi,j(H j,t

G )T I j

G (t)Hj,t G

(H j,t

i,t+1 )T I j

G (t)Hj,t

i,t+1


H i,t+1

G

G

= (Hi,t+1

G

t ≥ 0 i, j ∈ {1, . . . , n} H j,t

G

) T I i G(t + 1)H i,t+1

G ,


= Hj,t

i,t+1 Hi,t+1

G

n

j=1 Wi,j = 1

diam(G) i ∈ {1, . . . , n}

Lj

j = 1, . . . , n t ≥ 0 Li(diam(G)+t) = LG LG

= H(LG, LG) = I

H i,diam(G)+t

G


Hi

Ii G (t) t ≥ 0 i ∈ {1, . . . , n}


Îi G (t)

i

i

t = diam(G)



Ii G (t) t ≥ 0 i ∈ {1, . . . , n}



xi G (t) ∈ RMi(t) i ΣG (t) ∈ RMi(t)×Mi(t) i



x i G(t) = I i G(t) −1 i i G(t), Σ i G(t) = I i G(t) −1 .

i

t → ∞


t

ˆx i G (t)

E ˆx i G(t) = x,

t ≥ 0 i ∈ V Îi G (t)


Qi G (t) ∈ RMG×MG ˆx i G (t)

ˆ Σi G (t)

ˆx i

G(t) − x ˆx i G(t) − x T

Q i G(t) = E

=

−1 i

ÎG(t) n

j=1

([W t ]i,j) 2 H T j Σ −1

j Hj


Î −1 i

G(t) ,

t ≥ 0 i ∈ V Îi G (t) 0 ≤ [W t ]i,j ≤ 1

i, j ∈ V t ≥ 0 ([W t ]i,j) 2 ≤ [W t ]i,j

Q i G(t)

−1 i

ÎG(t) = ˆ Σ i G(t),


ˆ Σi G (t)−Qi G (t)

Qi G (t)

ˆ Σi G (t) ˆ Σi G (t) − QiG (t)

[ ˆ Σi G (t) − QiG (t)]r,r ≥ 0

[ ˆ Σi G (t)]r,r ≥ [Qi G (t)]r,r


ˆΣ i G (t)




Mmax


Mmax = max

i∈{1,...,n} Mi,

dmax

dmax = max

i∈{1,...,n} |Ni|.

G(t) = (V, E(t)) dmax

dmax = max |Ni(t)|.

i∈{1,...,n},t≥0



O(M3 max)

O(dmaxM2 G )



i Ii G (t)

O(dmaxM2 G ) Ii G (t)


(t)

Ii G

O(nM2 max) O(n + m)


O(dmaxnM2 max) O(dmax(n + m))

O(dmaxm)



I i G (t) O(nM2 max) O(n + m)


|λ2(W )|

γ = |λ2(W )|

[Ii G (t)]r,s [ii G (t)]r


|[I i G(t)]r,s − [IG]r,s| ≤ (γ) t√

n max [Ij ,

|[i i G(t)]r − [iG]r| ≤ (γ) t√ n max

j

j

G (0)]r,s − [IG]r,s


[ij G (0)]r


− [iG]r

,

i ∈ {1, . . . , n} r, s ∈ {1, . . . , Mi(t)} t ≥ 0

G(t) = (V, E(t))

τ

{G(t0 + 1), . . . G(t0 + τ)}

τ t0 τ

δ < 1

δ = max {|λ2(W)| | W }

W∈Wτ

Wτ τ W (t)

[Ii G (t)]r,s

[ii G (t)]r

τ

|[I i t


G(t)]r,s − [IG]r,s| ≤ (δ)

|[i i t


G(t)]r − [iG]r| ≤ (δ)

τ √

n max [Ij G

j

(0)]r,s


− [IG]r,s

,


[ij G (0)]r


− [iG]r

,

τ √ n max

j

t

t

τ τ




W = (1/n)11T |λ2(W )| = 0

|[Ii G (t)]r,s − [IG]r,s| ≤ 0 |[ii G (t)]r − [iG]r| ≤ 0 t ≥ 1 i ∈ {1, . . . , n}

r, s ∈ {1, . . . , Mi(t)}


10

8

6

4

2

0

−2

−4

−6

−8

−10

10

8

6

4

2

0

−2

−4

−6

−8

−10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10


−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10


−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10


9

t = 1 t = 10

t → ∞

10

8

6

4

2

0

−2

−4

−6

−8

−10

Robot 7. t= 1

−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

Robot 7. t= 10


−10 −5 0 5 10

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −5 0 5 10

t = 1 t = 10 t → ∞

t = 1

t = 10

t → ∞

7

20 × 20 m

70 30 m


σx, σy = 0.4 cm σθ = 1


6 m

0.5






xi G (t), ΣiG (t)


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

20 × 20 m





1.5

1

0.5

0

−0.5

−1

−1.5

−2

R5

R1

R4

R3

R2

R7

R6

5 5.5 6 6.5 7 7.5 8 8.5 9

G


3 m

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


n i, j r, s

G k ∈ N

t ∈ N I n × n

0 n × n

n1 × n2 1 ∈ Rn

W [W ]i,j (i, j) λi(W )

i− vi(W ) λ(W )


n ∈ N

k i mk i ∈ N



i ∈ {1, . . . , n} k

ˆx k i ∈ RMk i Σk i ∈ RMk i ×Mk i Mk i = + mk i

xk i ∈ RMk i mk i

ˆx k i = x k i + v k i ,

v k i Σ k i

k n

n

k m ∈ N

x ∈ R MG n

m MG = n + m i ∈ {1, . . . , n} k

m k i ≤ m H k i ∈ {0, 1} Mk i ×MG

x x k i x k i = H k i x i

x

ˆx k i = H k i x + v k i ,

= 3 (x, y) θ = 2

= 3


obot

feat feat.

robot feat. feat

n

I k G ik G Ik i i k i i ∈

{1, . . . , n}

i


j = i

v k i , v k′

j i = j

k, k ′ ∈ N

i k

k ′ < k v k i , v k′

i I k i ∈ R MG×MG

i k i ∈ R MG i k


I k i = (H k i ) T (Σ k i ) −1 H k i , i k i = (H k i ) T (Σ k i ) −1 ˆx k i ,

i ∈ {1, . . . , n} Ik G ikG k


I k G =

n

I k i , i k G =

i=1

n

i k i .


k


ˆx k G = (I k G) −1 i k G, Σ k G = (I k G) −1 .

(r, s) r

I k i i k i i

R ⊆ {1, . . . , n}

(r, s) r [I k G ]r,s

[i k G ]r [I k i ]r,s [i k i ]r i ∈ R

i=1

[I k G]r,s =

[I k i ]r,s, [i k G]r =

[i k i ]r.

i∈R

i∈R


i [I k j ]r,s [i k j ]r

j ∈ R i i

j = i (r, s) r [I k j ]r,s = 0

[i k j ]r = 0 i

[I k G]r,s = [I k i ]r,s, [i k G]r = [i k i ]r,



I k i i k i j i = j




n ∈ N

k = 1, 2, . . .

Gk = (V, E k )

k V = {1, . . . , n}

i j (i, j) ∈ E k

N k

i i k

N k

i = {j | (i, j) ∈ E k , j = i}.

i ∈ V

N k

i





i ∈ {1, . . . , n} ui ∈ R

xi(t), wi(t) ∈ R

˙xi(t) = −γ xi(t) −

[WP ]i,j [xi(t) − xj(t)] +

[WI]j,i [wi(t) − wj(t)] + γ ui,

j=i

˙wi(t) = −

[WI]i,j [xi(t) − xj(t)] ,

j=i

WP WI

[WP ]i,j = [WP ]j,i = [WI]i,j =

[WI]j,i = 0 i j j /∈ Ni γ > 0




j=i


n u ∈ R n =

(u1, . . . , un) T x ∈ R n = (x1, . . . , xn) T w ∈ R n = (w1, . . . , wn) T



˙x(t)

=

˙w(t) −LI

−γI − LP L T I

0

x(t)

w(t)


γI

+

0


u,

LP LI WP

WI

LP = diag(WP 1) − WP , LI = diag(WI1) − WI.

ex(t) ∈ R n

ex(t) = x(t) − 11T

n u,

Π Π = I − 11T i ∈ {1, . . . , n}

n

γ LP LI

rank(LI) = n − 1,

ε ∈ R is such that Π(LP + L T P )Π 2εΠ,

γ > 0 is chosen such that γ + ε > 0,

u x(0), w(0) ex(t)

0 t → ∞

xi(t) i ∈ V


i xi(t), wi(t)

[WP ]i, j = [WI]i, j = 0 j /∈ Ni


WM ∈ R n×n


WM [WM]i,j = [WM]j,i = 0 i j

WM

WM = W T M WM1 = 1 1 T WM = 1 T 1

λ(WM) ∈ (−1, 1)

LM = diag(WM1) − WM = I − WM,

0

λ(LM) ∈ (0, 2) LM

ε = 0 Π(LM + L T M )Π = 2LM LM

LM γ > 0


WP = WI = WM


LW ∈ R n LP = LI = LW L T W = LW W


LW = diag(W 1) − W

W = WM

λi(A) i A vi(A)

λi LW r = 1 √ 1 LW

n

0 [r S2 . . . Sn] = [r S] LW



k i ∈ V

ui ∈ R xG ∈ R ui

xavg ∈ R

xG =

n

i=1

ui, xavg = 1

n

n

i=1

ui = 1

n xG.

i ∈ V xi(t) ∈ R xavg

Ni








LW ∈ R n LP = LI = LW

L T W = LW W LW = diag(W 1) − W

h

γ

W = WM

LW = LM

r ∈ R n LW λ1(LW ) = 0

r = 1/ √ n.

S2, . . . , Sn n − 1 LW [r S2 . . . Sn] = [r S]

LW

[r S] T LW [r S] = diag (λ1(LW ), . . . , λn(LW )) ,

λ1(LW ) ≤, . . . , ≤ λn(LW )

LW

λ2(LW ), . . . , λn(LW ) L (−1)

W

L (−1)

W = (I − rrT ) LW + rr T −1 (I − rr T ).


i ∈ V bi = b(λi(LW ))


bi = b(λi(LW )) = (γ + λi(LW )) 2 − (2 λi(LW )) 2 .


LW h > 0


x(t + 1) x(t) hγI

= A + u, with

w(t + 1) w(t) 0


−γI − µLW LW

A = I2n×2n + h

−LW 0


.

µ > 0

µ = 1

h γ


h γ




xi(t) → xavg t → ∞

x(t) → 1xavg x∗ w∗ ∈ R n

r L (−1)

W

x∗ = rr T u = 1xavg, w∗ = rr T w(0) − γL (−1)

W u,



A

LW


λi(LW ) LW

λi(A) λn+i(A) A


λi(A) = 1 − h (γ + λi(LW ) + bi) /2, λn+i(A) = 1 − h (γ + λi(LW ) − bi) /2,

i ∈ V bi λ1(LW ) = 0 λ1(A) =

1 − hγ λn+1(A) = 1

−γI − LW LW


Z =

, A = I2n×2n + hZ

−LW 0

A Z i ∈ {1, . . . , 2n}

λi(A) = 1 + hλi(Z), vi(A) = vi(Z).


Y = P T ZP


r S2 . . . Sn 0

P =

0 r S2 . . . Sn


,

[r S2 . . . Sn] LW

Z Y

λi(Z) = λi(Y ), vi(Z) = P vi(Y ).

Y


T −γI − [r S] LW [r S] [r S]

Y =

T LW [r S]

−[r S] T LW [r S] 0



diag(−γ − λ1, . . . , −γ − λn) diag(λ1, . . . , λn)

Y =

−diag(λ1, . . . , λn) 0


,

λi = λi(LW ) (LW ) Y vi(Y ) =

λi(Y )vi(Y ) Y i ∈ V

λi(Y ) = − γ + λi + bi

2

, λn+i(Y ) = − γ + λi − bi

.

2

vi(Y ) vn+i(Y ) i−

(n + i)−

[vi(Y )]i = 1, [vi(Y )]n+i = −λi(LW )/λi(Y ),

[vn+i(Y )]i = 1, [vn+i(Y )]n+i = −λi(LW )/λn+i(Y ).

A


h γ


,

γ ≥ 3/2 λn(LW ),

hγ < 3/2,

λn(LW ) LW A

λn+1(A) = 1


A LW

λi(LW ) LW

γ h λi(A) ±Im [bi]

0 A


λi > 0 i ∈

{2, . . . , n} bi γ ≤ bi ≤ 2/ √ 3γ

λi(Y ) λn+i(Y ) −4/3 γ ≤ λi(Y ) < −γ

−1/3 γ ≤ λn+i(Y ) < 0 i ∈ {2, . . . , n}

λ1(A) = 1 − hγ 1 h > 0

γ > 0 −1/2

1 i ∈ {2, . . . n} λi(A) = 1 + hλi(Y ) < 1 λn+i(A) = 1 + hλn+i(Y ) <

1 h > 0 λi(Y ) < 0, λn+i(Y ) < 0 λi(A) ≥ 1 − 4/3hγ > −1

λn+i(A) ≥ 1 − 1/3hγ > 1/2 λi(A) λn+i(A)

1 λn+1(A) = 1

γ ≥ 3 h < 3/(2γ)

LM

λ1(LM) = 0 0 < λi(LM) < 2 i ∈ {2, . . . , n}

λeff(A) A

λn+i(A) ≥ 1/2

λn+i(A) i ∈ {2, . . . , n} λn+2(A) λi(A)

i λi(A) ≥ 0 λn+i(A)

i λi(A) < 0

λn(A) λeff(A) = max{λn+2(A), −λn(A)}



LW

G

h > 0 γ > 0

u ∈ R n x(0) ∈ R n w(0) ∈ R n

x(t) ∈ R n w(t) ∈ R n

lim

t→∞ x(t) = x∗, lim w(t) = w∗,

t→∞

t → ∞ x∗ w∗ β = 2 √ 10/3

exw(t) = x(t) T , w(t) T T

T − x∗ , wT T

∗ t

exw(t) 2 ≤ βλ t eff(A) exw(0) 2 .


h γ

|λn+1(A)| = 1 |λi(A)| < 1

λeff(A) λt eff (A) 0 t → ∞

exw(t)2

t


exw(t)

exw(t + 1) = A exw(t), exw(t) = A t exw(0).


C = P T AP = I + hY,

P Y ezy


ezy(t) = P T exw(t), exw(t) = P ezy(t),

exw(t) 2 = ezy(t) 2


ezy(t + 1) = C ezy(t), ezy(t) = C t ezy(0),

ezy(0) = (z(0) T , y(0) T ) T − (z T ∗ , y T ∗ ) T



T r u

z∗ =

0n−1



, y∗ =

r T w(0)

−γ(S T LW S) −1 S T u


,

r T w(0) = [y(0)]1 [ezy(0)]n+1


C

ezy(0) =

2n

i=1

i ∈ {1, . . . , 2n}

ai vi(C),

λi(C) = λi(A), and vi(C) = vi(Y ),

λi(A) vi(C)

ai an+i [ezy(0)]i [ezy(0)]n+i

ezy(0) ai an+i i ∈ V

[ezy(0)]i = ai + an+i,

[ezy(0)]n+i = −aiλi(LW )/λi(Y ) − an+iλi(LW )/λn+i(Y ),

i = 1 [ezy(0)]1 = a1 + an+1 [ezy(0)]n+1 = 0

a1 a1 = [ezy(0)]1


an+1 = 0

ai an+i i ∈ {2, . . . , n}

ai = − λi(LW


)

[ezy(0)]n+i +

bi

λi(Y )

λi(LW ) [ezy(0)]i


,

an+i = λi(LW


)

[ezy(0)]n+i + λn+i(Y )

λi(LW ) [ezy(0)]i


,

bi

bi

t

ezy(t) = C t ezy(0) =


i ∈ {2, . . . , n}


2n

ai λi(C) t vi(C),

[ezy(t)]1 = (λ1(A)) t [ezy(0)]1, [ezy(t)]n+1 = 0,

i=1

[ezy(t)]i = ci,n+i [ezy(0)]n+i + ci,i [ezy(0)]i,

[ezy(t)]n+i = −ci,n+i [ezy(0)]i + cn+i,n+i [ezy(0)]n+i,

ci,n+i = λi(LW ) (λn+i(A)) t − (λi(A)) t /bi,

ci,i = λn+i(Y )(λn+i(A)) t − λi(Y )(λi(A)) t /bi,

cn+i,n+i = −λi(Y )(λn+i(A)) t + λn+i(Y )(λi(A)) t /bi.

ezy(t) 2

2 t


ezy(t) 2

2 =

+

+

n

i=2

n

i=1

([ezy(t)]i) 2 + ([ezy(t)]n+i) 2 = (λ1(A)) 2t ([ezy(0)]1) 2 +

(c 2 i,n+i + c 2 i,i)([ezy(0)]i) 2 +

n

i=2

(c 2 i,n+i + c 2 n+i,n+i)([ezy(0)]n+i) 2 +

n

2ci,n+i(ci,i − cn+i,n+i)[ezy(0)]i[ezy(0)]n+i,

i=2

2ci,n+i(ci,i − cn+i,n+i) = − 2λi(LW )(γ + λi(LW ))

b 2 i

(λn+i(A)) t − (λi(A)) t 2 .


k1a k2b |k1a−k2b| ≤ max{k1, k2} max{a, b}

(λn+i(A)) t (λi(A)) t ≥ 0 t 1/bi ≤

1/γ λi ≤ 2/3γ max{−λi(Y ), −λn+i(Y )} ≤ 4/3γ


n

|[ezy(0)]i[ezy(0)]n+i| ≤

i=2

c 2 i,n+i ≤ (2/3) 2 λ 2t

eff(A),

max{c 2 n+i,n+i, c 2 i,i} ≤ (4/3) 2 λ 2t

eff(A),

|2ci,n+i(ci,i − cn+i,n+i)| ≤ (20/3 2 )λ 2t

eff(A).

n

i=2


exw(t) 2 = ezy(t) 2

(max{|[ezy(0)]i|, |[ezy(0)]n+i|}) 2 ≤ ezy(0) 2

2 .

ezy(t) 2

2 ≤ 40/32 λ 2t


eff(A) ezy(0) 2

.

exw(t) 2 ≤ 2 √ 10/3 λ t eff(A) exw(0) 2 ,

λeff(A) = max{λn+2(A), −λn(A)}

λ2(LW ), λn(LW ) LW



λn(LW ) λ2(LW )


λn(A) λn+2(A)

h ∗ −λn(A) = λn+2(A)

h ∗ = 4/(2γ + λn(LW ) + λ2(LW ) + bn − b2).


i u k i

k = 1, . . . , K

1

n

n

i=1 uk i k

k = 1, . . . , K

k


L l

L − l(K − 1)


2


Consensus iterations

...

t=1

t=2

t=l

Input update steps

k=1 k=2 ... k=K

i ∈ {1, . . . , n} u k i

k = 1, . . . , K

l



L

L



l




l k ∗


ɛ k

k exw(lk ∗) / 2 k exw(0) ≤ ɛ l 2 ∗ k

l k ∗ ≥ (log (ɛ) − log (β)) / log (λeff(A)) .


k k + 1



k L k W


Gk = V, E k k [r Sk 2 . . . Sk n] = [r Sk ]

Lk W r

Lk W Ak Lk W

k uk = (uk 1, . . . , uk n)

xk (t) = (xk 1(t), . . . , xk n(t)) wk (t) = (wk 1(t), . . . , wk n(t))

k xk ∗ wk ∗ ∈ Rn

...


k

x k ∗ = rr T u k = 1x k avg, w k ∗ = rr T w k (0) − γ(L k W ) (−1) u k ,

r (L k W )(−1) λ⋆

λ⋆ = max

k∈{1,...,K} λeff(A k ).


h > 0 γ > 0

k




i

k = 1

x k i (0) = 0w k i (0) = 0 u k i ←


k = 1, . . . , K − 1

t = l

[x k i (t), w k i (t)] = consensusalg u k i , x k i (0), w k i (0)


x k i (0) = x k i (t) w k i (0) = w k i (t)

u k i ←


k = K

t = L − (K − 1)l

[x k i (t), w k i (t)] = consensusalg u k i , x k i (0), w k i (0)


(i) k = 1 (ii)

α σ


α = αk for k = 1, σ = max

k∈{1,...,K−1} σk,

with αk = r T u k 2 2 + γ 2 (L k W ) (−1) u k 2 2

1/2 ,

and σk = (r T (u k − u k+1 ) 2 2 + γ 2 (L k W ) (−1) u k − (L k+1

W )(−1) u k+1 2 2) 1/2 .


G k x k i (t) i x k avg

k


V

G k

k ∈ {1, . . . , K} x k (t) ∈ R n w k (t) ∈ R n


lim

t→∞ xk (t) = x k ∗, lim w

t→∞ k (t) = w k ∗,

t → ∞ x k ∗ w k ∗ e k xw(t) =

[(x k (t)) T , (w k (t)) T ] T −[(x k ∗) T , (w k ∗) T ] T k ∈ {1, . . . , K} t

t


e k xw(t) ∞ ≤ e k xw(t) 2 ≤ αfk(t) + σgk(t),

fk(t) = β k λ t+(k−1)l

⋆ , gk(t) = βλ t k−2

⋆ (βλ l ⋆) p ,

l

β = 2 √ 10/3 λ⋆ α σ

x k (t) ∈ R n w k (t) ∈ R n x k ∗ w k ∗


l

p=0


k

exw(l) ≤ βλ 2 l

k

⋆ exw(0) .

2

ek xw(l) k ek+1 xw (0)

k + 1

e k+1

xw (0) = e k xw(l) +


x k ∗

w k ∗



x k+1


w k+1



k

exw(t) ≤ β 2 k λ t+(k−1)l


1

⋆ exw(0)

2

+ βλ t k−2

⋆ (βλ l ⋆) p


k−p−1

x∗

p=0

w k−p−1


− x k−p


− w k−p



.

2

,

k = 1

k = 1 t = 0 e1 xw(0) = [(−x1 ∗) T , (−w1 ∗) T ] T

e1 xw(0)2 α

T T T (a , b ) 2 = a2

2 2 + b2 2

k = 1, . . . , K − 1 k ((x∗ − xk+1 ∗ ) T , (wk ∗ − wk+1 ∗ ) T ) T σk

2

σ


k − 1 k


β k λ t+(k−1)l


α1 + βλ t ⋆

p=0

k−2

(βλ l ⋆) p σk−p−1,

βkλ t+(k−1)l

⋆ α1 βλt ⋆(βλl ⋆) k−p−1σp p = 1, . . . , k−2


βλt ⋆σk−1

k − 1 k

k

k


βλ t ⋆αk.

αk

σk

αk



k

l uk Gk t < l

k +1 l

Gk+1 uk = uk+1

σk+1 γ(L k+1

W )(−1) uk2

σk+1

t l





MG×MG

MG

(r, s)

r r, s ∈ {1, . . . , MG}

{I, r, s} {i, r} u k i , x k i (t), w k i (t)

M 2 G + MG

1

2 MG(MG + 1)

MG + 1

2 MG(MG +1) MG +M 2 G


k [I k i ]r,s [i k i ]r

i ∈ V

u k i {I,r,s} = [I k i ]r,s, u k i {i,r} = [i k i ]r,

r, s ∈ {1, . . . , MG} x k i (t)

t i ∈ V I k i (t) ∈ R MG×MG

i k i (t) ∈ R MG

[I k i (t)]r,s = x k i (t) {I,r,s}, [i k i (t)]r = x k i (t) {i,r},

r, s ∈ {1, . . . , MG} i ∈ V k ∈ {1, . . . , K} t = 0, 1, . . .

ˆx k i (t) ∈ R MG Σ k i (t) ∈ R MG×MG

t k

ˆx k i (t) = I k i (t) −1 i k i (t), Σ k i (t) = I k i (t) −1 /n.


(i) I k i i k i j (ii)






i k i (t) I k i (t)







I k i (t)



i ∈ V


Gk k ∈ {1, . . . , K}

K ˆx K i (t) ΣK i (t) i ∈ V

ˆx K G ΣKG

lim

t→∞ ˆxK i (t) = ˆx K G , lim

t→∞ Σ K i (t) = Σ K G .


i ∈ V


G k k ∈ {1, . . . , K}

I k avg i k avg k

I k avg = 1

n

n

j=1

I k j , i k avg = 1

n

n

i k j .

α{I,r,s} σ{Ir,s} α σ u k i {I,r,s} = [Ik i ]r,s

α{i,r} σ{i,r} u k i {i,r} = [ik i ]r αI σI αi

σi I αi

αI = max

r,s∈{1,...,MG} α{I,r,s}, σI = max

r,s∈{1,...,MG} σ{I,r,s},

αi = max α{i,r}, σi = max σ{i,r}.

r∈{1,...,MG}

r∈{1,...,MG}

i ∈ V k ∈ {1, . . . , K} r, s ∈ {1, . . . , MG} t ≥ 0 [I k i (t)]r,s

[i k i (t)]r

i t

|[I k i (t)]r,s − [I k avg]r,s| ≤ αIfk(t) + σIgk(t),

|[i k i (t)]r − [i k avg]r| ≤ αifk(t) + σigk(t),

fk(t) gk(t)



ˆθ k i (t) i x




ˆx k i (t) i ∈ V

k ∈ {1, . . . , K} t

x

E ˆx k i (t) Ik = E i (t)

−1 k

ii (t) = x.

I k i (t) i k i (t)

I 1 j , . . . , I K j i 1 j, . . . , i K j


j=1


k = 1 t = 0

I k i (t) i k i (t)

I k i (t) =

i k i (t) =

n

[Φ(k, t)]i,j I k j +

j=1

n

[Φ(k, t)]i,j i k j +

j=1

n

k−1

[Ω(k, t, p)]i,j I

p=1 j=1

p

j ,

k−1

n

p=1 j=1

Φ(k, t), Ω(k, t, p), Ψ(t1, t2) ∈ R2n×2n

t


hγI

Φ(k, t) = Ψ (τ + (k − 1)l, t − 1 + (k − 1)l)

0

τ=1

l


hγI

Ω(k, t, p) = Ψ (τ + (p − 1)l, t − 1 + (k − 1)l)

0

τ=1

Ψ(t1, t2) = A(t2) . . . A(t1 + 1)A(t1),

[Ω(k, t, p)]i,j i p

j ,

A(t + kl) = Ak (t) t k

ˆx k j j x

ˆx k j = H k i x + v k j , with E v k j = 0,

i k j = (H k j ) T (Σ k j ) −1 ˆx k j

i k j = (H k j ) T (Σ k j ) −1 v k j + I k j x.

ik i (t)

i k n

i (t) =

j=1

n

[Φ(k, t)]i,j(H k j ) T (Σ k j ) −1 v k j +

k−1

[Ω(k, t, p)]i,j(H

p=1 j=1

k−p

j


n

[Φ(k, t)]i,j I k j +

j=1

) T (Σ k−p

n

j

) −1 v k−p

j

+

k−1

[Ω(k, t, p)]i,j I

p=1 j=1

p

j

Ik i (t)x Ik i (t) ˆx k i (t) = (Ik i (t)) −1ik i (t)


ˆx k i (t) = x + (I k i (t)) −1


n


+

(I k i (t)) −1

k−1


n

j=1

[Ω(k, t, p)]i,j(H

p=1 j=1

k−p

j


[Φ(k, t)]i,j(H k j ) T (Σ k j ) −1 v k j

) T (Σ k−p

j

x,

) −1 v k−p

j

v k j k ∈ {1, . . . , K} j ∈ V

ˆx k i (t) x



,


,

.


A(t + kl)

t k Ψ (t1 + kl, t2 + kl)

(A k ) t2−t1+1



K = 5

20

10

0

−10

−20

−30

−40

−20 −10 0 10 20

9





k k + 1 20

9

k = 1 k = 2 L = 1000


l = 0

l = 1(L/K)


4

l = (L/K)

l = 0

k = K l = 1(L/K)


4

50 k = 1 50

k = K

200

l = (L/K) l = 1

4 (L/K)

k = 1, 2, . . . , 4

k = K 800

L ˆ θK i (L) ΣK(L)

θi


15

10

5

0

−5

−10

−15


Robot 1

Robot 2

Robot 3

Robot 4

Robot 5

Robot 6

Robot 7

Robot 8

Robot 9

−5 0 5 10 15 20 25

15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

k = 1 k = 2

9 k = 1 k = 2

20 40

k = 2


5

4

3

2

1

0

x 10 4

l=0

l=1/4(L/K)

l=L/K

map updates, l=1/4(L/K)

map updates, l=L/K

0 200 400 600 800 1000

160

140

120

100

80

60

40

20

0

0 200 400 600 800 1000

|| ˆ θk i (t) − ˆ θk c || L ||Tr(Σk θi ˆ (t)) − Tr Σk c (t) || L

L || ˆ θk i (t)− ˆ θk c ||

||Tr(Σk θi

ˆ (t)) − Tr Σk c(t) || l = 0

k = 5 l = 1

4 (L/K)

50

200

l = (L/K) 200

800


15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

1

i = 1 ˆ θK i (L)

(L/K)

ΣK θi

ˆ (L) l = 1

4

ˆ θK G ΣKG


ˆ θ K G ΣK G


l = 1

4 (L/K)














µ


15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

1

i = 1 ˆ θK i (L)

(L/K)

ΣK θi

ˆ (L) l = 1

4

ˆ θK G ΣKG


ˆ θ K G ΣK G


l = 1

4 (L/K)














µ


15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25

1

i = 1 ˆ θK i (L)

(L/K)

ΣK θi

ˆ (L) l = 1

4

ˆ θK G ΣKG


ˆ θ K G ΣK G


l = 1

4 (L/K)














µ


n ∈ N n


Gcom = (Vcom, Ecom)

Vcom = {1, . . . , n} i j

(i, j) ∈ Ecom Ni i

Ni = {j | (i, j) ∈ Ecom}.

i Si mi

Si = {f i 1, . . . , f i mi }.

Si

Sj j ∈ Ni

Si





Robot C

f C

1

f C

2

f B

2

Robot B

f B

1

f D

1

X

f D

2

Robot D

f A

2

f A

1

Robot A

A B C D

A f A 1 f D 2 f B 1 B

f B 1 f C 1 C f C 1 f D 1

f D 1 f D 2


i, j k

r, r ′ , s, s ′ , r th


i th f i r A, Ar,s [A]r,s

(r, s) Aij (i, j)

Ik k × k 0k1×k2 k1 × k2



F

Si Sj, ij ∈ Nmi×mj


i 1 if fr and f j s are associated,

[ij]r,s =

0 otherwise,

r = 1, . . . , mi s = 1, . . . , mj F

F Si

F (Si, Si) = ii = I

ij


mi

r=1

[ij]r,s ≤ 1

r = 1, . . . , mi s = 1, . . . , mj

mj

s=1

[ij]r,s ≤ 1,

i j

Si Sj F (Si, Sj) = ij = T

(F (Sj, Si))

ji =

T










n

n

n F

Si Sj i, j ∈ {1, . . . , n}

Gcen = (Fcen, Ecen) Fcen f i r i = 1, . . . , n

r = 1, . . . , mi f i r f j s [ij]r,s = 1


Camera A Camera B

Camera D Camera C

Camera A Camera B

Camera D Camera C

Camera A Camera B

Camera D Camera C




(f A 1 , f B 1 ) (f A 2 , f B 2 ), (f A 2 , f B 1 ).


A C B D

Gcom.


Gcen




Si


Gcen

Gcen






n


n


Gcom = (Vcom, Ecom)




Gdis = (Fdis, Edis) ⊆ Gcen,

Fdis = Fcen, f i r f j s

Gcen i j

Edis = {(f i r, f j s ) | (f i r, f j s ) ∈ Ecen ∧ (i, j) ∈ Ecom}.


msum |Fdis| = n i=1 mi = msum

df Gdis

Gdis, dv Gcom

df ≤ msum dv ≤ n. ∈ Nmsum×msum Gdis




ij =

=



11 . . . 1n




n1 . . . nn

F (Si, Sj) if j ∈ {Ni ∪ i},

0 otherwise.


⎦ ,



i

ij, j = 1, . . . , n.

Gcom,






Gdis

Gdis

Gdis

Gdis |V|

∈ {0, 1} |V|×|V| , i, j ∈

{1, . . . , |V|} t ∈ N (i, j) t th t ,

t i j.



i ∈ Vcom t

Xij(t) ∈ N mi×mj , j = 1, . . . , n, t ≥ 0,

Xij(0) =

I, j = i,

0, j = i,



Xij(t + 1) =

ikXkj(t),

k∈{Ni∪i}

ik


[ t ]ij ∈ N mi×mj t

i j Xij(t) i

[ t ]ij

i, j ∈ {1, . . . , n} t ∈ N

Xij(t) = [ t ]ij,


t = 0 0 = I i, j ∈ {1, . . . , n} [ 0 ]ii = I

[ 0 ]ij = 0 Xij

t > 0

[ t ]ij =

n

ik[t−1 ]kj =

k=1

k∈{Ni∪i}

ik[ t−1 ]kj,

ik = 0 k /∈ {Ni ∪ i} i, j ∈ {1, . . . , n}

t > 0, Xij(t − 1) = [t−1 ]ij

Xij(t) =

ikXkj(t − 1) =

ik[t−1 ]kj = [t ]ij.

k∈{Ni∪i}

k∈{Ni∪i}

Xij(t) = [ t ]ij t > 0.

Xij(t)

t i j.



.

Gdis

[Xij(t)]r,s > 0

f i r f j s


df

df msum


¯

∼ ¯ , r s

[]r,s > 0 ⇔ [ ¯ ]r,s > 0 []r,s = 0 ⇔ [ ¯ ]r,s = 0.

Xij(t)


i, ti t, Xij(t) ∼

Xij(t − 1) j = 1, . . . , n. i ti

¯ Xij(t) Xij(t), [Xij(t−1)]r,s = 0 [Xij(t)]r,s >

0. | ¯ Xij(t)|, f j s ∈ Sj

Gdis f i r ∈ Si t ti, Xij(ti) ∼

Xij(ti − 1) ∀j n

j=1 | ¯ Xij(ti)| = 0


ti f i r Gdis.

ti,

ti + 1

i


df + 1

Gdis

df, n

j=1 | ¯ Xij(df + 1)| = 0 i = 1, . . . , n.

j t Xij(t) i

i

Xij(t) ∼ Xij(t − 1).



Xij(ti).

f i r


f i r ′ r = r′

f j s f j

s ′, s = s ′ ,

[Xii(ti)]r,r ′ > 0;

[Xij(ti)]r,s > 0 [Xij(ti)]r,s ′ > 0.





i Xij(ti)


t


Gdis.




i r(0) = {[i1]r,1, . . . , [in]r,mn} ∈ {0, 1} msum

f i r [ i r(0)]u, u = 1, . . . , msum, u th


i



i r(t), r ∈ Si j ∈ Ni

j s(t), j ∈ Ni, s ∈ Sj

r ∈ Si

j ∈ Ni, s ∈ Sj | [ij]r,s = 1

i r(t + 1) = i r(t) ∨ j s(t)


r ′ ∈ Si ∃ u ∈ {1, . . . , msum} [ i r(t)]u =

[ i r ′(t)]u = 1

i r(t + 1) = i r(t) ∨ i r ′(t)



i r(t + 1) = i r(t) ∀r ∈ Si












2m 2 sum.


i r


i ∈ Vcom, r ∈ Si.

i r


m 2 sum .


2m 2 sum.




y i r(t) i r(t), [ i r(t − 1)]u = 0

[ i r(t)]u = 1, u = 1, . . . , msum.

Gdis t Si.


ti i r(ti) = i r(ti − 1) ∀r y i r(ti) = 0


Si,


ti, ti +1.

ti +1




i

j t i r(t), r = 1, . . . , mi

i i

i r(t) = i r(t − 1).



min(df, 2n)

df

df ≤ 2n


Gcom, ℓ.

n − ℓ



3ℓ

+ 1

4

ℓ/2 ℓ + 1


ℓ/2 + 1

(ℓ/2)


u, ℓ/4



ℓ + 1




n−ℓ. 2n− 5

4 ℓ+1.

3 2n.


n − ℓ n − ℓ



Cam. A

Cam. B

Cam. C

Cam. D

1 1

1 1 1

1 1

1 1 1

1 1

1 1 1

1 1 1

1 1 1

1 1

1 1 1

1 1 1

Cam. A

Cam. B

Cam. C

Cam. D

1 1 1

1 1 1 1 1

1 1 1

1 1 1 1 1

1 1 1

1 1 1

1 1 1 1 1

1 1 1 1

1 1 1

1 1 1 1

1 1 1 1 1

Cam. A

Cam. B

Cam. C

Cam. D

1 1 1 1 1 1 1

1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1

1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

Cam. A

Cam. B

Cam. C

Cam. D

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

1 1 1

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1












f A 1 ,

A 1 (2) = A 1 (1) ∨ D 1 (1).

f B 1 f D 2 A 2 (2) = A 2 (1)∨ B 1 (1)∨ D 2 (1). A 3 (2) = A 3 (1)∨ B 3 (1).

f A 1 f A 2 f C 1


3 < df = 7

f A 1 f A 2 f B 1 , f B 2 , f C 1 , f C 2 , f D 1

f D 2 .


Gdis


C Gdis

C i r ∈ {1, . . . , mi} f i r ∈ C.

C R ⊆ Vcom

i ∈ R C ˜mi Gdis C = 0

Gdis

C Gdis

C


C Cq

∪ Cq = C, and Cq ∩ Cq ′ = ∅,

q

q, q ′ = 1, 2, . . .



f i r f i r ′


˜mi ≥ 2 C i ∈ R

˜mj ≥ 2 j ∈ R







R C i⋆

C

i⋆ = arg max

i∈R

˜mi.

C

˜mi⋆

i ∈ R

maxi∈R ˜mi = ˜mi⋆


i


C i i = i⋆

˜mi⋆

f i⋆

r ∈ C Cq





f j s f i r


[ij]r,s = 0

j


f i r




f j s f i r

[ij]r,s = 0


˜mi⋆

i

Xi1(ti), . . . , Xin(ti)




˜mi⋆ Cq

f i⋆ ∈ C Cq

f i⋆ ∈ Cq f j s Cq f i r

[ij]r,s = 1 j i i


f i r Cq

f i r

f i r ′ Cq

f i r i Cq

(a) f i r Cq i

(b) (c) f i r Cq i [ij]r,s

j j


[ji]s,r (d) i f i r Cq


i ∈ Vcom

Gdis G ′ dis

t = n Cq


Cq G ′ dis

Cq

Cq

q ∈ {1, . . . , ˜mi⋆}

(i) n

n

Cq


(ii) Gdis

C Gdis

Cq Gdis

Cq G ′ dis

Cq

G ′ dis (iii)

Cq

(iv)

Cq



C

2 ˜mi⋆ ˜mi⋆ ≥ 2

C

˜mi⋆

G ′ dis


i


C

2 ˜mi⋆ ≥ 4

|C| < 4 3 f i r, f i r ′, f j s

(f i r, f j s ) (f i r ′, f j s )

2




Xij(ti)


ij


ij j ∈ {1, . . . , n}





f i r i Li r = (i⋆, r⋆) ∈ N2 i⋆ r⋆ f i r

f j s , f j′

s ′ , . . . Gdis

i, j, j ′ , . . .

(i, r), (j, s), (j ′ , s ′ ), . . . (i⋆, r⋆)

i f i r Li r = (i⋆, r⋆)

j, j ′ , . . .

L i r = L j s = L j′

s ′ = · · · = (i⋆, r⋆) .

f i r Gdis

i L i r (i, r)

f i r



Gdis

f i r f j s L i r = L j s

Gdis




˜ Si ⊆ Si

i ∈ {1, . . . , n} | ˜ Si|

i Li L i r

f i r ∈ Si \ ˜ Si Xij | ˜ Si| × | ˜ Sj| ¯r = row (f i r)


f i r ∈ ˜ Si Xij

¯r ∈ {1, . . . , | ˜ Si|} ¯s = col(f j s ) ˜ Sj

Ãij ∈ N | ˜ Si|×| ˜ Sj| Aij

i j

i

˜ Si ← {f i 1, . . . , f i mi } Li ← ∅


L i r = (i, r) f i r f i r

| ˜ Si| > 0


f i r f i⋆

r⋆

L i r = (i⋆, r⋆) f i r


f i r [i⋆, r⋆]

L i r = (i⋆, r⋆) f i r

f i r

L i r = (i, r) f i r


L i r f i r

Li ← Li ∪ {L i r} ˜ Si ← ˜ Si \ {f i r}


i

˜Si = {f i 1, . . . , f i mi }, Li = ∅.

i j ∈ Ni

ij ∈ N mi×mj

f i r

[ij]r,s = 0 for all j ∈ Ni, j = i, and all s ∈ {1, . . . , mj}.

f i r i L i r


L i r = (i, r), Li = Li ∪ L i r, ˜ Si = ˜ Si \ {f i r}.

˜ Si


Gdis

˜Si i ∈ {1, . . . , n} i Xij ∈ N | ˜ Si|×| ˜ Sj|

j = 1, . . . , n (Gdis) ˜ Si

˜ Sj f i r f j s

[Xij]¯r,¯s > 0,


¯r = row(f i r) ¯s = col(f j s ) i



i f i r

i i

(i⋆, r⋆)


i⋆


f i⋆ i⋆

r⋆ , fr ′ , . . .



Xi1, . . . , Xin i ˜mj

j f i r ¯r = row(f i r)


˜mj = f j s | [Xij]¯r,¯s > 0, with ¯s = col(f j s ) .

˜m⋆ ˜mj j ∈ {1, . . . , n} i⋆

f i⋆ i⋆

r⋆ , fr ′ ⋆ , . . . f i r ¯r = row(f i r)

i⋆ = min {j | ˜mj = ˜m⋆} , {r⋆, r ′ ⋆, . . . } = s | [Xii⋆]¯r,¯s > 0 with ¯s = col(f i⋆

s ) .

f i r i⋆

f i⋆

r⋆


i⋆ = min j | [Xij]¯r,¯s > 0 for some ¯s ∈ {1, . . . , | ˜

Sj|} ,

r⋆

= s | [Xii⋆]¯r,¯s > 0 with ¯s = col(f i⋆

s ) ,


¯r = row(f i r) i f i r L i r = (i⋆, r⋆)


L i r = (i⋆, r⋆), Li = Li ∪ L i r, ˜ Si = ˜ Si \ {f i r}.





Cq (i⋆, r⋆)

i⋆ r⋆

f i r

(i⋆, r⋆)


i f i r

L i r = (i⋆, r⋆), Li = Li ∪ L i r, ˜ Si = ˜ Si \ {f i r}.


f i r ∈ ˜ Si

i f i r

[ Ãij]¯r,¯s = 0, j ∈ Ni, j = i, ¯s ∈ {1, . . . , | ˜ Sj|},

¯r = row(f i r)

i

˜ Si



i Sij Xij








C.


[]r,s =

ers if []r,s = 1,

−1 otherwise,

ers fr fs.


• err = 0, ∀r

• ers ≥ 0, ∀r, s

• ers = esr, ∀r, s


• ers = er ′ s ′ ⇔ [r = r′ ∧s = s ′ ]∨[r = s ′ ∧s =

r ′ ]


C. C









i

C

Gdis

C


r(0) = {[C]r,1, . . . , [C]r,c}, r = 1, . . . , ˜mi


r(t + 1) = maxs∈C, [C]r,s≥0(r(t), s(t)rs)

r(t + 1) = r(t) ∀r ∈ ˜mi


i r r ′

(s, s ′ ) :

[r]s = [r ′]s ′, s = s′

s ′′ = s, [r]s = [r]s ′′,

s ′′ = s ′ , [r ′]s ′ = [r ′]s ′′





r(0) = {[C]r,1, . . . , [C]r,c}, r ∈ {1, . . . , ˜mi}.

˜mi


r(t + 1) = max

s∈C, [C]r,s≥0 (r(t), s(t)rs),

rs

r s.



r, s ∈ C [C]r,s ≥ 0 r

srs.


, s ∈ C [C]r,s ≥ 0,

C, r s

r ≥ srs s ≥ rsr


rs = sr = −1

sr , srs ≥

r, r = srs.


fr u th

[r(t + 1)]u,

[r(t + 1)]u =


⎨max([r(t)]u,

[s(t)]s) [C]r,s ≥ 0 ∧ u = r

max([r(t)]u, [s(t)]r) [C]r,s ≥ 0 ∧ u = s


max([r(t)]u, [s(t)]u) [C]r,s ≥ 0 ∧ r = u = s

,




C

r ∈ C, [r(t)]r = 0, ∀t ≥ 0.

[r(t)]s ′ → [C]r ′ ,s ′ = er ′ s ′

r ′ = arg min d(r, r

[] r ′′ ,s ′=1

′′ ),

d(r, r ′′ ) r ′′ r

fr ′ fr fs ′.

fr, r th

r, [r(t + 1)]r,

[r(t + 1)]r = max

s∈C, [C]r,s≥0 ([r(t)]r, [s(t)]s).

[r(0)]r = err = 0,

r, [r(t)]r = 0, ∀t ≥ 0.


(fr ′, fs ′).

C

Cr ′ = {r | d(r, r′ ) < d(r, s ′ )},

Cs ′ = {s | d(s, s′ ) < d(s, r ′ )}.


′ ∈ Cr ′ s′ ∈ Cs ′

s ′th

Cr ′ r′th Cs ′

[r(t)]s ′, r ∈ Cr ′, [s(t)]r ′, s ∈ Cs ′.

r ∈ Cr \ r ′ ,

[r(t + 1)]s ′ = max

r ′′ ([r(t)]s ′, [r ′′(t)]s ′).

∈Cr ′, [C] r,r ′′≥0

r = s ′ = r ′′ . Cs ′

C fr ′

[r ′(t + 1)]s ′ = max ([r ′(t)]s ′, [r(t)]s ′, [s ′(t)]r ′).

r∈Cr ′,[C] r ′ ,r≥0 Cs

[s(t + 1)]r ′ = max

s ′′ ([s(t)]r ′, [s ′′(t)]r ′)

∈Cs ′, [C] s,s ′′≥0

[s ′(t + 1)]r ′ = max ([s ′(t)]r ′, [s(t)]r ′, [r ′(t)]s ′).

s∈Cs ′,[C] s ′ ,s≥0 Cr ′ Cs ′


max ([r(0)]s ′, [s(0)]r ′),

r∈Cr ′, s∈Cs ′

r ∈ Cr ′ \ r′ fs ′, [r(0)]s ′ = −1.

s ∈ Cs ′ \ s′ , [s(0)]r ′ = −1. r′ s ′ ,

[r ′(0)]s ′ = er ′ s ′ = es ′ r ′ = [s ′(0)]r ′ ≥ 0 > −1.

c

(fr ′, fs ′), er ′ s ′. r ∈ Cr ′, [r]s, s ∈ Cs ′ \s′ ,

[s ′]s.




Cℓ


∀r ′ , s ′ ∈ Cℓ, s ′ = r ′

∀r ′ ∈ Cℓ, s ′ ∈ Cℓ, s ′ = arg min s∈Cℓ d(r′ , s),

[r ′]s ′ → max ers.

r,s∈Cℓ

[r ′]s ′ → max ers.

r,s∈Cℓ


1, . . . , rℓ, Cℓ.

(fr1, fr2), (fr2, fr3) . . . , (frℓ , fr1).


r2 fr1,

[r1]r2 = max([r1]r2, [r2]r1, [rℓ ]r2),

fr1

fr2 Cℓ


[r1]r2 [r2]r1.


[r2]r1 = max([r2]r1, [r1]r2, [r3]r1),

[r3]r1.

[rℓ−1 ]r1 [rℓ ]r1,

[rℓ ]r1 = max([rℓ ]r1, [rℓ−1 ]r1, [r1]rℓ ).

[r1]r2 = [r2]r1, [r2]r1 = [r3]r1, . . . ,

[rℓ−1 ]r1 = [rℓ ]r1 [rℓ ]r1 = [r1]rℓ

[r1]r2 = [r1]rℓ = [r]r1, ∀r ∈ Cℓ \ r1.

Cℓ

r ∈ Cℓ, [r]r ′ = [r]r ′′, ∀r′ , r ′′ ∈ Cℓ \ r.

ℓ − 1




¯ Cℓ = C \ Cℓ.

s ∈ ¯ Cℓ

• ∃ r ∈ Cℓ fr fs

• s Cℓ

∈ ¯ Cℓ r ∈ Cℓ

r r ′ ∈ Cℓ,

s r, s

r s

Cℓ.

r

Cℓ r ′ ∈ Cℓ \ r, [s]r ′ [r]r ′

[s]r

r fr fs Cℓ,


Camera B

1

7

Camera A Camera F

9

Errors

8

Camera C Camera D

6

3

4

2

Camera E

Camera B

Camera A Camera F

Camera C Camera D

Camera E

Camera B

1

7

Camera A Camera F

9

8

Camera C Camera D





0 1

0 9 6

1 0 7

9 0

7 0 8 2

8 0 4

2 4 0 3

6 3 0

0 1 7

0 9 3 6

1 0 7 8 2

9 0 6

1 7 0 8 4 3

7 8 0 4 3

6 7 8 8 0 3

6 9 2 4 3 0

0 1 7 8 2

0 9 2 4 3 6

1 0 7 8 4 3

9 0 3 6

1 6 7 0 8 8 3

1 6 7 8 0 8 3

1 6 7 9 8 8 0 3

6 7 9 8 8 3 0

0 1 7 8 4 3

0 7 9 8 8 3 6

1 6 0 7 8 8 3

9 0 2 4 3 6

1 6 7 9 0 8 8 3

1 6 7 9 8 0 8 3

1 6 7 9 8 8 0 3

1 6 7 9 8 8 3 0

0 6 1 7 8 8 3

1 0 7 9 8 8 3 6

1 6 0 9 7 8 8 3

9 7 0 8 8 3 6

1 6 7 9 0 8 8 3

1 6 7 9 8 0 8 3

1 6 7 9 8 8 0 3

1 6 7 9 8 8 3 0

6

3

4

2

Camera E

0 6 1 9 7 8 8 3

1 0 7 9 8 8 3 6

1 6 0 9 7 8 8 3

1 9 7 0 8 8 3 6

1 6 7 9 0 8 8 3

1 6 7 9 8 0 8 3

1 6 7 9 8 8 0 3

1 6 7 9 8 8 3 0





r

(fs, fs ′),

fr fr ′

[r]s = [r ′]s ′, s = s′

s ′′ = s, [r]s = [r]s ′′,

s ′′ = s ′ , [r ′]s ′ = [r ′]s ′′,



[r]s, [r]s = [r ′]s ′ = ess ′.




˜mi

˜mi


0 6 1 9 7 8 8 3

1 0 7 9 8 8 3 6

1 6 0 9 7 8 8 3

1 9 7 0 8 8 3 6


(f A 2 , f B 2 ).

f B 1 f C 1













f D 1

f C 1



r


f B 1 ,


f A 1 f C 1 .



f D 1 f E 1




f A 2 f B 2 .


f B 1 f C 1



7 20 × 20 m

70

30 m

σx, σy = 0.4 cm

σθ = 1


6 m 0.5


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

20 × 20 m











3 m

F (Si, Sj)

(i, j) ∈ Ecom


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8






1.5

1

0.5

0

−0.5

−1

−1.5

−2

R5

R1

R4

R3

R2

R7

R6

5 5.5 6 6.5 7 7.5 8 8.5 9



3 m










Gdis


10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8






1.5

1

0.5

0

−0.5

−1

−1.5

−2

R5

R1

R4

R3

R2

R7

R6

5 5.5 6 6.5 7 7.5 8 8.5 9



3 m










Gdis


Gdis
















10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


Gdis
















10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


Gdis
















10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


Gdis
















10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

xi G (t), ΣiG (t) t = 5


i, j

e, e ′ e i

j e = (i, j) A, Ar,s [A]r,s

(r, s) ⊗ Ir

r × r 0r×s r × s

A Aij A = [Aij] A = (B1, . . . , Br)

A Aii = Bi Aij = 0 i = j



n ∈ N

n ∈ N

{p G 1 , . . . , p G n } G p G i = x G i , y G i , θ G i

∈ SE(3)

i ∈ {1, . . . , n} m ∈ N


p i j ∈ SE(3)

j i

G = (V, E) V = {1, . . . , n} E m

|E| = m e = (i, j) ∈ E i j i


j G

i

Ni

Ni = {j | (i, j) ∈ E or (j, i) ∈ E}.

A ∈ {0, 1, −1} n×m

Ai,e =




−1 e = (i, j)

1 e = (j, i)

0

, for i ∈ {1, . . . , n}, e ∈ {1, . . . , m},

Wi,j

G n



G



−π/2 < θi < π/2 i ∈ V

a ∈ V a = 1

p a a = 03×1

a

V a = V \ {a}

A a ∈ {0, 1, −1} n−1×m a

A


e = (i, j) ∈ E G = (V, E)

z θ e z xy

e j i

Σ z θ e Σ z xy

e



ˆp a i i ∈ V a a


−π/2 < θi < π/2 i ∈ V


i ∈ V p− xi ∈ R p ze ∈ R p

e = (i, j) ∈ E i j

ze = xj − xi + ve,

ve ∼ N (0p×p, Σze)





ˆx cen

i i ∈ V

ˆx a i


0 < θi < π i ∈ V

p G i = (R G i , T G

i ) ∈ SE(3) i

G R G i T G

i


R G i = ⎣

cos θ G i − sin θ G i 0

sin θ G i cos θ G i 0

0 0 1


⎦ , T G

i =



x G i

y G i

0


⎦ ,

z = 0 z

p i j = (R i j, T i j ) ∈ SE(3) j i

p i j



p

i = (R

i , T

i ) ∈ SE(3) i



p i j

G


p

i = (R

i , T

i )

T

G

T

G = 1

n

n

j=1


T

j .

R G


(RG , T G ) (R

G , TG )

RG = (R G )T T G = − (R G )T T

G i ∈ V

pG i = (RG i , T G

i )

R G i = R G R

i , T G

i = R G T

i + T G .

G

p G i


p G i = (R G i , T G

i )


G i

pi G = (Ri G , T i G ) pGi pi G


R G i = R i T G

G , Ti = − R i T i

G TG, R i G = R GT i

i , TG = − R GT G

i Ti .


pi j

(Ri j, T i j ) ∈ SE(3)


G

i ∈ V

T i G (t) ∈ R3 T i G (0) = 0 t ∈ N


T i G(t + 1) =

j∈Ni∪{i}

Wi,j(R i jT j

G (t) + T i j ),

Wi,j G


G i ∈ V

t → ∞ T i G (t) i ∈ V


i T i G

lim

t→∞ T i G(t) = T i G.




Ti(t) = R

i T i G(t) + T

i , T i G(t) = (R

i ) T Ti(t) − (R

i ) T T

i ,

T i G (t) (R i , T

i )


Ti(t + 1) =

Wi,jR

i R i jR j Tj(t) +

Wi,j(R

i R i jT j + R

i T i j + T

i )

j∈Ni∪{i}

=

j∈Ni∪{i}

j∈Ni∪{i}

Wi,jTj(t),


j∈Ni∪{i} Wi,j = 1 i ∈ V R i Ri jRj = R = I R i Ri jT j + R i T i j + T

i =

T

= 0 i, j ∈ V


lim

t→∞ Ti(t) = 1

n

n

j=1

Tj(0) = 1

n

n

R

j=1

jT j

G


(0) + Tj = 1

n

n

j=1

T

j = T

G,


lim

t→∞ T i G(t) = (R

i ) T T

G − (R

i ) T T

i = − (R

i ) T R G

= − R G i


T R G T

i + T G

= − R G i

T T G − (R

i ) T R G

T R G T

i

T T G

i = T i G,



R i R G


R

G = arg min

R n

d 2 (R , R

i ),

d 2 (R

i , R) R R

i

i=1

d 2 (R , R

i ) = − 1

2 Tr [log((R ) T R

i )] 2 .

log log : SO(3) → so(3)

log(R) =


03×3

β = 0,

β

2 sin β (R − RT ) β = 0,


β = arccos Tr{R}−1

n 2

i=1 d2 (R i , R)

θi

π/2


Ri G (t) i

SO(3)

i Ri G (0) = I

t ∈ N

R i G(t + 1) = R i G(t) exp(ui(t)), ui(t) =

Wi,j log(R i G(t) T R i jR j

G (t)),

j∈Ni∪{i}

Wi,j G

exp exp : so(3) → SO(3)


I α = 0,

exp(ui(t)) = sin α I + α ui(t) 1−cos α + α2 u2 i (t) α = 0,


1

α = 2Tr{uTi (t)ui(t)}

± π

G

2


lim

t→∞ R

i R i G(t) = R

G,


i ∈ V

n 2 log(ɛ −1 )

ɛ



n ∈ N

{p a 1, . . . , p a n} p a i = [x a i , y a i , θ a i ] i ∈ {1, . . . , n}

a m ∈ N

a ∈ V p a a = 03×1

a = 1 V a = V \{a}




e = (i, j) ∈ E G = (V, E)

z θ e z xy

e j

i Σ z θ e Σ z xy

e zθ ∈ R m zxy ∈ R 2m

Σzθ ∈ Rm×m Σzxy ∈ R 2m×2m m

zθ = (z θ 1, . . . , z θ m) T , zxy = ((z xy

1 ) T , . . . , (z xy

m ) T ) T ,

Σzθ = Diag(Σ z θ 1 , . . . Σ z θ m ), Σzxy = blkDiag(Σ z xy

1 , . . . Σ z xy

m ).


i ∈ V ˆp a i



˜ θ a V ∈ Rn

a V

zxy


ˆp a V = ((ˆxa V )T , ( ˆ θ a V )T ) T





˜ θV a ∈ Rn−1

a


zθ ∈ Rm Σzθ ∈ Rm×m

± π


2


˜θ a V a = Σ˜ θ a V a Aa Σ −1

zθ zθ, Σ˜ θ a V a = A a Σ −1

zθ (Aa ) T −1 ,

Aa ∈ {0, 1, −1} n−1×m

a A

˜ θa i = 0 i = a ˜ θa V ∈ Rn Σ˜ θa R

V

n×n

V a


˜θ a V = (0, ( ˜ θ a V a)T ) T , Σ˜ θ a V

= Diag(0, Σ˜ θa ).

Va z θ e e = (i, j)

i

˜ θ a V

e = (i, j) ∈ E ˜ Re ∈ R 2×2 ˜ Se ∈ R 2×2

˜ θi i

˜Re = R( ˜ θ a

cos ˜a θi i ) =

sin ˜ θa i

− sin ˜ θa i

cos ˜ θa i


,

Se

˜ = S( ˜ θ a

− sin ˜a θi i ) =

− cos ˜ θa i

cos ˜ θa i

− sin ˜ θa i


,

˜ R ∈ R 2m×2m m

˜R = R( ˜ θ a V) = blkDiag( ˜ R1, . . . , ˜ Rm).

w ∈ R2m+(n−1)

Σw


˜zxy

w = ˜θV a


˜R 0 zxy

=

0 In−1

˜θV a


,


T K J Σzxy 0 K 0

Σw =

,

0 In−1

0 Σ˜ θV a

J T In−1

K ∈ R 2m×2m J ∈ R 2m×(n−1)

zxy ˆ θ

K = ˜ R, and Je,i = ˜ Se z xy

e if e = (i, j) for some j, and Je,i = 02×1 otherwise.


ˆx a V a ∈ R2(n−1)

a ˆ θ a V a ∈ Rn−1


˜θ a V a ˆpa V a ∈ R3(n−1)


ˆp a Va =

ˆx a V a

ˆθ a V a


= Σˆp a VaBΣ−1 w w, Σˆp a Va = BΣ −1

w B T −1 ,

B = blkDiag ((A a ⊗ I2), In−1) Σw w

ˆp a V ∈ R3n V a


ˆp a V = (0 T 3×1, (ˆp a Va)T ) T , Σˆp a V = blkDiag(03×3, Σˆp a ).

V

ˆx a V a ˆ θ a V a


ˆx a V a = L−1 (A a ⊗ I2)Υ˜zxy


I2m + JΣˆ θ a V a J T Υ˜zxyE

˜R zxy,

ˆθ a V a = (Aa Σ −1

zθ (Aa ) T ) −1 A a Σ −1

zθ zθ + Σˆ θ a V a J T Υ˜zxyE ˜ R zxy, where

Υ˜zxy = ( ˜ RΣzxy ˜ R T ) −1 , E = (A a ⊗ I2) T L −1 (A a ⊗ I2)Υ˜zxy − I2m,

Σˆ θ a V a = ((Σ˜ θ a V a )−1 − J T Υ˜zxyEJ) −1 , L = (A a ⊗ I2)Υ˜zxy(A a ⊗ I2) T ,

ˆp a V


ˆx a V ˆ θa V





˜ θ a V a Σ˜ θ a V a

˜θ a V a = Σ˜ θ a V a Aa Σ −1

zθ zθ, Σ˜ θ a V a = (Aa Σ −1

zθ (Aa ) T ) −1 .

w

Σw

w =

˜zxy

˜θ a V a


˜Rzxy

=

˜θ a V a



˜RΣzxy

, Σw =

˜ RT + JΣ˜ θa Va J T JΣ˜ θa Va Σ˜ θa Va J T Σ˜ θa Va


ˆp a Va =

ˆx a V a

ˆθ a V a

T

.

= (BΣ −1

w B T ) −1 BΣ −1

w w.


ˆx a Va ˆ θa Va Υw = Σ−1 w


Υw =

Υ˜zxy −Υ˜zxyJ

−J T Υ˜zxy Σ −1

˜θ a Va + J T Υ˜zxyJ

,

Υ˜zxy Υ˜zxy = ( ˜ RΣzxy ˜ R T ) −1


A B

C D

−1 =

E F

G H



E = A −1 + A −1 B D − CA −1 B −1 CA −1 = A − BD −1 C −1 ,

F = −A −1 B D − CA −1 B −1 = − A − BD −1 C −1 BD −1 ,

G = − D − CA −1 B −1 CA −1 = −D −1 C A − BD −1 C −1 ,

H = D − CA −1 B −1 = D −1 + D −1 C A − BD −1 C −1 BD −1 .

Υˆp a = (BΣ−1

Va w BT ) Σˆp a

Va Υˆp a = Va Σˆp a = Va a (A ⊗ I2)Υ˜zxy(Aa ⊗ I2) T −(Aa ⊗ I2)Υ˜zxyJ

−J T Υ˜zxy(Aa ⊗ I2) T Σ −1

˜θ Va + J T Υ˜zxyJ


Σˆx Σ ˆx, ˆ θ

Σ T

ˆx, ˆ θ

Σˆ θ

Σˆ θ = ((Σ˜ θ a V a )−1 − J T Υ˜zxyEJ) −1 ,


,

, with

Σˆx = L −1 + L −1 (A a ⊗ I2)Υ˜zxyJΣˆ θ J T Υ˜zxy(A a ⊗ I2) T L −1 ,

Σ ˆx, ˆ θ = L −1 (A a ⊗ I2)Υ˜zxyJΣˆ θ ,

E = (A a ⊗ I2) T L −1 (A a ⊗ I2)Υ˜zxy − I,

L = (A a ⊗ I2)Υ˜zxy(A a ⊗ I2) T .



˜ θa V

i ∈ V

˜ θa i (t) ∈ R i = a

t ∈ N

˜θ a i (0) = 0, ˜ θ a i (t + 1) = ˜ θ a i (t), for i = a.

i ∈ Va t = 0 ˜ θa i (0)

t ∈ N


(Σzθ) e −1˜a θj (t) + C −1


i (Σzθ) e −1˜a θj (t),

˜θ a i (t + 1) = C −1

i ci + C −1

i

e=(i,j)∈E

e=(j,i)∈E


ci = −

Ci =

e=(i,j)∈E

e=(i,j)∈E

(Σ z θ e ) −1 z θ e +

(Σ z θ e ) −1 +

e=(j,i)∈E

e=(j,i)∈E

(Σ z θ e ) −1 z θ e,

(Σ z θ e ) −1 .

Υ˜ θ a V a

η˜ θ a V a ˜ θ a V a

Υ˜ θ a V a = (Σ˜ θ a V a )−1 = A a Σ −1

zθ (Aa ) T , η˜ θ a V a = Aa Σ −1

zθ zθ.

C Υ˜ θ a V a

C = Diag([Υ˜ θ a V a ]2,2, . . . , [Υ˜ θ a V a ]n,n),

D D = C − Υ˜ θa

Va

Υ˜ ˜θ θa Va a Va = η˜ θa Va , θ˜ a

Va = C −1 D˜ θ a Va + C−1η˜ θa .

Va ˜θ a Va(t + 1) = C−1D ˜ θ a Va(t) + C−1η˜ θa ,

Va t = 0 ˜ θ a V a(0) Aa Σ −1

zθ zθ A a Σ −1

zθ (Aa ) T

i − th ˜ θa Va

˜ θa i (t) ˜ θa i i ∈ Va

C−1D

ρ(C −1 D) < 1,

˜ θ a i (t) i = a t

ρ(C −1 D) ρ(C −1 D)

Σzθ ˜ θ a i (t)

i ˜ θ a i ˜ θ a V a



Ci ci i ∈ V a

z θ e Σ z θ e e = (j, i)

e = (i, j) ˜ θ a j (t) ˜ θ a i (t + 1)

j ∈ Ni


tmax

¯ θ a i tmax ¯ θ a i = ˜ θ a i (tmax)

zxy

˜zxy

¯ θ a i ¯ R =

R( ¯ θ a V a) ¯ θ a i ˜ θ a i

¯R i ∈ V

¯z xy

e = ¯ Rez xy

e , for all e = (i, j) ∈ E.

¯ θ ˜ θ

¯zxy ˜zxy



ˆp a Va

Σw

Σ˜ θa

Va ˆp a V i ∈ V ˆp a i (t) ∈ R3

ˆx a i (t) ∈ R2 ˆ θa i (t) ∈ R ˆp a V (t)

pa i (t) i ∈ V


ˆp a i (0) = 03×1, ˆp a i (t + 1) = ˆp a i (t), for i = a.

i ∈ V a t = 0 ˆp a i (0)

ˆpi(t) t ∈ N


ˆp a i (t + 1) =


M1 M2

Mi =

M3 M4

ˆx a i (t + 1)

ˆθ a i (t + 1)



, fi(p a

f1

V(t)) =

f2

= M −1

i (fi(ˆp a V(t)) + mi) ,


m1

, mi =

m2


.

Υ ˜z xy

e Υ˜zxy e = (i, j) ∈ E

Υ ˜z xy

e

= ˜ Re(Σ z xy

e )−1 ( ˜ Re) T .


Mi

M1 =

Υ xy

˜z + e

e=(i,j)∈E

M2 =

e=(i,j)∈E

M3 =

e=(i,j)∈E

M4 =

e=(i,j)∈E

e=(i,j)∈E

Υ ˜z xy

e

e=(j,i)∈E

˜Se z xy

e ,

(z xy

e ) T ( ˜ Se) T Υ ˜z xy

e ,

Υ ˜z xy

e ,

(z xy

e ) T ( ˜ Se) T Υ xy ˜Se ˜z z e

xy

e +

(Σ xy

ze e=(i,j)∈E

)−1 +

(Σ xy

ze e=(j,i)∈E

)−1 .

fi(ˆp a V (t))

ˆp a V (t) = (ˆxa V (t)T , ˆ θ(t) a

V )T

f1 =

Υ xy ˜Se ˜z z e

xy

e ˆ θ a j (t),

f2 =

e=(i,j)∈E

Υ ˜z xy

e ˆxa j (t) +

e=(j,i)∈E

(z xy

e ) T ( ˜ Se) T Υ ˜z xy

e ˆxa j (t) −

mi

m1 = −

e=(i,j)∈E

m2 = −


e=(i,j)∈E

e=(i,j)∈E

Υ xy

˜z e ˜zxy e +

e=(j,i)∈E

Υ ˜z xy

e ˆxa j (t) +

e=(i,j)∈E

Υ xy

˜z e ˜zxy e +

(z xy

e ) T ( ˜ Se) T Υ xy

˜z e ˜zxy e +

(Σ z θ e ) −1˜ θ a j −

e=(j,i)∈E

e=(i,j)∈E

e=(j,i)∈E

(Σ z xy

e )−1ˆ θ a j (t) −

e=(i,j)∈E

(Σ z θ e ) −1˜ θ a j +

Υ ˜z xy

e

e=(j,i)∈E

˜Sez xy

e ˜ θ a i −

(z xy

e ) T ( ˜ Se) T Υ xy ˜Sez ˜z e

xy

e ˜ θ a i

e=(i,j)∈E

(Σ z θ e ) −1˜ θ a i +

(Σ z xy

e )−1ˆ θ a j (t).

e=(j,i)∈E

e=(j,i)∈E

Υ ˜z xy

e

(Σ z θ e ) −1˜ θ a i .

˜Sez xy

e ˜ θ a j ,


ˆpi(t) i ∈ V

ˆp a i = [(ˆx a i ) T ˆ θ a i ] T G


i = a ˆp a i (t) = 0

Va ˆp a i

i ∈ Va Υˆp a

Va ˆp a Va Υˆp a Va = (Σˆp a Va)−1

Υˆp a = Va

L −A a Υ˜zxyJ

−J T Υ˜zxy(A a ⊗ I2) T A a Σ −1

zθ (Aa ) T + J T Υ˜zxyJ


,

L Υ˜zxy BΣ −1

w


BΣ −1


a (A ⊗ I2)Υ˜zxy

w =

−J T Y˜zxy

−(A a ⊗ I2)Υ˜zxyJ

A a Υ −1

zθ (Aa ⊗ I2) T + J T Υ˜zxyJ


.


i

ˆp a i =

Υˆp a Va ˆpaV a = B Σ−1 w w,

ˆx a i

ˆθ a i


= M −1

i (fi(ˆp a V) + mi) ,

Mi fi(pa V (t)) mi

M = blkDiag(M2, . . . , Mn)

ˆq a Va ˆpaV a ˆqa Va =


(ˆx a 2) T θˆ a

2, . . . , (ˆx a n) T T θˆ a

n

Υˆp a V a Υˆq a V a ˆpa i (t) i ∈ V a

ˆp a i = [(ˆx a i ) T θˆ a

i ] T ρ(M −1 (M − Υˆq a Va)) < 1

ρ(I − M −1 Υˆq a Va) < 1.

λ(I − M −1Υˆq a Va) = 1 − λ(M −1Υˆq a Va) 0 < λ(M −1Υˆq a V

0 < λ(M −1Υˆq a V

M −1 Υˆq a Va λ(M −1Υˆq a V

λmin(M −1 )

λmax(Υˆq a

Va ) 0 < λmin(M −1 )

λmax(Υˆq a

Va ) 0 < λ(M −1Υˆq a Va) a) < 2.

a)

a) ≥

λ(M −1Υˆq a Va) < 2

Υˆq a Va

adj(G) ⊗ I3 adj(G)

I3 3 × 3


s = 1

adj(G)ij = 0 for |i − j| > s.

Υˆq a V a s′ = 1


λmax(M −1 Υˆq a Va) < 2s′ = 2.




¯ θa V ˜ θa V


pi

i

xi

i ∈ {1, . . . , n} p− xi ∈ R p

xj −xi


e = (i, j) ∈ E G = (V, E)

i ze ∈ R p j

ze = xj − xi + ve,

ve ∼ N (0p×p, Σze) z ∈ R mp Σz ∈ R mp×mp

m

z = (z T 1 , . . . , z T m) T , Σz = blkDiag(Σz1, . . . , Σzm),

G

i Ni


n z


a ∈ V a = 1

ˆx a a = 0p a

ˆx a V a ∈ R(n−1)p

ˆx a V a = ((ˆxa 2) T , . . . , (ˆx a n) T ) T V a = V\{a} a


ˆx a V a = Σˆx a V a (Aa ⊗ Ip) Σ −1

z z, Σˆx a V a = (A a ⊗ Ip)Σ −1

z (A a ⊗ Ip) T −1 ,

A a ∈ R (n−1)×m G

a ˆx a V = (0Tp , (ˆx a Va)T ) T Σˆx a V =

blkDiag 0p×p, Σˆx a Va

a


i

Ni


ˆx a V





ˆx a Va

Υˆx a Va = η η Υ

ˆx a Va Σˆx a Va η = (A a ⊗ Ip) Σ −1

z z, Υ = (A a ⊗ Ip) Σ −1

z (A a ⊗ Ip) T .

ˆx a Va(t) ∈

R (n−1)p ˆx a Va(0) t


D, N Υ = [Υij]

ˆx a V a(t + 1) = D−1 N ˆx a V a(t) + D−1 η,

D = blkDiag(Υ22, . . . , Υnn), N = D − Υ.

ˆx a V a(t) ˆxa V a J = D−1 N

ρ(J) = ρ(D −1 N) < 1

Υ

j /∈ Ni Υij = Υji = 0p×p

Υ η i ∈ V a

i

i ∈ V ˆx a i

ˆx a V a = ((ˆxa 2) T , . . . , (ˆx a n) T ) T

i ∈ V ˆx a i (t) ∈ R p t = 0

ˆx a i (0) = 0p t i ∈ V ˆx a i (t)

ˆx a i (t + 1) =

MiBij ˆx a j (t) +

j∈Ni

e=(j,i)∈E

MiΣ −1

ze ze −

e=(i,j)∈E

MiΣ −1

ze ze,

Mi Bij p×p Mi = 0 i = a Mi = (

j∈Ni Bij) −1 i = a



⎨ Σ

Bij =


−1

ze + Σ−1 ze ′ if e = (i, j), e ′ Σ

= (j, i) ∈ E

−1

ze

Σ

if e = (i, j) ∈ E, (j, i) /∈ E

−1

ze if e = (j, i) ∈ E, (i, j) /∈ E

.


G Σz1, . . . , Σzm

m

p− Σz

D −1 N D N


ρ(D −1 N) < 1.


Υ Υ Z p

n−1

Υ

M p

n−1 Υ+ΥT ∈ M p

n−1

n − 1 n

Υ = [Υij] i, j ∈ Va |Va | = n − 1

Υ

Υij Υ

Υij =

−Bij if j ∈ Ni, j = i

0 if j /∈ Ni, j = i

, and Υii =

Bij,

i, j ∈ V a Bij Bij

Bij ≻ 0 −Bij ≺ 0 Υ Z p

n−1



J ⊂ V a i ∈ J

j∈J Υij ≻ 0

J = V a Υ

a Υ = [Υij] i, j ∈ V a i a

a ∈ Ni Υii Bia

j∈V a Υij 0 i ∈ V a



Υij = 0 if a /∈ Ni, and

j∈V a

j∈Ni


Υij = Bia ≻ 0, when a ∈ Ni.

j∈V a

A ≻ B A B A−B


G a ∈ Ni i ∈ Va

J Va i ∈ J Va


Υij = 0 if Ni ⊆ J , and Υij =

Bij ≻ 0, otherwise.

j∈J

j∈J

j∈Ni\J

G J Va

i ∈ J J a


j∈J Υij ≻ 0

u2, . . . , un = 1 Υ ∈ M p

n−1 Υ

Υ + ΥT ∈ M p

n−1 ρ(D−1N) < 1

G Σz1, . . . , Σzm

m p− Σz

i ∈ V

ˆx a i (t) i ∈ V

lim

t→∞ ˆxa i (t) = ˆx a i ,

ˆx a i


ˆx a V Σˆx a

V

a ˆx cen

V Σˆx cen

V


ˆx cen

V = (I − Hcen) ˆx a V, Σˆx cen

V = (I − Hcen) Σˆx a V (I − Hcen) T ,

where Hcen = (1n ⊗ Ip) (1n ⊗ Ip) T /n.

ˆx cen

V

Σˆx cen

V




blkTr


Σˆx cen

V

Σˆx a V

blkTr Σˆx cen

V


a ∈ V


blkTr Σˆx a

, V


Tr Σˆx cen


≤ Tr Σˆx V

a

. V

Pij Qij p × p

Σˆx a V = [Pij] Σˆx cen

V = [Qij] i, j ∈ V


blkTr Σˆx a V

=

n

Pii.

P = [Pij] i, j ∈ {1, . . . , n}

blkTr(P ) = n

i=1 Pii


i=1


Σˆx cen

V


Qii = Pii − 1

n

n

j=1

(Pij + Pji) + 1

n 2

n

n

j=1 j ′ =1


Pjj ′,

i ∈ V

blkTr Σˆx cen


n n

= Qii = Pii − V

i=1 i=1

1

n n

Pij

n

i=1 j=1


− (1n ⊗ Ip) T Σˆx a V (1n ⊗ Ip)/n.

= blkTr Σˆx a V

Σˆx a V (1n ⊗ Ip) T Σˆx a V (1n ⊗ Ip) 0

blkTr Σˆx cen


− blkTr Σˆx V

a

0

V

A Tr(blkTr(A)) = Tr(A)

blkTr Σˆx cen


− blkTr Σˆx V

a

0 blkTr Σˆx V

cen



V

blkTr Σˆx a


V

Tr(Σˆx cen)

= Tr(blkTr(Σˆx cen

V V )) ≤ Tr(blkTr(Σˆx a V )) = Tr(Σˆx a V ).

Tr(Σˆx a V

) − Tr(Σˆx cen

V

) = 1

n

n n i=1 j=1 Tr(Pij).

ˆx a a

0

k Σˆx a V = [Pij] Pij = 0

i, j ∈ {1, . . . , k} blkTr(Σˆx cen

V ) = blkTr(Σˆx a V ) − n i=k+1

n

i=k+1

n

j=k+1 Pij/n 0

n

j=k+1 Pij/n,

i ∈ V ˆx cen

i

ˆx cen

V = ((ˆxcen 1 ) T , . . . , (ˆx cen

n ) T ) T

ˆx cen

1 + · · · + ˆx cen

n = 0 (1n ⊗

Ip)(I − Hcen) = 0 i j ˆx cen

i = ˆx cen

j − ˆx a j + ˆx a i

ˆx a V = ((ˆxa 1) T , . . . , (ˆx a n) T ) T

ˆx a V

ˆx cen

i (0) = 0 i ∈ V

t

ˆx cen

i (t + 1) =

Wi,j(ˆx cen

j (t) − ˆx a j + ˆx a i )

j∈Ni∪{i}

i ∈ V W = [Wi,j] Wi,j > 0

(i, j) ∈ E Wi,j = 0 j /∈ Ni Wi,i ∈ [α, 1] Wi,j ∈ {0} ∪ [α, 1]

i, j ∈ V α ∈ (0, 1]

−ˆx a j + ˆx a i ze

e = (j, i)

ˆze ˆz = (A ⊗ Ip) T ˆx a V , ˆz = ((ˆz1) T , . . . , (ˆzm) T ) T

t

ˆx a i , ˆx a j ˆx a i (t), ˆx a j (t)


i ∈ V

ˆx cen

i (t) ∈ R p t = 0 ˆx cen

i (0) = 0 t

i ∈ V ˆx cen

i (t)

ˆx cen

i (t + 1) =

j∈Ni∪{i}

Wi,j(ˆx cen

j (t) + ˆx a i (t) − ˆx a j (t)),

ˆx a i (t), ˆx a j (t) i j t

Wi,j


i ∈ V G

ˆx cen

i (t) i ∈ V

i ˆx cen

i

lim

t→∞ ˆxcen i (t) = ˆx cen

i .

ecen(t) = (ˆx cen

1 (t) − ˆx cen

1 ) T , . . . , (ˆx cen

n (t) − ˆx cen

n ) T T

n t G

t

||ecen(t)||2 ≤ λ t eff(W)||ecen(0)||2 + 2p(n − 1)σJλ t t

k ρ(J)

eff(W)

,

λeff(W)

J J = D −1 N D N

σJ J W

ecen(0)

t = 0


ecen(t)

ea(t) ∈ R (n−1)p

ecen(t) = (ˆx cen

1 (t)) T , . . . , (ˆx cen

n (t)) T T cen − ˆx ˆxcen 1 ) T , . . . , (ˆx cen

n ) T T

V

k=1

V = (ˆx cen

˜ea(t) = (ˆx a 2(t) T , . . . , ˆx a n(t) T T − ˆx a V a ˆx a V a = (ˆx a 2) T , . . . , (ˆx a n) T T

i = 1 a ea(t)

(0 T p , ˜ea(t) T ) T

j∈Ni∪{i} ˆxa i (t) = ˆx a i (t)

ˆx cen

V ˆx cen

V = (I − Hcen)ˆx a V


ecen(t) = (W ⊗ Ip)ecen(t − 1) + ((In − W) ⊗ Ip)ea(t − 1) + P ˆx a V,

P ˆx a V

P = I − (W ⊗ Ip) − (I − (W ⊗ Ip))(I − Hcen) = (I − (W ⊗ Ip))Hcen.


(W ⊗ Ip)Hcen = Hcen P = 0


ecen(t) = (W ⊗ Ip)ecen(t − 1) + ((In − W) ⊗ Ip)ea(t − 1) =

= (W ⊗ Ip) t t−1

ecen(0) + (W ⊗ Ip) t−k−1 ((I − W) ⊗ Ip) ea(k).

k=0

ecen(t)

ecen(t)2 ≤ λ t t−1

eff(W)ecen(0)2 + 2 λ t−k−1

eff (W)ea(k)2,

((W − I) ⊗ Ip) 2 ≤ 2 W


ea(t)2

˜ea(t) ∈ R (n−1)p ea(t) = (0, ˜e T a (t)) T J

VJ = [vp+1(J), . . . , vnp(J)] λJ = Diag (λp+1(J), . . . , λnp(J))

J = VJ λJ V −1

J ||vi(J)||2 = 1

˜ea(t)

k=0

˜ea(t) = J˜ea(t − 1) = J t ˜ea(0).

˜ea(0) σp+1, . . . , σnp

˜ea(0) =

np

i=p+1

σivi(J),

˜ea(t) t


˜ea(t) = VJλ t JV −1

J VJ [σp+1, . . . , σnp] T =

np

i=p+1

σivi(J)λ t i(J).

σJ = max np

i=p+1 |σi| ρ(J) = max np

i=p+1 |λi(J)| t ≥ 0

||˜ea(t)||2

||ea(t)||2 = ||˜ea(t)||2 ≤ p(n − 1)σJρ t (J).


ecen(t)2 ≤ λ t t−1

eff(W)ecen(0)2 + 2p(n − 1)σJ λ t−k−1

eff (W)ρ k (J),


k=0


ρ(J) = λeff(W)


||ecen(t)||2 ≤ λ t eff(W)||ecen(0)||2 + 2p(n − 1)σJλ t−1

eff (W)t,

ρ(J) = λeff(W)

||ecen(t)||2 ≤ λ t eff(W)||ecen(0)||2 +

2p(n − 1)σJ

ρ(J) − λeff(W) (ρt (J) − λ t eff(W)).

λeff(W) < 1 G λt eff (W)||ecen(0)||2

t → ∞

ecen(0) ρ(J) = λeff(W) λt eff (W)t

t ≥ λeff(W)

1−λeff(W) t → ∞ ρ(J) = λeff(W)

(ρ t (J) − λ t eff (W)) λeff(W)

ρ(J) < 1 limt→∞ ||ecen(t)||2 = 0

||ecen(t)||2 = 0 ecen(t) = 0

Z p n M p n



A = [Aij] A ∈ R np×np Aij p × p

i, j ∈ {1, . . . , n}

A Zp n Aij i, j ∈ {1, . . . , n}

Aij 0 i, j ∈ {1, . . . , n}, j = i. ˆ Zp n A ∈ Zp n

Aii ≻ 0 i ∈ {1, . . . , n} A M p n A ∈ ˆ Zp n

u1, . . . , un > 0

n

ujAij ≻ 0 for all i ∈ {1, . . . , n}.

j=1

A ∈ Zp n ∀J ⊂ {1, . . . , n}

i ∈ J

j∈J Aij

≻ 0 π

j≥i Aπ(i),π(j) ≻ 0 i ∈ {1, . . . , n}

A ∈ Zp n u1, . . . , un > 0

n

Aijuj 0, for all i ∈ {1, . . . , n}.

j=1

π {1, . . . , n}


Aπ(i),π(j)uπj ≻ 0, for all i ∈ {1, . . . , n}.

A ∈ M p n

j≥i


A + A T ∈ M p n, D = blkDiag (A11, . . . , Ann) , and A = D − N.

ρ (D −1 N) < 1




7

20 × 20 m

10

8

6

4

2

0

−2

−4

−6

−8

−10

−10 −8 −6 −4 −2 0 2 4 6 8

20 × 20 m






1.5

1

0.5

0

−0.5

−1

−1.5

−2

−2.5

R1

R3

R4

R2

R5

R7

R6

−9.5 −9 −8.5 −8 −7.5 −7 −6.5 −6 −5.5 −5



3 m


3 m

G

i

(T i G (t), Ri G (t))


(T

G , R G ) (T i G (t), Ri G (t))

i pG i G

x− y−

0.11 cm 2.12 cm

0.43

2

1.5

1

0.5

0

−0.5

−1

−1.5

−2

−2.5

−3

t=0

t=1

t=3

t=7

−9 −8.5 −8 −7.5 −7 −6.5 −6 −5.5 −5


t = 0 i

(T i G (t), Ri G (t))

(T i G (t), Ri G (t))

(T i j (t), Ri j(t))

t = 1, 3, 7


, R )

(T

G

G





n = 20

4 m ± π

2



x y 6 cm 1


−6

−6.5

−7.5

−8.5

0 4 8 10

1

0

−1

−2

0 4 8 10

1

0.5

0

−0.5

0 4 8 10

x− y−




R1 100

8

7

6

5

4

3

2

1

0

R 15

R 20

R 1

R 10

−5 −4 −3 −2 −1 0 1 2 3 4 5

4m





25 cm x− y−

13 cm 4

1.7





ρ(C −1 D)

ρ(I − M −1Υˆq a Va)



ρ(C −1 D) ρ(I−M −1 Υˆq a V a)

t = 50 ¯ θ a V

˜ θ a V 0.16◦

0.01 t = 100 8.5e−05

R 5


3.38 ◦ 1.87 ◦

x cm cm

y cm cm

4.03 ◦ 1.66 ◦


ρ(C−1D)

ρ(I − M −1Υˆq a Va)

t = 50 t = 100 t = 200

0.16 ◦ 0.01 ◦ 8.5e − 05 ◦

x cm cm cm

y cm cm cm

0.29 ◦ 0.12 ◦ 0.11 ◦

t = 200

200

¯ θa V ˜ θa V t = 200


1.64 cm 0.48 cm x− y− 0.11

t = 100 t = 50

1.74 cm 0.84 cm x− y− 0.29


˜θ a V i

j

i j

d 5% d x 0.7% d

y

2.5


t = 0 0


5

4

3

2

1

0

−1

−2

−3

−4

R 2

R 1

R 7

R 3

R 4

R 8

R 5

R 6

R 9

R 10

0 2 4 6 8 10




6

4

2

0

−2

−4

R 2

R 1

R 7

R 3

R 4

R 8

R 9

R 6

R 5

R 10

−6

−2 0 2 4 6 8 10 12

6

4

2

0

−2

−4

R 2

R 1

R 7

R 3

R 4

R 8

R 9

R 5

R 6

R 10

−6

−2 0 2 4 6 8 10 12

6

4

2

0

−2

−4

R 1

R 2

R 7

R 3

R 4

R 8

R 5

R 9

R 6

R 10

−6

−2 0 2 4 6 8 10 12















n = 20


4

i ∈ V Σ ˆx i V

blkTr(Σ ˆx i V )


−2

−2.5

−3

−3.5

−4

Centr.

t=0

t=1

t=3

t=7

t=15

t=31

t=63

t=100

−4.5

8 8.5 9 9.5 10 10.5 11 11.5




1

0

−1

−2

−3

−4

Centr.

t=0

t=200

t=400

t=600

t=800

t=1000

t=1200

t=1400

−5

−2 0 2 4 6 8 10

t = 0



10

8

6

4

2

0

R 6

R 14

R 20

R 7

R 8

R 2

R 13

R 19

R 11

R 15

R 3

R 16

R 18

R 17

R 4

R 9

R 5

R 12

R 10

R 1

0 2 4 6 8 10

11

10

9

8

7

6

R 8

R 14

R 19

R 13

R 18

R 3

5

1 2 3 4 5 6

R19

10

×10 e = (i, j) ∈ E

4 i

ze j e = (i, j)


i ∈ V

blkTr(Σˆx i )

V

blkTr(Σˆx cen)


V

R3 R12

R1

ˆx cen

V Σˆx cen

V

a = R1 ˆx a V Σˆx a

V




R1

λ 2

2.5

2

1.5

1

0.5

cen

R 3

R 12

R 1

0

0 0.5 1 1.5 2

λ

1

10

8

6

4

2

0

cen

R 1

0 2 4 6 8 10

blkTr(Σ ˆx i V )

blkTr(Σ ˆx i V ) i ∈ V

blkTr(Σˆx cen

V )

R1

R3 R12



R1

n 1000

ˆx a i (t) ˆx cen

i (t)

100


n

t i ∈ V

R1 R3 R12




100


35

30

25

20

15

10

5

0

0 200 400 600 800 1000

18

16

14

12

10

8

6

4

2

2.6

2.4

2.2

2

1.8

1.6

1.4

1.2

1

0.8

900 920 940 960 980 1000

100

0

0 200 400 600 800 1000

1.1

1.08

1.06

1.04

1.02

1

0.98

0.96

0.94

900 920 940 960 980 1000

100

100


100

i ∈ V

R1 R3 R12


9001000

6.5.5

9001000

6.5.5


35

30

25

20

15

10

5

0

0 200 400 600 800 1000

18

16

14

12

10

8

6

4

2

2.6

2.4

2.2

2

1.8

1.6

1.4

1.2

1

0.8

900 920 940 960 980 1000

100

0

0 200 400 600 800 1000

1.1

1.08

1.06

1.04

1.02

1

0.98

0.96

0.94

900 920 940 960 980 1000

100

100


100

i ∈ V

R1 R3 R12


9001000

6.5.5

9001000

6.5.5


35

30

25

20

15

10

5

0

0 200 400 600 800 1000

18

16

14

12

10

8

6

4

2

2.6

2.4

2.2

2

1.8

1.6

1.4

1.2

1

0.8

900 920 940 960 980 1000

100

0

0 200 400 600 800 1000

1.1

1.08

1.06

1.04

1.02

1

0.98

0.96

0.94

900 920 940 960 980 1000

100

100


100

i ∈ V

R1 R3 R12


9001000

6.5.5

9001000

6.5.5


k−


k


k














k k−



max − max −

k + 1


n


n








n









P

S




S



i Si

Si = HiP(k + 1|k)H T i + σ 2 z.


P(k + 1|k) = I Si

Si = 1 + σ 2 z + 2/r 2 ,

r = (xk+1 − xi) 2 + (yk+1 − yi) 2

Si

r


Pxx 0

P(k + 1|k) = blkDiag I,

0 Pyy


, I ,

Pyy = kPxx k > 1

y r xk+1 = r cos α

yk+1 = r sin α Si

Si = 1 + σ 2 z +

1 + Pxx

r 2

+ (k − 1)Pxx

r2 cos2 (α),

Si Si α =

0 + nπ, n ∈ Z α = π + nπ, n ∈ Z

2

∂2Si = 2(k − 1)Pxx

∂α2 r2 (1 − 2 cos2 (α)).

k > 1 Si α = 0 + nπ, n ∈ Z α =

π

2 + nπ, n ∈ Z Si

Si


r


α


π

2

90º

70º

50º

90º


α = 90, 70 50

α < π/2


A = (B1, . . . , Br) A Aii = Bi Aij = 0 i = j


70º

50º


ˆxr(k) = (ˆxk, ˆyk, ˆ θk) T

k ˆxi(k) = (ˆxi, ˆyi) T

k i ∈ {1, . . . , m} ˆx(k) = (ˆxr(k) T , ˆx1(k) T . . . , ˆxm(k) T ) T

k P(k) xg = (xg, yg, θg) T


xk k+1 ˆxr(k) xg

x k k+1 = (⊖ˆxr(k)) ⊕ xg,



−ˆxk cos

⊖ˆxr(k) = ⎣

ˆ θk − ˆyk sin ˆ θk

ˆxk sin ˆ θk − ˆyk cos ˆ θk

−ˆ ⎤

⎦ .

θk


ˆxr(k) xg

xk k+1 k

k + 1


x k ⎡

(xg − ˆxk) cos

k+1 = ⎣

ˆ θk + (yg − ˆyk) sin ˆ θk

−(xg − ˆxk) sin ˆ θk + (yg − ˆyk) cos ˆ ⎤

θk

⎦ .

θg − ˆ θk

¯x(k +

1) = x(k+1|k) = (¯xr(k+1) T , ¯x1(k+1) T . . . ¯xm(k+1) T ) T P(k+

1|k) ˆx(k), P(k) xk k+1 =

(xodom, yodom, θodom) Podom ¯xr(k + 1) = (¯xk+1, ¯yk+1, ¯ θk+1) T

¯xi(k+1) = (¯xi, ¯yi) T i ∈ {1, . . . , m} xk k+1


Podom = Diag σ2 x, σ2 y, σ2

θ σx = Kxd

σy = Kyd d = (xg − ˆxk) 2 + (yg − ˆyk) 2


¯x(k + 1) = ((¯xr(k + 1)) T , ¯x1, ¯y1, . . . , ¯xm, ˆym) T = ((ˆxr(k) ⊕ x k k+1) T , ˆx1, ˆy1, . . . , ˆxm, ˆym) T ,

P(k + 1|k) = J1P(k)J T 1 + J2PodomJ T 2 ,

⊕ ˆxr(k) x k k+1

ˆxr(k) ⊕ x k ⎡

ˆxk + xodom cos

k+1 = ⎣

ˆ θk − yodom sin ˆ θk

ˆyk + xodom sin ˆ θk + yodom cos ˆ ⎤

θk

⎦ ,

ˆθk + θodom

J1 J2


J1 = blkDiag(j1, I), J2 = ((j2) T , 0) T ⎡

1 0 −xodom sin

j1 = ⎣

,

ˆ θk − yodom cos ˆ θk

0 1 xodom cos ˆ θk − yodom sin ˆ ⎤

θk

⎦ ,


cos

j2 = ⎣

0 0 1

ˆ θk − sin ˆ θk

sin

0

ˆ θk cos ˆ θk 0

0 0 1



⎦ .

¯xi(k + 1) = (¯xi, ¯yi) T


¯xr(k + 1) = (¯xk+1, ¯yk+1, ¯ θk+1) T


−(¯xi − ¯xk+1) sin

hi(¯xr(k + 1), ¯xi(k + 1)) = atan2

¯ θk+1 + (¯yi − ¯yk+1) cos ¯ θk+1

(¯xi − ¯xk+1) cos ¯ θk+1 + (¯yi − ¯yk+1) sin ¯

.

θk+1



∂hi

Hi =

0 · · · 0

∂¯xr(k+1)

∂hi

∂¯xr(k + 1) =


∂hi ∂hi

∂¯xk+1 ∂ ¯yk+1

∂hi/∂ ¯ θk+1 = −1

∂hi

∂¯xk+1

∂hi

∂¯xi

= −

=

¯yk+1 − ¯yi

∂hi

∂ ¯ θk+1

(¯xk+1 − ¯xi) 2 2 ,

+ (¯yk+1 − ¯yi)

¯yk+1 − ¯yi

(¯xk+1 − ¯xi) 2 2 ,

+ (¯yk+1 − ¯yi)

∂hi

∂¯xi(k+1)


,

∂hi

∂¯yk+1

∂hi

∂¯yi

0 · · · 0


,

∂hi

∂¯xi(k + 1) =

=

= −

∂hi

∂¯xi

∂hi

∂ ¯yi

¯xk+1 − xi

(¯xk+1 − ¯xi) 2 2 ,

+ (¯yk+1 − ¯yi)

¯xk+1 − ¯xi

h H


,

(¯xk+1 − ¯xi) 2 2 ,

+ (¯yk+1 − ¯yi)

h = (h T 1 , . . . , h T n) T , H = (H T 1 , . . . , H T n) T .


ˆx(k + 1) = x(k + 1|k + 1) P(k + 1|k + 1)


ˆx(k + 1) = ¯x(k + 1) + K(z − h), P(k + 1|k + 1) = (I − KH)P(k + 1|k),

K = P(k + 1|k)H T S −1 , S = HP(k + 1|k)H T + R,

ν = z − h S K

z

z = h ν

ˆx(k + 1) = ¯x(k + 1) R

σ 2 zI σz





x = (xk+1, yk+1) f(x)

f(x) = Tr(P(k + 1|k + 1)),

P(k + 1|k + 1)





x(0) = (xk, yk), ˙x = −∇f(x),

x(0) = (xk, yk), x(t + 1) = x(t) − h∇f(x(t)),

(xk, yk) x(t) x t ∇f(x(t)

f x(t) h

˙x = 0 x(t + 1) − x(t)


8

6

4

2

0

−2

F1

Initial map

−4

−6 −4 −2 0 2

MAP at Step 2, features: 1

4 6

−4

−6 −4 −2 0 2 4 6

8

6

4

2

0

−2

Cost function for a map with a single feature

F1

0.25

0.2

0.15

0.1

0.05

5.5

5

4.5

4

3.5

3

Cost function for a map with a single feature

−3.5 −3 −2.5 −2 −1.5 −1







(x, y)









n

ˆxi Σi = Pi(k + 1|k + 1)

i ∈ {1, . . . , n}


ˆxG =

n

i=1

H T i Σ −1

i Hi

−1 n

i=1

H T i Σ −1

i ˆxi, ΣG = (

n

i=1

H T i Σ −1

i Hi)

−1

,

Hi



F1

0.25

0.2

0.15

0.1

0.05


8

6

4

2

0

−2


F1

Initial map

−4

−6 −4 −2 0 2

MAP at Step 2, features: 1

4 6

−4

−6 −4 −2 0 2 4 6

8

6

4

2

0

−2

F1

Cost function

x 10

10

−4

9

8

7

6

5

4

3

5.5

5

4.5

4

3.5

3

Cost function

−3.5 −3 −2.5 −2 −1.5 −1







(x, y)







F


F (x1, · · · , xn) =

m

j=1

min

i fij(xi)),

xi i k + 1 i ∈ {1, . . . , n} fij

j i

i xi

fij(xi) Pi(xi)

i xi

[Pi(xi)]jj Pi(xi) j



fij

fij(xi)) = ([Pi(xi)]jj).

F1

x 10

10

−4

9

8

7

6

5

4

3


i

1 · · · m

x 1 i fi1(x 1 i ) · · · fim(x 1 i )




xK i fi1(xK i ) · · · fim(xK i )



x l1

1


x ln

n

1 · · · m

f11(x l1

1 ) · · · f1m(x l1


1 )


fn1(xln n ) · · · fnm(xln n )

min mini fi1(x li

i ) · · · mini fim(x li

i )



i

K

K ≤ mi i ∈ {1, . . . , n}

i ∈ {1, . . . , n} K

x 1 i , · · · , x K i

x 1 i , · · · , x K i i

fij(x l i)

l ∈ {1, . . . , K} j ∈ {1, . . . , m} i ∈ {1, . . . , n} j

i fij(x l i) = ∞ l ∈ {1, . . . , K}

i fij


x l1

1 , · · · , xln n m j=1 mini fij(x li

i ))


F


W

λ2(W ) W


λ2(W )

λ2(LW )

λn(LW ) LW




λ2(LW ) W

λ2(LW ) LW = I − W

λ2(W ) = 1−λ2(LW )









n ∈ N i ∈ {1, . . . , n}


G = (V, E) V = {1, . . . , n} E

(i, j) ∈ E i j n × n

C G Cij = 0 (i, j) /∈ E j = i

Cii A ∈ {0, 1} n×n


G


n k k ∈ N

i, j t t ∈ Rt≥0


Aij [A]ij

(i, j) A

(b1, . . . , br) A Aii = bi Aij = 0

λi(A) vi(A) ith A

λA

(λ1(A), . . . , λr(A))

VA

A∞

[v1(A), . . . , vr(A)]

n ∞− A maxi j=1 |Aij|

A2


A maxi λi(AT A)

ρ(A)


A maxi |λi(A)|

Ir

r × r

0r 1r r

A

L L = diag(A1) − A

λ⋆(L) λi(L)

Aij =

1 (i, j) ∈ E

0

, for i, j ∈ V.

G Ni

i i Ni = {j | (i, j) ∈ E}

di i Ni dmax = maxi∈V di

L ∈ R n×n G

L = diag(A1) − A.

A L

L

λ1(L) ≤ λ2(L) ≤ · · · ≤ λn(L).

L

v1(L) = 1/ √ n λ1(L) = 0 L1/ √ n =

0

G


0 = λ1(L) < λ2(L) ≤ · · · ≤ λn(L); and


λn(L) ≤ 2dmax

G λ⋆(L) λ2(L)

L


i ∈ V

˙xi(t) = ui(t), with xi(t), ui(t) ∈ R,

ui i

ui(t) = −

(xi(t) − xj(t)).

j∈Ni

x = [x1, . . . , xn] T u = [u1, . . . , un]

˙x(t) = −Lx(t), x(0) = x0.

G


lim

t→∞ xi(t) =

n

xi(0)/n.




xi(t)



i

ei(t) i

xi(t) t ˆxi(t)

i=1

ei(t) = ˆxi(t) − xi(t), for i ∈ V,

e(t) = [e1(t), . . . , en(t)] T c1, α


c1 > 0 and 0 < α < λ⋆(L).

fi(t, ei(t)) = |ei(t)| − c1e −αt ,

i fi(t, ei(t)) > 0 i

ˆxi


i xi(t) xj(t)

(i) (ii)



x0


λ⋆(L)



λ⋆(L)

α

λ⋆(L)


λ⋆(L)

(i)

C∞

max−

(ii) C k 1

k

∞ ρ(C) C






C





n

i ∈ V

Ĉij(k) (i, j) k− C [C k ]ij j ∈ V

k = 0 i Ĉij(k)

Ĉii(0) = 1, and Ĉij(0) = 0 for j ∈ V \ {i}.

k ≥ 1 i

Ĉij(k + 1) =

Cij ′Ĉj ′ j(k), for j ∈ V.

j ′ ∈Ni∪{i}


i

j ∈ Ni

i n n k


C

k ≥ 0 k− C C k

i i− Ck k = 0

I C0 k ≥ 1

Ck+1 = CC k (i, j)

[C k+1 n

]ij = Cij ′[Ck ]j ′ j.

j ′ =1

C Cij ′ = 0 j′ /∈ Ni ∪ {i}


[C k+1 ]ij =

Cij ′[Ck ]j ′ j,

j ′ ∈Ni∪{i}





i

[ Ĉ(k)]ij = [C(k)C(k − 1) . . . C(0)]ij, for j ∈ V.


n

k j, j ′

Ĉj ′ j(k)

i

ID(i)

i ∈ V

li(k) ID(j) j ˜ Cij(k)

(i, j) k− C [C k ]ij j ID(j) ∈ li(k)

k = 0 i ∈ V ˜ Cii(k)

˜Cii(0) = 1, li(0) = {ID(i)},

Ni

k ≥ 1 i lj(k)

li(k)

li(k + 1) =

lj(k).

j∈Ni∪{i}


i ˜ Cij(k) ˜ Cij(k) = 0

j

ID(j) ∈ li(k + 1) and ID(j) /∈ li(k).

i ˜ Cij(k) ID(j) ∈ i(k + 1)


˜Cij(k + 1) =

j ′ ∈Ni∪{i},ID(j)∈ j ′(k)

Cij ′ ˜ Cj ′ j(k),

˜ Cij(k) ID(j) ∈ li(k)

li(k)

i ∈ V k ≥ 0

˜Cij(k) = 0 ID(j) /∈ li(k) C


k− C C k



li(0) = {ID(i)}, Ĉii(0) = 1, Ĉij(0) = 0, for j = i,


li(k + 1) =

j∈Ni∪{i}

{lj(k)}, and Ĉij(k + 1) =

j ′ ∈Ni∪{i}

Cij ′Ĉj ′ j, for all j ∈ V.

j /∈ li(k) Ĉij

k = 0 k i ∈ V

j /∈ li(k) Ĉij = 0 j k + 1 j /∈ li(k + 1)

j /∈

j ′ ∈Ni∪{i} {lj ′(k)} j′ ∈ Ni ∪ {i} Ĉj ′ j = 0


Ĉij(k + 1) =

j ′ ∈Ni∪{i}

Cij ′0 = 0.

˜ Cij(k) Ĉij(k)

k ≥ 0 i ∈ V j ∈ li(k) k ≥ 0 i ∈ V j /∈ li(k)

˜ Cij(k) Ĉij(k) = 0


ID(j) ∈ i(k + 1)


˜Cij(k + 1) =

j ′ ∈Ni∪{i},ID(j)∈ j ′(k)

Cij ′ ˜ Cj ′ j(k).

j /∈ lj ′(k) Ĉj ′ j(k) = 0 Ĉj ′ j(k) = ˜ Cj ′ j(k) k k + 1


˜Cij(k + 1) =

Cij ′Ĉj ′

j(k) +

Cij ′0,

j ′ ∈Ni∪{i},ID(j)∈ j ′(k)

Ĉij(k + 1)

j ′ ∈Ni∪{i},ID(j)/∈ j ′(k)


i [C k ]ij j ∈ V

i − th C k

i ID(i)



n

i ∈ V ki li(k) =

li(k − 1)

n = |li(ki)|

ki = min{k | li(k) = li(k − 1)}.

li(k − 1) (k − 1)− i

k

k li(k −1)

i

n = |li(ki)|



C

.

ρ(C) ≤ C, and ρ(C) = lim C

k→∞ k 1

k .

∞− .∞


C k ∞ = max

i∈V

n

|[C k ]ij|,

max−

i i − th C k


C k

ci(k) ˜ Cij(k)

ci(k) =

| ˜ Cij(k)|.

j=1

ID(j)∈li(k)

max − ci(k)

βi(k) = ci(k), βi(k + τ + 1) = max

j∈Ni∪{i} βj(k + τ),


T = diam(G) βi(T )

i ∈ V ci(k)

β1(k + T ) = · · · = βn(k + T ) = max

i∈V ci(k).

β ∗ i (k) i k ≥ 1

β ∗ i (k) = (βi(k + T )) 1

k = (max

j∈V

cj(k)) 1

k .

ci(k) i

i − th C k β ∗ i (k)

k T k + T

max −

T

max −


C

i

C k → ∞ β ∗ i (k)

ρ(C) C

k ≥ 1

lim

k→∞ β∗ i (k) = ρ(C), for all i ∈ V,

( √ n) −1

k β ∗ i (k) ≤ ρ(C) ≤ β ∗ i (k).

k−

C k ˜ Cij(k) = 0 ID(j) /∈ li(k) ci(k)

i − th C k max −

C k β ∗ i (k)

β ∗ i (k) = (C k ∞) 1

k

ρ(C) = lim

C

k→∞ k 1

k



ρ(C) = (ρ(C k )) 1

∞ = lim β

k→∞ ∗ i (k),

k ≤ C k 1

k

∞ = β ∗ i (k),

C

C2 = maxi λi(C 2 ) ρ(C) = maxi |λi(C)|

ρ(C) = C2 = C k 1

k

2 .


C k 2 C k ∞ C k

( √ n) −1 C k ∞ ≤ C k 2

( √ 1


n) k β ∗ i (k) = ( √ 1


n) k C k 1

k

∞ ≤ C k 1

k


2 = ρ(C),

n

λ⋆(L)



L

G C

λ⋆(L) ρ(C)

λ⋆(L)

ρ(C)




C L

C = I − βL − 11 T /n.

λ1(C) = 0, λi(C) = 1 − βλi(L), for i ∈ {2, . . . , n}.

ρ(C) λ⋆(L)

λ⋆(L) = (1 − ρ(C))/β, if 0 < β < 1/λn(L).



L + r11 T /n L i ∈

{2, . . . , n} r

λ1(L + r11 T /n) = r, and λi(L + r11 T /n) = λi(L) for i ∈ {2, . . . , n}.

VL L

VL = 1/ √ n, v2(L), . . . , vn(L)

= 1/ √ n, ˜

VL ,


L

V T L LVL = λL = diag(0, λ2(L), . . . , λn(L)),

L + r11 T /n

V T L


T 1T 1/n 1T VL/ ˜

L + r11 /n VL = λL + r

√ n

˜V T L 1/√n ˜ V T L 11T VL

˜



1 0

= λL + r

0 0

= diag(r, λ2(L), . . . , λn(L)),

1T 1 = n 1T VL

˜ = 0

L C

T

λi(C) = 1 − βλi L + (1/β)11 /n , for i ∈ V,

r = 1/β

β

C

β = ε/λn(L), for some ε ∈ (0, 1),

λ1(C) = 0, and for i ∈ {2, . . . , n},

λi(C) = 1 − ελi(L)/λn(L).

λn(L) ≥ λi(L) > 0 i ∈ {2, . . . , n}

1 > λ2(C) ≥ · · · ≥ λn(C) > λ1(C) = 0,

C λ2(C)

C = I − βL − 11 T /n


1/ √ n v1(C) C

λ1(C) = 0 k ≥ 1

C k = (I − βL − 11 T /n) k = (I − βL) k − 11 T /n,

I − βL



n n β

n β = ε/(2dmax)

β < 1/λn(L) max −

β

k


Ĉ = I − βL

ˆ λi(k) n k

n

n


ε ∈ (0, 1)

β = ε/(2n)

Ĉ = I − βL k ≥ 1 i ∈ V

Ĉij(k) (i, j) k− Ĉ [Ĉk ]ij

ID(j) ∈ li(k)

[ Ĉk ]ij = [C k ]ij + 1/n,

C

k i

ĉi(k) =

| Ĉij(k) − 1/n| + (n − |li(k)|)/n,

ID(j)∈li(k)

max − maxj∈V ĉj(k) ˆ β ∗ i (k)

ˆβ ∗ 1

i (k) = (max ĉj(k)) k .

j∈V

i ∈ V k ≥ 1


ˆλi(k) = 1 − ˆ


βi (k) /β.

i

k → ∞ ˆ λi(k)

λ∗(L)

lim ˆλi(k) = λ∗(L), for i ∈ V,

k→∞

k ≥ 1 λ⋆(L)

ˆλi(k) ≤ λ⋆(L) ≤ ( √ n) −1

k ˆ λi(k) + (1 − ( √ n) −1

k )/β.

β = ε/(2n) 0 < β < 1/λn(L) ε ∈ (0, 1) λn(L) ≤

2dmax < 2n dmax

λ∗(L) = (1 − ρ(C))/β C

C = I − βL − 11 T /n = Ĉ − 11T /n

i ∈ V Ĉij(k) [ Ĉk ]ij ID(j) ∈ li(k) [ Ĉk ]ij = 0

ID(j) /∈ li(k)

[C k ]ij = [ Ĉk ]ij − 1/n, for ID(j) ∈ li(k),

[C k ]ij = −1/n, for ID(j) /∈ li(k),

i ∈ V k ≥ 1 ĉi(k) i−

C k ˆ β ∗ i (k) ˆ β ∗ i (k) = C k 1

k


( √ n) −1

k C k 1

k

∞ ≤ ρ(C) ≤ C k 1

k

∞,

C


˙x(t) = −Lˆx(t) = u(t), ei(t) = ˆxi(t) − xi(t),


|ei(t)| ≤ c1e −α(t)t ,

α


ˆα(k) = ˆ λ(k), for k ∈ N, and α(t) = γ ˆα(k), for t ∈ [k, k + 1),

0 < γ < 1 ˆ λ(k)


ˆα(k) = ˆ λ(k) ≤ λ⋆(L).










σz = 1 σx = 0.01d

σy = 0.01d σθ = 2.5

d


15

10

5

0

−5

−10

r1

r2

r3

gt

F6

−15

−20 −10 0 10

F16

15

10

5

0

−5

−10

gt

Global map

F20

F13 F15

F1

F6

F19

F14

F5

F4

F18

F7

F16

F10

F12 F8 F11

F9

F2

F17

F3

−15

−20 −10 0 10


15

10

5

0

−5

−10

r1

r2

r3

gt

F6

−15

−20 −10 0 10

F16

15

10

5

0

−5

−10

gt

Global map

F9

F13

F1

F5

F6 F7 F8

F12

F2

F17

F19

F3

F20

F15

F14

F18

F4 F16

F10

F11

−15

−20 −10 0 10


15

10

5

0

−5

−10

r1

r2

r3

gt

F6

−15

−20 −10 0 10

F16

15

10

5

0

−5

−10

gt

Global map

F9

F13

F1

F5

F6 F7 F8

F12

F2

F17

F19

F3

F14

F18

F4 F16

F10

F11

F20

F15

−15

−20 −10 0 10





ˆxG, ΣG


F 16 r1


F 16

F 16

F 16



F 6, F 7, F 8, F 3







n = 20

ˆ λi(k) i ∈ V k

ˆ λi(k)

k λ∗(L)

λ∗(L) ( √ n) −1

k λi(k) ˆ +

(1 − ( √ n) −1

k )/β λ∗(L) k

λ∗(L)


I − βL − 11T /n

y(k) = w(k)/normalization cons.(w(k)),

w(k + 1) = (I − βL)y(k) − deflation cons.(y(k)),

Tcons = 10, 25, 50, 100

I − βL w(k)

z(t + 1) = Wz(t)

W

ˆ λi(k)

( √ n) −1

k ˆ λi(k) + (1 − ( √ n) −1

k )/β


i ∈ V w(k +

1) T y(k)/y T (k)y(k)


10

8

6

4

2

0

0 2 4 6 8 10

10 × 10

e = (i, j) ∈ E

4

30

20

10

0

−10

−20

−30

λ * (L)

Estimated λ(k)

Upper−bound

−40

0 20 40 60 80 100

ˆ λi(k) i ∈ V

k

λ∗(L)

λ∗(L) ( √ n) −1

k ˆ λi(k) + (1 − ( √ n) −1

k )/β

λ∗(L) k λ∗(L)




λ∗(L)

Tcons



n


14

12

10

8

6

4

2

0

−2

λ * (L)

Estimated λ(k)

Upper−bound

PowerIt 10

PowerIt 25

PowerIt 50

PowerIt 100

0 200 400 600 800 1000

Iterations


λ∗(L)

Tcons = 10, 20, 50 100


λ∗(L) λ∗(L)

Tcons


14

12

10

8

6

4

2

0

−2

λ * (L)

Estimated λ(k)

Upper−bound

PowerIt 10

PowerIt 25

PowerIt 50

PowerIt 100

0 200 400 600 800 1000

Messages per agent

i n


14

12

10

8

6

4

2

0

−2

λ * (L)

Estimated λ(k)

Upper−bound

PowerIt 10

PowerIt 25

PowerIt 50

PowerIt 100

0 200 400 600 800 1000

Iterations


λ∗(L)

Tcons = 10, 20, 50 100


λ∗(L) λ∗(L)

Tcons


14

12

10

8

6

4

2

0

−2

λ * (L)

Estimated λ(k)

Upper−bound

PowerIt 10

PowerIt 25

PowerIt 50

PowerIt 100

0 200 400 600 800 1000

Messages per agent

i n


60m × 45m













30 m 30 m


20

10

0

−10

−20

−30

−40

−20 −10 0 10 20

9


15

10

5

0

−5

−10

−15

F23

−5 0 5 10 15 20 25






R8

R7

R9

R6

R1

R5

R2

R3

R4

R8

R9

R7

R6

R1





t ((t − 1) mod 9) + 1 (t

mod 9) + 1





I i G (t) ii G (t)

40 IG iG

x



4





R5

R2

R3

R4

R8

R7

R9

R6

R1

X

R5

R2

R3

R4


20000

10000

0

2000

1000

−1000

0 20 40

20000

10000

0

0 20 40

20000

10000

0

0 20 40

Ii G (t) Ii G (t) Ii G (t)

0

0 20 40

2000

1000

0

−1000

0 20 40

2000

1000

0

−1000

0 20 40

ii G (t) ii G (t) iiG (t)

x− 40

Ii G (t)

ii G (t)


xi G (t) ΣiG (t)

i xG

ΣG Qi G (t)

xi G (t) xG Σi G (t) nΣG

Qi G (t) ΣiG (t)


4

2

0

3000

2000

1000

x 10 4

0 20 40

4

2

0

x 10 4

0 20 40

4

2

0

x 10 4

0 20 40

Ii G (t) Ii G (t) Ii G (t)

0

0 20 40

3000

2000

1000

0

0 20 40

3000

2000

1000

0

0 20 40

ii G (t) iiG (t) ii G (t)

Ii G (t) iiG (t)

Ii G (t) x−



IG

γ = |λ2(W )|


W = 1


2

⎢ 1


3 ⎢ 0



1

1

0

1

. . .

0

0

0


0

1

1

0

1

0 0 0 1 2

γ = 0.96 1










t ((t − 1)

mod 9) + 1 (t mod 9) + 1 τ = 8



⎥ .



5.8

5.6

5.4

0.2

−0.2


0 20 40

5.8

5.6

5.4

0 20 40

5.8

5.6

5.4

0 20 40

xi G (t) xiG (t) xiG (t)

0

0 20 40

0.2

0

−0.2

0 20 40

0.2

0

−0.2

0 20 40

Σi G (t) ΣiG (t) ΣiG (t)

x− 40

xi G (t)

Σi G (t) i


xi G (t)

Qi G (t)

ΣG

Qi G (t)

Σi G (t)

nΣG


W (t) t

δ = 0.89




τ = 1

δ = 0.80


0.80 t ≤ 0.96 t ≤ 0.89 t/8 t = 0, 1, . . .





1 5 20


23

22.5

22

0.5

−0.5

0 20 40

23

22.5

22

0 20 40

23

22.5

22

0 20 40

xi G (t) xiG (t) xiG (t)

0

0 20 40

0.5

0

−0.5

0 20 40

0.5

0

−0.5

0 20 40

Σi G (t) ΣiG (t) ΣiG (t)

x− 40

xi G (t)

Σi G (t) i


xi G (t)

Qi G (t)

ΣG

Qi G (t)

Σi G (t)

nΣG

9

1

5 1

1 6 7

9

20






Ii G (t)

ii G (t)



5 5


15

10

5

0

−5

−10

−15


−5 0 5 10 15 20 25

15

10

5

0

−5

−10

−15

−5 0 5 10 15 20 25






xG ΣG



Time (secs.)

1.4

1.2

1

0.8

0.6

0.4

0.2

Fixed graph

Switching graph

Link failure graph

0

0 5 10 15 20 25 30 35 40

Iterations



Ii G (5)



5 5 9

I i G

(t) 9


9


8 9 8 9 15


1 7

16

















γ = 1.8 h = 0.8 λ⋆ = 0.97

L = 500 K = 5

k k + 1 10



l = 25 k =

1, . . . , 4 L − (K − 1)l = 400

l = 50 300 k = K

l = (L/K) = 100

|[I k i (t)]r,s − [I k avg]r,s|/σI

|[i k i (t)]r − [i k avg]r|/σi L

k = 1, . . . , K − 1 l = 100

l = 50

l = 25


6

4

2

0

−2

−4

ST

R1

R2

R3

R4

R5

R6

R7

R8

−5 0 5 10 15 20




5

4

3

2

1

0

−1

−2

−3

−4

−5

R 1

R 8

R 7

R 2

R 3

R 5

R 4

R 6

0 2 4 6 8 10 12


7.5 m

k = K l = 25

l = 50 l = 100


l = 50

l = 100

l = 25 L ˆx k i (t) Σk i (t)

ˆx k G Σk G


k = K

l = 100 L





t l = 100

l = 100 k = 1


6

4

2

0

−2

k=1

k=2

k=3

k=4

k=5

−4

−8 −6 −4 −2 0 2 4 6 8


k = 1, . . . , 5

10 k = 4 k = 5

k = 4 k = 5

k = 4

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

l=25

l=50

l=100

0

k=1 k=2 k=3 k=4 k=5 end

3

2.5

2

1.5

1

0.5

l=25

l=50

l=100

0

k=1 k=2 k=3 k=4 k=5 end

maxi,r,s |[I k i (t)]r,s − [Ik avg ]r,s|/σI maxi,r |[i k i (t)]r − [ik avg ]r|/σi

L maxi,r,s |[I k i (t)]r,s −

[I k avg]r,s|/σI maxi,r |[i k i (t)]r −[i k avg]r|/σi l = 25

k = 1, . . . , K − 1

l = 100

k = 1, . . . , K − 1

l = 50



k = 2, . . . , K


6

4

2

0

−2

−4

ST

GM

R1

R2

R3

R4

R5

R6

R7

R8

−5 0 5 10 15 20

ˆx k i (t) Σk i (t) i = 1

L l = 100


ˆx k G ΣkG k = K


3

2.5

2

1.5

1

0.5

Bound

Algorithm 1

Zero−init

0

k=1 k=2 k=3 k=4 k=5 end

3

2.5

2

1.5

1

0.5

Fixed graph

Switching graph

0

k=1 k=2 k=3 k=4 k=5 end

maxi,r |[i k i (t)]r − [ik avg]r|/σi fk(t)αi/σi + gk(t) maxi,r |[i k i (t)]r − [ik avg]r|/σi

L l = 100


t l = 100



G

t k G


l = 100



G t = k

G G

l = 100


9 9

9












t




C







10%


10

0

−10

−20

−30

−40

−50

−20 −10 0 10 20

8

6

4

2

0

−2

−4

−5 0 5 10 15


Robot 1

Robot 2

Robot 3

Robot 4

Robot 5

Robot 6

Robot 7

Robot 8

Robot 9


9


9


1


(C)

msum




















msum Gcom


Gcom




n


m


(C)

msum




















msum Gcom


Gcom




n


m


(c)

msum


y

1000

500

−500

2000

1000


0

z 0

−1000 −500 0 500 1000 1500

x

y

1000

500

2000

0

1000

z 0

−500

0

x

500

1000

y

1000

0

−1000

2000

1500

1000

500

z

0

−500

x

0

500

y

1000

0

−1000

3000

2000

1000

z 0

−1000

0

x

1000

1000

500

0

−500

−1000

−1000 −500 0 500 1000

x


















y

0 1000

2000

z


y

−1000 500 2000

x



y

z

−2000 0 2000

x












z


y

1000

0

−1000

3000

2000

1000

z

0

−1000

−2000

−2000

−1000







8000

6000

4000

2000

0

−2000

R3

R2

−3000 −2000 −1000 0 1000 2000 3000 4000 5000 6000


R2, R3, R6,

R9

R9

x

0

R6

1000

2000

3000


−200

200 400

0

R9

R7

R5

R2

R6

−600 −400−200 0 200 400

R8

R4

500

R3

0

R1

−500



−1000

R8

R8

R8

R2

R2

R2

R5

R4

R5

R4

R4

R5

500

500

500

R9

R9

R9

R3

R3

R3

0

0

0

R1

−200

200 400

R1

0 R7

R6

−500−200

200 400

0 R7

R6

−500−200

200 400

R1

0 R7

R6

−500

−600

−400−200 −600 −600

−400−200 −400−200

0 −1000

200 0 −1000

0 −1000

400

200 200

400

400


R9

R5

R2

R8

R4

500

R3 0

R9

R5

R2

R8

R4

500

R3

0

R9

R5

R2

R8

R4

500

R3

0

−200

200 400

0 R7

−600

−400−200

R1

R6

−500−200

200 400

0 R7

−600

−400−200

0 −1000

200

400

R1

R6

0

200

400

−500−200

200 400

0 R7

−600

−400−200

−1000

R1

R6

0

200

400

−500

−1000







R1 R1


R1


R1




2000

1000

0

−1000

−1000

2000

1000

0

−1000

−1000

0

0

1000

1000

2000

3000

2000

1000

−1000

−1000

Common

Exclusive (R1)

4000

3000

2000

1000

0

−1000

0

0

1000

2000

3000

0

−1000

Common

Exclusive (R1)

Exclusive (R3)

Exclusive (R6)

2000

1000

0

−1000

−1000

4000 0

3000

2000

1000

1000

2000


2000

3000

0

−1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R9)

4000

3000

2000

1000

2000

1000

0

−1000

−1000

0

1000

2000

3000

0

−1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8)

Exclusive (R9)

4000

3000

2000

1000


2000

1000

0

−1000

−1000

0

1000

2000

3000

3000

0

−1000

0

−1000

Common

Exclusive (R1)

Exclusive (R3)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

4000

3000

2000

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive 4000 (R8)

Exclusive 3000 (R9)

2000

1000

R1 0 − 5


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


2000

1000

0

−1000

−1000

0

1000

2000

3000

−1000

0

1000

Common

Exclusive (R1)

Exclusive (R2)

Exclusive (R3)

Exclusive (R4)

Exclusive (R5)

Exclusive (R6)

Exclusive (R7)

Exclusive (R8) 4000

Exclusive 3000 (R9)

2000

R1


O(n)


O(n)


p


p


p


i ∈ V zi(0) ∈ R

W ∈ R n×n

≥0 Wi,j > 0 (i, j) ∈ E Wi,j = 0

j /∈ Ni Wi,i ∈ [α, 1] Wi,j ∈ {0} ∪ [α, 1] i, j ∈ V

α ∈ (0, 1] G i ∈ V

zi(t) t ≥ 0

n

zi(t + 1) = Wi,j zj(t),

j=1

t → ∞ zi(t) i ∈ V


lim

t→∞ zi(t) = z⋆ = 1

n

zj(0),

n

i ∈ V i zi(t)

Wi,j = 0

j /∈ Ni e(t) = (z1(t), . . . , zn(t)) T − (z⋆, . . . , z⋆) T

t t ||e(t)||2/||e(0)||2 < ɛ

n 2 log(ɛ −1 )


W ∈ R n×n


Wi,j =




1

1+max{|Ni|,|Nj|}

j=1

if j ∈ Ni, j = i,

0 if j /∈ Ni, j = i,

1 −

j∈Ni Wi,j, if j = i,


i, j ∈ V j = i |Ni| |Nj| i j


W


60×45m


20

10

0

−10

−20

−30

−40

−20 −10 0 10 20

9

Hooray! Your file is uploaded and ready to be published.

Saved successfully!

Ooh no, something went wrong!