CS 4610/5335 Kalman Filter
Robert Platt Northeastern University 2/10/20
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Sebastian Thrun, Wolfram Burgard, & Dieter Fox,
Probabilistic Robotics
4. Marc Toussaint, U. Stuttgart Robotics Course
Kalman Filtering
Another way to adapt Sequential Bayes Filtering to continuous state spaces
– relies on representing the probability distribution as a Gaussian
– first developed in the early 1960s (before general Bayes filtering); used in Apollo program
Image: UBC, Kevin Murphy Matlab toolbox
Kalman Filter
initial position prediction measurement update yyyy
xxxx
Image: Thrun et al., CS233B course notes
Example: Localizing against known map
Example: Localizing against known map
Example: Localizing against known map
Kalman Filter (1-D)
initial position prediction measurement update
x ̇ x ̇ x ̇ x ̇ xxxx
Image: Thrun et al., CS233B course notes
Kalman Idea
Image: Thrun et al., CS233B course notes
Measurement evidence
posterior
prior
Image: Thrun et al., CS233B course notes
System:
Process update:
Measurement update:
Image: Thrun et al., CS233B course
Kalman in 1D
notes
Measurement evidence
prior
posterior
Image: Thrun et al., CS233B course notes
Last time on the board…
Last time on the board…
● Univariate Gaussian:
● Multivariate Gaussian:
Gaussians
Playing w/ Gaussians
● Suppose:
● Calculate:
y
y
x
x
● Suppose:
● Then:
In fact
Illustration
Image: Thrun et al., CS233B course notes
And
Suppose:
Then:
Marginal distribution
Does this remind us of anything?
Does this remind us of anything?
Process update (discrete):
Process update (continuous):
Process update (discrete):
Process update (continuous):
Does this remind us of anything?
transition dynamics
prior
Process update (discrete):
Process update (continuous):
Does this remind us of anything?
transition dynamics
prior
Observation update:
Observation update
Where:
Observation update:
Observation update
Where:
Observation update:
Observation update
Where:
But we need:
Observation update
Suppose: Calculate:
Another Gaussian identity…
Observation update
But we need:
To summarize the Kalman filter System:
Prior:
Process update:
Measurement update:
Suppose there is an action term… System:
Prior:
Process update:
Measurement update:
To summarize the Kalman filter Prior:
Process update:
Measurement update:
This factor is often called the “Kalman gain”
Things to note about the Kalman filter Process update:
Measurement update:
– covariance update is independent of observation
– Kalman is only optimal for linear-Gaussian systems – the distribution “stays” Gaussian through this update
– the error term can be thought of as the different between the observation and the prediction
Example: filling a tank
Level of
tank Fill rate
Process:
Observati on:
Example: estimate velocity
prediction
past measurements
Image: Thrun et al., CS233B course notes
Example: estimate velocity
Example: Localizing against known map
Example: Localizing against known map
But, my system is NON-LINEAR! What should I do?
But, my system is NON-LINEAR! ● What should I do?
Well, there are some options…
But, my system is NON-LINEAR! ● What should I do?
Well, there are some options… But none of them are great.
But, my system is NON-LINEAR! ● What should I do?
Well, there are some options…
But none of them are great.
Here’s one: the Extended Kalman Filter
Extended Kalman filter Take a Taylor expansion:
Where:
Where:
Extended Kalman filter Take a Taylor expansion:
Where:
Where:
Then use the same equations…
Prior:
Process update:
Measurement update:
To summarize the EKF
Prior:
Process update:
Measurement update:
Compare with regular KF
EKF
EKF
Extended Kalman filter
Image: Thrun et al., CS233B course notes
Extended Kalman filter
Image: Thrun et al., CS233B course notes
EKF Mobile Robot Localization
Process dynamics:
state Process noise is assumed to be Gaussian:
Odometry measurement
Suppose we have a mobile robot wandering around in a 2-d world …
noise
EKF Mobile Robot Localization
Actual path of robot
Estimated path based on odometry
But, wheels slip – odometry is not always correct… How do we localize? Extended Kalman Filter!
Process dynamics:
EKF Mobile Robot Localization
noise
Odometry measurement
EKF uncertainty estimate
Dynamics: Linearized dynamics:
Where:
EKF Process Update
EKF Process Update
EKF uncertainty estimate
Dynamics: Linearized dynamics:
Where:
Process update:
With no observations, uncertainty grows over time…
Landmarks
(i.e. features of the env’t)
Observations
Observations:
– range and bearing of a landmark
Observations:
range bearing
Observations Observations:
Landmarks
(i.e. features of the env’t)
where:
EKF Mobile Robot Localization Process Update:
Observation Update:
Mapping using the EKF
How do we use the EKF to estimate landmark positions? State:
Positions of each of the M landmarks (base frame)
SLAM using the EKF
Vehicle/landmark covariance
Vehicle covariance
Estimate both robot position and landmark positions:
robot position
Landmark positions
Landmark covariance
SLAM using the EKF
Landmark covariance drops significantly as soon as “loop closure” occurs.
Image: Thrun
Mud Cards / Feedback
Do not write your name / e-mail / ID number!
1. What did you like?
2. What was unclear / “muddy”?
Feel free to leave any other feedback!
59