Download presentation

Presentation is loading. Please wait.

1
Introduction to Kalman Filter and SLAM Ting-Wei Hsu 08/10/30

2
What is Kalman Filter? (cont.)

3
What’s used for ? Tracking missiles Tracking heads/heads Extracting lip motion from video Fitting Bezier patches to points data Lots of computer vision Economics Navigation ……

4
Basic Idea z[n] = A + u[n] Measure1: State:

5
Basic Idea Measure2: State = ?

6
Basic Idea (cont.) Measure from 1 & 2

7
Kalman Filter Model z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 z6z6 x 2, σ 2 z7z7 x 3, σ 3

8
Extend to System Model x = Hθ+w y = θ

9
Estimate from Two Distributions If x and y are distributed according to Gaussian PDF with [E(x) E(y)] T And covariance matrix

10
Extend to System Model

12
z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 3

13
Pre-limit of Kalman Filter Linear dynamical system Markov Chain Zero mean Gaussian noise

14
Prediction to Correction

15
System Model F k state transition model w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k Observation model: v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

16
System Model z1z1 z2z2 z3z3 z4z4 z5z5 x 1, σ 1 F z6z6 x 2, σ 2 F z7z7 x 3, σ 3

17
Predict and Update Predict Predicted state Predicted estimate covariance Update innovation or measurement residual Innovation (or residual) covariance

18
Predict and Update (cont.) Update Optimal Kalman gain Updated state estimate Updated estimate covariance http://en.wikipedia.org/wiki/Kalman_filter

19
Example: 2D PV Model Position-velocity model u(n): change in velocity v(n): measurement error

20
Example: 2D PV Model (cont.) Process Noise Covariance Measurement Noise Covariance

21
EKF-Extended Kalman Filter Processes to be estimate or measurement is non-linear. Model: Predict:

22
EKF-Extended Kalman Filter Update: Transition and observation matrix

23
Disadvantage of the Extended Kalman Filter Use only first level Taylor series. If the initial estimate of the state is wrong, the filter may quickly diverge. Solution: Unsented Kalman filter

24
SLAM Simultaneous localization and mapping Technique used by robots and autonomous vehicles to build up a map within an unknown environment.

25
SLAM Problem

26
Overview of the Process 1.Update the current state estimate using the odometry data. 2.Update the estimated state from re- observing landmarks. 3.Add new landmarks to the current state.

27
Spring Network Analogy

28
System Model F k state transition model w k is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution with covariance Q k Observation model: v k is the observation noise which is assumed to be zero mean Gaussian white noise with covariance R k

29
The Matrix The system state: x x r, y r, theta r for robot x 1,y 1 …x n, y n position of each landmark.

30
The Matrix Covariance Matrix P 3x33x2 2x3

31
The Matrix Measurement model : H

32
The Matrix Jacobian of H of robot

33
The Matrix H for SLAM EKF as landmark number two observed.

34
The Matrix

35
Prediction model : A

36
The Matrix The SLAM specific Jacobians

37
Step 1: Update current state using the odometry data Update current state using odometry data P rr is the top left 3 by 3 matrix of P Update the robot to feature correlation

38
Step 2.Update the Estimated State from Re-observing Landmarks X = X + K*(z-h)

39
The Matrix Process noise Measure noise c, d represent the accuracy of measure device =

40
Step 3: Add New Landmarks to Current State X = [X x N y N ] T

41
FastSLAM Integrates particle filter and extend Kalman Filter. Cope with non-linear robot models better.

42
FastSLAM Robot Trajectory

43
Factoring the SLAM Posterior

44
Symbol Θ MAP, consists of collection of features[θ 0 θ 1 …θ n ] s t robot post at time t s t = s 1, s 2, s 3 …s t z t, n t : measurement feature n at time t u t : control of vehicle

45
Fast SLAM Algorithm z t depend only on s t, n t, θ n t

46
Particle Filter in FastSLAM

47
Step 1. Extend the Path Posterior by Sampling New Poses. s t robot pose u t contorl

48
Step 2 Updating the Observed Landmark Estimate z t sensor measurement θlandmark

49
Step 3. Resampling

50
Step 3. Resampling (cont.)

Similar presentations

© 2021 SlidePlayer.com Inc.

All rights reserved.

To make this website work, we log user data and share it with processors. To use this website, you must agree to our Privacy Policy, including cookie policy.

Ads by Google