Probabilistic Fundamentals in Robotics. DAUIN Politecnico di Torino July 2010

Transcription

1 Probabilistic Fundamentals in Robotics Probabilistic Models of Mobile Robots Robot localization Basilio Bona DAUIN Politecnico di Torino July 2010

2 Course Outline Basic mathematical framework Probabilistic bili i models of mobile robots Mobile robot localization problem Robotic mapping Probabilistic planning and control Reference textbook Thrun, Burgard, Fox, Probabilistic Robotics, MIT Press, robotics.org/ Basilio Bona July

3 Mobile robot localization problem Mobile robot localization: Markov and Gaussian Introduction Markov localization EKF localizationl Multi hypothesis tracking UKF localization Mobile robot localization: Grid and Monte Carlo Grid localization Monte Carlo localization Localization in dynamic environments Basilio Bona July

4 Introduction Localization or position estimation is the problem of determining the pose of a rover relative to a given map of the environment Localization is the process of establishing the correspondence between the map reference frame and the local rover reference frame Hence coordinate transformation is necessary: from robot centered local lcoordinate system to global lone The pose x t =(x y θ) of the robot is not known and cannot be measured directly with a sensor The pose must be inferred from environment data (and odometry) A single measurement is not sufficient to recover the pose Localization techniques depend on the map type The algorithms presented tdare variants of the basic Bayes filter Basilio Bona July

5 Example: localization with known landmarks Basilio Bona July

6 Localization as a state system A graphical state description of the localization problem The map m, the measurements z t and the controls u t are known Basilio Bona July

7 Maps A 2D metric map A topological map (graph like) An occupancy grid map A mosaic map of a ceiling Basilio Bona July

8 A taxonomy of localization problems Position tracking The initial robot pose is known; the initial error is assumed small; the pose uncertainty is often assumed to be unimodal; the problem is essentially a local one Global localization The initial robot pose in unknown; the initial error cannot be assumed to be small; global localization includes position tracking Kidnapped robot problem During operation the robot get kidnapped and suddenly reappears in another location; the robot believes to be able to estimate its position, but this is not true Basilio Bona July

9 Static vs dynamic environments Static environments Environments where the only moving object is the robot. All other objects (features, landmarks, etc.) are fixed wrt the map Dynamic environments Otherobjectsmove move, i.e., changetheirlocationover over time. Changes usually are not episodic, i.e., they persist over time and are recognized by sensor readings. Changes that affect only a single or few measurements are treated t as noise. Examples are: people moving around the robot, doors that open and close, movable furniture, daylight illumination (for robot with vision sensors) Two approaches: moving object are included into the state Sensor data are filtered to eliminate the effect of unmodeled dynamics Basilio Bona July

10 Passive vs active localization Passive localization The localization module only observes the robot operating; the localization results do not control the robot motion. Motion is random or determined by other task accomplishment Ati Active localizationli The localization module controls the robot motion to maximize a performancecriterionon criterion on localization error Basilio Bona July

11 Single robot vs multi robot Single robot The localization is performed by a single robot that moves (actively or passively) in the environment. No communication issues are present (except that with the supervisor) Multi robot The localization is jointly performed by a team or robots. In principle each one can perform single robot localization, but if they can detect each other, this information can be shared and each robot belief can influence other robots belief. Detection and communication issues are important, as well as relative pose measurement. Basilio Bona July

12 Markov localization Probabilistic localization algorithms are variants of the Bayes filter The simplest localization filter (pure Bayesian) is called Markov localization Prediction bel ( x ) = p ( x x, u ) bel ( x )d x t t t 1 t t 1 t 1 Correction bel( x ) = η p( z x ) bel( x ) t t t t Basilio Bona July

13 Initial belief Position tracking bel( x ) det(2 π ) T = Σ exp ( x x ) Σ ( x x ) N ( xx ;, Σ ) 0 Normal distribution Nxμ (;, Σ) bel( x ) Global localization 0 = 1 X Volume of the poses space Basilio Bona July

14 KF summary x = Ax + Bu + ε t t t 1 t t t ( ) px ( x, u ) = N x ; Ax + Bu, R t t 1 t t t t 1 t t t belx ( ) = px ( x, u) belx ( ) d x t t t 1 t t 1 t 1 ( ; +, ) ( ;, Σ ) ~ N x Ax Bu R ~ N x μ t t t 1 t t t t 1 t 1 t 1 Basilio Bona July

