Modeling and state estimation Examples State estimation Probabilities Bayes filter Particle filter. Modeling. CSC752 Autonomous Robotic Systems

Transcription

1 Modeling CSC752 Autonomous Robotic Systems Ubbo Visser Department of Computer Science University of Miami February 21, 2017

2 Outline 1 Modeling and state estimation 2 Examples 3 State estimation 4 Probabilities 5 Bayes filter 6 Particle filter

3 Modeling The model represents the current state of the environment.

4 Modeling The model represents the current state of the environment. All sensors of a physical robot are noisy. The model can never be exact.

5 Modeling The model represents the current state of the environment. All sensors of a physical robot are noisy. The model can never be exact. Robots can only estimate states using probabilistic methods for example.

6 State estimation Determines a state X t that changes over time using a sequence of measurements z t and u t. z t: measurement u t: state transition measurement

7 State estimation Determines a state X t that changes over time using a sequence of measurements z t and u t. z t: measurement u t: state transition measurement Useful if a state can not be accurately and directly measured (which means every state for a physical robot).

8 State estimation Determines a state X t that changes over time using a sequence of measurements z t and u t. z t: measurement u t: state transition measurement Useful if a state can not be accurately and directly measured (which means every state for a physical robot). filter noise

9 State estimation Determines a state X t that changes over time using a sequence of measurements z t and u t. z t: measurement u t: state transition measurement Useful if a state can not be accurately and directly measured (which means every state for a physical robot). filter noise infer a state from measurements

10 State estimation Determines a state X t that changes over time using a sequence of measurements z t and u t. z t: measurement u t: state transition measurement Useful if a state can not be accurately and directly measured (which means every state for a physical robot). filter noise infer a state from measurements Modeling in our soccer agent Ball tracking, opponent localization (and teammates), self-localization, orientation estimation (upright vector).

11 Examples

12 Examples How noisy can measurements be?

13 Examples How noisy can measurements be? How can a state estimation be robust despite all the errors?

14 Example 1 RoboCup Small-Size League:

15 Example 1 RoboCup Small-Size League: x,y positions as measurement z t.

16 Example 1 RoboCup Small-Size League: x,y positions as measurement z t. Only noisy measurements, we need the actual state (the model).

17 Example 1 RoboCup Small-Size League: x,y positions as measurement z t. Only noisy measurements, we need the actual state (the model).

18 Example 1 RoboCup Small-Size League: x,y positions as measurement z t. Only noisy measurements, we need the actual state (the model). Problem with two robots: wrong perceptions on other robot.

19 Example 2 Obstacle avoidance using a laser range finder: There can be several different errors in the measurements. source: slides from

20 Example 2 Obstacle avoidance using a laser range finder: There can be several different errors in the measurements. source: slides from

21 Example 2 Obstacle avoidance using a laser range finder: There can be several different errors in the measurements. source: slides from

22 Example 2 Obstacle avoidance using a laser range finder: There can be several different errors in the measurements. The general model for a beam based sensor is a mixture of several distributions. source: slides from

23 Example 2 Obstacle avoidance using a laser range finder: There can be several different errors in the measurements. The general model for a beam based sensor is a mixture of several distributions. Knowledge about the behavior of a sensor (the sensor model) is very important for a robust state estimation. source: slides from

24 Example 3 3D ball-tracking with a camera:

25 Example 3 3D ball-tracking with a camera: Uncertainty, especially the distance of the ball to the camera.

26 Example 3 3D ball-tracking with a camera: Uncertainty, especially the distance of the ball to the camera. State in world coordinates and should include the velocity. A single observation does not contain much information.

27 Example 3 3D ball-tracking with a camera: Uncertainty, especially the distance of the ball to the camera. State in world coordinates and should include the velocity. A single observation does not contain much information. Consider only possible trajectories to reduce uncertainty.

28 Example 3 3D ball-tracking with a camera: Uncertainty, especially the distance of the ball to the camera. State in world coordinates and should include the velocity. A single observation does not contain much information. Consider only possible trajectories to reduce uncertainty. Knowledge about the behavior of the ball and physics is useful (state transition model).

29 Example 4 Self-localization in 1D with limited sensors:

