Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete...
Transcript of Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete...
![Page 1: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/1.jpg)
Probabilistic Robotics
Bayes Filter Implementations
Gaussian filters
![Page 2: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/2.jpg)
Kalman Filter Localization
![Page 3: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/3.jpg)
4
Bayes Filter Reminder
1. Algorithm Bayes_filter( Bel(x),d ): 2. η=0
3. If d is a perceptual data item z then 4. For all x do 5. 6. 7. For all x do 8.
9. Else if d is an action data item u then
10. For all x do 11. 12. Return Bel’(x)
![Page 4: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/4.jpg)
• Prediction
• Correction
Bayes Filter Reminder
![Page 5: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/5.jpg)
Kalman Filter
• Bayes filter with Gaussians • Developed in the late 1950's • Most relevant Bayes filter variant in practice • Applications range from economics, wheather forecasting,
satellite navigation to robotics and many more.
• The Kalman filter "algorithm" is a couple of matrix multiplications!
6
![Page 6: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/6.jpg)
Gaussians
€
p(x) ~ N(µ,σ 2) :
p(x) =12πσ
e−12(x−µ )2
σ 2
-σ σ
µ
Univariate
µ
Multivariate
![Page 7: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/7.jpg)
Gaussians
1D
2D
3D
Video
![Page 8: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/8.jpg)
Properties of Gaussians
• Multivariate
• Univariate
• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations
![Page 9: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/9.jpg)
Introduction to Kalman Filter (1)
• Two measurements no dynamics
• Weighted least-square
• Finding minimum error
• After some calculation and rearrangements
• Another way to look at it – weigthed mean
![Page 10: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/10.jpg)
11
Discrete Kalman Filter
• Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
• with a measurement
Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise. Matrix (nxl) that describes how the control ut changes the state from t to t-1. Matrix (kxn) that describes how to map the state xt to an observation zt. Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
![Page 11: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/11.jpg)
12
Kalman Filter Updates in 1D
prediction measurement
correction
It's a weighted mean!
![Page 12: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/12.jpg)
13
Kalman Filter Updates in 1D
![Page 13: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/13.jpg)
14
Kalman Filter Updates in 1D
![Page 14: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/14.jpg)
15
Kalman Filter Updates
![Page 15: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/15.jpg)
16
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed:
![Page 16: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/16.jpg)
17
• Dynamics are linear function of state and control plus additive noise:
Linear Gaussian Systems: Dynamics
![Page 17: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/17.jpg)
18
Linear Gaussian Systems: Dynamics
![Page 18: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/18.jpg)
19
• Observations are linear function of state plus additive noise:
Linear Gaussian Systems: Observations
![Page 19: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/19.jpg)
20
Linear Gaussian Systems: Observations
![Page 20: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/20.jpg)
21
Kalman Filter Algorithm
1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):
2. Prediction: 3. 4.
5. Correction: 6. 7. 8.
9. Return µt, Σt
![Page 21: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/21.jpg)
Kalman Filter Algorithm
![Page 22: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/22.jpg)
Kalman Filter Algorithm
Prediction Observation
Matching Correction
![Page 23: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/23.jpg)
24
The Prediction-Correction-Cycle
Prediction
![Page 24: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/24.jpg)
25
The Prediction-Correction-Cycle
Correction
![Page 25: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/25.jpg)
26
The Prediction-Correction-Cycle
Correction
Prediction
![Page 26: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/26.jpg)
27
Kalman Filter Summary
• Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
• Optimal for linear Gaussian systems!
• Most robotics systems are nonlinear!
![Page 27: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/27.jpg)
28
Nonlinear Dynamic Systems
• Most realistic robotic problems involve nonlinear functions
• To be continued
![Page 28: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/28.jpg)
29
Linearity Assumption Revisited
![Page 29: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/29.jpg)
30
Non-linear Function
![Page 30: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/30.jpg)
31
EKF Linearization (1)
![Page 31: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/31.jpg)
32
EKF Linearization (2)
![Page 32: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/32.jpg)
33
EKF Linearization (3)
Depends on uncertainty
![Page 33: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/33.jpg)
34
• Prediction:
• Correction:
EKF Linearization: First Order Taylor Series Expansion
![Page 34: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/34.jpg)
35
EKF Algorithm
1. Extended_Kalman_filter( µt-1, Σt-1, ut, zt):
2. Prediction: 3. 4.
5. Correction: 6. 7. 8.
9. Return µt, Σt
![Page 35: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/35.jpg)
Kalman Filter for Mobile Robot Localization Robot Position Prediction
• In a first step, the robots position at time step k+1 is predicted based on its old location (time step k) and its movement due to the control input u(k):
f: Odometry function
![Page 36: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/36.jpg)
Kalman Filter for Mobile Robot Localization Robot Position Prediction: Example
![Page 37: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/37.jpg)
Kalman Filter for Mobile Robot Localization Observation
• The second step it to obtain the observation Z(k+1) (measurements) from the robot’s sensors at the new location at time k+1
• The observation usually consists of a set n0 of single observations zj(k+1) extracted from the different sensors signals. It can represent raw data scans as well as features like lines, doors or any kind of landmarks.
• The parameters of the targets are usually observed in the sensor frame {S}. • Therefore the observations have to be transformed to the
world frame {W} or • the measurement prediction have to be transformed to the
sensor frame {S}. • This transformation is specified in the function hi (seen
later) – measurement prediction
![Page 38: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/38.jpg)
Kalman Filter for Mobile Robot Localization Observation: Example
Raw Date of Laser Scanner
Extracted Lines Extracted Lines in Model Space
Set of discrete line features
![Page 39: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/39.jpg)
Kalman Filter for Mobile Robot Localization Measurement Prediction
• In the next step we use the predicted robot position and the map M(k) to generate multiple predicted observations zt – what you should see
• They have to be transformed into the sensor frame
• We can now define the measurement prediction as the set containing all ni predicted observations
• The function hi is mainly the coordinate transformation between the world frame and the sensor frame
• Need to compute the measurement Jacobian to also predict uncertainties
![Page 40: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/40.jpg)
Kalman Filter for Mobile Robot Localization Measurement Prediction: Example
• For prediction, only the walls that are in the field of view of the robot are selected.
• This is done by linking the individual lines to the nodes of the path
![Page 41: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/41.jpg)
Kalman Filter for Mobile Robot Localization Measurement Prediction: Example
• The generated measurement predictions have to be transformed to the robot frame {R}
• According to the figure in previous slide the transformation is given by
and its Jacobian by
![Page 42: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/42.jpg)
Kalman Filter for Mobile Robot Localization Matching
• Assignment from observations zj(k+1) (gained by the sensors) to the targets zt (stored in the map)
• For each measurement prediction for which an corresponding observation is found we calculate the innovation:
and its innovation covariance found by applying the error propagation law:
• The validity of the correspondence between measurement and prediction can e.g. be evaluated through the Mahalanobis distance:
![Page 43: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/43.jpg)
Kalman Filter for Mobile Robot Localization Matching: Example
![Page 44: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/44.jpg)
Kalman Filter for Mobile Robot Localization Matching: Example
• To find correspondence (pairs) of predicted and observed features we use the Mahalanobis distance
with
![Page 45: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/45.jpg)
Kalman Filter for Mobile Robot Localization Estimation: Applying the Kalman Filter
• Kalman filter gain:
• Update of robot’s position estimate:
• The associate variance
![Page 46: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/46.jpg)
Kalman Filter for Mobile Robot Localization Estimation: 1D Case
• For the one-dimensional case with we can show that the estimation corresponds to the Kalman filter for one-dimension presented earlier.
![Page 47: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/47.jpg)
Kalman Filter for Mobile Robot Localization Estimation: Example
• Kalman filter estimation of the new robot position : • By fusing the prediction of robot
position (magenta) with the innovation gained by the measurements (green) we get the updated estimate of the robot position (red)
![Page 48: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/48.jpg)
50
Localization
• Given • Map of the environment. • Sequence of sensor measurements.
• Wanted • Estimate of the robot’s position.
• Problem classes • Position tracking • Global localization • Kidnapped robot problem (recovery)
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
![Page 49: Probabilistic Robotics - George Mason Universitykosecka/cs485/lect14-kalman-ekf.pdf11 Discrete Kalman Filter • Estimates the state x of a discrete-time controlled process that is](https://reader033.fdocument.org/reader033/viewer/2022053001/5f055be77e708231d4129271/html5/thumbnails/49.jpg)
51
Landmark-based Localization