15 KF summary bel( x ) = p( x x, u ) bel( x ) dx t t t 1 t t 1 t 1 ( ; + u, R ) ( ; μ Σ ) ~ N x Ax B ~ N x, t t t 1 t t t t 1 t 1 t 1 1 T 1 bel ( x ) = η exp ( ) ( ) t x Ax Bu R x Ax Bu t t t 1 t t t t t t 1 t t exp T ( x μ ) Σ ( x μ ) dx t t t t t t 1 2 μ = A μ t t t + Bu 1 t t bel( x ) = t T Σ t = AΣ A + R t t 1 t t Basilio Bona July

16 KF summary z = C x + δ t t t t ( ) pz ( x) = N z; Cx, Q t t t t t t bl bel ( x ) = η p ( z x ) bl bel ( x ) t t t t (, ) N ( x ; μ, ) t t t t t t t Σ ~ N z ; C x Q ~ Basilio Bona July

17 KF summary bel( x ) =η p( z x ) bel( x ) t t t t ( ;, Q ) N ( x μ Σ ) ~ N z C x ~ ;, t t t t t t t 1 bel x z C x Q z C x exp T ( x μ ) Σ ( x μ ) t t t t t 2 T 1 ( ) = η exp ( ) ( ) t t t t t t t t μ = μ + K ( z C μ ) t t t t t t bel( x ) = with K =ΣC Σ = ( I KC ) t t t Σ t T t t t t T ( C Σ C + Q ) t t t t 1 Basilio Bona July

18 EKF localization We assume that the map is represented by a collection of features (landmark based localization) We assume the velocity model and the measurement model presented before All features are uniquely identifiable (correspondence variables are known) = known landmarks Range and heading measurements are available pz ( x, mc, ) t t t Door 1 Door 2 Door 3 One dimensional environment with three landmarks (doors) Correspondence is known Basilio Bona July

19 Initial belief (uniform) Measurement model Correction/update step Motion model (prediction) Measurement model Correction/update step Motion model (prediction) Basilio Bona July

20 EKF_known_correspondence_localization Prediction x x x μ μ μ t 1, x t 1, y t 1, θ gu (, μ ) t t 1 y y y 1: G = = t x μ μ μ t 1 t 1, x t 1, y t 1, θ θ θ θ μ μ μ t 1, x t 1, y t 1, θ x x v ω t t gu (, μ ) t t 1 y 2: V y = = t u t v t ω t θ θ v ω t t 2 ( α v + α ω 1 2 ) 0 t t 3: M = t 2 0 ( α v α ω + 3 t 4 t ) 4: μ = gu (, μ ) 5: Σ t t t 1 = GΣ G + VMV (,, u, z, c, m) μt Σ 1 t 1 t t t Jacobian of g wrt location Jacobian of g wrt control Motion noise Predicted d mean T T Predicted covariance t t t 1 t t t t Basilio Bona July

21 EKF_known_correspondence_localization (,, u, z, c, m) μt Σ 1 t 1 t t t Correction ( m μ ) ( m μ ) x t, x y t, y ( m μ, m μ y t, y x t, x) μ atan2 t, θ Predicted measurement mean 2: r r r t t t h( μ, m) t μ μ μ tx, ty, t, θ H = = t x φ φ φ t t t t μ μ μ tx, ty, t, θ Jacobian of h wrt location 3: 2 σ 0 r Q = t 2 0 σ r 4: S = H t t Σ t H t + Q Predicted measurement covariance t T 1 5: K = Σ H S Kl Kalman gain : zˆ = t t t t t t t 6: μ = μ + K ( z zˆ ) Updated mean t t t t t 7 : Σ = ( I K H ) Σ Updated covariance t t t t Basilio Bona July

22 EKF prediction step different motion noise parameters Small rotation and translation noise High rotation noise High translation noise High rotation and translation noise Basilio Bona July

23 EKF measurement prediction step innovation z t zˆ t innovation Basilio Bona July

24 EKF correction step measurement uncertainty Prediction uncertainty uncertainty in robot location Basilio Bona July

