Chapter 5 Optimal Estimation

Transcription

1 Chapter 5 Optimal Etimation Part State Space Kalman Filter 1

2 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 2

3 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 3

4 Rudolph. E. Kalman Born in Budapet, Hungary, on May 19, Magnetic peronality Did EE at MIT Profeor at Stanford U 4

5 Impact One of the greatet and broadly applied dicoverie in the hitory of tatitical etimation theory. Navigation and Guidance Application Robotic Aircraft Automobile Spacecraft orbit determination 5

6 Impact Control and Etimation Application Continuou manufacturing procee (Poer, Chemical) Target tracking Computer viion Economic Forecating Stock Market Prediction!!! 6

7 Impact Subytem Within Robotic Perception, Localization Control Subproblem of Robotic State etimation Data aociation Calibration, ytem identification Trade tudie Built-in imulation 7

8 Characterization Uually, the ituation i more generic ith meaurement that are: incomplete: related to ome but not all of the variable of interet indirect: related indirectly to the quantitie of interet intermittent: available at irregularly-paced intant of time Alo, the tate vector of interet may be changing ith repect to time. The Kalman Filter can handle all of thi. 8

9 Characterization An algorithm. Not hardare. Recurively etimate tate of a dynamic ytem from noiy data. Sytem dynamic perturbed by hite noie. Meaurement perturbed by hite noie. For optimal (or even correct) reult, error mut be: Unbiaed (have zero mean for all time) Gauian (have a Gauian ditribution for all time) White (contain all frequencie) 9

10 5.3.1 Introduction Recall the form of tate pace model of a ytem: 10

11 5.3.1 Overall Operation Phyical Sytem Sytem State Meaurement Proce Meaure ment x (t) z(t) xˆ ( t) Initial Etimate Kalman Filter -Sytem model -Meaurement model Sytem Noie v (t) (t) Meaurement Noie xˆ ( t) State Etimate 11

12 5.3.1 Additional Capabilitie of SS KF An SS KF can: Predict tate beteen and beyond the meaurement. Ue rate meaurement that are derivative of required tate variable. Explicitly account for modeling aumption and diturbance in a more precie ay than jut noie. Identify a ytem (calibrate parameter) in real-time. Correlation that it track make it poible to remove effect of hitorical error once they become knon. 12

13 Need for State Prediction Let ubcript denote time thu: x 1 = xt ( 1 ) z 2 = zt ( 2 ) Not all of the difference beteen x1 and x2 i no due to error. Some of it i motion. Mut compute x2 from x1 and then compare z2 to that. That alo involve predicting the error in the prediction recall ho error compound in dead reckoning. 15

14 Dicrete Time Sytem Model Continuou Time: State or Proce Model Meaurement or Obervation Model Dicrete Time: Continuou form rarely ued in practice State or Proce Model Meaurement or Obervation Model 16

15 Nomenclature n = # tate m = # meaurement 17

16 Aume: Noie Proce and meaurement noie are hite (uncorrelated ith themelve in time). Uncorrelated ith each other. T E ( k i ) = δ ik Q k T Ev ( k v i ) = δ ik R k T E ( k v i ) = 0, ( ik, ) 18

17 Tranition Matrix Convert continuou time ODE to dicrete time one: The time continuou, matrix ODE: x = Ft ()x Can alay be converted to: But it may not be eay. 19

18 Matrix Exponential When F(t) i actually time-independent (F): Don t panic! It jut add and multiplie, ah, forever. For time varying F(t), even hen t i ufficiently mall relative to ytem time contant, can ue: 20

19 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 21

20 The Filter Equation 2 Set + mean after incorporation of meaurement into etimate 22

21 Time and Update z k K xˆ P xˆ [ H T T k = Pk H k kpk H k + Rk xˆ + k = k + Kk[ zk H k k k = [ I Kk H k ] Pk xˆ = Kalman Filter ˆ k x k + ] P k = P k ] 1 Delay xˆ k+1= Φk ˆ + k = x k xˆ k+ 1 k k + P k = P k Sytem Model = Φ xˆ P P Φ + G Q T k k k k G T k xˆ k Sytem model run continuouly (i.e. at high rate). Kalman filter run hen meaurement are available. 23

