COS Lecture 16 Autonomous Robot Navigation

Transcription

1 COS Lecture 16 Autonomous Robot Navigation Instructor: Chris Clark Semester: Fall Figures courtesy of Siegwart & Nourbakhsh

2 Control Structure Prior Knowledge Operator Commands Localization Cognition Perception Motion Control

3 Introduction to the Kalman Filter 1. KF Representations. Two Measurement Sensor Fusion 3. Single Variable Kalman Filtering 4. Multi-Variable KF Representations 3

4 KF Representations What do Kalman Filters use to represent the states being estimated? Gaussian Distributions! 4

5 KF Representations 5 Single variable Gaussian Distribution Symmetrical Uni-modal Characterized by Mean µ Variance σ Properties Propagation of errors Product of Gaussians

6 KF Representations Single Var. Gaussian Characterization Mean Expected value of a random variable with a continuous Probability Density Function p(x) µ = E[X] = x p(x) dx For a discrete set of K samples K µ = Σ x k /K k=1 6

7 KF Representations Single Var. Gaussian Characterization Variance Expected value of the difference from the mean squared σ =E[(X-µ) ] = (x µ) p(x) dx For a discrete set of K samples σ = Σ (x k µ ) /K K k=1 7

8 KF Representations Single variable Gaussian Properties Propagation of Errors Product of Gaussians 8

9 KF Representations Single variable Gaussian Properties We stay in the Gaussian world as long as we start with Gaussians and perform only linear transformations. 9

10 Introduction to the Kalman Filter 1. KF Representations. Two Measurement Sensor Fusion 3. Single Variable Kalman Filtering 4. Multi-Variable KF Representations 10

11 Fusing Two Measurements Example Given two measurements q 1 and q, how do we fuse them to obtain an estimate q? Assume measurements are modeled as random variables that follow a Gaussian distribution with variance σ 1 and σ respectively 11

12 Fusing Two Measurements Example (cont ): 1

13 Fusing Two Measurements Example (cont ): Lets frame the problem as minimizing a weighted least squares cost function: 13

14 Fusing Two Measurements Example (cont ): If n= and w i = 1/σ i q = q 1 + σ 1 (q - q 1 ) σ 1 + σ 14

15 Introduction to the Kalman Filter 1. KF Representations. Two Measurement Sensor Fusion 3. Single Variable Kalman Filtering 4. Multi-Variable KF Representations 15

16 Single Variable KF Example: Fusing two Measurements q = q 1 + σ 1 (q - q 1 ) σ 1 + σ We can reformulate this in KF notation 16 x t = x t-1 + K t (z t - x t-1 ) K t = σ t-1 σ t-1 + σ t

17 Single Variable KF KF for a Discrete Time System x t = x t-1 + K t (z t - x t-1 ) K t = σ t-1 σ t-1 + σ t σ t = σ t-1 -K t σ t-1 17 Where x t is the current state estimate σ t is the associated variance z t is the most recent measurement K is the Kalman Gain

18 Kalman Filter Introduction 1. KF Representations. Two Measurement Sensor Fusion 3. Single Variable Kalman Filtering 4. Multi-Variable KF Representations 18

19 Representations in KF 19 Multi-variable Gaussian Distribution Symmetrical Uni-modal Characterized by Mean Vector µ Covariance Matrix Σ Properties Propagation of errors Product of Gaussians

20 Representations in KF Multi-Var. Gaussian Characterization Mean Vector Vector of expected values of n random variables µ = E[X] = [ µ 0 µ 1 µ µ n ] T µ i = x i p(x i ) dx i 0

21 Representations in KF 1 Multi-Var. Gaussian Characterization Covariance Expected value of the difference from the means squared σ ij =Cov[X i, X j ] = E[(X i µ i ) (X j µ j ) ] Covariance is a measure of how much two random variables change together. Positive σ ij when variable i is above its expected value, then the other variable j tends to also be above its µ j Negative σ ij when variable i is above its expected value, then the other variable j tends to be below its µ j

22 Representations in KF Multi-Var. Gaussian Characterization Covariance For continuous random variables σ ij = (x i µ i ) (x j µ j ) p(x i, x j ) dx i dx j For discrete set of K samples σ ij = Σ (x i,k µ i )(x j,k µ j )/K K k=1

23 Representations in KF Multi-Var. Gaussian Characterization Covariance Matrix Covariance between each pair of random variables σ 00 σ 01 σ 0n Σ = σ 10 σ 11 σ 1n : 3 s Note: σ ii =σ i σ n0 σ n1 σ nn

24 Representations in KF Multi variable Gaussian Properties Propagation of Errors Product of Gaussians 4

25 Next Apply the Kalman Filter to multiple variables in the form of a KF. 5