Course 8. An Introduction to the Kalman Filter

Transcription

1 Course 8 An Introduction to the Kalman Filter

2 Speakers Greg Welch Gary Bishop

3 Kalman Filters in 2 hours? Hah! No magic. Pretty simple to apply. Tolerant of abuse. Notes are a standalone reference. These slides are online at cs.unc.edu/~tracker/ref/s2001/ /~tracker/ref/s2001/kalman/

4 Rudolf Emil Kalman Born 1930 in Hungary BS and MS from MIT PhD 1957 from Columbia Filter developed in Now retired

5 What is a Kalman Filter? Just some applied math. A linear system: f(a+b) = f(a) + f(b). Noisy data in hopefully less noisy out. But delay is the price for filtering... Pure KF does not even adapt to the data.

6 What is it used for? Tracking missiles Tracking heads/hands/drumsticks Extracting lip motion from video Fitting Bezier patches to point data Lots of computer vision applications Economics Navigation

7 A really simple example

8 Gary makes a measurement z σ 2 1, z 1 Conditional Density Function x ˆ = z 1 1 N(z 1,σ 2 z1 ) σ ˆ 2 = σ 2 1 z

9 Greg makes a measurement z σ 2 2, z 2 Conditional Density Function x ˆ =...? 2 N(z 1,σ 2 z1 ) σ ˆ 2 =...?

10 Combine estimates

11 Combine variances

12 Combined Estimates Conditional Density Function x ˆ = x ˆ 2 N( x,ˆ ˆ σ 2 ) σ ˆ 2 = σ Online weighted average!

13 But suppose we re moving Not all the difference is error Some may be motion KF can include a motion model Estimate velocity and position

14 Process Model Describes how the state changes over time The state for the first example was scalar The process model was nothing changes A better model might be State is a 2-vector [ position, velocity ] position n+1 = position n + velocity n * time velocity n+1 = velocity n

15 Measurement Model What you see from where you are not Where you are from what you see

16 Predict Correct KF operates by Predicting the new state and its uncertainty Correcting with the new measurement predict correct

17 Example: 2D Position-Only (Greg Welch)

18 Apparatus: 2D Tablet

19 Process Model x y k k state x k x = y state transition A k 1 k 1 xstate k 1 x = Ax + w k k 1 k 1 x + ~ k 1 ~ y k 1 wnoise k 1

20 Measurement Model u v k k z k measurement H = 0 x 0 H measurement matrix H y x y k k state x k u + ~ k ~ v k noise v k z = Hx + v k k k

21 Preparation A = State Transition Q Q E w w T xx = { 0 }= 0 Q R R E v v T xx = { 0 }= 0 R yy yy Process Noise Covariance Measurement Noise Covariance

22 Initialization x = Hz P = ε 0 0 ε

23 PREDICT x k k = Axk 1 P = AP A + Q k 1 transition uncertainty T

24 CORRECT ( ) k k k k x = x + K z Hx P = I actual KH P k ( ) k predicted ( ) T T 1 k k K = P H HP H + R denominator (measurement space)

25 Summary x k = Axk 1 T P = AP A + Q k k 1 ( ) T T K = P H HP H + R 1 k k ( ) x = x + K z Hx k k k k k = ( ) k P I KH P

26 Results: XY Track y [meters] Truth Estimate x [meters]

27 Y Track: Moving then Still y [meters] Truth Estimate Time [seconds]

28 Motion-Dependent Performance Truth Estimate Measurement significant latency when moving relatively smooth when not

29 Example: 2D Position-Velocity (PV Model)

30 Process Model (PV) state transition 1 0 dt dt state x y dx dt dy dt

31 Measurement Model (Same) measurement matrix H x 0 H 0 0 y state x y dx dt dy dt

32 Different Performance Truth Estimate Measurement improved latency when moving relatively noisy when not

33 Example: 6D HiBall Tracker (x, y, z, roll, pitch, yaw)

34 Apparatus HiBall with six optical sensors Ceiling panel with LEDs

35 State Vector (PV) x d d T τ ρ τ ρ λ dt dt = [ ] τ = ρ = dτ = dt dρ = dt λ = translation (3D) rotation (3D) linear velocity (3D) angular velocity (3D) LED position (3D)

36 Non-Linear Measurement Model Non-Linear Measurement Model u v c c c c x z y z = c c c V x y z = ( ) ( ) rotate ρ λ τ view matrix

