data fusion using kalman filter - mcgill cimyiannis/417/.../15-kalmanfilter.pdf · the kalman...
Post on 01-Aug-2020
40 Views
Preview:
TRANSCRIPT
Ioannis Rekleitis
Data Fusion using Kalman Filter
Example of a Parameterized Bayesian Filter:
Kalman Filter
Kalman filters (KF) represent posterior belief by a
Gaussian (normal) distribution
2
2
2
)(
2
1)(
x
exP
A 1-d Gaussian distribution is given by:
)()(2
1 1
||)2(
1)(
xx
n
T
exP
An n-d Gaussian distribution is given by:
2CS-417 Introduction to Robotics and Intelligent Systems
Kalman Filter : a Bayesian Filter• Initial belief Bel(x0) is a Gaussian distribution
– What do we do for an unknown starting position?
• State at time t+1 is a linear function of state at time t:
• Observations are linear in the state:
• Error terms are zero-mean random variables which are normally distributed
• These assumptions guarantee that the posterior belief is Gaussian– The Kalman Filter is an efficient algorithm to compute the posterior
– Normally, an update of this nature would require a matrix inversion (similar to a least squares estimator)
– The Kalman Filter avoids this computationally complex operation
CS-417 Introduction to Robotics and Intelligent Systems 3
)(1 actiontttt BuFxx
)( nobservatiottt Hxo
The Kalman Filter
• Motion model is Gaussian…
• Sensor model is Gaussian…
• Each belief function is uniquely characterized by its mean and covariance matrix
• Computing the posterior means computing a new mean and covariance from old data using actions and sensor readings
• What are the key limitations?
1) Unimodal distribution
2) Linear assumptions
4CS-417 Introduction to Robotics and Intelligent Systems
The Kalman Filter
• Linear process and measurement models• Gaussian noise (or white )• Gaussian state estimate
• Process model is
• Measurement model is
Prior Measurement Kalman filter posterior
111 tttt qBuAxx
ttt rHxz
Kalman, 1960
Images courtesy of Maybeck, 1979 5CS-417 Introduction to Robotics and Intelligent Systems
What we know…
What we don’t know…
• We know what the control inputs of our process are– We know what we’ve told the system to do and have a model for what the
expected output should be if everything works right
• We don’t know what the noise in the system truly is– We can only estimate what the noise might be and try to put some sort of upper
bound on it
• When estimating the state of a system, we try to find a set of values that comes as close to the truth as possible– There will always be some mismatch between our estimate of the system and the
true state of the system itself. We just try to figure out how much mismatch there is and try to get the best estimate possible
6CS-417 Introduction to Robotics and Intelligent Systems
Minimum Mean Square Error
Reminder: the expected value, or mean value, of aContinuous random variable x is defined as:
dxxxpxE )(][
Minimum Mean Square Error (MMSE)
)|( ZxPWhat is the mean of this distribution?
This is difficult to obtain exactly. With our approximations,we can get the estimate x̂
]|)ˆ[( 2
tZxxE …such that is minimized.
According to the Fundamental Theorem of Estimation Theorythis estimate is:
dxZxxpZxExMMSE )|(]|[ˆ
7CS-417 Introduction to Robotics and Intelligent Systems
Fundamental Theorem of Estimation Theory
• The minimum mean square error estimator equals the expected (mean) value of xconditioned on the observations Z
• The minimum mean square error term is quadratic:
– Its minimum can be found by taking the derivative of the function w.r.t x and setting that value to 0.
• It is interesting to note that when they use the Gaussian assumption, Maximum A Posteriori estimators and MMSE estimators find the same value for the parameters. – This is because mean and the mode of a Gaussian distribution are the same.
CS-417 Introduction to Robotics and Intelligent Systems 8
]|)ˆ[( 2
tZxxE
0])|)ˆ[(( 2 ZxxEx
Kalman Filter Components(also known as: Way Too Many Variables…)
Linear discrete time dynamic system (motion model)
ttttttt wGuBxFx 1
Measurement equation (sensor model)
1111 tttt nxHz
State transitionfunction
Control inputfunction
Noise inputfunction with covariance Q
State Control input Process noise
StateSensor reading Sensor noise with covariance R
Sensor function Note:Write these down!!!
9CS-417 Introduction to Robotics and Intelligent Systems
Computing the MMSE Estimate of the State and Covariance
CS-417 Introduction to Robotics and Intelligent Systems 10
Given a set of measurements: }1,{1 tizZ it
According to the Fundamental Theorem of Estimation, the stateand covariance will be:
]|)ˆ[(
]|[ˆ
1
2
1
t
MMSE
t
MMSE
ZxxEP
ZxEx
We will now use the following notation:
]|[ˆ
]|[ˆ
]|[ˆ
1|1
|
111|1
tttt
tttt
tttt
ZxEx
ZxEx
ZxEx
Computing the MMSE Estimate of the State and Covariance
CS-417 Introduction to Robotics and Intelligent Systems 11
What is the minimum mean square error estimateof the system state and covariance?
ttttttt uBxFx ||1ˆˆ Estimate of the state variables
ttttt xHz |11|1ˆˆ
Estimate of the sensor reading
T
ttt
T
tttttt GQGFPFP ||1 Covariance matrix for the state
11|11|1 t
T
tttttt RHPHS Covariance matrix for the sensors
At last! The Kalman Filter…
CS-417 Introduction to Robotics and Intelligent Systems 12
Propagation (motion model):
T
ttt
T
tttttt
ttttttt
GQGFPFP
uBxFx
//1
//1ˆˆ
Update (sensor model):
tttt
T
ttttttt
tttttt
t
T
tttt
t
T
ttttt
ttt
tttt
PHSHPPP
rKxx
SHPK
RHPHS
zzr
xHz
/11
1
11/1/11/1
11/11/1
1
11/11
11/111
111
/111
ˆˆ
ˆ
ˆˆ
…but what does that mean in English?!?
CS-417 Introduction to Robotics and Intelligent Systems 13
Propagation (motion model):
Update (sensor model):
- State estimate is updated from system dynamics
- Uncertainty estimate GROWS
- Compute expected value of sensor reading
- Compute the difference between expected and “true”
- Compute covariance of sensor reading
- Compute the Kalman Gain (how much to correct est.)
- Multiply residual times gain to correct state estimate
- Uncertainty estimate SHRINKS
T
ttt
T
tttttt
ttttttt
GQGFPFP
uBxFx
//1
//1ˆˆ
tttt
T
ttttttt
tttttt
t
T
tttt
t
T
ttttt
ttt
tttt
PHSHPPP
rKxx
SHPK
RHPHS
zzr
xHz
/11
1
11/1/11/1
11/11/1
1
11/11
11/111
111
/111
ˆˆ
ˆ
ˆˆ
Kalman Filter Block Diagram
CS-417 Introduction to Robotics and Intelligent Systems 14
Example 1: Simple 1D Linear System
Given: F=G=H=1, u=0Initial state estimate = 0Linear system:
111
1
ttt
ttt
nxz
wxx
Propagation: Update:
ttttt
tttt
QPP
xx
//1
//1ˆˆ
Unknown noiseparameters
tttttttt
tttttt
tttt
tttt
tttt
ttt
PSPPP
rKxx
SPK
RPS
xzr
xz
/1
1
1/111/1
11/11/1
1
1/11
1/11
/111
/11
ˆˆ
ˆ
ˆˆ
15CS-417 Introduction to Robotics and Intelligent Systems
State Estimate
16CS-417 Introduction to Robotics and Intelligent Systems
State Estimation Error vs 3 Region of Confidence
17CS-417 Introduction to Robotics and Intelligent Systems
Sensor Residual vs 3 Region of Confidence
18CS-417 Introduction to Robotics and Intelligent Systems
Kalman Gain and State Covariance
19CS-417 Introduction to Robotics and Intelligent Systems
Example 2: Simple 1D Linear System with Erroneous Start
Given: F=G=H=1, u=cos(t/5)Initial state estimate = 20
Linear system:111
1 )5/cos(
ttt
ttt
nxz
wtxx
Propagation: Update: (no change)
ttttt
tttt
QPP
txx
//1
//1 )5/cos(ˆˆ
Unknown noiseparameters
tttttttt
tttttt
tttt
tttt
tttt
ttt
PSPPP
rKxx
SPK
RPS
xzr
xz
/1
1
1/111/1
11/11/1
1
1/11
1/11
/111
/11
ˆˆ
ˆ
ˆˆ
20CS-417 Introduction to Robotics and Intelligent Systems
State Estimate
21CS-417 Introduction to Robotics and Intelligent Systems
State Estimation Error vs 3 Region of Confidence
22CS-417 Introduction to Robotics and Intelligent Systems
Sensor Residual vs 3 Region of Confidence
23CS-417 Introduction to Robotics and Intelligent Systems
Kalman Gain and State Covariance
24CS-417 Introduction to Robotics and Intelligent Systems
Some observations
• The larger the error, the smaller the effect on the final state estimate– If process uncertainty is larger, sensor updates will dominate state
estimate
– If sensor uncertainty is larger, process propagation will dominate state estimate
• Improper estimates of the state and/or sensor covariance may result in a rapidly diverging estimator– As a rule of thumb, the residuals must always be bounded within a ±3
region of uncertainty
– This measures the “health” of the filter
• Many propagation cycles can happen between updates
25CS-417 Introduction to Robotics and Intelligent Systems
Using the Kalman Filter for Mobile Robots
• Sensor modeling– The odometry estimate is not a reflection of the robot’s control
system is rather treated as a sensor
– Instead of directly measuring the error in the state vector (such as when doing tracking), the error in the state must be estimated
– This is referred to as the Indirect Kalman Filter
• State vector for robot moving in 2D– The state vector is 3x1: [x,y,q]
– The covariance matrix is 3x3
• Problem: Mobile robot dynamics are NOT linear
26CS-417 Introduction to Robotics and Intelligent Systems
Problems with the Linear Model Assumption
• Many systems of interest are highly non-linear, such as mobile robots
• In order to model such systems, a linear process model must be generated out of the non-linear system dynamics
• The Extended Kalman filter is a method by which the state propagation equations and the sensor models can be linearized about the current state estimate
• Linearization will increase the state error residual because it is not the best estimate
27CS-417 Introduction to Robotics and Intelligent Systems
Approximating Robot Motion Uncertainty with a Gaussian
28CS-417 Introduction to Robotics and Intelligent Systems
Linearized Motion Model for a Robot
X
Y
w
xy
G
vtt
t
tt
y
Vx
w
0From a robot-centric perspective, the velocities look like this:
From the global perspective, the velocities look like this:
tt
ttt
ttt
Vy
Vx
w
sin
cos
The discrete time state estimate (including noise) looks like this:
tw
twVyy
twVxx
t
t
t
ttt
tVttt
tVttt
w
w )(ˆˆ
ˆsin)(ˆˆ
ˆcos)(ˆˆ
1
1
1
Problem! We don’t know linear and rotational velocity errors. The state estimate will rapidly diverge if this is the only source of information! 29CS-417 Introduction to Robotics and Intelligent Systems
Linearized Motion Model for a Robot
111
111
111
~ˆ
~ˆ
~ˆ
ttt
ttt
ttt
yyy
xxx
The indirect Kalman filter derives the pose equations from theestimated error of the state:
In order to linearize the system, the following small-angle assumptions are made:
~~
sin
1~
cos
Now, we have to compute the covariance matrix propagationequations.
30CS-417 Introduction to Robotics and Intelligent Systems
tw
twt
t
tttt
ttt
w
w
ww
~)(ˆ
ˆ~111
1
~tCalculation of
CS-417 Introduction to Robotics and Intelligent Systems 31
Calculation of111
111
ˆ~
ˆ~
ttt
ttt
yyy
xxx
111ˆ~
ttt xxx
...
ˆ~)ˆcos()ˆsin(
~~
)ˆcos()ˆcos()ˆsin(~
)ˆcos(~
)ˆcos()ˆcos()]ˆsin()~
sin()ˆcos()~
[cos(~
)ˆcos()ˆcos()ˆ~cos(~
)ˆcos()ˆcos()cos(ˆ
)ˆcos()(ˆ)cos(
ˆ~
111
111
ttt
tvtttt
tvtttttttt
tvtttttttt
tvtttttt
tvtttttt
tvttttt
ttt
yyy
twtvx
twtvtvtvx
twtvtvx
twtvtvx
twtvtvxx
twvxtvx
xxx
32CS-417 Introduction to Robotics and Intelligent Systems
Linearized Motion Model for a Robot
ttttt
v
t
t
t
t
t
t
t
t
t
t
WGXFX
w
w
t
t
t
y
x
tV
tV
y
x
~~
0
0sin
0cos
~
~
~
100
ˆcos10
ˆsin01
~
~
~
1
1
1
1
w
From the error-state propagation equation, we can obtain theState propagation and noise input functions F and G :
From these values, we can easily compute the standard covariance propagation equation:
T
ttt
T
tttttt GQGFPFP //1
33CS-417 Introduction to Robotics and Intelligent Systems
Covariance Estimation
34CS-417 Introduction to Robotics and Intelligent Systems
2
2
/
11/1
0
0][
where
][]~~
[
])~
)(~
[(
]~~
[
w
vT
ttt
T
ttt
T
tttt
T
t
T
ttt
T
t
T
ttt
T
tttttttt
T
tttt
wwEQ
GQGFPF
GwwEGFXXEF
wGXFwGXFE
XXEP
t
t
t
w
fF
tv
tv
X
fF
wFXFXX
wFXXFXX
wXfX
t
t
Xw
tt
tt
Xx
twtxtt
twttxtt
ttt
t
t
0
0)ˆsin(
0)ˆcos(
|
100
)ˆcos(10
)ˆsin(01
|
)~
(ˆ~)ˆ(ˆ
),(
ˆ
ˆ
11
11
1
Alternative Calculation
CS-417 Introduction to Robotics and Intelligent Systems 35
Propagation• Finally, for a mobile robot EKF propagation step we
have:
CS-417 Introduction to Robotics and Intelligent Systems 36
2
2
//1
1
1
1
0
0 ,
0
0sin
0cos
,
100
ˆcos10
ˆsin01
:where
ˆˆ
ˆsinˆˆ
ˆcosˆˆ
w
w
v
tt
t
tt
t
t
T
ttt
T
tttttt
m
ttt
t
m
ttt
t
m
ttt
Q
t
t
t
GtV
tV
F
GQGFPFP
t
tVyy
tVxx
Sensor Model for a Robot with a Perfect Map
X
Y
xy
G
Li =[xLi, yLi
]T
z
q
q
q
n
n
xxyy
yyxx
n
nz
rrLrL
rLrLt
ii
ii
,2atan
22
1
From the robot, the measurement looks like this:
The measurement equation is nonlinear and must also be linearized!
37CS-417 Introduction to Robotics and Intelligent Systems
1
0
22
22
rLrLi
rLirLi
rLirLi
xxyy
yyxx
H
yyxx
i
Update
CS-417 Introduction to Robotics and Intelligent Systems 38
2
*
**)*(**)*(
*
**
**
ˆ
1/1/1
1
T
TT
tttt
T
T
PPP
KRKHKIPHKIP
rKxx
SHPK
RHPHS
zzr
See Matlab Examples
CS-417 Introduction to Robotics and Intelligent Systems 39
Sensor Model for a Robot with a Perfect Map
X
Y
xy
G
L
z
n
n
n
y
x
z y
x
L
L
L
t
t
t
t
1
1
1
1
From the robot, the measurement looks like this:
From a global perspective, the measurement looks like:
n
n
n
yy
xx
z y
x
tL
tL
tL
tt
tt
t
t
t
t
1
1
1
11
11
1
1
1
1
100
0cossin
0sincos
The measurement equation is nonlinear and must also be linearized!
40CS-417 Introduction to Robotics and Intelligent Systems
Sensor Model for a Robot with a Perfect Map
CS-417 Introduction to Robotics and Intelligent Systems 41
Now, we have to compute the linearized sensor function. Onceagain, we make use of the indirect Kalman filter where the error in the reading must be estimated.
In order to linearize the system, the following small-angle assumptions are made:
~~
sin
1~
cos
The final expression for the error in the sensor reading is:
n
n
n
y
x
yyxx
yyxx
y
x
y
x
t
t
t
tLttLttt
tLttLtt
L
L
L t
t
t
t
1
1
1
11111
1111
~
~
~
100
)ˆ(ˆsin)ˆ(ˆcosˆcosˆsin
)ˆ(ˆcos)ˆ(ˆsinˆsinˆcos
~
~
~1
1
1
1
Updating the State Vector
CS-417 Introduction to Robotics and Intelligent Systems 42Propagation only Propagation and update
Extended Kalman Filter for SLAM
• State vector
– Expanded to contain entries for all landmarks positions:
– State vector can be grown as new landmarks are discovered
– Covariance matrix is also expanded
TT
L
T
L
T
R nXXXX
1
NNNN
N
N
LLLLRL
LLLLRL
RLRLRR
PPP
PPP
PPP
1
1111
1
43CS-417 Introduction to Robotics and Intelligent Systems
Extended Kalman Filter for SLAM
• Kinematic equations for landmark propagation
tLtL
tLtL
tLtL
ttt
tVttt
tVttt
ii
ii
ii
t
t
t
yy
xx
tw
twVyy
twVxx
w
w
ˆˆ
ˆˆ
ˆˆ
)(ˆˆ
ˆsin)(ˆˆ
ˆcos)(ˆˆ
1
1
1
1
1
1
44CS-417 Introduction to Robotics and Intelligent Systems
Extended Kalman Filter for SLAM
• Sensor equations for update:
• Very powerful because covariance update records shared information between landmarks and robot positions
0000
~~~~~1
i
ni
LR
T
L
T
L
T
L
T
R
HHH
XXXXX
45CS-417 Introduction to Robotics and Intelligent Systems
EKF for SLAM
46CS-417 Introduction to Robotics and Intelligent Systems
Enhancements to EKF
• Iterated Extended Kalman Filter
Iterate state update equation until convergence
T
ttkkkk
ttkkkk
T
kkt
t
T
kktt
ttt
t
tt
t
KSKPP
rKXX
SHPK
RHPHS
zzr
1
11
1
11/11/1
11/11/1
1
/11
1/111
111
ˆˆ
ˆ
State and covariance update
47CS-417 Introduction to Robotics and Intelligent Systems
Enhancements to the EKF
• Multiple hypothesis tracking
– Multiple Kalman filters are used to track the data
– Multi-Gaussian approach allows for representation of arbitrary probability densities
– Consistent hypothesis are tracked while highly inconsistent hypotheses are dropped
– Similar in spirit to particle filter, but orders of magnitude fewer filters are tracked as compared to the particle filter
48CS-417 Introduction to Robotics and Intelligent Systems
top related