3 Datafus Kalman Selfloc v08 Ajs

Data Fusion, Kalman Filtering, Self Localiz.
Data Fusion
Kalman Filtering
Self Localization
Armando Jorge Sousa

http://www.fe.up.pt/asousa
asousa@fe.up.pt
Faculty of Engineering, University of Porto, Portugal

Department of Electrical and Computer Engineering
Institute of Systems and Robotics, Porto
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
Summary
Data Fusion
Definition, need, what is improved, strategies, healthy start
Kalman Filtering
State Space, discrete time, stchastic, KF algorithm, iterative
EKF algorithm, pitfalls
Self Localization as an example of EKF as an example of Data Fusion

Example wheelchair, using EKF, System Model, Measurements &
Model, Results
Final Comments
Definition of Data Fusion

Multisensory data fusion is the process of combining observations
from a number of different sensors to provide a robust and
complete description of an environment or process of interest.
Data fusion finds wide application in many areas of robotics such as
object recognition (perception), environment mapping, and
localization
[adapted from Springer Handbook of Robotics
Bruno Siciliano; Oussama Khatib (Eds.) 2008]
Combine data from multiple sources will be more efficient

than if they were achieved by means of a single source
[www.wikipedia.org]
Need for Data Fusion

Sensor Imperfection
Sensor Malfunctioning
Technological limitations
Physical working model limitations (ranges, etc)
Complexity / Indirect Measuring
Sensory Redundancy => COTS are very interesting
Real Time issues
Distributed sensing needs distributed data fusion
Improvements After Data Fusion

Representation
Example: resolution
Certainty
Example: likelihood
Accuracy
Example: reduce outliers
[Mitchell, 2007]
Strategies for Data Fusion

Fusion across different Sensors
Example: several temperature sensors
Fusion across different Attributes

Example: Temperature, pressure humidity to determine air
refractive index
Fusion across different Domains

Example: different ranges / domains
Fusion across different Time

Example: Sampling over space and time
[adapted Boujemaa and Forbes, 2007]
Going to Kalman Filtering

Sensor Model
Example: probabilistic
Framework
The Kalman Filter <= the only addressed
Bayes Rule
Sequential Monte Carlo Methods
Probabilistic Grids
Alternatives to Probability Example: Heuristics!
Kalman Filtering
R. Kalman, 1960
Known as Integration Workhorse Levy, 1997
Filter Weighed merging of System Estimate and
Measurements originated Updates
The Kalman filter is a recursive optimal linear estimator
with theoretical guarantee of convergence
Starting with State Space representation
Where measurements affect system estimate!
State Space & Discrete Time

d x t
= Ax t Bu t
dt
y t = Cx t
x =[ x 1 x 2x m ]
dim A=mm
u=[u1 u2u p ]
dim B= pm
T
y=[ y 1 y 2y q ]
dim C =mq
t k =t k tk 1
x k
y k
=
x k 1
= Hx k
Gu k 1
k =e A t
tk
G= e A t B dt
k
tk 1
H =C
State Space - Stochastic
xk = x k 1Gu k 1wk 1
y t = Hx k v k 1
{
{
Rw k , i=
Qk se i=k
0
se ik
Rv k , i =
R k se i=k
0
se ik
E w k viT =0 , k , i
cov x 0=P0
P w~ N 0, Q
P v~N 0, R
Kalman Filter always keeps

track of averages and
covariances !
E x 0= X 0
cov y 1 , y 1 cov y 1 , y 2
2
cov y 2 , y 1 cov y 1 , y 1
P =
cov y n , y 1 cov y 1 , y 1
cov y 1 , y n
cov y 1 , y n
cov y n , y n
10
KF - Algorithm
Predict Stage
Update Stage
Kalman Gain Calculation:
1
K k = P k H T HP k H T R k
Propagate State:
State Update with new Measure:
x k = x k 1Bu k 1
x k = x k K k y k H x k
Propagate Covariance:
Update Covariance with new

measurement:
P k = P k 1 T Q k
P k = I K k H P k
Initial Estimates: x0 , P0
11
KF - hints
The Kalman Gain weighs changes on system state:

If measurement noise is low => update based
on measure is more important
If measurement noise is high => update based
on system model is more important
What if no measure is available ?
(also: read literature for information filters)
12
KF Iterative
Initial(State, Covariance)
Cycle:
Propagate (Stat,Covar,SysModel,Inputs)
Cycle Meas(i):
If Valid(Meas(i)) then
Update(Stat,Covar,MeasModel(i),Meas(i))
This is an abuse => no guarantees!
13
Extended Kalman Filter

d X t
= f X t , u t k , t ,
dt
t]t k 1 , t k ]
Ak =
f
x
x=xt k
u=ut k
t=t k
Ak =exp Ak t k t k1
x k x k A x k 1 x k W w k 1
y k y k h x k x k V v k
[ ] [ ] [ ][ ]
f1
x1
f2
f
A=
= x1
x
fn
x1
f1
f1
x2
xn
f2
f2
x2
xn
fn
fn
x2
xn
h1
x1
h2
h
H = = x1
x
hq
x1
h1
h2
h2
h2
hq
h2
h 1
xn
h 2
xn
hq
xn
f1
w1
f2
f
W=
=
w w1
fn
w1
f1
f1
w2
wn
f2
f2
h
w2
w n V = v =
fn
fn
w2
wn
h1
v1
h 2
v1
h q
v1
h1
h1
v2
vn
h 2
h2
v2
vn

h q
hq
v2
vn
14
EKF - Algorithm
Predict Stage
Update Stage
Kalman Gain Calculation:
K k =P k H
Propagate State:
HPk
H Rk
T
State Update with new Measure:
x k = x k 1Buk 1
x k =x k K k y k H x k
Propagate Co-Variance:
Update Covariance with new