22 Interpreting Uncertainty Matrice Q k : you provide thi intantaneou uncertainty hich corrupt the ytem model random phyical diturbance and proce model error R k : you provide thi too intantaneou uncertainty hich corrupt the meaurement model random error in enor output P k : Filter motly manage. You provide only P0 total integrated uncertainty in tate etimate 24

23 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 25

24 Linearizing Nonlinear Problem Full nonlinear model: x = fxt (, ) + gt ()() t z = hxt (, ) + vt () Linearize about a reference trajectory x*(t) x = f ( x *, t) x+ gt ()t () x z hx ( *, t) = h * ( x, t) x+ vt () x 26

25 Linear (Feedforard) Kalman Filter Doe not update the reference trajectory: Sytem Model x r + h( x r ) δ x r z + z h( x r ) Kalman Filter State vector i the error. Advantage: more reponive to dynamic (computed in reference trajectory). Diadvantage: diverge more quickly. 27

26 Extended Kalman Filter Doe update the reference trajectory: Sytem Model h( x r ) x r z + z h( x r ) Kalman Filter δ x r State vector i the tate. Diadvantage: le reponive to dynamic. Advantage: diverge le quickly. 28

27 Kalman Filter: Extended Kalman Filter Jacobian: Compute Kalman gain: Update tate etimate: Update it covariance: Sytem Model: Project tate: Project covariance: Thee are the one you ill ue for almot any filter. 29

28 State Tranition Nonlinear Problem When the ytem model i nonlinear: x = fxt ( ()t, ) The previou expreion: xˆk + 1 = φ k ( xˆ k ) I jut code for olve the ODE. The tranition matrix can be generated from time linearization: + = x k + fx ( k, t k ) t x k 1 30

29 Uncertainty Propagation Nonlinear Problem The tate covariance propagation i: Thi approximation can be ued: Φ F = I + F t k 31

30 Sytem Identification A poorly knon contant can be computed automatically if there are enough meaurement to oberve it. It tate equation i: x i = 0 Jut add it to the tate vector and make ure to update H to encode ho meaurement error depend linearly on it error. 32

31 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 33

32 State Vector: D Mobile Robot Filter y b ψψ Care about thi Meaurement Need thi to propagate tate x Tranmiion Encoder Gyro 34

33 Sytem and Meaurement Model (Sytem Model) Generally of the form: x Here, it i: dx = = dt fxt (, ) y b ψψ Nonlinear! Aume contant velocity beteen meaurement, but no orrie becaue: Meaurement can change velocity. Meaurement may arrive at 100 Hz. x 35

34 Sytem and Meaurement Model Recall: (Sytem Jacobian) Clearly : ψ F 36

35 Dicretize and Linearize Linearize: Expre in matrix form: v k+1 ω k+1 Thi i a linearized (called Euler ) approximation. THIS IS NOT Φ! Maybe it eaier to code thi. 37

36 Dicretize and Linearize (State Uncertainty Propagation) Recall, it of the form: We approximate the tranition matrix ith: Where: Note difference from F matrix 2 lide ago! 38

37 Be careful ith P0: Initialization Too little P0 and meaurement ill be ignored. Too much P0 and numerical problem. Here aume: Mean: the matrix hoe diagonal i thi vector 39

38 Sytem Diturbance Error groth beteen meaurement Ue it to capture: Incorrectne of flat terrain aumption. Incorrectne of no Wheel lip aumption. Incorrectne of contant velocity aumption. Would like it to be larger for larger t. In the abence of real data, try omething related to the Taylor remainder Firt neglected term in dynamic linearization. 40

39 Sytem Diturbance Try: But hat i GG kk? contant Let kk xxxx and kk yyyy be interpreted in the body frame to allo aymmetric error magnitude in direction of travel. Then GG kk convert coordinate: 41

40 OK. Breathe. We re 1/4 Done We have the dynamic