25 Estimation Sequence (1) Accurate detection sensor trajectories estimated from the motion control ground truth trajectories corrected trajectories uncertainty before after Basilio Bona July

26 Estimation Sequence (2) Less accurate detection sensor Basilio Bona July

27 Estimation with unknown correspondences When landmark correspondence is not certain (as it is often the case) one should adopt a strategy to determine the identity of the landmarks during localization The most simple and popular technique is the maximum likelihood correspondence One determines the most likely value of the correspondence variable ibl and use it as the true one This strategy is fragile when different landmarks have equally likely values How to reduce the risk Choose landmarks that are far apart, so not to be confused Make sure that the robot pose uncertainty remains small Basilio Bona July

28 ML data association The maximum likelihood estimator (MLE) determines the correspondence c t that maximizes the likelihood cˆ = arg max p( z c, z, u, m) t t 1: t 1: t 1 1: t c t This expression is conditioned on all prior correspondences that are treated as always correct c1: t 1 When the number of landmarks is high, the number of possible correspondence grows too much and the problem becomes intractable A solution is to maximize each term singularly and then proceed as in i i c ˆ = arg max p ( z c, z, u, m ) = t t 1: t 1: t 1 1: t i c t i i arg max Nz ( ; h ( μ, c, m ), H Σ H T + Q ) c i t t t t t t t t Basilio Bona July

29 Multi hypothesis tracking filter Each color represents a different hypothesis Basilio Bona July

30 Localization with MHTF Current belief is represented by multiple hypotheses Each hypothesis is tracked by a Kalman filter Additional problems: Data association: Which observation corresponds to which hypothesis? Hypothesis management: When to add/delete hypotheses? Large body of literature on target tracking, motion correspondence etc. Basilio Bona July

31 MHT example The blue robot sees the door, but cannot resolve its position since also the other hypothesis (in red) are likely Basilio Bona July

32 MHT (1) Hypotheses are extracted from laser range finder (LRF) scans Each hypothesis is given by pose estimate, covariance matrix and probability of being the correct one: H = { xˆ, Σ, PH ( )} i i i i Hypothesis probability is computed using Bayes rule sensor report Pr ( H) PH ( ) t i i PH ( r) = i t Pr () Hypotheses with low probability are deleted New candidates are extracted from LRF scans t C = {, z R} j j j Basilio Bona July

33 MHT (2) Basilio Bona July

34 MHT (3) Basilio Bona July

35 MHT: Implemented System (3) Example run # hypotheses P(H best ) Map and trajectory t #hypotheses vs. time Basilio Bona July

36 UKF localization The UKF localization algorithm uses the unscented transform to linearize the motion and the measurement models Instead of computing Jacobians of these models it computes Gaussians using sigma points and transforms them through the model The landmark association (correspondence) is certain Basilio Bona July

37 UKF Algorithm part a) Sigma points Basilio Bona July

38 UKF Algorithm part b) Cross covariance Basilio Bona July

39 M Q t t a Σt UKF_localization (,, u, z, m) μt Σ 1 t 1 t t 2 ( α v + α ω 1 2 ) 0 t t = 2 0 ( α v α ω Motion noise + 3 t 4 t ) 2 σ 0 r = 2 Measurement noise 0 σ r T T a T μ = μ ( 00) ( 00) t 1 t 1 Σ 0 0 t 1 = 0 M 0 1 t Augmented covariance 0 0 Qt Prediction Augmented state tt mean ( ) χ = μ μ + γ Σ μ γ Σ a a a a a a t 1 t 1 t 1 t 1 t 1 t 1 Sigma points χ x t = g( χ) Prediction of sigma points 2L i x μ = t w χ m i, t Predicted mean i= 0 2L L T i x x Σ = t w ( χ μ, )( χ μ c i t t i, t t) Predicted covariance i= 0 Basilio Bona July

40 UKF_localization (,, u, z, m) μt Σ 1 t 1 t t Correction 1 x ( ) Ζ = h χ + χ Measurement sigma points z t t t 2L i t m i, t i=0= 0 zˆ = w Ζ Predicted measurement mean ( Ζ ˆ) ( Ζ ˆ) 2L i = t c i, t t i, t t i=0 S w z z ( χ μ ) ( zˆ,, ) 2L xz, i x = w t c i t t i t t i= 0 Σ χ μ Ζ T T Pred. measurement covariance Cross covariance K = Σ S xz, 1 t t t Kalman gain μ Σ = μ + K ( z zˆ ) t t t t t = Σ KSK T t t t t t Updated mean Updated covariance Basilio Bona July