30 Example 4 Self-localization in 1D with limited sensors:

31 Example 4 Self-localization in 1D with limited sensors: Door sensor ambiguous. Even a sequence of measurements z t is not enough to localize.

32 Example 4 Self-localization in 1D with limited sensors: Door sensor ambiguous. Even a sequence of measurements z t is not enough to localize. Another sensor needed: sensor to measure wheel rotations.

33 Example 4 Self-localization in 1D with limited sensors: Door sensor ambiguous. Even a sequence of measurements z t is not enough to localize. Another sensor needed: sensor to measure wheel rotations. Measurements u t needed (odometry motion model).

34 General state estimation

35 General state estimation For one given observation there is a high uncertainty and ambiguity.

36 General state estimation For one given observation there is a high uncertainty and ambiguity. The state estimation gets a sequence of measurements, so the estimation of X t is based on all measurements z 0,..., z t and u 0,..., u t.

37 General state estimation General state estimation:

38 General state estimation General state estimation:

39 General state estimation General state estimation: Problems: more measurements with every time time step increasing amount of computation.

40 Markov assumptions Markov assumption 1: The measurement z t depends only on the state X t and a random error. Markov assumption 2: The state transition measurement u t only depends on the states X t and X t+1 and a random error.

41 Markov process Bayesian network with the measurements u t and z t : x t 1 x t x t+1 ut 1 ut u t+1 z t 1 z t z t+1 The states x t are hidden.

42 Recursive state estimation / filter Recursive state estimation:

43 Recursive state estimation / filter Recursive state estimation: X t includes all the knowledge from the measurements before.

44 Recursive state estimation / filter Recursive state estimation: X t includes all the knowledge from the measurements before. Needed for X t is only X t 1, z t and u t.

45 Recursive state estimation / filter Recursive state estimation: X t includes all the knowledge from the measurements before. Needed for X t is only X t 1, z t and u t. Belief X t is updated using only the new measurements constant time for each step.

46 State estimation Sensor model and state transition model needed. Update belief X t using z t and sensor model. u t and motion model and knowledge about dynamics in the environment.

47 Example state estimation

48 Example state estimation

49 Example state estimation

50 Example state estimation

51 Example state estimation

52 Example state estimation

53 Example state estimation

54 Example state estimation

55 Example state estimation

56 Example 1: Small-Size League State, z t, u t, the sensor model and prediction?

57 Example 1: Small-Size League State, z t, u t, the sensor model and prediction? State: position x, y, θ and speed x, y, θ

58 Example 1: Small-Size League State, z t, u t, the sensor model and prediction? State: position x, y, θ and speed x, y, θ z t : x,y,θ

59 Example 1: Small-Size League State, z t, u t, the sensor model and prediction? State: position x, y, θ and speed x, y, θ z t : x,y,θ u t : Driving command sent to the robot.

60 Example 1: Small-Size League State, z t, u t, the sensor model and prediction? State: position x, y, θ and speed x, y, θ z t : x,y,θ u t : Driving command sent to the robot. Sensor model: Gaussian distribution around robot Maybe also small probabilities at other robots

61 Example 1: Small-Size League State, z t, u t, the sensor model and prediction? State: position x, y, θ and speed x, y, θ z t : x,y,θ u t : Driving command sent to the robot. Sensor model: Gaussian distribution around robot Maybe also small probabilities at other robots Prediction using X t 1, u t, odometry motion model

62 Example 3: Ball tracking State, z t, u t, the sensor model and prediction?

63 Example 3: Ball tracking State, z t, u t, the sensor model and prediction? state: position x, y, z and velocity x, y, z

64 Example 3: Ball tracking State, z t, u t, the sensor model and prediction? state: position x, y, z and velocity x, y, z z t : image x, y

65 Example 3: Ball tracking State, z t, u t, the sensor model and prediction? state: position x, y, z and velocity x, y, z z t : image x, y u t : none

66 Example 3: Ball tracking State, z t, u t, the sensor model and prediction? state: position x, y, z and velocity x, y, z z t : image x, y u t : none Sensor model: transformation from state to image, Gaussian distribution in image

