Kalman Filter Examples - eecs.yorku.ca
Transcript of Kalman Filter Examples - eecs.yorku.ca
![Page 1: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/1.jpg)
Kalman Filter Examples
3/16/20181
![Page 2: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/2.jpg)
Static State Estimation
3/16/20182
recall the static state estimation problem we have been studying the process or plant model
the observation model
1
1
t
tttttt
xuBxAx
ttt xz
0,0,1 ttt RBA
2,1 ttt QC
![Page 3: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/3.jpg)
Static State Estimation
3/16/20183
how well does the Kalman filter work
![Page 4: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/4.jpg)
Static State Estimation
3/16/20184
notice that we need to specify the measurement noise covariance Qt
how sensitive is the Kalman filter to Qt ? e.g., what if we use a Qt that is much smaller than the actual
measurement noise? e.g., what if we use a Qt that is much larger than the actual
measurement noise?
![Page 5: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/5.jpg)
Static State Estimation
3/16/20185
specified Qt = 0.01 * actual Qt
![Page 6: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/6.jpg)
Static State Estimation
3/16/20186
specified Qt = 100 * actual Qt
![Page 7: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/7.jpg)
Static State Estimation
3/16/20187
suppose our measurements get progressively noisier over time
noise variance increases 10% for each successive measurement
![Page 8: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/8.jpg)
Tank of Water
3/16/20188
estimate the level of water in the tank; the water could be static, filling, or emptying not sloshing or sloshing
![Page 9: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/9.jpg)
Tank of Water
3/16/20189
static level
1 tt xx
ttt xz
plant model
measurement model
![Page 10: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/10.jpg)
Tank of Water: Static and Not Sloshing
3/16/201810
![Page 11: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/11.jpg)
Tank of Water: Static and Not Sloshing
3/16/201811
notice that in this case the Kalman filter tends towards estimating a constant level because the plant noise covariance is small compared to the measurement noise covariance the estimated state is much smoother than the measurements
what happens if we increase the plant noise covariance?
![Page 12: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/12.jpg)
Tank of Water: Filling and Not Sloshing
3/16/201812
![Page 13: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/13.jpg)
Tank of Water: Static and Not Sloshing
3/16/201813
notice that in this case the Kalman filter tends towards estimating values that are closer to the measurements
increasing the plant noise covariance causes the filter to place more weight on the measurements
![Page 14: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/14.jpg)
Tank of Water: Filling and not Sloshing
3/16/201814
suppose the true situation is that the tank is filling at a constant rate but we use the static tank plant model i.e., we have a plant model that does not accurately model the state
transition
![Page 15: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/15.jpg)
Tank of Water: Filling and not Sloshing
3/16/201815
![Page 16: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/16.jpg)
Tank of Water: Filling and not Sloshing
3/16/201816
notice that in this case the estimated state trails behind the true level estimated state has a much greater error than the noisy
measurements
if the plant model does not accurately model reality than you can expect poor results however, increasing the plant noise covariance will allow the filter to
weight the measurements more heavily in the estimation…
![Page 17: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/17.jpg)
Tank of Water: Filling and not Sloshing
3/16/201817
![Page 18: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/18.jpg)
Tank of Water: Filling and not Sloshing
3/16/201818
it is not clear if we have gained anything in this case the estimated state is reasonable but it is not clear if it is more
accurate than the measurements
what happens if we change the plant model to more accurately reflect the filling?
![Page 19: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/19.jpg)
Tank of Water
3/16/201819
filling at a (noisy) constant rate and we do not care about the rate
ut is the change in the water level that occurred from time t-1to t
tu
LtLt
t
xxx 1,
ttt xz
plant model
measurement model
![Page 20: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/20.jpg)
Tank of Water: Filling and not Sloshing
3/16/201820
![Page 21: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/21.jpg)
Tank of Water: Filling and not Sloshing
3/16/201821
notice that the estimated state is more accurate and smoother than the measurements
what about the filling rate?
![Page 22: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/22.jpg)
Tank of Water
3/16/201822
filling at a (noisy) constant rate and we want to estimate the rate
t
x
tL
L
A
t
tt
xx
x
1
11011
tt
C
t xzt
01
plant model
measurement model
![Page 23: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/23.jpg)
Tank of Water: Filling and not Sloshing
3/16/201823
![Page 24: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/24.jpg)
Tank of Water: Filling and not Sloshing
3/16/201824
notice that the estimated filling rate seems to jump more than the estimated level this should not be surprising as we never actually measure the filling
rate directly it is being inferred from the measured level (which is quite noisy)
![Page 25: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/25.jpg)
Tank of Water: Static and not Sloshing
3/16/201825
can we trick the filter by using the filling plant model when the level is static? hopefully not, as the filter should converge to a fill rate of zero!
![Page 26: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/26.jpg)
Tank of Water: Static and not Sloshing
3/16/201826
![Page 27: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/27.jpg)
Tank of Water: Static and not Sloshing
3/16/201827
![Page 28: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/28.jpg)
Projectile Motion
3/16/201828
projectile launched from some initialpoint with some initial velocity underthe influence of gravity (no drag) 00 , yx
yx vv ,
gtvtvvtv
gttvyty
tvxtx
yy
xx
y
x
)()(
)(
)(2
21
0
0
![Page 29: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/29.jpg)
Projectile Motion
3/16/201829
convert the continuous time equations to discrete recurrence relations for some time step
tgvvvv
tgtvyy
tvxx
tyty
txtx
tytt
txtt
1,,
1,,
221
1,1
1,1
t
![Page 30: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/30.jpg)
Projectile Motion
3/16/201830
rewrite in matrix form
tttt ux
ty
x
Ax
ty
x
gt
tg
vvyx
tt
vvyx
0
0
10000100
010001
221
1
1
![Page 31: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/31.jpg)
Omnidirectional Robot
3/16/201831
an omnidirectional robot is a robot that can move in any direction (constrained in the ground plane) http://www.youtube.com/watch?v=DPz-ullMOqc http://www.engadget.com/2011/07/09/curtis-boirums-robotic-car-
makes-omnidirectional-dreams-come-tr/
if we are not interested in the orientation of the robot then its state is simply its location
tt y
xx
![Page 32: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/32.jpg)
Omnidirectional Robot
3/16/201832
a possible choice of motion control is simply a change in the location of the robot
with noisy control inputs
tt u
t
x
tt y
xyx
x
1
1
t
u
t
x
tt
tt
yx
yx
x
1
1
![Page 33: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/33.jpg)
Differential Drive
3/16/201833
recall that we developed two motion models for a differential drive using the velocity model, the control inputs are
24
23
22
21
tt
tt
v
v
t
tt
vu
![Page 34: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/34.jpg)
Differential Drive
3/16/201834
using the velocity motion model the discrete time forward kinematics are
t
tytx
yx
x vc
vc
t
)cos()sin(
Eqs 5.9
t
tytx
vv
vv
)cos(cos)sin(sin
![Page 35: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/35.jpg)
Differential Drive
3/16/201835
there are two problems when trying to use the velocity motion model in a Kalman filter1. the plant model is not linear in the state and control
2. it is not clear how to describe the control noises as a plant covariance matrix
t
tytx
x
t
tvv
tvv
t t
t
t
t
t
t
t
t
)cos(cos)sin(sin
24
23
22
21
tt
tt
v
v
t
tt
vu
![Page 36: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/36.jpg)
Measurement Model
3/16/201836
there are potentially other problems any non-trivial measurement model will be non-linear in terms of
the state
consider using the known locations of landmarks in a measurement model
![Page 37: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/37.jpg)
Landmarks
3/16/201837
a landmark is literally a prominent geographic feature of the landscape that marks a known location
in common usage, landmarks now include any fixed easily recognizable objects e.g., buildings, street intersections, monuments
for mobile robots, a landmark is any fixed object that can be sensed
![Page 38: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/38.jpg)
Landmarks for Mobile Robots
3/16/201838
visual artificial or natural
retro-reflective beacons LORAN (Long Range Navigation): terrestrial radio; now being
phased out GPS: satellite radio
acoustic scent?
![Page 39: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/39.jpg)
Landmarks: RoboSoccer
3/16/201839
![Page 40: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/40.jpg)
Landmarks: Retroreflector
3/16/201840
![Page 41: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/41.jpg)
Landmarks: Active Light
3/16/201841
![Page 42: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/42.jpg)
Trilateration
3/16/201842
uses distance measurements to two or more landmarks suppose a robot measures the distance d1 to a landmark the robot can be anywhere on a circle of radius d1 around the
landmark
landmark
d1
![Page 43: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/43.jpg)
Trilateration
3/16/201843
without moving, suppose the robot measures the distance d2to a second landmark the robot can be anywhere on a circle of radius d2 around the
second landmark
landmark
d1
landmark
d2
![Page 44: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/44.jpg)
Trilateration
3/16/201844
the robot must be located at one of the two intersection points of the circles tie can be broken if other information is known
landmark
d1
landmark
d2
![Page 45: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/45.jpg)
Trilateration
3/16/201845
if the distance measurements are noisy then there will be some uncertainty in the location of the robot
![Page 46: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/46.jpg)
Trilateration
3/16/201846
notice that the uncertainty changes depending on where the robot is relative to the landmarks
uncertainty grows quickly ifthe robot is in line with thelandmarks
![Page 47: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/47.jpg)
Trilateration
3/16/201847
uncertainty grows as therobot moves fartheraway from the landmarks but not as dramatically
as the previously slide
![Page 48: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/48.jpg)
Triangulation
3/16/201848
triangulation uses angular information to infer position http://longhamscouts.org.uk/content/view/52/38/
![Page 49: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/49.jpg)
Triangulation
3/16/201849
in robotics the problem often appears as something like: suppose the robot has a (calibrated) camera that detects two
landmarks (with known location) then we can determine the angular separation, or relative bearing, α
between the two landmarks
α
knownposition known
position
unknownposition
D1
Z2
Z1
![Page 50: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/50.jpg)
Triangulation
3/16/201850
the unknown position must lie somewhere on a circle arc Euclid proved that any point on the shown circular arc forms an
inscribed triangle with angle α we need at least one more beacon to estimate the robot’s location
α
knownposition known
position
D1
α
![Page 51: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/51.jpg)
Triangulation
3/16/201851
the unknown position must lie somewhere on a circle arc Euclid proved that any point on the shown circular arc forms an
inscribed triangle with angle α we need at least one more beacon to estimate the robot’s location
α
D1
β
D2
![Page 52: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/52.jpg)
Kalman Filter with Landmarks
3/16/201852
suppose that we have an omnidirectional robot with plant model:
suppose that the robot can measure the vector from its current position to a point landmark located at a known position in the world what is the measurement model?
t
u
t
x
tt
tt
yx
yx
x
1
1
![Page 53: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/53.jpg)
Kalman Filter with Landmarks
3/16/201853
measurement model:
is the measurement model linear?
![Page 54: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/54.jpg)
Kalman Filter with Landmarks
3/16/201854
can we make the measurement model linear?
![Page 55: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/55.jpg)
Kalman Filter with Landmarks
3/16/201855
![Page 56: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/56.jpg)
Kalman Filter with Landmarks
3/16/201856
![Page 57: Kalman Filter Examples - eecs.yorku.ca](https://reader030.fdocuments.net/reader030/viewer/2022012502/617c60d8498962566f60a1ca/html5/thumbnails/57.jpg)
Kalman Filter with Landmarks
3/16/201857