The Kalman Filter (part 1) Definition. Rudolf Emil Kalman. Why do we need a filter? Definition. HCI/ComS 575X: Computational Perception.

Transcription

1 The Kalman Filter (part 1) HCI/ComS 575X: Computational Perception Instructor: Alexander Stoytchev March 5, 2007 HCI/ComS 575X: Computational Perception Iowa State University, SPRING 2007 Copyright 2007, Alexander Stoytchev Rudolf Emil Kalman Definition A Kalman filter is simply an optimal recursive data processing algorithm Under some assumptions the Kalman filter is optimal with respect to virtually any criterion that makes sense. [ Definition The Kalman filter incorporates all information that can be provided to it. It processes all available measurements, regardless of their precision, to estimate the current value of the variables of interest. Why do we need a filter? No mathematical model of a real system is perfect Real world disturbances Imperfect Sensors 1

2 Application: Radar Tracking Application: Lunar Landing Application: Missile Tracking Application: Sailing Application: Robot Navigation 2

3 Application: Other Tracking Application: Head Tracking Face & Hand Tracking Brown and Hwang (1992) Introduction to Random Signals and Applied Kalman Filtering Ch 5: The Discrete Kalman Filter Maybeck, Peter S. (1979) Chapter 1 in ``Stochastic models, estimation, and control'', Mathematics in Science and Engineering Series, Academic Press. Arthur Gelb, Joseph Kasper, Raymond Nash, Charles Price, Arthur Sutherland (1974) Applied Optimal Estimation MIT Press. 3

4 A Simple Recursive Example Problem Statement: Given the measurement sequence: z 1, z 2,, z n find the mean First Approach 1. Make the first measurement z 1 Store z 1 and estimate the mean as µ 1 =z 1 2. Make the second measurement z 2 Store z 1 along with z 2 and estimate the mean as µ 2 = (z 1 +z 2 )/2 First Approach (cont d) First Approach (cont d) 3. Make the third measurement z 3 Store z 3 along with z 1 and z 2 and estimate the mean as µ 3 = (z 1 +z 2 +z 3 )/3 n. Make the n-th measurement z n Store z n along with z 1, z 2,, z n-1 and estimate the mean as µ n = (z 1 + z z n )/n Second Approach Second Approach (cont d) 1. Make the first measurement z 1 Compute the mean estimate as µ 1 =z 1 2. Make the second measurement z 2 Compute the estimate of the mean as a weighted sum of the previous estimate µ 1 and the current measurement z 2: µ 2 = 1/2 µ 1 +1/2 z 2 Store µ 1 and discard z 1 Store µ 2 and discard z 2 and µ 1 4

5 Second Approach (cont d) Second Approach (cont d) 3. Make the third measurement z 3 Compute the estimate of the mean as a weighted sum of the previous estimate µ 2 and the current measurement z 3: n. Make the n-th measurement z n Compute the estimate of the mean as a weighted sum of the previous estimate µ n-1 and the current measurement z n: µ 3 = 2/3 µ 2 +1/3 z 3 µ n = (n-1)/n µ n-1 +1/n z n Store µ 3 and discard z 3 and µ 2 Store µ n and discard z n and µ n-1 Comparison Analysis The second procedure gives the same result as the first procedure. It uses the result for the previous step to help obtain an estimate at the current step. Batch Method Recursive Method The difference is that it does not need to keep the sequence in memory. Second Approach (rewrite the general formula) Second Approach (rewrite the general formula) µ n = (n-1)/n µ n-1 +1/n z n µ n = (n-1)/n µ n-1 +1/n z n µ n = µ n-1 + 1/n (z n - µ n-1 ) 5

6 Second Approach (rewrite the general formula) Second Approach (rewrite the general formula) µ n = (n-1)/n µ n-1 +1/n z n µ n = µ n-1 + 1/n (z n - µ n-1 ) Old Estimate Gain Factor Difference Between New Reading and Old Estimate The Gaussian Function Gaussian Properties Gaussian pdf Properties If and Then 6

7 pdf for Properties Summation and Subtraction A simple example using diagrams Conditional density of position based on measured value of z 1 Conditional density of position based on measured value of z 1 uncertainty position measured position 7

8 Conditional density of position based on measurement of z 2 alone Conditional density of position based on measurement of z 2 alone uncertainty 2 measured position 2 Conditional density of position based on data z 1 and z 2 Propagation of the conditional density uncertainty estimate position estimate Propagation of the conditional density Propagation of the conditional density movement vector movement vector expected position just prior to taking measurement 3 expected position just prior to taking measurement 3 8

9 Propagation of the conditional density Updating the conditional density after the third measurement position uncertainty uncertainty 3 σ x (t 3 ) σ x (t 3 ) z 3 x(t3) z 3 measured position 3 position estimate Questions? How should we combine the two measurements? Now let s do the same thing but this time we ll use math σ Z2 σ Z1 9

10 Calculating the new mean Calculating the new mean Calculating the new mean Calculating the new variance σ Z2 σ Z1 Why is this not z 1? Calculating the new variance Calculating the new variance 10

11 Calculating the new variance Calculating the new variance Calculating the new variance Calculating the new variance Why is this result different from the one given in the paper? Remember the Gaussian Properties? 11

12 Remember the Gaussian Properties? The scaling factors must be squared! If and Then This is a 2 not a The scaling factors must be squared! Therefore the new variance is Try to derive this on your own. Another Way to Express The New Position Another Way to Express The New Position 12

13 Another Way to Express The New Position The equation for the variance can also be rewritten as Adding Movement Adding Movement Adding Movement Properties of K If the measurement noise is large K is small 0 13

14 THE END 14