67 Example 3: Ball tracking State, z t, u t, the sensor model and prediction? state: position x, y, z and velocity x, y, z z t : image x, y u t : none Sensor model: transformation from state to image, Gaussian distribution in image Prediction: state transition model using physics

68 Bayes filter Previous slides have shown the principle of a Bayes filter. Why does this work exactly? Probabilities Bayes rule Recursive Bayesian estimation Source for the following slides: Thrun et al., Probabilistic Robotics;

69 Discrete random variables X denotes a random variable. X can take on a countable number of values in {x 1, x 2,..., x n }. P(X = x i ) is the probability that X takes on value x i.

70 Continuous random variables X takes on values in the continuum. p(x = x) (or short p(x)) is a probability density function. b Example: Pr(x [a, b]) = p(x)dx a

71 Joint and Conditional Probabilities P(X = x and Y = y) = P(x, y). If X and Y are independent then P(x, y) = P(x)P(y). P(x y) is the probability of x given y. If X and Y are independent then P(x y) = P(x).

72 Law of total probability Discrete case: P(x) = 1 x P(x) = P(x, y) y P(x) = P(x y)p(y) y

73 Law of total probability Discrete case: P(x) = 1 x P(x) = P(x, y) y Continuous case: p(x)dx = 1 p(x) = p(x, y)dy P(x) = y P(x y)p(y) p(x) = p(x y)p(y)dy

74 Bayes rule p(x y)p(y) = p(x, y) = p(y x)p(x)

75 Bayes rule p(x y)p(y) = p(x, y) = p(y x)p(x) p(x y) = p(y x)p(x) p(y)

76 Bayes rule p(x y)p(y) = p(x, y) = p(y x)p(x) p(x y) = p(y x)p(x) p(y) const y p(y x)p(x)

77 Bayes rule p(x y)p(y) = p(x, y) = p(y x)p(x) p(x y) = p(y x)p(x) p(y) const y p(y x)p(x) Bayes rule with background knowledge: p(x y, z) = p(y x, z)p(x z) p(y z)

78 Example for a simple measurement The robot obtains the measurement z. What is P(open z)?

79 Diagnostic vs. causal reasoning P(open z) is diagnostic. P(z open) is causal. Often the causal knowledge is much easier to obtain (the sensor models).

80 Diagnostic vs. causal reasoning P(open z) is diagnostic. P(z open) is causal. Often the causal knowledge is much easier to obtain (the sensor models). The bayes rule allows us to use causal knowledge to get P(open z): P(open z) = P(z open)p(open) P(z)

81 Example P(z open) = 0.6 P(z open) = 0.3 P(open) = P( open) = 0.5

82 Example P(z open) = 0.6 P(z open) = 0.3 P(open) = P( open) = 0.5 P(open z) = P(z open)p(open) P(z)

83 Example P(z open) = 0.6 P(z open) = 0.3 P(open) = P( open) = 0.5 P(open z) = P(z open)p(open) P(z) P(open z) = P(z open)p(open) P(z open)p(open) + P(z open)p( open)

84 Example P(z open) = 0.6 P(z open) = 0.3 P(open) = P( open) = 0.5 P(open z) = P(z open)p(open) P(z) P(open z) = P(open z) = P(z open)p(open) P(z open)p(open) + P(z open)p( open) =

85 Example P(z open) = 0.6 P(z open) = 0.3 P(open) = P( open) = 0.5 P(open z) = P(z open)p(open) P(z) P(open z) = P(open z) = P(z open)p(open) P(z open)p(open) + P(z open)p( open) = The measurement z raises the probability that the door is open.

86 Actions Actions increase uncertainty.

87 Actions Actions increase uncertainty. Update belief with action model (e.g. odometry, motion model): P(x u, x )

88 Actions Actions increase uncertainty. Update belief with action model (e.g. odometry, motion model): P(x u, x ) Outcome of actions: Discrete: P(x u) = x P(x u, x )P(x )

89 Actions Actions increase uncertainty. Update belief with action model (e.g. odometry, motion model): P(x u, x ) Outcome of actions: Discrete: P(x u) = x P(x u, x )P(x ) Continuous: p(x u) = p(x u, x )p(x )dx

90 Markov assumptions Measurement z t only depends on x t : p(z t x t,...) = p(z t x t )

