Probabilistic Robotics

Transcription

1 University of Rome La Sapienza Master in Artificial Intelligence and Robotics Probabilistic Robotics Prof. Giorgio Grisetti Course web site: Giorgio Grisetti Dynamic Bayesian Networks 1 / 32

2 Overview Probabilistic Dynamic Systems Dynamic Bayesian Networks (DBN) Inference on DBN Recursive Bayes Equation Giorgio Grisetti Dynamic Bayesian Networks 2 / 32

3 Dynamic System, Deterministic View u t 1 x t 1 x t f(x t 1,u t 1 ) h(x t ) z t t f(x t 1,u t 1 ): transition function h(x t ): observation function x t 1 : previous state x t : current state u t 1 : previous control/action z t : current observation t : delay Giorgio Grisetti Dynamic Bayesian Networks 3 / 32

4 Dynamic System, Probabilistic View u t 1 x t 1 x t p(x t x t 1,u t 1 ) p(z t x t ) z t t p(x t x t 1,u t 1 ): transition model p(z t x t ): observation model x t 1 : previous state x t : current state u t 1 : previous control/action z t : current observation t : delay Giorgio Grisetti Dynamic Bayesian Networks 4 / 32

5 Evolution of a Dynamic System x 0 Let s start from a known initial state distribution p(x 0 ). Giorgio Grisetti Dynamic Bayesian Networks 5 / 32

6 Evolution of a Dynamic System u 0 x 0 A control u 0 becomes available Giorgio Grisetti Dynamic Bayesian Networks 6 / 32

7 Evolution of a Dynamic System u 0 x 0 x 1 The transition model p(x t x t 1,u t 1 ) correlates the current states x 1 with the previous control u 0 and the previous state x 0. Giorgio Grisetti Dynamic Bayesian Networks 7 / 32

8 Evolution of a Dynamic System u 0 x 0 x 1 z 1 The observation model p(z t x t ) correlates the observation z 1 and the current state x 1. Giorgio Grisetti Dynamic Bayesian Networks 8 / 32

9 Evolution of a Dynamic System u 0 u 1 u t 1 u t x 0 x 1 x t 1 x t x T z 1 z t 1 z t z T This leads to a recurrent structure, that depends on the time. Giorgio Grisetti Dynamic Bayesian Networks 9 / 32

10 Dynamic Bayesian Networks (DBN) u 0 u 1 u t 1 u t x 0 x 1 x t 1 x t x T z 1 z t 1 z t z T Graphical representations of stochastic dynamic processes Characterized by a recurrent structure Giorgio Grisetti Dynamic Bayesian Networks 10 / 32

11 States in a DBN The domain of the states x t, the controls u t and the observations z t are not restricted to be boolean or discrete. Examples: Robot localization, with laser range finder states x t SE(2), isometries on a plane observations z t R #beams, laser range measurements controls u t R 2, translational and rotational speed HMM states x t [X 1,...,X Nx ], finite states observations z t [Z 1,...,Z Nz ], finite observations controls u t [U 1,...,U Nu ], finite observations Inference in a DBN requires to design a data structure that can represent a distribution over states. Giorgio Grisetti Dynamic Bayesian Networks 11 / 32

12 Typical Inferences in a DBN In a dynamic system, usually 1 we know: the observations made by the system z 1:T, because we measure them the controls u 1:T 1 because we issue them Typical inferences in a DBN: name query known Filtering p(x T u 0:T 1,z 1:T ) u 0:T 1,z 1:t Smoothing p(x k u 0:T 1,z 1:T ), 0 < k < T u 0:T 1,z 1:t Max a Posteriori argmax x0:t p(x 0:T u 0:T 1,z 1:T ) u 0:T 1,z 1:T 1 usually does not mean always Giorgio Grisetti Dynamic Bayesian Networks 12 / 32

13 Typical Inferences in a DBN Using the traditional tools for Bayes Networks is not a good idea. too many variables (potentially infinite) render the solution intractable the domains are not necessarily discrete However, we can exploit the recurrent structure to design procedures that take advantage of it. Giorgio Grisetti Dynamic Bayesian Networks 13 / 32

14 DBN Inference: Filtering u 0 u 1 u t 1 u t known query x 0 x 1 x t 1 x t x T z 1 z t 1 z t z T Given the sequence of all observations z 1:T up to the current time T, the sequence of all controls u 0:T 1 we want to compute the distrubution over the current states p(x T u 0:T 1,z 1:T ). Giorgio Grisetti Dynamic Bayesian Networks 14 / 32