41 Tranmiion Encoder Meaurement Velocity encoder: Model Alay expre meaurement a a prediction baed on: The preent tate No other meaurement If you are ure you can t predict the meaurement from the tate, add more tate variable til you can. 43

42 Tranmiion Encoder Meaurement Model (Error Model) Expre uncertainty a ditance dependent random alk. In continuou time: That i, hen integrated rt time, gro linearly rt ditance becaue Vdt = d Multiply by tt ee to get: Why? Produce a poition variance that gro linearly ith ditance beteen meaurement. 44

43 Gyro Meaurement Model/Uncertainty Gyro meaurement: For R, go ith time dependent random alk: To convert to dicrete time (multiply by tt gg ). Make the variance of angle rate contant hile variance of computed angle gro linearly ith time. 45

44 1/2 DONE no have: z = h(x) & H & R

45 Time for a Fe Good Z 47

46 Dead Reckoning So far, e have a lot of code that doe thi: Any proce that only integrate noiy velocitie mut eventually (quickly?) get lot. Without poe fixe, even an optimal etimate i not much ue. 48

47 Suppoe: Landmark A map of here the landmark are in the orld. A enor hich meaure landmark poition relative to itelf. ψψ Note: The book preent a forced formulation hich i better but not conitent ith the homeork aignment, o thee lide cover an unforced formulation here velocitie remain in the tate vector. y x b 49

48 Forced Formulation Can treat velocity meaurement a input u rather than meaurement z. Error in the velocitie are then modeled in Q rather than R. The tate vector i maller: Sytem Model: 50

49 Forced Formulation Sytem model in matrix form: Sytem Jacobian: Φ k matrix: State Uncertainty Propagation: - P k + 1 T = F P k T Φ k Φ Fk + G k Q k G k T 51

50 Incorporating a Map (Landmark Meaurement Model) Thi i of the form z = h(x) here: r d m x = x b y b ψ θ αα x 52

51 Incorporating a Map (Landmark Meaurement Model) Jacobian.r.t robot H x z poe: z = = b b ρ b z d bd H dhbdhx αα r d m Jacobian.r.t landmark poe: z H m z = = b b ρ m z d bd d H dhbdhdhm x m 53

52 Oberver and Jacobian A real enor doe not meaure in Carteian coordinate. Polar i more likely: y d coα inα = = x d y d r d r d Forard Kinematic x d r d αα x y z en α = = fr ( d ) = r d y d atan( ) x d ( x d ) 2 + ( y d ) 2 Invere Kinematic enor 54

53 Senor Referenced Obervation H x z b z z d bd z = = H b dhbdhx H m ρ d ρ b Occur in 2 place z = = b Nothing here but ton of math Recall: z en α = = fr ( d ) = r d y d x d atan( ) ( x d ) 2 + ( y d ) 2 b ρ m z d bd d H dhbdhdhm 55 z H d α z H ( r d ) 2 y d x d = = = = r d 1 H r --- xd yd r d α cα r d cα α

54 Body To Senor H x z b z z d bd z = = H b dhbdhx H m ρ b Occur in 2 place We need: b Invere i: Compound-Right Poe Jacobian! b z = = b b ρ m z d bd d H dhbdhdhm d 56

55 World to Body: Firt Jacobian H x z z z d bd z = = H dhbdhx H m b b ρ b z = = b b ρ m z d bd d H dhbdhdhm We need: b ρd Invere i: ρd b d Compound-Right Poe Jacobian 57

56 World to Body: Second Jacobian H x z z z d bd z = = H dhbdhx H m b b ρ b b We need: Right-Left Poe Jacobian ρb Minu Sign z = = b b ρ m z d bd d H dhbdhdhm d 58 Thi mean there i info here on x and y and θ.

57 Model to World H x z z z d bd z = = H dhbdhx H m b b ρ b z = = b b ρ m z d bd d H dhbdhdhm We need: ρm Compound-Left Poe Jacobian d d H m ρ 10 ( y d ym ) d = = 01 x ( d x m ) ρ m