91 Markov assumptions Measurement z t only depends on x t : p(z t x t,...) = p(z t x t ) State x t only depends on x t 1 and u t 1 : p(x t u t 1, x t 1,...) = p(x t u t 1, x t 1 )

92 Bayes filter Given: Measurements z 1,..., z t and action data/transition measurements u 1,..., u t. Sensor model: p(z x). Action model: p(x u, x ). Prior probability of the state p(x). Wanted: Belief of the state: Bel(x t) = p(x t z t, u t 1,..., u 1, z 1)

93 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...)

94 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes = p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) p(z t u t 1, z t 1,...)

95 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) = p(z t u t 1, z t 1,...) z t const. = ηp(z t x t, u t 1, z t 1,...)p(x t u t 1, z t 1,...)

96 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) = p(z t u t 1, z t 1,...) z t const. = ηp(z t x t, u t 1, z t 1,...)p(x t u t 1, z t 1,...) Markov = ηp(z t x t)p(x t u t 1, z t 1,...)

97 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) = p(z t u t 1, z t 1,...) z t const. = ηp(z t x t, u t 1, z t 1,...)p(x t u t 1, z t 1,...) Markov = ηp(z t x t)p(x t u t 1, z t 1,...) Total prob. = ηp(z t x t) p(x t x t 1, u t 1, z t 1,...)p(x t 1 u t 1, z t 1,...)dx t 1

98 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) = p(z t u t 1, z t 1,...) z t const. = ηp(z t x t, u t 1, z t 1,...)p(x t u t 1, z t 1,...) Markov = ηp(z t x t)p(x t u t 1, z t 1,...) Total prob. = ηp(z t x t) p(x t x t 1, u t 1, z t 1,...)p(x t 1 u t 1, z t 1,...)dx t 1 Markov = ηp(z t x t) p(x t u t 1, x t 1)p(x t 1 z t 1, u t 2...)dx t 1

99 Recursive Bayesian estimation Bel(x t) = p(x t z t, u t 1, z t 1,...) Bayes p(zt xt, ut 1, zt 1,...)p(xt ut 1, zt 1,...) = p(z t u t 1, z t 1,...) z t const. = ηp(z t x t, u t 1, z t 1,...)p(x t u t 1, z t 1,...) Markov = ηp(z t x t)p(x t u t 1, z t 1,...) Total prob. = ηp(z t x t) p(x t x t 1, u t 1, z t 1,...)p(x t 1 u t 1, z t 1,...)dx t 1 Markov = ηp(z t x t) p(x t u t 1, x t 1)p(x t 1 z t 1, u t 2...)dx t 1 = ηp(z t x t) p(x t u t 1, x t 1)Bel(x t 1)

100 Bayes filter implementations Bel(x t ) = ηp(z t x t ) p(x t u t 1, x t 1 )Bel(x t 1 )

101 Bayes filter implementations Bel(x t ) = ηp(z t x t ) p(x t u t 1, x t 1 )Bel(x t 1 ) Some methods based on this equation: Grid-based estimator Kalman filter Particle filter

102 Grid-based estimator Probability density function (belief) is represented using a discretized state space. Can be a simple grid with a constant step size. Tree-based methods using e.g. octrees for more efficiency.

103 Grid-based estimator Can be useful e.g. for localizations using a grid-based environment map.

104 Kalman filter The belief is represented by multivariate normal distributions. Very efficient. Optimal for linear Gaussian systems.

105 Kalman filter The belief is represented by multivariate normal distributions. Very efficient. Optimal for linear Gaussian systems. Most robotics systems are nonlinear. Limited to Gaussian distributions.

106 Kalman filter The belief is represented by multivariate normal distributions. Very efficient. Optimal for linear Gaussian systems. Most robotics systems are nonlinear. Limited to Gaussian distributions. Extensions of the Kalman Filter for nonlinearity: Extended Kalman Filter Unscented Kalman Filter

107 Particle filter Belief represented by samples (particles). State estimation for non-gaussian, nonlinear systems.

108 Particle filter Belief represented by samples (particles). State estimation for non-gaussian, nonlinear systems. Particles have weights. A high probability in a given region can be represented by many particles. few particles with higher weights.

