Professional Documents
Culture Documents
Data Fusion
Kalman Filtering
Self Localization
asousa@fe.up.pt
Summary
Data Fusion, Kalman Filtering, Self Localiz.
Data Fusion
Definition, need, what is improved, strategies, healthy start
Kalman Filtering
State Space, discrete time, stchastic, KF algorithm, iterative
EKF algorithm, pitfalls
Final Comments
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
Certainty
Example: likelihood
Accuracy
Example: reduce outliers
[Mitchell, 2007]
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
Framework
The Kalman Filter <= the only addressed
Bayes Rule
Sequential Monte Carlo Methods
Probabilistic Grids
Alternatives to Probability Example: Heuristics!
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
Kalman Filtering
Data Fusion, Kalman Filtering, Self Localiz.
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!
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
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
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
Data Fusion, Kalman Filtering, Self Localiz.
Predict Stage
Update Stage
Kalman Gain Calculation:
1
K k = P k H T HP k H T R k
Propagate State:
x k = x k 1Bu k 1
x k = x k K k y k H x k
Propagate Covariance:
P k = P k 1 T Q k
P k = I K k H P k
Initial Estimates: x0 , P0
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
11
KF - hints
Data Fusion, Kalman Filtering, Self Localiz.
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
12
KF Iterative
Data Fusion, Kalman Filtering, Self Localiz.
Initial(State, Covariance)
Cycle:
Propagate (Stat,Covar,SysModel,Inputs)
Cycle Meas(i):
If Valid(Meas(i)) then
Update(Stat,Covar,MeasModel(i),Meas(i))
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
13
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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
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
Data Fusion, Kalman Filtering, Self Localiz.
Predict Stage
Update Stage
Kalman Gain Calculation:
K k =P k H
Propagate State:
HPk
H Rk
T
x k = x k 1Buk 1
x k =x k K k y k H x k
Propagate Co-Variance:
P k = P k 1T Q k
P k = I K k H P k
Initial Estimates: x0 , P0
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
15
EKF Trouble
Data Fusion, Kalman Filtering, Self Localiz.
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
16
KF Main Ideas
Data Fusion, Kalman Filtering, Self Localiz.
KF
Weighed merging of prediction and updates
Optimal
Iterative
No guarantees but works
EKF
Non-linear
No guarantees but works
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
17
Self Localization
Data Fusion, Kalman Filtering, Self Localiz.
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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
18
Two measurements
of (angle, distance)
do not localize robot
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
19
20
X t=[ x r t
y r t r t
v r t r t ]T
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
v t=
v1 v 2
r=
2
v1 v 2
b
21
[ ][
v t cos t
x t
d
y t = v t sin t
dt
t
t
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
22
[ ][
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
*
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
23
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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
line
2d '
0
0
2 '
24
Results
Data Fusion, Kalman Filtering, Self Localiz.
8 < = t < 16
1
(m )
y w (m )
0 <= t < 8
-1
-1
-2
-2
-3
-3
-6
-4
-2
-6
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
(m )
-4
-2
x
(m )
25
Final Comments
Data Fusion
Data Fusion, Kalman Filtering, Self Localiz.
(E)KF
KF: Guaranteed optimal and convergent
Iterative => no assurances
EKF: No assurances
26
Literature
Data Fusion
Data Fusion, Kalman Filtering, Self Localiz.
(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
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
27
Any
Questions
?
Data Fusion and Kalman Filtering - Presentation for MAP-I May 26, 2008
28