58 Total Meaurement Model: Point Compute it like thi: Feature State Variable Landmark Poition From Map Predicted Senor Reading Jacobian r d z en = T b *T b α = = fr ( d ) = r d m ( ρ b )*T m ( rm )*r d y d atan( ) x d ( x d ) 2 + ( y d ) 2 Find Robot H x z z = = b b ρ b z d bd H dhbdhx Find Landmark z H m z = = b b ρ m z d bd d H dhbdhdhm 60

59 3/4 DONE! no have ome really good z

60 Still

61 Not

62 Done!

63 Data Aociation The Achille Heel of the Kalman Filter. There are lot of landmark out there. Ho do you kno hich one you are looking at? One mitake and it all over: A potentially maive change in the vehicle poe ill occur. Thi ill caue more rong aociation and feer or no right one. The filter ill diverge, and the ytem ill rapidly get lot. 66

64 Thi i the expreion: Innovation Covariance S = HPH T + R in the Kalman Gain calculation. Repreent the covariance of the innovation z- h(x). I.E. ho doe the tate error P [in h(x)] and the meaurement error R [in z] combine to give the error in my prediction right no. 67

65 Validation Gate Recall the Mahalanobi ditance - multidimenional deviation from the mean: d = z T S 1 z Compute thi for every landmark giving n d to look at. It turn out if the innovation i Gauian, then the MHD i Chi quare ditributed. Confidence threhold can be derived: Variance Gate 68

66 Validation Gate Thi lead to ome good idea for data aociation: Require that any candidate aociation have a MD < about 3. Require that there be no other candidate aociation ith an MD < 6 or an even bigger number. Require that an aociation be table for everal cycle before it i actually ued. 69

67 Done! Thi ha been a... really really ueful Kalman Filter

68 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Harder Example Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 71

69 Jut Kidding! Here are ome graph of a 3D filter. 72

70 3D AHRS Filter Reult 73

71 3D AHRS Filter Reult 74

72 3D AHRS Filter Reult 75

73 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 76

74 Sequential Meaurement Proceing All meaurement do not have to come in at the ame rate. Jut proce em hen you have em after predicting tate for their time of arrival. State_Update() /* enter every cycle */ { ytemmodel(dt); if( Doppler meaurement available) run Kalman() on Doppler; if( Encoder meaurement avail run Kalman() on encoder; if( AHRS meaurement available) run Kalman() on AHRS; if( Steering meaurement available) run Kalman() on teering; } Kalman() { } 77

75 Single Meaurement Efficiency: Kalman Recall: K k Gain - T - T = P kh k [ Hk P khk + R k ] 1 Suppoe only one direct meaurement: R = [] r Meaurement Jacobian i: Define: p = P 1 at index H = Then, Kalman Gain i a calar time th column of P: 1 K = p + r Pi Sth column of P 78

76 The formula: Uncertainty Propagation take n2(1+m)+n3 flop [1200 for n=10,m=1] Can be computed more efficiently a: hich take n2(1+m) + mn2 flop [300 for n=10,m=1] P = [ I ( KH) ]P P = P K( HP) 79

77 Uncertainty Propagation For a calar meaurement, recall: 1 at index H = KHP i jut a contant time the outer product: ( KHP) 1 ij p+ r Pi P j = i j 80

78 R matrix and cycle time It i lightly better to have every element of R be proportional to dt. Thi tend to make your filter behave appropriately if you change the time tep. If not, you can get ierd behavior like a filter hich produce ore aner if you run it fater (becaue you are adding up more random number of the ame variance). 81

79 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter - SKIP Summary 82

80 Outline 5.3 State Space Kalman Filter Introduction Linear Dicrete Time Kalman Filter Kalman Filter for Nonlinear Sytem Simple Example: 2D Mobile Robot Pragmatic Information for Kalman Filter Other Form of the Kalman Filter Summary 83

81 Summary A SS KF i conceptually to et of equation. Mot cae require linearization. The extended form i the mot ueful. Handle the tricky iue of integration dead reckoning and poition fixe automatically. Mot meaurement are calar and e often aume decorrelation. Lead to proceing efficiencie. 84