measurement:
P k = P k 1T Q k
P k = I K k H P k
Initial Estimates: x0 , P0
15
EKF Trouble
EKF offer no theoretical guarantees

Not Optimal
May not converge
Expect troubles with periodical functions such as
trigonometrical
Should:
Design and implement VERY carefully
Test thouroughly
16
KF Main Ideas
KF
Weighed merging of prediction and updates
Optimal
Iterative
No guarantees but works
EKF
Non-linear
No guarantees but works
17
Self Localization
Fundamentals:
Where am I ?
Robotic Autonomy
Redundancy of sensors
Needs multisensory data fusion (example EKF)
Data internal to robot to find world pose
SemiStructed environment
Must match seen markers and world positions
18
Self Localization - example

Two angles (internal
to the robot) do not
localize it (x,y,)
Two measurements
of (angle, distance)
do not localize robot
(shown are 4 positions of an

infinite number of
solutions)
(still two solutions eligible)
19
Self Localiz Using EKF

Let us consider using Self Localization of a wheel
chair robot by using wall (line) ranging
Let us consider a pair of distance meters that allow
us to have distance and angle information
In order to use KF/EKF we need:
System Model
Measurement Model
20
System Model - wheelchair
X t=[ x r t
y r t r t
v r t r t ]T
v t=
v1 v 2
r=
2
v1 v 2
b
21
System Model - redux
[ ][
v t cos t
x t
d
y t = v t sin t
dt
t
t
Low dynamics allow

system order reduction
v and w are now inputs to
the system
22
System Model - discrete
[ ][
v t cos t
x t
d
y t = v t sin t
dt
t
t
x k
y k
x k A x k 1 x k W w k 1
y k h x k x k V v k
0 0 vt sin t k
A k = 0 0 vt cos t k
0 0
0
k
P w~ N 0, Q
P v~N 0, R
A=exp A k t k t k 1
*
23
Self Localization Walls / Lines

{
L =' L R
d L=d ' L a
b=ac
b=x R sin L
c= y R cos L
L =' L R
d L=d ' L x R sin L y R cos L
[ ]{
y=
' L
d 'L
line
' L= L R
d ' L=d L x R sin L y R cos L
d ' L , ' L
0
0
1
=
=
x r , y r , r sin L cos L 0
line
2d '
0
0
2 '
24
Results
8 < = t < 16
1
(m )
y w (m )
0 <= t < 8
-1
-1
-2
-2
-3
-3
-6
-4
-2
-6
(m )
-4
-2
x
(m )
25
Final Comments
Data Fusion
Many sensors to better perception

One method possible is (E)KF
(E)KF
KF: Guaranteed optimal and convergent
Iterative => no assurances
EKF: No assurances
Self Localization is hard problem even in semi

structured environment
Lines / walls (angle and distance) example
Must match seen marker
26
Literature
Data Fusion
Handbook of Robotics, Siciliano, Bruno; Khatib, Oussama

(Eds.), Springer 2008
Multi-Sensor Data Fusion: An Introduction, H.B. Mitchell,
Springer 2007
(E)KF
Applied Optimal Estimation, Arthur Gelb, 1974 MIT Press
A new approach to linear filtering and prediction problems,
R. E. Kalman 1960, Journal of Basic Engineering 82 (1): 35-45
Self Localization
http://www.fe.up.pt/asousa
27
Any
Questions
?
28

3 Datafus Kalman Selfloc v08 Ajs

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

3 Datafus Kalman Selfloc v08 Ajs

Încărcat de

Drepturi de autor:

Formate disponibile

Data Fusion, Kalman Filtering, Self Localiz.

Armando Jorge Sousa

Faculty of Engineering, University of Porto, Portugal

Self Localization as an example of EKF as an example of Data Fusion

Data Fusion, Kalman Filtering, Self Localiz.

Definition of Data Fusion

Combine data from multiple sources will be more efficient

Need for Data Fusion

Data Fusion, Kalman Filtering, Self Localiz.

Improvements After Data Fusion

Data Fusion, Kalman Filtering, Self Localiz.

Strategies for Data Fusion

Fusion across different Attributes

Fusion across different Domains

Fusion across different Time

Data Fusion, Kalman Filtering, Self Localiz.

Going to Kalman Filtering

State Space & Discrete Time

Data Fusion, Kalman Filtering, Self Localiz.

State Space - Stochastic

Kalman Filter always keeps

State Update with new Measure:

Update Covariance with new

The Kalman Gain weighs changes on system state:

This is an abuse => no guarantees!

Data Fusion, Kalman Filtering, Self Localiz.

Extended Kalman Filter

State Update with new Measure:

Update Covariance with new

EKF offer no theoretical guarantees

Data Fusion, Kalman Filtering, Self Localiz.

Self Localization - example

(shown are 4 positions of an

(still two solutions eligible)

Data Fusion, Kalman Filtering, Self Localiz.

Self Localiz Using EKF

Data Fusion, Kalman Filtering, Self Localiz.

System Model - wheelchair

Data Fusion, Kalman Filtering, Self Localiz.

System Model - redux

Low dynamics allow

Data Fusion, Kalman Filtering, Self Localiz.

System Model - discrete

Self Localization Walls / Lines

Data Fusion, Kalman Filtering, Self Localiz.

Many sensors to better perception

Self Localization is hard problem even in semi

Handbook of Robotics, Siciliano, Bruno; Khatib, Oussama

Data Fusion, Kalman Filtering, Self Localiz.

S-ar putea să vă placă și