41 UKF_localization (,, u, z, m) μt Σ 1 t 1 t t Correction 2 zˆ t H Q t t 2 2 ( m μ, ) + x t x ( m μ y t, y) = Predicted measurement mean atan 2 ( m μ, m μ ) μ y t, y x t, x t, θ r r r t t t h ( μ, m) t μ μ μ tx, ty, t, θ = = x φ φ φ Jacobian of h w.r.t. location t t t t μ μ μ t, x t, y t, θ 2 σ 0 r = 2 0 σ r S = H Σ H + Q K μ Σ T t t t t t = Σ H S Kalman gain T 1 t t t t = μ + K ( z zˆ ) t t t t t ( I K H ) Σt t t t Pred. measurement covariance Updated mean = Updated covariance Basilio Bona July

42 UKF Prediction Step 3) High noise in rotation small in translation 1) Small noise in translation and rotation Initial estimate 2) High noise in translation small in rotation 4) High noise in translation and in rotation Basilio Bona July

43 UKF Observation Prediction Step The left plots show the sigma points predicted from two motion updates along with the resulting uncertainty ellipses. The true robot and the observation are indicated by the white circle and the bold line, respectively The right plots show the resulting measurement prediction sigma points. The white arrows indicate the innovations, the differences between observed and predicted measurements Basilio Bona July

44 UKF Correction Step The panels on the left show the measurement prediction The panels on the right the resulting corrections, which update the mean estimate and reduce the position uncertainty ellipses Basilio Bona July

45 EKF Correction Step Basilio Bona July

46 Estimation Sequence Robot trajectory according to the motion control (dashed lines) and the resulting true trajectory (solid lines) Landmark detections are indicated by thin lines EKF PF UKF Basilio Bona July

47 Prediction Quality EKF prediction UKF prediction Approximation error due to linearization The robot moves on a circle The reference covariances are extracted from an accurate, sample based prediction Basilio Bona July

48 Mobile robot localization Grid and Monte Carlo methods These algorithms can process raw sensor measurement, i.e., there is no need to extract features from measurements These methods are non parametric, e.g., they are not limited to unimodal distributions as with the EKF localization method They can solve global localization problem and kidnapped robot problems (in some cases); the EKF algorithm is not able to solve such problems The first method dis called grid localization li i The second method is called Monte Carlo localization Basilio Bona July

49 Grid localization Grid localization uses a histogram filter to represent posterior belief The grid dimension is a key factor for the performances of the method When the cell grid dimension is small, the algorithm can be extremely slow When the cell grid dimension is large, there can be an information loss that makes the algorithm not working in some cases Basilio Bona July

50 Example Measurement model Correction/update step Motion model (prediction) Measurement model Correction/update step Motion model (prediction) Basilio Bona July

51 Topological grid map A topological map is a graph annotationofof theenvironmentenvironment Topological maps assign nodes to particular places and edges as paths if direct passage bt between pairs of places (end nodes) exist Humans manage spatial knowledge primarily by topological information This information is used to construct a hierarchical topological map that describes the environment Basilio Bona July

52 Topological grid map Topological grid map representation: Coarse gridding Cells of varying size Resolution influenced by the structure of the environment (significant places and landmarks as doors, corridors windows, T junctions, deadends ends, act as grid elements) Grid elements Basilio Bona July

53 Metric grid map Metric grid representation: Finer gridding g Cells of uniform size Resolutionnot not influenced by the structure of the environment Usually cell sizes of 15 cm x 15 cm up to 1 m x 1m Motionmodel affected by robot velocity and cell size θ Basilio Bona July

54 Cell size influence Cell size influence the performance of the grid localization algorithm Average localization error as a function of grid cell size, for ultrasound sensors and laser range finders Average CPU time needed for global localization as a function of grid resolution, shown for both ultrasound sensors and laser range finders Basilio Bona July

55 Example with metric grids 1 Basilio Bona July

