Mobile Robots Localisation

cole Centrale de Nantes

Gatan GARCIA

Objectives
Understand the general concepts of localization. Understand the interest of hybrid localization algorithms. Grasp the basics of the Kalman filter, which is the most common algorithm in localization problems. Be able to use observability criteria to analyze localization problems.

Course outline
Chapter I Introductory material

!efinitions "pplication e#amples

Chapter II "bsolute localisation methods Chapter III $elative localisation methods% odometry Chapter I& '#ample of se(uential use of relative and absolute localisations.

Course outline
Chapter & )ybrid localisation method using a Kalman filter Chapter &I *ocalisation and observability Chapter &II +he G,- system

Relations with other mo ules

+he .inematic model is the basic model used in localization. +he last part of the course /observability0 has relations with non linear systems. +he Kalman 1ilter has relations with -ignal ,rocessing. But, as a whole, this part of the course is fairly independant from the rest.

Cha!ter I
Intro uctor" material

Mobile robot $ %unctional

" priori .nowledge *ocalisation2 mapping -peed, accel... !istances, angles 7bstacle positions ... ,roprioceptive data
'n

ia&ram

3ission

4 ,osition 5, map
4 ,o

,lanning !esired tra6ectories

sitio n

,erception

'#te rocep ti data ve

onment r i v

,osition, speed, actuator tor(ues

Control

'e(initions
Pose of a solid% its position and orientation in 8!, characterized by 9 independent parameters. Localisation% determination of some or all variables of the pose /usually only those necessary to e#ecute a given tas.0. -everal ways to represent the pose. :ot always 9 dof to determine% for e#ample localisation in ;!.

)he classical (rames

Classes o( localisation
Static localisation% the vehicle is motionless while sensor data necessary for localisation is read and processed. Dynamic localisation% the vehicle moves while the sensor data is read and processed. Quasi-static localisation% the vehicle does move but while sensor data are read and processed, but the motion is negligible. /always chec. the hypothesis0.

Other classes o( localisation

Relative localisation% performed wrt the /.nown0 initial location
sensor data allows to calculate elementary displacements of the vehicule, which are integrated. Uses proprioceptive sensors /encoders, accelerometers, gyrometers, !oppler radars...0

Absolute localisation% performed wrt a fi#ed frame in the environment.

Uses exteroceptive sensors /goniometers, range finders...0.

Absolute localisation* characteristics

*ocalisation wrt environment. 1re(uency of sensor readings often low. -ensor data availability often asynchronous /sometimes due to vehicule motion0. !ata processing sometimes creates time delays /e.g. &ision0. :umber of e#ternal measurements can vary /e.g. loss of a G,- satellite0. ,recision varies with the number of beacons and their position.

Relative localisation* characteristics

:o reference to environment, and hence always available /e.g. even in the fog for the sailor0. ,ossibility to use high fre(uencies. ,ossibility to use a constant calculation period. ,rocessing time of data usually short. ,recision degrades with time or traveled distance.

)he +ol , sailor metho * relative an absolute localisations

sensors% log < compass /< map of currents0 'stimated tk path i t1 t0 Initial position vi 1 2 $eal path :

-"bri localisation* intermi.in& absolute an relative

Idea% associate e#ternal and internal sensors. Goal% $etain advantages of absolute and relative localisations, which are complementary.
begin Calculate initial location loop (fast) Read internal sensors (e.g. Encoders) Calculate relative localisation if external sensor data available then Update estimated location endif endloop end

Cha!ter II
Absolute localisation metho s

)he surve"or/s !roblem

*ocalisation of a point from two .nown ones.
P Known point Kn.nown point A 1 ; B

1 /resp. 20 is measured by setting a goniometer above A /resp. B0 and measuring the angular separation between A /resp. B0 and P. )ow to calculate the position of P =

)he surve"or/s metho

y b1 1 d b2 2 Known point Un.nown point # Baseline /.nown0

0recision o( the surve"or/s metho

the measurement vector /inputs0 the covariance matri# of M the position vector of point P

:B % +his method is general.

)hin&s to remember
It is often possible to simplify calculations by choosing a convenient local frame. In the case of goniometric systems, it is often useful to use distances as intermediate variables. By linearizing an input>output relation, it is possible to calculate the covariance matri# of the errors on output variables as a function of the covariance matri# of input variables.

Localisation usin& istances to 1nown !oints

Un.nown point Known point

-ystem%

;nd solution with two measures

2' static localisation usin& a2imuth an&les

beacons azimuth angles

Un.nown robot posture

-ow to calculate the !osture

Beacon coordinates in robot frame Rm%

Inter>beacon distances are invariant /8 e(uations0%

Note% the use of distances as @ !educe beacon coordinates in Rm. intermediate variables yields a simpler system @ Calculate mTb. /try with /x,y,0 if @ Calculate 0Tm. you?re not convinced0 @ 1ind b1, b2 and b3.

3' static localisation usin& a2imuth an elevation an&les

,roblem% .nowing 1, 1, 2, 2, 3, 3, calculate the position and attitude of the sensor frame RS for .now beacon positions B1 , B2 , B3 .

A !ossible im!lementation
rotation a#is CC! azimuth Beacon /.nown position0 elevation optic a#is encoder

-ow to calculate the !osition an attitu e

Beacon coordinates in robot frame Rm%

Inter>beacon distances%

+hen b1, b2, b3 yield 0Tm as in the 2D case.

Using the intermediate variables allows to solve a nonlinear system with only 3 unknowns instead of 6!

'"namic localisation b" a eterministic metho

Goal% localise the robot while it moves, with the beacons detected at different locations. Requires% hypotheses on the motion.
OT B

/.nown0 (c o n n u e )

BT C i /calculated0 (c a lc u l e )

C i

B
tra6ectory t r a je c t o i r e
O M i

/constant C iT M i ( t r a n s la t io n

c.nown o n s tan te co n n u e ) translation0

OT r s u l taresult0 t f i n a l) M i (/final

)he measurements in the "namic case

+he graph corresponds to a straight line motion between measurement instants t0, ti, tj.

)he sim!lest h"!otheses