15 DBN Inference: Smoothing u 0 u 1 u k 1 u k known query x 0 x 1 x k 1 x k x T z 1 z k 1 z k z T Given the sequence of all observations z 1:T up to the current time T, the sequence of all controls u 0:T 1 we want to compute the distrubution over a past state p(x k u 0:T 1,z 1:T ). Knowing also the controls u k:t 1 and the observations Z k:t after time k, leads to more accurate estimates than pure filtering. Giorgio Grisetti Dynamic Bayesian Networks 15 / 32

16 DBN Inference: Maximum a Posteriori u 0 u 1 u t 1 u t known query x 0 x 1 x t 1 x t x T z 1 z t 1 z t z T Given the sequence of all observations z 1:T up to the current time T, the sequence of all controls u 0:T 1 we want to find the most likely trajectory of states x 0:T. In this case we are not seeking for a distribution. Just the most likely sequence. Giorgio Grisetti Dynamic Bayesian Networks 16 / 32

17 DBN inference: Belief Algorithms for performing inference on a DBN keep track of the estimate of a distribution of states. This distribution should be stored in an appropriate data structure. The structure depends on the knowledge of the characteristics of the distribution (e.g. Gaussian) the domain of the state variables (e.g. continuous vs discrete) When we write b(x t ) we mean our current belief of p(x t...). The algorithms for performing inference on a DBN work by updating a belief. Giorgio Grisetti Dynamic Bayesian Networks 17 / 32

18 DBN inference: Belief In the simple case of a system with discrete state x {X 1:n }, the belief can be represented through and array x of float values. Each cell of the array x[i] = p(x = X i ) contains the probability of that state. If our system has a continous state and we know it is distributed according to a Gaussian, we can represent the belief through its parameters (mean and covariance matrix). If the state is continuous but the distribution is unknown, we can use some approximate representation (e.g. weighed samples of state values). Giorgio Grisetti Dynamic Bayesian Networks 18 / 32

19 Filtering: Bayes Recursion We want to compute p(x T u 0:T 1,z 1:T ) =? We know: the observations z 1:T the controls u 0:T 1 p(x t x t 1,u t 1 ): the transition model. It is a function that given the previous state x t 1 and control u t 1, tells us how likely it is to lend in state x t. p(z t x t ): the transition model. It is a function, that given the current state x t 1, tells us how likely it is to observe z t. b(x t 1 ), which is our previous belief about p(x t 1, u 0:t 2,z 1:t 1 ) Giorgio Grisetti Dynamic Bayesian Networks 19 / 32