56 Example with metric grids 2 Basilio Bona July

57 Example with metric grids 3 Basilio Bona July

58 Example with sonar data 1 Basilio Bona July

59 Example with sonar data 2 Basilio Bona July

60 Example with sonar data 3 Basilio Bona July

61 Example with sonar data 4 Basilio Bona July

62 Example with sonar data 5 Basilio Bona July

63 Monte Carlo localization Monte Carlo localization (MCL) is a relatively new, yet very popular algorithm It is a versatile method, where the belief is represented by a set of particles (i.e., a particle filter) It can be used both for local and for global localization problems In the context of localization, the particles are propagated according to the motion model They are then weighted according to the likelihood of the observations In a re sampling step, new particles are drawn with a probability proportional to the likelihood of the observation The method first appeared in 70 s, and was re discovered by Kitagawa and Isard & Blake in computer vision Basilio Bona July

64 Particle filter localization (MCL) Particle filters based localization (Monte Carlo Localization MCL) uses a set of weighted random samples to approximate the robot pose belief Particle set size n ( ) [] i [] i ω δ( ˆ ) bel p p p Pose of the particle t t t t i= 1 Weight of the particle n [] i with ω = 1 t i= 1 Basilio Bona July

65 Particle filter localization (MCL) Particle based Representation of position belief Particle based approximation Gaussian approximation (ellipse: 95%acceptance region) Basilio Bona July

66 Particle filter localization (MCL) Using particle filters approximation, the Bayes Filter can be reformulated as follows (starting from the robot initial belief at time zero) 1. PREDICTION: Generate a new set of particles given the motion model and the applied controls 2. UPDATE: Assign to each particle an importance weight according to the sensor measurements 3. RESAMPLING: Randomly resample particles in function of their importance weight Basilio Bona July

67 Prediction Prediction Generate a new set of particles given the motion model and the applied controls For each particle: Given the particle pose at time step t-1 and the commands, the particle pose at time t is predicted using the motion model ˆ ( i ˆ ) [] i p = f p [], u, ω t t 1 t t The variable is randomly extracted according to the noise distribution We obtain a set of predicted particles Basilio Bona July

68 Update Update Assign to each particle an importance weight according to the sensor measurements For each particle: Compare particle s prediction of measurements with actual [ i ] measurements z vs h( pˆ ) t t Particles whose predictions match the measurements are given a high weight In red the particles il with high weights Basilio Bona July

69 Resampling Resampling Randomly resample particles in function of their importance weight For eachparticle: For n times draw (with replacement) a particle from Γ with t probability given by the importance weights and put it in the set Γ t Particles whose predictions match the measurements are given a high weight The new set Γ provides the particle based approximation of t the robot pose at time t Basilio Bona July

70 Example Measurement model Correction/update step Motion model (prediction) Measurement model Correction/update step Motion model (prediction) Basilio Bona July

71 Example Basilio Bona July

72 MC_localization (, u, z, m) χt 1 t t 1: χ χ t t = = 2: for m = 1to M do 3: [ m] [ m] x = sample_motion_model ( u, x ) t t t 1 4 : [ m w ] = (, [ m measurement_model z x ], m) 5: χ = χ + x, w 6: endfor t t t [ m] [ m] t t t t 7: for m = 1to M do 8: draw i with probability [] i 9: add x to χ t t 10: endfor 11 : return χt w [] i t see previous slides Basilio Bona July

73 Monte Carlo Localization within a sensor infrastructure Fixed sensors deployed in known positions in the environment STEP 1: Acquire odometry Basilio Bona July

74 STEP 1: Acquire odometry Filter Prediction Basilio Bona July

75 STEP 1: Acquire odometry Filter Prediction Acquire meas. Basilio Bona July

76 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Basilio Bona July

77 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling Basilio Bona July

78 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 1: Acquire odometry Basilio Bona July

79 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 1: Acquire odometry Filter Prediction Basilio Bona July

80 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Basilio Bona July

81 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Basilio Bona July

82 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling Basilio Bona July

83 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 3: Acquire odometry Basilio Bona July

84 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 3: Acquire odometry Filter Prediction Basilio Bona July

85 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 3: Acquire odometry Filter Prediction Acquire meas. Basilio Bona July

