data fusion using kalman filter - mcgill cimyiannis/417/.../15-kalmanfilter.pdf · the kalman...
TRANSCRIPT
![Page 1: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/1.jpg)
Ioannis Rekleitis
Data Fusion using Kalman Filter
![Page 2: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/2.jpg)
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
![Page 3: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/3.jpg)
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
![Page 4: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/4.jpg)
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
![Page 5: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/5.jpg)
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
![Page 6: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/6.jpg)
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
![Page 7: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/7.jpg)
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
![Page 8: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/8.jpg)
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
![Page 9: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/9.jpg)
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
![Page 10: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/10.jpg)
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
![Page 11: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/11.jpg)
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
![Page 12: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/12.jpg)
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
ˆˆ
ˆ
ˆˆ
![Page 13: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/13.jpg)
…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
ˆˆ
ˆ
ˆˆ
![Page 14: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/14.jpg)
Kalman Filter Block Diagram
CS-417 Introduction to Robotics and Intelligent Systems 14
![Page 15: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/15.jpg)
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
![Page 16: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/16.jpg)
State Estimate
16CS-417 Introduction to Robotics and Intelligent Systems
![Page 17: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/17.jpg)
State Estimation Error vs 3 Region of Confidence
17CS-417 Introduction to Robotics and Intelligent Systems
![Page 18: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/18.jpg)
Sensor Residual vs 3 Region of Confidence
18CS-417 Introduction to Robotics and Intelligent Systems
![Page 19: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/19.jpg)
Kalman Gain and State Covariance
19CS-417 Introduction to Robotics and Intelligent Systems
![Page 20: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/20.jpg)
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
![Page 21: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/21.jpg)
State Estimate
21CS-417 Introduction to Robotics and Intelligent Systems
![Page 22: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/22.jpg)
State Estimation Error vs 3 Region of Confidence
22CS-417 Introduction to Robotics and Intelligent Systems
![Page 23: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/23.jpg)
Sensor Residual vs 3 Region of Confidence
23CS-417 Introduction to Robotics and Intelligent Systems
![Page 24: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/24.jpg)
Kalman Gain and State Covariance
24CS-417 Introduction to Robotics and Intelligent Systems
![Page 25: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/25.jpg)
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
![Page 26: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/26.jpg)
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
![Page 27: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/27.jpg)
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
![Page 28: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/28.jpg)
Approximating Robot Motion Uncertainty with a Gaussian
28CS-417 Introduction to Robotics and Intelligent Systems
![Page 29: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/29.jpg)
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
![Page 30: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/30.jpg)
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
![Page 31: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/31.jpg)
tw
twt
t
tttt
ttt
w
w
ww
~)(ˆ
ˆ~111
1
~tCalculation of
CS-417 Introduction to Robotics and Intelligent Systems 31
![Page 32: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/32.jpg)
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
![Page 33: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/33.jpg)
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
![Page 34: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/34.jpg)
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
![Page 35: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/35.jpg)
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
![Page 36: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/36.jpg)
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
![Page 37: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/37.jpg)
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
![Page 38: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/38.jpg)
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
![Page 39: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/39.jpg)
See Matlab Examples
CS-417 Introduction to Robotics and Intelligent Systems 39
![Page 40: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/40.jpg)
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
![Page 41: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/41.jpg)
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
![Page 42: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/42.jpg)
Updating the State Vector
CS-417 Introduction to Robotics and Intelligent Systems 42Propagation only Propagation and update
![Page 43: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/43.jpg)
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
![Page 44: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/44.jpg)
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
![Page 45: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/45.jpg)
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
![Page 46: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/46.jpg)
EKF for SLAM
46CS-417 Introduction to Robotics and Intelligent Systems
![Page 47: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/47.jpg)
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
![Page 48: Data Fusion using Kalman Filter - McGill CIMyiannis/417/.../15-KalmanFilter.pdf · The Kalman Filter •Linear process and measurement models •Gaussian noise (or white) •Gaussian](https://reader034.vdocuments.us/reader034/viewer/2022042606/5f855c101089cf15d13b87b4/html5/thumbnails/48.jpg)
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