[SOLVED] CS matlab CS 4610/5335 Kalman Filter

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.
Heres 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 envt)
Observations
Observations:
range and bearing of a landmark
Observations:
range bearing

Observations Observations:
Landmarks
(i.e. features of the envt)
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