20 Filtering (1) splitting z t : = p( x t }{{} A p(x t u 0:t 1,z 1:t ) = (1),u }{{} 0:t 1,z 1:t 1 ) (2) }{{} B C z t recall the conditional bayes rule p(a B,C) = p(b A,C)p(A C) p(b C) B A C A C = p( {}}{{}}{{}}{{}}{{}}{ z t x t, u 0:t 1,z 1:t 1 )p( x t u 0:t 1,z 1:t 1 ) p( z }{{} t u 0:t 1,z 1:t 1 ) }{{} B C (3) Giorgio Grisetti Dynamic Bayesian Networks 20 / 32

21 Filtering: Denominator Let the denominator η t = 1/p(z t u 0:t 1,z 1:t 1 ). (4) Note that η t does not depend on x, thus to the extent of our computation is just a normalizing constant. We will come back to the denominator later. Giorgio Grisetti Dynamic Bayesian Networks 21 / 32

22 Filtering: Observation model Our filtering equation becomes η t p(z t x t,u 0:t 1,z 1:t 1 )p(x t u 0:t 1,z 1:t 1 ) (5) Note that p(z t x t,u 0:t 1,z 1:t 1 ) means this known u 0 u 1 u t 1 query x 0 x 1 x t 1 x t z 1 z t 1 z t If we know x t, we do not need to know u 0:t 1,z 1:t 1 to predict z t, since the state x t encodes all the knowledge about the past (Markov assumption) : p(z t x t,u 0:t 1,z 1:t 1 ) = p(z t x t ) (6) Giorgio Grisetti Dynamic Bayesian Networks 22 / 32

23 Filtering: Transition Model Thus, our current equation is p(x t u 0:t 1,z 1:t ) = η t p(z t x t )p(x t u 0:t 1,z 1:t 1 ) (7) Still the second part of the equation is obscure. Our task is to manipulate it to get something that matches our preconditions. Giorgio Grisetti Dynamic Bayesian Networks 23 / 32

24 Filtering: Transition Model If we would know x t 1, our life would be nuch easier, as we could repeat the trick done for the observation model: u 0 u 1 u t 1 known query x 0 x 1 x t 1 x t z 1 z t 1 z t thus p(x t x t 1,u 0:t 1,z 1:t 1 ) = p(x t x t 1,u t 1 ) (8) Giorgio Grisetti Dynamic Bayesian Networks 24 / 32

25 Filtering: Transition Model The sad truth is that we do not have x t 1, however Recall the following probability identities p(a C) = b p(a, B C) (9) combining the two above we have p(a,b C) = p(a B,C)p(B C) (10) p(a C) = b p(a B, C)p(B C) (11) Giorgio Grisetti Dynamic Bayesian Networks 25 / 32

26 Filtering: Transition Model The sad truth is that we do not have x t 1, however let s look again at our problematic equation, and put some letters p( x }{{} t u 0:t 1,z 1:t 1 ) = (12) }{{} A C = x t x }{{} t 1,u }{{} 0:t 1,z 1:t 1 )p(x }{{} t 1 u }{{} 0:t 1,z 1:t 1 ) (13) }{{} x t 1p( A B C B C putting in the result of Eq. 8, we highlight the transition model = x t 1p(x t x t 1,u t 1 )p(x t 1 u 0:t 1,z 1:t 1 ) (14) Giorgio Grisetti Dynamic Bayesian Networks 26 / 32

27 Filtering: Wrapup After our efforts, we figure out that the recursive filtering equation is the following: p(x t u 0:t 1,z 1:t ) = (15) η t p(z t x t ) x t 1p(x t x t 1,u t 1 )p(x t 1 u 0:t 1,z 1:t 1 ) (16) yet, if in the last term of the product in the summation, we would not have a dependancy from u t 1, we would have a recursive equation. luckily, p(x t 1 u 0:t 1,z 1:t 1 ) = p(x t 1 u 0:t 2,z 1:t 1 ), since the last control has no influence on x t 1 if we don t know x t. Giorgio Grisetti Dynamic Bayesian Networks 27 / 32

28 Filtering: Wrapup We can finally write the recursive equation of filtering as: b(x t) {}}{ p(x t u 0:t 1,z 1:t ) (17) = η t p(z t x t ) x t 1p(x t x t 1,u t 1 )p(x t 1 u 0:t 2,z 1:t 1 ) }{{} b(x t 1 ) during the estimation, we do not have the true distribution, but rather beliefs estimate. Equation 18 tells us how to update a current belief once new observations/controls become available b(x t ) = η t p(z t x t ) x t 1p(x t x t 1,u t 1 )b(x t 1 ) (18) Giorgio Grisetti Dynamic Bayesian Networks 28 / 32

29 Normalizer: η t η t is just a constant ensuring that b(x t ) is still a probability distribution. 1 η t = x t p(x t x t 1,u t 1 )b(x t 1 ) Giorgio Grisetti Dynamic Bayesian Networks 29 / 32

30 Filtering: Discrete case float transition model ( int to, int from, int control ); float observation model ( int observarion, int state ); void f i l t e r ( float b, int n states, int z, int u) { // clear the states float b pred [ n states ]; for ( int i =0; i<n states ; i++) b pred [ i ]=0; } // predict for ( int i =0; i<n states ; i++) for ( int j=0; j<n states ; j++) b pred [ i]+=transition model (i, j,u) b[ j ]; // integrate the observation float inverse eta = 0; for ( int i =0; i<n states ; i++) inverse eta += b pred [ i] =observation model (z, i ); // normalize float eta = 1./ inverse eta ; for ( int i =0; i<n states ; i++) b[ i ] = b pred [ i ] eta ; Giorgio Grisetti Dynamic Bayesian Networks 30 / 32

31 Filtering: Alternative Formulation Predict: incorporate in the last belief b t 1, the most recent observation. From transition model and the last state, compute the following joint distribution through chain rule p(x t,x t 1 z 1:t 1,u 1:t 1 ) = p(x t x t 1,u t 1 )p(x t,x t 1 z 1:t 1,u 1:t 2 ) }{{} b t 1 (19) From the joint, remove x t 1 through marginalization p(x t z 1:t 1,u 1:t 1 ) = p(x }{{} t,x t 1 z 1:t 1,u 1:t 1 ) (20) x b t 1 t t 1 Giorgio Grisetti Dynamic Bayesian Networks 31 / 32

32 Filtering: Alternative Formulation Update: from the predicted belief b t t 1, compute the joint distribution that predicts the observation. From transition model and the last state, compute the following joint distribution through chain rule p(x t,z t z 1:t 1,u 1:t 1 ) = p(z t x t )p(x t, z 1:t 1,u 1:t 1 ) (21) Incorporate the current observation through conditioning, on the actual measurement p(x t,z t z 1:t 1,u 1:t 1 ) p(x t z 1:t,u 1:t 1 ) = }{{} p(z t z 1:t 1,u 1:t 1 ) b t t (22) Note: since we already know the value of z t, we do not need to compute the joint distribution for all possible values of z Z, but just for the current measurement. Giorgio Grisetti Dynamic Bayesian Networks 32 / 32