37 SCAAT vs. MCAAT Single or Multiple Constraint(s) at a Time Dimension of the measurement Nothing about KF mathematics restricts it Can process in batch or sequential mode SCAAT Estimate 15 parameters with 2D measurements Temporal improvements Autocalibration of LED positions

38 HiBall Initialization Initialize pose using a brute-force (relatively slow) MCAAT approach Initial velocities = 0 Initial process covariance P 0 = ~cm/degrees Transition to SCAAT Kalman filter

39 Nonlinear Systems (Gary Bishop)

40 Kalman Filter assumes linearity Only matrix operations allowed Measurement is a linear function of state Next state is linear function of previous state Can t t estimate gain Can t t handle rotations (angles in state) Can t t handle projection

41 Extended Kalman Filter Nonlinear Process (Model) Process dynamics: A becomes a(x) Measurement: H becomes h(x) Filter Reformulation Use functions instead of matrices Use Jacobians to project forward, and to relate measurement to state

42 Jacobian? Partial derivative of measurement with respect to state If measurement is a vector of length M And state has length N Jacobian of measurement function will be MxN matrix of numbers (not equations) Often evaluating h(x) and Jacobian(h(x)) at the same time cost only a little extra

43 Tips Don t t compute giant symbolic Jacobian with a symbolic algebra package Do use an automatic method during development Check out tools from optimization packages Differentiating your function line-by-line is usually pretty easy

44 New Approaches Several extensions are available that work better than the EKF in some circumstances

45 System Identification Model Form and Parameters (Greg Welch)

46 Measurement Noise (R)

47 Sampled Process Noise (Q) For continuous model dx dt = Fx + Q c The sampled (discrete) Q is dt Q = e Fτ Q e F T τ dτ d c 0

48 Example: 2D PV Model For continuous model dx dt = 0 1 x q The sampled (discrete) Q is dt q Qd = dt 2 q 3 dt q 2 2 dt q 3 2

49 Parameter Optimization "Clean" and realistic state data Measurement model Truth True measurement data + Noise True State Error ( Cost) Estimated State - Kalman filter

50 Multiple-Model Configurations Off or On-Line Model Selection

51 Off-Line Model Selection simulated measurement sequence Z 1,Z 2,,Z k Optimizer 1 Optimizer 2 Optimizer n

52 On-Line Multiple-Model Estimation Actual meas. seq. Z* p(µ 1 z, µ1 ) KFµ p(µ 1 1 z, ) µ1 KFµ 2 KFµ n ~ X µ1 ~ X µ2 ~ X µn p(µ 2 z, ) µ2 p(µ n z, ) µn Σ x ( = { x µn k, P k, H, R }

53 Probability of Model µ For model µ with Π ={ xphr,,, } µ 1 ( z Hx) C ( z Hx) T p( 1 1 µ z, Π )= µ e 2 ( n π C ) 2 2 where C = HPH T + R

54 Final Combined Estimate ) x = µ xƒ µ ν p( µ z, Π ) µ p( ν z, Π ) ν

55 Example: P/PV Multiple-Model Truth Estimate y [meters] Time [seconds]

56 MME Weighting 1 y [meters] Time [seconds] p(µ P z,π µ P )

57 Low-Latency During Motion Truth ooooo P Estimate PV Estimate MM Estimate Time [seconds]

58 Smooth When Still Truth ooooo P Estimate PV Estimate MM Estimate Time [seconds]

59 Conclusions Suggestions and Resources (Greg Welch)

60 Many Applications (Examples) Engineering Robotics, spacecraft, aircraft, automobiles Computer Tracking, real-time graphics, computer vision Economics Forecasting economic indicators Other Telephone and electricity loads

61 Kalman Filter Web Site Electronic and printed references Book lists and recommendations Research papers Links to other sites Some software News

62 Java-Based KF Learning Tool On-line 1D simulation Linear and non-linear Variable dynamics

63 KF Course Web Page ( ) Electronic version of course pack (updated) Java-Based KF Learning Tool KF web page See also notes for Course 11 (Tracking)

64 Closing Remarks Try it! Not too hard to understand or program Start simple Experiment in 1D Make your own filter in Matlab, etc. Note: the Kalman filter wants to work Debugging can be difficult Errors can go un-noticed

65 End