*ocally%
+he rolling surface is appro#imated by a plane. +he motion is a straight line at constant speed. +he speed is parallel to a#is xm.

Conse(uences%
-even un.nowns% position < attitude < speed. +he hypotheses must be satisfied during the time needed to detect a sufficient number of beacons to determine the seven un.nowns.

-ow to solve
It is necessary to detect four beacons /one of three beacons seen twice at different locations0 -imilar to previous cases%
Arite beacon coordinates in sensor frame corresponding to time t0 /first beacon detection0. Arite dij2 /there are more than necessary0. -olve in the least s(uares sense. !etermine position and attitude at time t0 plus speed.

4hat are the !roblems5

If the model of vehicle motion is very simple, it may not correspond to reality. If the model is more general, the number of un.nowns grows. )ence, the model must be valid for a longer time period /detecting more beacons is necessary to obtain more e(uations0. In any case, the need for a motion model limits the motion dynamics that can be handled by the system.

Cha!ter III
A relative localisation metho * o ometr"

Recall* 6inematic mo el o( a +278, t"!e robot

Under pure rolling hypothesis /see .inematic modelling course0%

Mo el un er iscrete (orm 9:uations o( o ometr"

Beware% there are several forms for the e(uations of odometry /first order e(uivalence0.

Alternate (orms

0osture errors in robot (rame

Preal Pest yest

ey

ex

xest

e % heading error e x % longitudinal error Interpretation in the case of small e% ey % lateral error

0osture errors in robot (rame

Re(erence trajector"
1 0 .8 0 .6 0 .4 x R e f, y R e f (m ) 0 .2 y (m ) 0 -0 .2 -0 .4 -0 .6 -0 .8 -1 -0 .5 0 x (m ) 0 .5 1 0 .8 0 .6 0 .4 xR ef and yR ef

Initial location

0 .2 0 -0 .2 -0 .4 -0 .6 -0 .8 -1 0 1 2 3 t (s ) 4 5 6 7

$eal robot path

x/t0 and y/t0

@ +he real tra6ectory is always the circle% imagine that something forces the robot to follow the circle at a constant speed. @ +he various estimated tra6ectories will be different.

O ometr" in a near$!er(ect worl

1 0 .5 0 -0 .5 -1 -0 .5 0 0 .5 1 1 0 .5 0 -0 .5 -1 0 x 10
-3

x 10

-3

y /m0

x /m0
0 .0 1 5 0 0 .0 0 5 -5 0 -1 0

ex /m0

2 4 6 8 t /s0 t /s0 0 :o model errors. )igh sampling rate. )igh encoder resolution 0 2 4 6 8

ey /m0

e /B0
t /s0

O ometr" with a wheel ra ius error

0 0 .5 -5 -1 0 -1 5

y /m0

0 -0 .5 -1 0 -0 .5 0 0 .5 1

x /m0

0 .0 2 0 -0 .0 2 -0 .0 4 0 2 4 6 8 -0 .0 6 0 2 4 6 8

-0 .0 5

ex /m0

-0 .1 -0 .1 5 -0 .2

t /s0 t /s0 <CD wheel radius error /right wheel0 is already a severe error E

ey /m0

e /B0
t /s0

O ometr" with a wheelbase error

1 0 .5 5 4 3 2 1 -1 -0 .5 0 0 .5 1 0 0 2 4 6 8

y /m0

0 -0 .5 -1

x /m0

0 .0 8 0 .0 6 0 .0 4 0 .0 2 0 0 2 4 6 8

0 .0 3 0 .0 2 0 .0 1 0 -0 .0 1 0 2 4 6 8

ex /m0

t /s0 t /s0 F mm 2 GHH mm error on wheelbase. -imilar problems.

ey /m0

e /B0
t /s0

O ometr"* low resolution enco ers

1 1

y /m0

0 -0 .5 -1 -0 .5 0 0 .5 1

0 -0 .5 -1 0 x 10
-3

x /m0

0 .0 1

2 1

ex /m0

0 .0 0 5

0 -1

t /s0- 2 0

t /s0

*ow resolution encoders /here CHH dots per revolution0 generate small errors compared to geometrical model errors

ey /m0

e /B0
t /s0

0 .5

0 .5

O ometr"* low sam!lin& rate

1 0 .5 0 .0 1

y /m0

0 -0 .0 0 5 -1 -0 .5 0 0 .5 1 -0 .0 1 0 2 4 6 8

-0 .5

x /m0

0 .0 2 0 .0 1 5

0 .0 1 0 .0 0 5 0 -0 .0 0 5 0 2 4 6 8 -0 .0 1

ex /m0

0 .0 1 0 .0 0 5 0

2 4 6 8 t /s0 t /s0 0 7dometry can do with low sampling rates /here FH )z0. "nd calculations are simple.

ey /m0

e /B0
t /s0

0 .0 0 5

;!ee estimation* hi&h resolution ecreases noise

13 12

/rd2s0

11

"ctual speed 'stimated speed /CHH dots0 'stimated speed /CHHH dots0
0 0 .1 0 .2 0 .3 0 .4 0 .5 0 .6 0 .7 0 .8 0 .9 1

10

t /s0 " higher resolution /CHHH dots in green, CHH in red0 reduces noise on speed estimate by numerical difference. Useful if you use speed estimation in your control for e#ample.
6

;!ee estimation* hi&h sam!lin& rate increases noise

13 12

11

"ctual speed 'stimated speed /CHHH )z0 'stimated speed /CHH )z0

/rd2s0

10

t /s0 " higher sampling rate /CHHH )z in red, CHH )z in green0 increases noise on speed estimate by numerical difference.
0 0 .0 1 0 .0 2 0 .0 3 0 .0 4 0 .0 5 0 .0 6 0 .0 7 0 .0 8 0 .0 9 0 .1

Classes o( o ometr" errors

-ystematic errors%
Aheel radius and wheelbase errors 3isalignment of wheels 1inite resolution and sampling rate

:on systematic errors%

Uneven floor /including une#pected ob6ects0 Aheel slippage due to%
@ @ @ @ -lippery floor 7veracceleration and fast turning /s.idding0. '#ternal forces 2 internal forces /castor wheels0 :on>point wheel>to>floor contact.

Relative im!ortance o( errors

-ystematic errors%
+hey are very serious because their effects accumulate over time. Usually dominant on smooth indoor surfaces.

:on systematic errors%

!ominate on rough, irregular terrains. !ifficult to handle because une#pected. Can cause complete failure of error evolution models based on statistical worst case analysis /for e#ample used to empirically determine inter>landmar. distances0.

<ni irectional s:uare$!ath test

'#amples from Borenstein and 1eng @ Use distances from three robot points to the wall to measure actual position. @ Compare odometry estimated position to actual position for several tests% set of return osition errors. @ !o not compare return position to start position /would include controller errors0.

error I final position

calculated position

<ni irectional s:uare$!ath test

+wo different problems yield the same result...

<ni irectional s:uare$!ath test

@ By modifying the wheelbase only, the user obtains a very small final error. @ But there are intermediate errors. 'rrors compensate in the final position, due to the path being performed in a single direction. @ !he unidirectional square" ath test is not a good test#

=i irectional s:uare$!ath test

@ By performing the test in both directions, the previous problem is overcome. @ +he errors which compensated in one direction sum up in the other. @+his test imagined by Borenstein J 1eng is also called the University of 3ichigan Benchmar. or U3Bmar..

0ractical consi erations

7rientation errors grows faster for vehicules with short wheelbase. Castor wheels can induce slippage when reversing direction, especially when they bear a large part of the load. 7dometry wheels should be thin and not compressible, which may not be compatible with load bearing. Usually possible for small, lightweight robots with little power.

O ometr" with au.iliar" wheels

,rinciple% add a pair of thin, non load bearing au#iliary wheels, typically made of metal with a thin rubber band.

1igure from Borenstein J 1eng

)he enco er trailer

In some cases, li.e trac.ed vehicles, it is e#tremely difficult to use odometry. +he encoder trailer is a solution... if ground allowsE

+he University of 3ichigan encoder trailer

)he robot >?Melo "?@

Results with Melo "

C h e m in ,ath

C;H

120

#B

B3

115 CCF 110 CCH

7dometry o d o m K trie
s e u le

y /m0 y / m CHF 0105

100 CHH 95 LF f i l t r e ; !path '#act B B ;# B2 85 90 95 100

NF LH LF CHH # / m 0 x /m0 +he e#periment was conducted on a lawn on uneven, wet terrain. 'rror C.CF m for a ;9 m path /appro#. G.G D0 with non $ero initial error#

LH9 0

# B1
C

MF

75

80

NH

O ometr" on non !lanar sur(aces

0osition an
-tate% w.r.t. x0 w.r.t. horizontal plane Comparison with O, ,, $% ya I !it"# I >d"
Q
H

attitu e !arameters
h o r i z o n t a l plane p la n e )orizontal le ft>h a n d e d o r t h ohanded gonal *eft trih e d ro n orthogonal
trihedron

dc dv dv d" z y

d"% declivity dv% orthogonal cross>fall

# P
H

d c % g r a d ie n t d v % o r th o g o n a l c r o s s "fa ll

Interest of d", dv% directly measurable by inclinometers.

O ometr" on non !lanar sur(aces

Internal sensors setu!

1rame

,endulum

&iscous fluid ,endular inclinometer

O ometr" on non !lanar sur(aces

,roblem% find relations
Q

ro llin g p la n e b e tw e e n tim e in s ta n ts i a n d i+ 1

a
7 zi

R n s
&
z i< C

&

v
y i< C

inclinometer measurements
#
i

yi

'rinci le i P # i< C 'ncoders give $ %i .nown 0TSi .nown 0R& .nown rolling plane & in this plane, odometric displacement using a ;! model pro6ection onto R0 gives 'x, 'y, '(, ') and hence %i<1.

i< C

O ometr" on non !lanar sur(aces

'lementary translation%

'lementary rotation%

O ometr" on non !lanar sur(aces

+he term since

is always defined

Cha!ter IA
9.am!le o( se:uential use o( relative an absolute localisations

;"stem

escri!tion
ym
*inear detector

e M

xm
,ermanent magnet

@ 3agnetic bloc.s are inserted in the ground at .nown locations. @ +he robot is e(uipped with a linear detector which measures the position of the bloc. wrt the center of the detector, when crossed.

0rinci!le o( o!eration
+he principle of operation is similar to the /old0 sailor method. Between two passages over magnetic stations, the robot uses its wheel encoders for relative localisation. Ahen it crosses a magnetic station, it calculates its posture relative to the station /and hence its absolute posture0 and forgets its previous relative localisation. !istance between stations depends on odometry performance.

Absolute localisation e:uations

*D Rm/t10 ym/t10 rel P2/x!2,y!20 3easurement d2 /here R H0 xm/t10 M2/x2,y20 3easurement d1 /here S H0 P1/x!1,y!10 (y othesis% the robot crosses the station in a straight line motion xm/t20

Rm/t20

Absolute localisation e:uations

$obot point above magnetic bloc. ;%

*ocalisation result%

Localisation results
1 5 0 -5 -1 0 -1 0 .1 -0 .5 0 0 .5 1 -1 5 0 2 4 6 8

y /m0

0 -0 .5 -1

x /m0

0 .0 5

ex /m0

-0 .0 5 -0 .1 0 2 4 6 8 -0 .1 5 0 2 4 6 8

-0 .0 5 -0 .1

$esults with right radius error of CHD

ey /m0
t /s0

0 .0 5

e /B0
t /s0

0 .5

%urther results

Let/s switch to an interactive emo un er MatlabBBB

Cha!ter A
-"bri localisation metho usin& a 6alman (ilter

)he robot an
B2 y0 ym y +l 2

its sensors
xm B1

+r

B3

x0

+he sensor is a rotating sensor. It detects only one beacon at a time, and not fre(uently /it is slow0. Its motion between two measurements cannot be neglected.

4hat

o we want to 1now5

Ae want to determine the vehicule position and heading ,ITx, y, U+. Ae shall be satisfied with .nowing , at discrete time instants% ,kITxk, yk, kUt with tk<1>tkIT. Ae suppose that ,0 is .nown with a certain precision% Vust start motionless and use the method seen in chapter II.

4hen no beacon is
Use odometry%

etecte

Under a more synthetic form%

+hese e(uations are the standard form of an evolution model. , is called the state vector.

=ut these e:uations are not !er(ect

" standard way to ta.e into account these imperfections is to represent them by an additive noise.

+ypically, k is characterised by its covariance matri# -. But what to do with - = )hat we want% reflect the fact that our confidence in , decreases with successive applications of odometry e(uations without e#ternal measurements.

Mo ellin& how !recision evolves urin& o ometr"

$emember% if , and . are two random vectors with a linear relation between them . I M.,, then % Py I M./Px./MT +he relation between ,k<1 and ,k is not linear, but it is possible to linearize it. "round which point = :ot much choice% around ,k.

)he sensor
3easurement% j

etects a beacon

+he measurement depends on the actual position and heading of the vehicule, in a way we can write e#plicitly%

Ahat to do with j = Ahat information does it bear = If actual and estimated positions were e(ual what would we see = Ae would measure%

Ahy not correct , proportionnally to

W=

4hat terms shoul the correction inclu e 5

@ % the innovation /.j, size m#C in the general case0 @ +he (uality of the measurements, characterised by -0 /m#m0 @ +he last available estimate% via its supposed (uality% /1#C0 /1#10

@ )ow a small vehicle displacement influences the measurement around the current estimated position% /m#10

)he u! ate e:uations

Covariance matri# of /m#m0 +he correction gain should be inversely proportional to this term. Classical Kalman filter e(uations%

Comment on u! ate &ain

B y x

+he update calculation will not influence state variable x

Overall al&orithm
)hat ha ens at time t k *1
++ 'rediction hase, 22 $ead sensors and calculate input% 22 Calculate linear appro#. of system% If an e#ternal measurement . has been obtained during last period then ++ Consider ./ corresponds to time 22 tk<1. ,erform estimation hase, 22 Calculate gain%

22 ,redict state % 22 ,ropagate error% endif

++ Update state%

Coherence test
Consider the sensor which detects light beacons. Ahat happens if it detects the reflection of a beacon on a window= It is possible to test the coherence of a measurement by calculating the associated 3ahalanobis distance%

If d S threshold, the measurement is coherent and can be used in the estimation phase. :ote that d depends on the beacon /via 20. Calculating the values of d for each beacon allows to identify which one yielded the measurement% no need for signed beacons

9.!erimental setu!
- I$ ' 3

" vant

H .C m

H .; N F m

C .C F H m

Because the sensor is not above point M, the e(uations used are slightly different from those presented before.

9.!erimental setu!
cam Kra
sen sor A h ite ro p e

C a sto r w h e e l

cam era

* in e a r c a m e r a to tr a c . th e w h ite ro p e

@ +he robot trac.s a white rope tightened between .now points. @ +he points have been precisely localised by surveyors with sub>centimeter accuracy.

;ensor characteristics
H.HLB in azimuth H.HCB in elevation /not used in ;! localisation0 1or a beacon at GH m, it corresponds to 9.F cm error in x>y and N mm in (. -low sensor% C revolution in F s.

9.!erimental results
C,ath h e m in
0 .0 8 0 .0 6 0 .0 4 0 .0 2

120

#B

<8syy curve

115 110 o d o m K trie 7dometry s e u le

/m 0

0 -0 .0 2 -0 .0 4 -0 .0 6 -0 .0 8 -0 .1 50 100 150 200 250 300 350 400 450

y /m 0 1 0 5

>8syy curve
t /s0

100 95 90

'#act path
B

filtre ; !

B2B #
;

75

80

85

90

95

100

# /m 0

@ +he /estimated0 is e#tracted from the covariance matri#. @ +he error signal should always be in the <2> 8 interval. @ +he increase of at the end of the path reflects a less favorable beacon>vehicle configuration. @ B2 is sometimes missed at the end% things get worse...

9.!erimental results
18000

0 .1
0 .0 8

16000

14000
0 .0 6 0 .0 4 0 .0 2

12000

10000

20

40

60

80

: u m b e r o f m e a s u re m e n ts o n B C

8000

3ahalanobis distance calculated with B1 when B1 is actually detected

6000

3ahalanobis distances calculated with B2 and B3 when B1 is actually detected

9.!erimental results
0 .8 0 .6 0 .4 0 .2 8 6 4 2

/d eg 0
0 -0 .2 -0 .4 -0 .6 -0 .8 0 10 0 200 300 400 500

/d eg 0
0 -2 -4 -6 -8 0 10 0 200 300 400 500

t /s0

t /s0

)eading error

)eading error with corresponding estimated <2> 8.

9.!erimental results
0 ,0 3 0 ,0 1 /m 0 -0 ,0 1 -0 ,0 3 96 9 1 .5 87 8 2 .5 ' s tim a te d P 78 7 3 .5 69

r o ll a n g le

O errors are repeatable over three tests along the same path

CB of roll angle generates 8 cm lateral error

9.!erimental results
f ir s t r e v o lu t io n 0 .2
/m 0

; n d r e v o lu t io n

K a lm a n f ilt e r

o d o m e try
B3 120 115

0 .1 0 -0 .1 -0 .2 -0 .3 -0 .4

y /m 0
110 105 100 95

* o c a lis a t io n w it h o u t t a . in g in t o a c c o u n t s e n s o r b a c . w a r d s h if t 0 100 200 300 400 500 600 700

90

B1 80 90

B2 100

# /m 0
t /s0

800

+rac.ed path

General e:uations

1undamental% +he output vector . must be a function of the state vector only. -ny arameter in the e. ression of 3 which is not from , must be a known constant.

9.ercise
xm y0 ym y d1 x B1/H,H0 d2 B2/d,H0 -tate % Tx,y,U+ Arite the observation e(uation. Calculate matri# C of the Kalman filter x0
M

;ame e.ercise
xm y0 ym y d1 x B1/H,H0
M

d2 B2/d,H0

x0

Cha!ter AI
Localisation an observabilit"

Cotion o( observabilit"

7bservability% possibility to reconstruct the state x .nowing the inputs 4 and the outputs y of the system. Xuestions%
Is state estimation always possible= "re there situations when the estimator diverges=

7bservability tests help determine the dangerous situations.

Continuous mo el o( our localisation !roblem

Aith three beacons

Generic observabilit" o( 'io! an %liess

A system is observable if and only if it is possible to express the state vector as a function of the input vector, the output vector and their time derivatives.

:ote%

+he system is generically observable if the set of points for which this condition is not satisfied is a zero>measure set /or null set0.

Lie

erivative

Let g be a differentiable real function, and f a real vector function. The Lie derivative of g with respect to f is the vector field:

+he derivatives of the output with respect to time yield%

Observabilit" ran1 con ition

*et the observability matri# 5 be defined as%

Ahere

is the gradient vector

of real>valued function # with respect to state vector ,. f the matrix ! has full ran" #n$ then the system is observable.

Observabilit" with three beacons

Consider the sub>matri# 7C% xm Ahenever det/7C0 is non zero, observability is granted. C Aith% B1 8 B3 /2 0

; B1

det/510IH on the circle /20 which passes through the three /non aligned0 beacons. 'lsewhere, observability is granted. 7n the circle, we do not .now yet.

Ran1 con ition on the circle

If v/ IH then +hen

and

)ence if the translational speed is zero, det/50 I det/510 and ra1k/50 S 8. -till, this does not prove that the system is not observable, because the rank condition is a sufficient condition but is not a necessary condition.

motionless robot on the circle

1 .2 1 0 .8 0 .6 0 .4 0 .2 0 -0 .2 -0 .4 -0 .6 0 100 200
) e a d in g e rro rs t in s degree

;imulation

300

400

500

600

+he estimator does not converge% it cannot drive the initial error to zero.

;imulation $ movin& on the circle

1 0 .8 0 .6 0 .4 0 .2 0 -0 .2 -0 .4 -0 .6 -0 .8 0 10 0 2 00

d e g re e

) e a d in g ' rro rs
3 00 400 500 600

C o n s ta n t s p e e d I H .; m 2s

t in s

"fter a transient phase, the error curves with and without initial error are 4 the same 5. ,recision is not as good as outside the circle. +his e#ample illustrates the influence of the input on observability, for a non linear system.

;"stem with two beacons

"s p S n, observation cannot be obtained using the azimuth angles /outputs0 alone. +he input /speed and rotation speed0 will have to play a part...

If v I H%

+he sufficient condition will never be met if the vehicule does not move. ,retty logical, though. But what if it moves=

)wo beacons an

movin& robot

+he analysis of the ran. of 7 becomes comple# as higher derivatives need to be used. By studying the determinants of the various s(uare sub>matrices of 5 with a formal calculation software, we found three potentially dangerous vehicle motions%

*1 *3 B1

*2

B2

;imulations on D D
1 .5

d e g re e

108 106

104 102

e s tim a te d p a th

0 .5

W ith I n itia l E r r o r

100 98

re a l p a th beacon
81 8 1 .1 8 1 .2 8 1 .3 8 1 .4 8 1 .5 8 1 .6 8 1 .7

96 94

-0 .5

) e a d in g ' rro rs
0 100 200 300 400 500

t in s
600

92 8 0 .9

Ahen the vehicle moves along *1, the observer does not converge. Remark% our observer converges for other straight line paths through B1, other than *1. 7ther researchers tested a least> s(uare observer which did not converge% observability does not guarantee that a particular observer will converge...

;"stem with one beacon

Oou didn?t e#pect a miracle, did you= +he system is never observable. In simulation, the algorithm seems to diverge slower than odometry. "fter all, some information is better than no information...

;ome conclusions
7bservability analysis allows to find dangerous situations which are sometimes not easy to suspect intuitively. 7bservability is not a 4 binary 5 phenomenon. Ahen the vehicle comes close to non observable situations, the precision degrades and numerical problems can appear. -o much for zero>measure setsE