86 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 3: Acquire odometry Filter Prediction Acquire meas. Weights Update Basilio Bona July

87 STEP 1: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 2: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling STEP 3: Acquire odometry Filter Prediction Acquire meas. Weights Update Resampling Basilio Bona July

88 Example for landmark based localization Basilio Bona July

89 Properties of MCL MCL can approximate any distribution, as it can represent complex multi modal distributions and blend them with Gaussian style distributions Increasing the total number of particles increases the accuracy of the approximation The number of particles M is a trade off parameter between accuracy and necessary computational resources Particles number shall remain large enough to avoid filter divergence Theparticles number may remain fixed or change adaptively Basilio Bona July

90 Adapting the particle size In the example below, the number of particles is very high ( ) to allow an accurate representation of the belief during early stages of the algorithm, but is unnecessarily high h in later stages, when the belief blif concentrates in smaller regions an adaptive strategy is required Basilio Bona July

91 KLD sampling Kullback Leibler divergence (KLD) sampling is a variant of MCL that adapts the number of particles over time KLD (also known as information divergence, information gain,, relative entropy) is a measure of the difference between two probability distributions For probability distributions P and Q of a discrete random variable their KLD is defined as For distributions P and Q of a continuous random variable, the KLD is defined as an integral Basilio Bona July

92 KLD sampling The idea is to set adaptively the number of particles based on a statistical bound on the sample based approximation quality At each iteration of the PF, KLD sampling determines the number of samples such that, with probability 1 δ, the error between the true posterior and the sample based approximation is less than ε To derive this bound, we assume that the true posterior is given by a discrete, piecewise constant distribution such as a discrete density tree or a multi dimensional i l histogram D. Fox, Adapting the sample size in particle filters through KLDsampling, International Journal of RoboticsResearch Research, 2003 Basilio Bona July

93 KLD sampling Given a discrete distribution with k bins, and given the number n of samples to be chosen, we determine n as follows 1 2 n = χk 1,1 δ 2ε 2 where χ is the chi square distribution with k 1 d.o.f. and k 1,1 δ probability 1 δ we can guarantee that with probability 1 δ the KLdistance between the MLE and the true distribution is less than ε Basilio Bona July

94 Approximation formula ( 1) 9( 1) k n z k k δ ε + where is the upper quantile of the nomal distribution 1 1 (0,1) z N δ δ Basilio Bona July

95 Dynamic environment Markov assumptions are good for a static environment Often the environment where robots operate is full of people moving here and there People dynamics is not modeled by the state x t Probabilistic bili i approaches are robust since they incorporate sensor noise, but... Sensor noise must be independent d at each time step... while people dynamics is highly dependent in time Basilio Bona July

96 Dynamic environment: which solution State Augmentation include the hidden state into the state estimated by the filter it is more general, but suffer from high computational complexity: the pose of each subject moving around the robot must be estimated, and the number of states varies in time Outlier rejection pre process sensor measurements to eliminate measurements affected by hidden state it may work well when the people presence affects the sensors (laser range finder, and, to lesser extent, vision sensors) reading Basilio Bona July

97 Outlier rejection The Expectation maximization (EM) learning algorithm is used for outlier detection and rejection It comes from the beam model of range finders, where z short and p short parameters relates to unexpected objects we have to introduce and compute a correspondence variable that can take one of four values {hit, short, max, rand} the desired probability is computed as this integral does not have a closed form solution k c t k p ( z x, m ) z bel ( x )d x k k short t t short t t = short = t t 1: t 1 1: t k p ( z x, m) z bel( x )dx c t t c t t c pc ( z, z, u, m) approximation with a representative sample of the posterior the measurement is rejected if the probability exceeds a given threshold Basilio Bona July

98 Comparison of different ML implementations EKF MHT Topological grid Metric grid MCL Measurements landmarks landmarks landmarks Measurement noise Posterior raw measurements raw measurements Gaussian Gaussian any any any Gaussian mixture of Gaussians histogram histogram particles Efficiency i (mem) Efficiency (time) Ease of implementation Resolution Robustness Global llocalizationli i no no yes yes yes Basilio Bona July

99 Example Basilio Bona July

100 Example Basilio Bona July

101 Example Basilio Bona July

102 Thank you. Any question? Basilio Bona July