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.
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