109 Importance sampling Suppose we want to approximate a target density f.

110 Importance sampling Assume we can only draw samples from a density g.

111 Importance sampling The target density f can be approximated by attaching the weight w = f (x)/g(x) to each sample x.

112 Example Monte Carlo localization Sensor information (importance sampling) Bel(x) αp(z x)bel(x)

113 Example Monte Carlo localization Sensor information (importance sampling) Bel(x) αp(z x)bel(x) w αp(z x)bel(x) Bel(x) = αp(z x)

114 Example Monte Carlo localization Sensor information (importance sampling) Bel(x) αp(z x)bel(x) w αp(z x)bel(x) Bel(x) = αp(z x)

115 Example Monte Carlo localization Robot motion (resampling and prediction) Bel(x) p(x u, x )Bel(x )dx

116 Example Monte Carlo localization Robot motion (resampling and prediction) Bel(x) p(x u, x )Bel(x )dx

117 Example Monte Carlo localization Sensor information (importance sampling): Bel(x) αp(z x)bel(x) w αp(z x)bel(x) Bel(x) = αp(z x)

118 Example Monte Carlo localization Sensor information (importance sampling): Bel(x) αp(z x)bel(x) w αp(z x)bel(x) Bel(x) = αp(z x)

119 Example Monte Carlo localization Robot motion (resampling and prediction): Bel(x) p(x u, x )Bel(x )dx

120 Example Monte Carlo localization Robot motion (resampling and prediction): Bel(x) p(x u, x )Bel(x )dx

121 Particle filter steps State transition/prediction: Sample new particles using p(x u t 1, x t 1 ). In the context of localization: Move particles according to a motion model. Sensor update: Set particle weights using the likelihood p(z x). Resampling: Draw new samples from the old particles according to their weights.

122 Particle filter algorithm 1: procedure particle filter(s t 1, u t 1, z t ) 2: S t, η 0 3: for i = 1,..., n do Generate new samples 4: Sample index j(i) from disc. distr. given by w t 1 5: Sample from p(x t x t 1, u t 1 ) using x j(i) t 1 and u t 1 6: wt i p(z t xt) i Compute importance weight 7: η η + wt i Update normalization factor 8: S t S t {< x i t, wt i >} Insert 9: end for 10: for i = 1,..., n do 11: wt i wt i /η Normalize weights 12: end for 13: end procedure

123 Resampling

124 Resampling

125 Resampling

126 Resampling

127 Resampling

128 Resampling Binary search, n log n High variance

129 Resampling Binary search, n log n High variance

130 Resampling Binary search, n log n High variance

131 Resampling Binary search, n log n High variance

132 Resampling Binary search, n log n High variance Systematic resampling Stochastic universal sampling Linear time complexity Low variance

133 Source: Murray, Lawrence M., Anthony Lee, and Pierre E. Jacob. Parallel resampling in the particle filter. arxiv preprint arxiv: (2013).

134 Resampling algorithm 1: procedure systematic resampling(s, n) 2: S, c 1 w 1 3: for i = 2,..., n do Generate cdf 4: c i c i 1 + w i 5: u 1 U]0, n 1 ], i 1 Initialize threshold 6: end for 7: for j = 1,..., n do Draw samples 8: while u j > c i do Skip until next threshold reached 9: i i : S S {< x i, n 1 } Insert 11: u j+1 u j + n Increment threshold 12: end while 13: end for 14: Return S 15: end procedure Also called: stochastic universal resampling

135 Summary Particle filters are an implementation of a recursive Bayesian filter. Belief is represented by a set of weighted samples. Samples can approximate arbitrary probability distributions. Works for non-gaussian, nonlinear systems. Relatively easy to implement. Depending on the state space a large number of particles might be needed. Re-sampling step: new particles are drawn with a probability proportional to the likelihood of the observation.

136 Problems Global localization problem (initial position). Robot kidnapping problem.

137 Problems Global localization problem (initial position). Robot kidnapping problem. Augmented Monte Carlo Localization: Inject new particles when the average weight decreases. New random particles or particles based on current perception.

138 Acknowledgement Acknowledgement The slides for this lecture have been prepared by Andreas Seekircher.