Bayes Filter (Cyrill Stachniss)

so in the next part we want to look into the recursive base filter and provide an introduction in how to derive the basic base filter framework for recursive state estimation so again recursive state estimation what does that mean state estimation so without word recursive means that we want to estimate the state of the system for example where is a robot in the environment where is a certain object located i want to estimate it typically based on sensor information and also often based on control commands and things that change things and recursive means here that we want to update our belief where for example which state the system is currently in based on new observations that come in reusing the previous distribution that we had and updating the distribution based on the new information and this will turn into a recursive formula recursion is something that you probably know from your basic programming education where you basically reuse your previous result and just update it based on the new information that is available so again what is state estimation state estimation can be described by this probability distribution and it says i want to estimate the state of the system x at time t given my all my control commands and my observations so this is the state of my system at time t for example the position of the robot at a certain point in time and we do this estimation given so we have our observations always referred to as that and my control commands and for all the history for all the past so i started at one point in time this was the first time step and then i have them all up to time t and i have this sequence of observations and this sequence of control commands and i want to estimate where am i right now for example what's the state of the system and that's what state estimation is about and the recursive part comes into the game when i said okay i want to estimate p of x t i want to change this posterior here so that will be say p um p of x t given the rest is something where p of x t minus 1 pops up again so that i can base my state estimation about what's the state of the system at time t based on the belief i had about the system at time t minus one and of course the estimate about t times the estimate about the distribution up to t minus one then also only uses the observations and controls up to t minus one and then i kind of get new observation get a new control and i advance belief that i had to the belief at the next point in time we can start as an illustration with an example uh from the probabilistic robotics book of um sebastian tune with bogart and fox it's a very prominent example that you've also seen this course a few times you basically have a robot that lives in a one-dimensional world just has this x dimension it has no idea where it is and the only thing it can sense is actually a door so if it stands in front of the door it sensor will say door and otherwise we'll say no door door no door that's the only thing that the system can do so we will react to door events basically and so in the beginning the system has no idea where it is so it's basically a uniform distribution so this is a belief the belief of x is basically this distribution before p of x t given the control commands and the and the observations and now let's say it stands in front of the door and the sensor shouts door okay where am i if i have no idea where i am i only know my sensor set door and i have a map of the environment which from which i know that there are three doors they would say oh i'm either here or here or here right yeah that's okay that's a pretty good estimate but it could also be the case that my sensor made a mistake right so it's still possible that i'm somewhere else but it's of course more likely to be in front of a door so what we can do is then we can use our observation in the observation model will tell me just based on the information that the sensor should door i'm either here or here or here or maybe somewhere else but with a smaller probability so i can basically update that belief into a new belief which now encodes this information it turned from the uniform distribution to this belief that it incorporated one observation and then the more the system basically moved through the environment so after intercooperating the sensor information the robot drove let's say a meter forward for example and then we can see that the system moved forward the road moved from here to here um but what we can also see is that then the probability distribution should move with it right because if i was in front of a door i need to shift this probability mass also meter forward i could shift it exactly a meter forward if i would perfectly know that the platform would exactly drive one meter forward but often that's not the case you know you send driver meter forward and the system drives a meter and five centimeters forward or it only drives 95 centimeters forward or 99 centimeters or a meter 20. you don't know so there's some uncertainty in the motion and this leads to the fact that this probability distribution kind of smears out it gets wider this distribution so the peaks are not as peaked anymore they get flatter and distribute probability mass over a larger area in state space and therefore this distribution gets flatter but then the system gets a new observation in the robot again shot the door or the sensor shouts against door and i again if i don't know anything where i am no prior knowledge and just know that the sensor shot at door i would have exactly the same distribution at before but now i need to combine this distribution with this distribution over here and this leads to effect where there's an overlap in the post between the two peaks this will give me the highest peak and a few other peaks here as well and those other peaks basically mean i was right once but wrong the second time or the other way around so as a result of this we will have this distribution and if we kind of move again further forward we will have this distribution over here and this is basically advancing the belief from one state to the next state using just the current sensor information and just the current motion control command is something which we refer to as recursive state estimation and that's what we want to solve we want to first solve it just in form of a mathematical framework so deriving the equations that tell us what do we need to do in order to estimate this recursive belief so again we start with this one that's one estimate p of x t so where are we right now given all the observations from the past and the control commands from the past so in this example before these were the two motion commands and the two observations ahead from that platform and i want to estimate where am i where where am i given that i have no idea where i was before and that's where we're going to head to now okay so we start by saying we're interested about the belief of the system where it is at time t and this is nothing else as the state estimation equation or the definition of the of the probability distribution that is my belief so where am i right now given all the observations and all the control commands so this is the definition of the belief and what you see here everything which is readily underlined is the part which has changed in this derivation okay so the next thing we're going to do we want to split up or we're going to modify this distribution here to simplify it for us and one way to simplify this is to apply bayes rule remember thing that we did just a few minutes ago in the previous lecture on probability theory we looked into bayes rule so bayes rule allows me to swap certain elements in here between what's written in front of the given sign what's written behind the given sign given some background knowledge so what we do now is what we want to swap is zt and xt so the variable zt so the last observation and the current state of the system we just want to swap them turn them around that means that one to t minus one and all the u's count as background knowledge and so we apply bayes rule in order to swap those over here so what we do is we kind of swap z t and x t so x t and z t we're going to swap so this is a normalization constant over here which encodes the evidence so divided by the the lower part of that equation that we defined in bayes rule which is nothing else in a normalization constant so i can just write it as a constant here okay so what has happened over here so in this in in this first part of bayes rule so this is the first part and the second part here zt and xt have been swapped okay so here we swap those two the rest stay the same and then we have the second part where everything is in there except the verb which is now here sitting in front so everything else is in there but not set t so this is p of x t given z 1 to t minus 1 e 1 to t so the plane vanilla application of bayes rule nothing else has happened here okay and now what i'm doing i want to modify this part over here what i'm doing is is given i know where the system is right now given the state of the world given xt so i know where i am what's the probability of observing something of saying let's say i'm in front of the door also given all the past observation the past control commands and here is something that we assume we make an assumption is called the markov assumption the mark of assumption tells me or says that given i know the state of the system what happened in the past is decoupled from the future or independent of the future so that means given i know the state given i know where i am all the past control commands how i steer in the path and the past observations they don't tell me anything about i'm knowing what i'm going to observe right now or in the future okay so this means all this part over here this z one to t minus one u one to t doesn't help me to estimate p of the likelihood of the observation so i can ignore them i can drop that because there's an independence this term and this term are conditionally independent given x t that's what it's said here and this is what the markov assumption does so through the markov assumption this simplifies to p of z t given x t so it basically says what's the likelihood of an observation given i know the state of the world and this comes in through the markov assumption the next thing we want to do is we want to modify this part of the equation here so this is what we're going to change now and what we're going to do here what we're going to do here is we apply the law of total probability so you know with the law of total probability applying it we can add a new variable we can introduce a new variable to the system that we are integrating over and we introduce the variable x t minus 1. why are we going to do this because we want to end up getting a recursive equation so an equation where we can move from x t to x t minus one therefore we introduce a new variable right now through the law of total probability that tells me where was the system at the previous point in time so not where it is right now where was it before so we just apply we just add p of x t minus one in here after the given sign and then times p of x t and integrate over p of x t minus one okay so this is what the result of the law of total probability looks like so the expression gets a bit more complex but there is no black magic behind it's just the plain vanilla application of the law of total probability so i added this new variable x t minus one so it sits this is the same distribution of what's written here just i added this x t minus one and then i have to also write the p of x t minus one given the previous background knowledge that i have and integrate over x t minus one so i'm integrating basically over all possible states where i've been before so basis thing is where am i right now given all the old observations and all the motion commands basically can boil down to where am i right now given i know we have been the previous point in time times how likely that actually was there and then integrating about all previous states so i'm basically integrating over all possible previous states and estimate given that i was there how likely it is that i'm now in the state where i am or where i want to be at the p of x t so we're adding this additional variable to kind of breaking down this chain of previous states and add just the the last most previous most recent state to it um through this law of total probability but integrating over it so that we don't change the overall probability distribution okay um and then we're going to simplify this expression over here so what's written here it's p of x t so we probability where i am right now given i know where i've been before given all old observations which happen up to that time x t minus one and all the old control commands plus a new control command so given i know where i've been in the past in the previous time step whatever i've seen before really doesn't matter anymore right it's again a markov assumption whatever happened in the past i don't care where i know if i know where i am right now the only thing which still matters is ut because ut tells me how do i evolve from t minus one to t so this is still valuable information it's basically the last motion command that i've executed to the platform but whatever however i steered before is irrelevant if i know where i've been at the last point in time so this equation simplifies to p of x t given x t minus 1 and u t and all the observations and all the motion commands between the time step one and t minus one don't matter anymore so we can eliminate them so as a result of this this simplifies to this expression over here just using this markov assumption basically knowing where we've been in the previous point in time helps me to ignore everything which happened before and then the last thing we need to optimize or to change is this expression over here so what do we have here we have the probability we have been in the previous point in time given all observations up to the previous point in time and all control commands up to the previous point in time plus ut plus the last control command so basically it's the same distribution that we started with here except that we have one further motion command so i know what i have steered which motion i execute in the system and what i'm doing right now i'm i'm making an independence assumption or an independence assumption i'm assuming it doesn't mean it's true i just assume that the motion command i'm executing in the future doesn't help me to estimate where i am right now okay so assuming i want to estimate where i'm right now here in that position where i am and i know that in the future i will i will go 20 centimeters forward something like this um does it help me to estimate where i am right now you would say probably not because what i'm executing in the future shouldn't help me to estimate where i am right now um if you think like this it holds for most of the cases you can now ever construct a situation where this is wrong so if i would be standing in front of a wall and you can assume that i have let's say a collision avoidance system running if i'm a robot and i'm standing in front of the wall it's very unlikely that it will actually bump into the wall so if i'm here and i'm saying go 20 meters 20 centimeters forward i would bump into that wall so it's unlikely that the system would have executed this motion command if i would stand so close to the wall so there are certain situations where this independence assumption of ignoring ut does not hold but in most of the cases it's a reasonable assumption so what i'm doing here i'm simply ignoring the latest motion command that has been executed i'm just ignoring it okay i'm losing information here but you can argue that quite often this information is not very useful but there are corner cases where it could matter but we are ignoring it something that we assume independence not true it's not a true independence it's assumed independence so if our model breaks we're not good in state estimation revisiting the assumptions that we made is a good idea but our model makes this assumption now and then we can look to this last term over here p of x t minus 1 given z from 1 to t minus 1 and u from 1 to t minus 1. this is exactly the initial belief that i had except that i replaced t minus 1 by t or t to t minus 1. so this is now my recursive my recursion so i reduced the estimate of where i am right now to the estimate where i've been before plus some updates which only depend on the current control command and the current observation so i can put in the belief this is the belief x t minus 1 to make this recursion explicit over here okay so it's just a replacement it's nothing there's no assumption nothing there's just the recursive term that i put in there and that's basically the complete derivation of the recursive base filter so we have started from here from the believe at time t we reduced it to the belief at time t minus one plus some update taking into account the most recent motion command and take into account the most recent observation and this is one of the key derivations and a lot of state estimation techniques rely on this derivation so if you think about the kalman filter or the extended kalman filter or unscented kalman filter or into a particle filter or a histogram filter or other recursive filtering tank that you know already or that you will get to know they all rely on this mathematical framework okay so it's the very basic derivation of the base filter from the beginning to the end and it's clear also now which assumption we have made so if we have applied base rule that's not an assumption everything good we made a mark of assumption that is an assumption that the past doesn't impact the future given i know the state in between if i wouldn't know the state between there would clearly be a dependency but given that i know the state of where the system in is in right now the past is independent from the future and we have done this twice once for our first model over here which will lead to something we call the observation model and here in there something we call the um the motion model the control the control model and what we have used we have used an independence assumption assuming that a future motion command doesn't help me in the estimating the state of the past and this was kind of the last step that we have done in here these are the three assumptions that we have made when we have been deriving um the general base filter framework and these are assumptions that the base filter does but under these assumptions there have been no simplifications in here of course depending on the realization that you do for the base filter so if you use gaussian distributions to represent those probability distributions then you start making assumptions or kind of make your model less general because you reduce everything to a gaussian distribution for example but then you may be able to solve this very efficiently and if your assumption is a gaussian you're close to a gaussian that's maybe a good idea to do but so far this was really the general derivation without making assumption about what kind of models we are using or what kind of of probability distributions we are taking into account here quite often the literature you find that the base filter is broken up into its called a prediction step and a correction step and so this is our final equation that we have this part over here the last part with the integral is typically called the prediction step and the this second part over here is called the correction step why prediction and correction because what this part does it takes the previous belief and advances the previous bleep based on the control command so based on for example which steering command i've executed it estimates where it could be it predicts where the system will be in the future so the prediction step and then we'll get an observation in the new state and this observation is used to correct potential mistakes that we have made in the prediction so we take our observation into account in order to perform a correction of my prediction and therefore this is called the correction step so if you have a bar over here this is typical notation for the predicted belief the predicted belief is the belief where we only use the um the motion model to advance the state and the correction step then multiplies this predictive belief with the um with our observation model and then this and the normalizer and this then leads me to the correct step where i take into account that the sense observation may tell me something different than the prediction step and often we find in these space filter formulations an alternating prediction correction prediction correction prediction correction step and in every time step i basically perform a prediction and a correction motion and observation motion observation and always advance the belief estimating in which state the system is and in the prediction step we use something we so-called the motion model which is p of x t given x t minus 1 and ut and we have our correction our correction step which uses so-called observation model sometimes also called measurement model or sensor model depending on the community you're in i will most of the time use the word observation model in here which is the likelihood of obtaining the observation that i got given i know the state so if i assume that i know which state i am how likely is actually to observe what i'm observing and that's the question that we typically have to answer in the setup and up to this point this was the general derivation of the basic base filter if you want to implement your base filter you need to do certain things or what you need to do is you need to specify your motion model and your observation model how do they actually look like how does my belief look like how do i represent this belief if this is a gaussian if this is a sum of gaussian this is an arbitrary probability distribution this will have an impact on how those things look like also how do i describe how i advance from time t minus 1 to t is this a linear model is this a non-linear model what do i know about the system is it a car that i know i have a steering wheel and a gas pedal that i can can push and this will constrain the motion how i'm going i can't move sidewards with a car for example these are all constrained that i can take into account need to integrate into my motion my observation model in order to perform state estimation in an efficient manner so the important thing to note in here is that there's a large number of different realizations of this base filter so the base filter is just a framework for recursive state estimation and there are different realizations which are out there and they depend based on the assumption that you make again linear versus non-linear models for example for describing the motion or for describing the observations can we assume gaussian distributions or do we want to represent arbitrary distributions do we need to be able to handle uniform distributions for example initially when we don't know in which state the system is in that's something which can it's tricky to do with the gaussian distribution do we need to represent multiple modes multiple hypotheses about the state do we want to represent the probability distribution in a parametric form a non-parametric form these are all design decisions that i have to make and they have an impact on how this filter actually looks like there are different popular filters which are out there kalman filter and extended kalman filters they typically use gaussian distributions for all our distributions and a linear model for the kalman filter or linearized models for the extended kalman filter which basically performs a tailor linearization or taylor approximation of the nonlinear function and this is a very popular filter that you find very often literature particle filters um are an alternative representation alternative realization of the base filter they have the advantage that they allow for for leaving the gaussian world so you don't assume that everything is gaussian anymore you can represent arbitrary probability distributions this comes at a cost at a computational cost and the memory cost but allows you to for example represent multimodal distributions in a very elegant way and but can handle basically arbitrary models at least for the motion model as long as we can sample from those models and can point wise evaluate our observation models then we can use actually arbitrary models are not constrained to linear or close to linear models just to give you a very brief idea how such a motion model observation model looks like i just want to give you two very simplistic examples without going into the details just to give you kind of visual understanding and how that could look like later in the course we will explicitly look into observation models and into motion models so for for today i just want to show you one short example one instance how that could look like so motion model um is this part over here was this part over here so where am i right now given i know where i've been before and given i know which motion command has been executed how does it look like and we can represent it or illustrate this in this form over here so what you see here is an illustration of a robot so kind of a circular robot it's looking over here at the point in time t x t minus one and next point in time it's looking over here so the system has actually been driving around over here or assumed to have been driven over here that's what the motion command tells the system and the question is where am i right now where am i actually and then it turns out i get this probability distribution this grayer so the darker the values the higher the probability we can see this as an experiment let's say i put the robot in a certain state and i execute the same motion command let's say 300 times then i get these 300 different dots of where the system actually ends up with ends up and so these are the outcomes you can see the outcomes of experiments and they represent the probability distribution or an approximation of the probability distribution where i am after executing this comma knowing where i've been before and these are different parameters for those models for example in this case the system has a very high uncertainty in the angular movement but can very precisely estimate how far it has traveled so in this case you you the uncertainty in this direction is very small but has is very large in the angular component here's the other way around a higher uncertainty along the um the forward motion direction but have a smaller uncertainty with respect to the angular components you can basically see this based on this kind of idea of experiments of repeating an experiment and seeing where the system ends up and this is something which we typically call a banana shape distribution so it's kind of this because the shape of a banana which describes the motion and this is a traditional so-called odometry based motion model where you count the revolution of the wheels of your platform and see where are you going to end up and this is an example how this model looks like so how p of x t given x t minus 1 and u t actually looks like these are just illustrations of this fact for the observation model we can do something different again a very small simple example as simple as possible just to give you a visual idea on how that could look like so consider we have a sensor let's say let's take my laser pointer over here that you see here and consider this as a measurement device so it measures the distance to the closest obstacle so whatever somewhere over here you see the laser pointer this is the it measures this distance just assuming this so it says whatever 40 centimeters 35 centimeters 50 centimeters but this measurement is affected by noise it's not perfect maybe it's 50 centimeters but in reality or then the system says it's 48 centimeters or in reality it's 60 centimeters away and the system tells it's 62 centimeters away so how often does it happen that i'm off and how far am i off and this is something which i can for example represent with a gaussian distribution representing gaussian measurement noise which is actually a not too bad approximation if at least we're in the outlier-free case so if you have a range sensor the robot sits here and here is an obstacle what's the probability of range measurement that i get and maybe i assume this is a gaussian distribution so it's very unlikely to measure a meter if my object is 50 centimeters away and it's also very unlikely to measure 10 centimeters if the object is 50 centimeters away so 50 centimeters is very likely to measure and 49 is also quite possible 51 as well but the further i move away from the actual distance the smaller the probability gets and this is an example for a simple observation model for range sensor with gaussian noise of a single ray range reading in this most simple case just to give you an idea how the culture model could look like so by using such a motion model and such an observation model i can actually estimate positions where a system is in the world and perform localization tasks and that's something that we will gradually work towards in this course first we'll look into maps and we'll look into how to describe motion and then go deeper into those models until we reach that point in time we can actually localize the platform but this is kind of the the guideline wherever we actually will move to which also brings me to the end of the lecture today so we have seen the first part that probabilities are important in order to estimate a lot of state estimation problems and their key tool that we are going to use we have derived the base filter framework which is a technique for recursive state estimation and allows me to estimate probability distributions um taking into account the previous belief that i had plus the new observations and the new control commands and there are different ways how we can realize the base filter so base filter was a general framework and then there are concrete realizations such as the extended kalman filter or the particle filter and we will dive into the details of these filtering techniques in the future here in this course if you implement such a base filter you need to specify what kind of distributions are you looking into what kind of models do you have you have to specify your observation model and your motion model as the central models in the base filter which you need to specify as well as how the probability distributions look like so that's important if you want to actually implement it and build such a filter as a concrete instantiation of this general framework that we have presented over here so if you want to dive a bit more into into that and want to read a bit more about it i can recommend the probabilistic robotics book chapter 2.1 and 2.2 basically is repetition of this probability primer that i have talked about and the chapter 2.3 is an introduction to the base filter using a similar notation that i have used here so it should be actually straightforward and if you want to dive more into the detail and read again what i have presented over here so that's it for my side for today thank you very much for your attention and i hope you enjoyed this starting part into the course and get an idea on how state estimation at least works on a general general concept and we will make that more concrete in the near future thank you for your attention

Channel: Cyrill Stachniss

Views: 7,664

Rating: 4.9713264 out of 5

Keywords: robotics, photogrammetry

Id: 0lKHFJpaZvE

Length: 32min 5sec (1925 seconds)

Published: Tue Aug 25 2020