Data Fusion Kalman Filtering Self Localization

Size: px

Start display at page:

Download "Data Fusion Kalman Filtering Self Localization"

Aileen Whitehead
5 years ago
Views:

Data Fusion Kalman Filtering Self Localization Armando Jorge Sousa http://www.fe.up.

1 Data Fusion Kalman Filtering Self Localization Armando Jorge Sousa Faculty of Engineering, University of Porto, Portugal Department of Electrical and Computer Engineering Institute of Systems and Robotics, Porto 1

2 Summary Data Fusion Definition, need, what is improved, strategies, healthy start Kalman Filtering State Space, discrete time, stchastic, KF algorithm, iterative EKF algorithm, pitfalls Self Localization as an example of EKF as an example of Data Fusion Example wheelchair, using EKF, System Model, Measurements & Model, Results Final Comments 2

3 Definition of Data Fusion Multisensory data fusion is the process of combining observations from a number of different sensors to provide a robust and complete description of an environment or process of interest. Data fusion finds wide application in many areas of robotics such as object recognition (perception), environment mapping, and localization [adapted from Springer Handbook of Robotics Bruno Siciliano; Oussama Khatib (Eds.) 2008] Combine data from multiple sources will be more efficient than if they were achieved by means of a single source [ 3

4 Need for Data Fusion Sensor Imperfection Sensor Malfunctioning Technological limitations Physical working model limitations (ranges, etc) Complexity / Indirect Measuring Sensory Redundancy => COTS are very interesting Real Time issues Distributed sensing needs distributed data fusion 4

5 Improvements After Data Fusion Representation Example: resolution Certainty Example: likelihood Accuracy Example: reduce outliers [Mitchell, 2007] 5

6 Strategies for Data Fusion Fusion across different Sensors Example: several temperature sensors Fusion across different Attributes Example: Temperature, pressure humidity to determine air refractive index Fusion across different Domains Example: different ranges / domains Fusion across different Time Example: Sampling over space and time [adapted Boujemaa and Forbes, 2007] 6

7 Going to Kalman Filtering Sensor Model Example: probabilistic Framework The Kalman Filter <= the only addressed Bayes Rule Sequential Monte Carlo Methods Probabilistic Grids Alternatives to Probability Example: Heuristics! 7

8 Kalman Filtering R. Kalman, 1960 Known as Integration Workhorse Levy, 1997 Filter Weighed merging of System Estimate and Measurements originated Updates The Kalman filter is a recursive optimal linear estimator with theoretical guarantee of convergence Starting with State Space representation Where measurements affect system estimate! 8

9 State Space & Discrete Time {d x t = Ax t Bu t dt y t = Cx t x k = x k 1 Gu k 1 y k = Hx k x =[ x 1 u=[u 1 y=[ y 1 x 2 x m ] T u 2 u p ] T y 2 y q ] T t k =t k t k 1 k =e A t k t k G= e A t k B dt t k 1 H =C dim A =m m dim B = p m dim C =m q 9

10 State Space - Stochastic { x k = x k 1 Gu k 1 w k 1 y t = Hx k v k 1 R w k, i ={ Q k se i=k 0 se i k R v k, i ={ R k se i=k 0 se i k Kalman Filter always keeps track of averages and covariances! E w k v i T =0, k,i P w ~ N 0,Q P v ~N 0, R E x 0 = X 0 cov x 0 =P 0 P 2 =[cov y 1, y 1 cov y 1, y 2 cov y 1, y n cov y 2, y 1 cov y 1, y 1 cov y 1, y n cov y n, y 1 cov y 1, y 1 cov y n, y n ] 10

11 KF - Algorithm Predict Stage Update Stage Propagate State: x k = x k 1 Bu k 1 Propagate Covariance: P k = P k 1 T Q k Kalman Gain Calculation: K k =P k H T HP k H T R k 1 State Update with new Measure: x k =x k K k y k H x k Update Covariance with new measurement: P k = I K k H P k Initial Estimates: x 0, P 0 11

12 KF - hints The Kalman Gain weighs changes on system state: If measurement noise is low => update based on measure is more important If measurement noise is high => update based on system model is more important What if no measure is available? (also: read literature for information filters) 12

13 KF Iterative Initial(State, Covariance) Cycle: Propagate (Stat,Covar,SysModel,Inputs) Cycle Meas(i): If Valid(Meas(i)) then Update(Stat,Covar,MeasModel(i),Meas(i)) This is an abuse => no guarantees! 13

14 Extended Kalman Filter A= f x =[ d X t = f X t,u t dt k,t, t ]t k 1,t k ] f 1 f 1 f 1 x 1 x 2 x n f 2 f 2 ] f 2 x 1 x 2 x n H f n f n f n x 1 x 2 x n x k x k A x k 1 x k W w k 1 y k y k h x k x k V v k =[ h = x h1 h 1 h 1 x 1 h 2 x n h 2 h 2 h 2 x 1 h 2 x n W h q x 1 n] h q h q h 2 x A k = f x x=x t k u=u t k t=t k =[ f 1 f 1 f 1 w 1 w 2 w n f f 2 f 2 = f 2 w w 1 w 2 w n A k =exp A k t k t k 1 = f n f n f n w 1 w 2 w n]v h v =[ h1 h 1 h 1 v 1 v 2 v n h 2 h 2 ] h 2 v 1 v 2 v n h q h q h q v 1 v 2 v n 14

15 EKF - Algorithm Predict Stage Update Stage Propagate State: x k = x k 1 Bu k 1 Propagate Co-Variance: P k = P k 1 T Q k Kalman Gain Calculation: K k =P k H T HP k H T R k 1 State Update with new Measure: x k =x k K k y k H x k Initial Estimates: x 0, P 0 Update Covariance with new measurement: P k = I K k H P k 15

16 EKF Trouble EKF offer no theoretical guarantees Not Optimal May not converge Expect troubles with periodical functions such as trigonometrical Should: Design and implement VERY carefully Test thouroughly 16

17 KF Main Ideas KF Weighed merging of prediction and updates Optimal Iterative No guarantees but works EKF Non-linear No guarantees but works 17

18 Self Localization Fundamentals: Where am I? Robotic Autonomy Redundancy of sensors Needs multisensory data fusion (example EKF) Data internal to robot to find world pose SemiStructed environment Must match seen markers and world positions 18

19 Self Localization - example Two angles (internal to the robot) do not localize it (x,y,θ) (shown are 4 positions of an infinite number of solutions) Two measurements of (angle, distance) do not localize robot (still two solutions eligible) 19

20 Self Localiz Using EKF Let us consider using Self Localization of a wheel chair robot by using wall (line) ranging Let us consider a pair of distance meters that allow us to have distance and angle information In order to use KF/EKF we need: System Model Measurement Model 20

21 System Model - wheelchair X t =[ x r t y r t r t v r t r t ] T {v t = v 1 v 2 2 r = v 1 v 2 b 21

22 System Model - redux d dt [x t y t t cos t v t sin t t ]=[v t ] Low dynamics allow system order reduction v and w are now inputs to the system 22

23 System Model - discrete d dt [x t y t t cos t v t sin t t ]=[v t ] x k x k A x k 1 x k W w k 1 y k y k h x k x k V v k A k =[0 0 v t k sin t k 0 0 v t k cos t k ] A=exp A * k t k t k 1 P w ~ N 0,Q P v ~N 0, R 23

24 Self Localization Walls / Lines { L = ' L R d L =d ' L a y= [ ' L d ' L] { ' L = L R d ' L =d L x R sin L y R cos L H line = d ' L, ' L x r, y r, r = [ ] sin L cos L 0 {b=a c b=x R sin L c= y R cos L { L = ' L R d L =d ' L x R sin L y R cos L 2 R line d ' 0 =[ L 0 ' L 2 ] 24

25 Results 0 < = t < 8 8 < = t < 16 y w (m ) x w (m ) y w (m ) x w (m ) 25

26 Final Comments Data Fusion Many sensors to better perception One method possible is (E)KF (E)KF KF: Guaranteed optimal and convergent Iterative => no assurances EKF: No assurances Self Localization is hard problem even in semi structured environment Lines / walls (angle and distance) example Must match seen marker 26

27 Literature Data Fusion Handbook of Robotics, Siciliano, Bruno; Khatib, Oussama (Eds.), Springer 2008 Multi-Sensor Data Fusion: An Introduction, H.B. Mitchell, Springer 2007 (E)KF Applied Optimal Estimation, Arthur Gelb, 1974 MIT Press A new approach to linear filtering and prediction problems, R. E. Kalman 1960, Journal of Basic Engineering 82 (1): Self Localization 27

28 Any Questions? 28

Kalman Filter. Predict: Update: x k k 1 = F k x k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q

Kalman Filter. Predict: Update: x k k 1 = F k x k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q Kalman Filter Kalman Filter Predict: x k k 1 = F k x k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q Update: K = P k k 1 Hk T (H k P k k 1 Hk T + R) 1 x k k = x k k 1 + K(z k H k x k k 1 ) P k k =(I