SLAM Course - 05 - EKF SLAM (2013/14; Cyrill Stachniss)

welcome to the course today on simultaneous localization and mapping using an extended Kalman filter and so last week we actually introduced the Kalman filter and extended Kalman filter so the key concepts of these algorithms and today we would like to I would like to go more into the detail how to apply this concept to the simultaneous localization mapping problem so that we are able to estimate the location of the robot as well as a map of the environment in this case a map of landmarks I am giving this framework so just to put into context we wanted to estimate the position of the robot and the position of landmarks in the environment this is all map and we need that map in order to perform navigation tasks and the key problem when a key problem but one of the challenges in this problem is that the process of estimating a map cannot be decoupled from the process of estimating the trajectory or the pose of the robot because both variables are depend on each other and so we have to solve both problems at the same time namely estimating the location of the landmarks and the location of the position of the robot so we started with kind of the definition of the slam problem what do we need what we want we have a sequence of control commands that the robot executed these are these variables u times M 1 2 times step T and these are either bra commands like go with a certain velocity or these are automated readings and we treat these odometry readings as control commands this kind of counting the revolution of the wheels and estimating where the weak event and the second thing we have our sensor observations there's typically a sensor which observes the environment such as the laser rangefinder camera solar sensor or some of those kinds of measurements and they typically provide me information about the environment that could be the bearing information to a landmark the range information to a landmark so how far is landmark away or it could also be both at the same time these are kind of typical sense observations that we get in this context and what we want to estimate from this data is first map of the environment to be referred to as M and our case here today this map is a set of landmark locations and these landmark locations are just things that the robot can identify with its laser range or with its sensor setup there's a range scanner or camera images and we want to estimate the pose of those landmarks if we are navigating outside here in front of our building that may be trees that we see but the Kenny ant can it can be any other type of object which populates the environment and that we want to use for localizing the robot for estimating a map and the next thing one 1/2 is gonna have the path of the robot so the positions of the robot XY as well as orientations for the 2d case where were the robot at every point in time in most cases we actually look into online slam that means we're only interested in estimating the current position of the robot occurring pose of the robot and because we kind of ignore the previous locations because they are depending on the application not necessarily interesting for us that really depends on the estimation technique and on the task at hand in this course if you look into three different paradigms on how to address a slam problem the carbon filter family Hartigan filters and graph based optimization approaches and what we do here today is we start with the Kalman filter so I've said the last lecture we introduced the Kalman filter as kind of an abstract concept for state estimation and today we want to look into the question how can we map that to the slam problem what needs to be taking into account here just as a reminder the common filter is a recursive based filter and the recursive based filter can be split up into two steps the first one is a prediction step and the second one is a correction step and what the prediction step does it it takes a current estimate of the about the state of the robot it executes or takes into account the commands that have been sent to the robot and estimates a new state a predicted state in which the system is in if we have landmarks in the environment in the pose of the robot as our state space and we want to estimate how the state changes given that the executing command to be kind of clear that at least or motion commands we sent to a robot this only changes the position of the robot just by going a metre forward that typically doesn't affect the position of the landmarks so in a lot of cities summation settings the prediction step only effects a subset of the variables okay so this is a prediction step there's our previous belief and we estimate how does the belief change if we are in a state XT or over the distribution XT which was the belief at the previous point in time we execute the command UT where do we end up these are predicted belief always with this head here on top and the second step is then the correction step which takes into account the sensor observation that the robot takes perceived about the environment and it relates what the robot sees and what the world is supposed to see and based on them of them based on the mismatch of these two observations the predicted observation and the obtained observation a correction is executed and an update of the state happens and this is the correction step and this prediction correction step was also something we have seen in the excellent Kalman filter so we explicitly had our prediction step where we take into account the commands or the controls that were issued to the robot and then we have the the correction step which takes into account the sensor information what we are considering here today is EKF for online slam that means we are not interested in estimating the full trajectory of the robot we are only interested in estimating the current pose of the robot together with all landmark locations so we looked at the graphical model that we introduced in the first or second lecture we are not estimating the whole sequence of robot positions we are only interested in always estimating the current one because what we want to know where the robot currently in the environment and what are the environment look like because these are typically the two quantities based on which we can make most of the navigation decisions for some applications it may be interesting to also know about the past reject arees that the robot has been taken but in the con what we are considering today this is not the case so it's everything clear so far there any questions up to this point okay perfect so what we then introduced last time was actually the extended carbon filter algorithm just will very very briefly in two minutes revisit this algorithm to keep you everything up to date so what happens here in the first two lines to line two and three this is the prediction step there we compute the predicted belief about the state we want to estimate and this is this mu bar and Sigma bar because these two variables the mean and the covariance matrix of our state and what happens here is we have our previous estimate about the state this me you need t minus one so the the kind of the output of the Kalman filter in the previous step and we apply the the the control command UT and this gives us our new state and this function G is a nonlinear function so we moved from the Kalman filter to the extended Kalman filter by allowing for nonlinear models and we have our update in the of the covariance matrix which takes account the Jacobian of this function the previous estimate of the the previous uncertainty estimate the previous covariance matrix plus the noise that is added to the process through the motion command UT so it means if we have a certain covariance matrix which represents our uncertainty and we execute a motion command let's say move a media forward so the robot moves a meter forward and this motion adds an additional uncertainty so the covariance grew so the uncertainty grows covariance matrix gets larger that's exactly what happens here in this line this is a prediction step and if you want to implement the friction step for specific application we first need to define our nonlinear function G which tells us how to move from a given state and the control command to our new state and we need to compute its Jacobian so it's the matrix of partial derivatives that's what we need for the prediction step then we have the correction step which is down here the first thing we do is to compute the so called Kalman gain K and the key idea of the Kalman gain was to to compute a weighted sum by saying how much certain is the robot about its predicted belief this guy over here and how certain is the robot about M it's the sense of property so what's the uncertainty associated with an observation and the more certain or the better the sensor is so the smaller the uncertainty in the sensor the more the robot will trust the sensor observation and the more will the weighted mean drag the current as the the the corrected estimate towards what the robot has seen if we in contrast have a very very bad sensor it's very high noise and a quite accurate motion model though what may trust much more it's predicted belief and this is this this the common gain K computes exactly this weighted sum so this term over here was the uncertainty of my sensor observation so if I'm very very uncertain about what I'm going to see this is very very large we invert this this sum of two elements where one element is extremely large so this term will be extremely small so if you haven't high uncertainty in our sense observations K will be very small as kind of a small weighting factor and if you then look to the to the next line what happens the the new mean is the predicted mean plus the Kalman gain and the difference between what we observe and what we expect to observe so this kind of the difference between the obtained observation and the predicted observation and if the Kalman gain is close to zero the robot will simply stay with its predicted belief the common gain is large it will lead to an update of the of the new mean estimate which moves towards the sense observation okay and then the next the step down here is just an update of the covariance matrix taking into account again the common gain the Jacobian of the observation function and the previous of the covariance matrix of the predictive belief it was the common filter algorithm as we introduced it last time now I want to take that algorithm and say how do we need to what do we need to do with this algorithm in order to solve the slam problem so how does G look like how does H look like and if there anything else we actually need to take into account when we do that we first need to start what as our state space look like so just a question to you what does the state space look like so what's the dimensionality of our mean what goes in there any eight years what are going to what what do we want to estimate okay so X Y and theta will definitely be part of our state's bed state space what else exactly the position of the length marks so it's kind of a little potentially very large a vector that the first three dimension described describe the pose of the robot the remaining dimensions describe the XY locations of our landmarks okay that's exactly what we do that is only for the two if the robot lives in 2d plane if the robot lives in a 3d world we will have a six dimensional vector for representing the robots pose because we have three angular rotations or three rotations and the landmark positions are then quite likely to be three-dimensional as well but for the 2d case we have a three dimensional robot pose and two dimensional Network locations okay so that's the first three dimension of the robots pose the fourth and fifth one are landmark one and so on until we end up with our in landmarks so if we look into our state vector and our covariance matrix they will look something like this so we have a three plus two n dimensional Gaussian distribution if n is the number of landmarks so we're here the pose of the robot with the corresponding covariance matrix we have the uncertainty pub landmarks and that correlations as well as here the correlations between the robots pose and the location of landmarks and this is what we're trying to estimate so sometimes if I invokes it's written in this way sometimes people group the landmarks and the robots pose together and you find kind of this kind of representation which is exactly the same except the landmarks are now M 1 to M and and not the individual dimensions split up or sometimes if you write it even more compactly you find it written in this way where all these these all guys are vectors and these are matrices the only thing you need to take care that sometimes the notations a little bit sloppy so this guy is called X but it's really not only the the X dimension of the robots poses X Y Theta so you have to be a little bit flexible with this X over here but it should be actually kind of clear from the context what I refer to in most papers or books actually find it often written in that way as well because otherwise you have too many indices if you haven't time index and next for the robot and all these things so people tend to neglect that so don't get confused if you read something like this then this is to be clear three-dimensional vector okay now we have to go take this representation go through the individual steps of our common filter we will first do it in kind of an abstract way just as using a small example and then we will do this more concretely deriving the individual functions so how does our function G look like how does our function H look like and we'll really look into the details so that in the end you should be able to actually implement the extended Kalman filter for this lamp problem which will be the exercise for next week in next week's week actually okay so let's start what are the key elements of the carbon filter cycle so the first thing we do is we do our state prediction we take the control and estimate the new position of the robot given the control the next thing we need to do we need to compute our predicted measurement this is evaluating our function age at the predicted mean this tells us what are we expected to observe given the best estimate that we have for robot so given the the belief where the robot is we can compute an expected observation well we then need to take me techne to take our real measurement then we have to make a data Association that's saying to which length mark is what I'm currently seeing actually corresponding to and we need to compute the difference between the expected observation and they obtained observation and this leads then to the update step or the correction step so these three steps are kind of sub steps which belong to the correction step or two two four two two five actually it's a correction step and just split up differently because these individual steps will lead to different effects on the filter so the first things we do with the robot was over here we do our state prediction for the road let's say the robot moves here and it's uncertainty is illustrated by this red ellipse and then in this case the ellipse increased and that's the new state where in which the robot currently is so as I said before the the motion command only changes the pose of the robot it does not change location of landmarks in our case assuming that the robot doesn't modify the environment so it bumps into a like market moves landmark away that may be taken into account but in the basic setting the robot is assumed to change its own state but it's not supposed to was not assumed that the robot can actually modify the environment with its commands so the only thing that changes in the state vector is actually an update of the first three dimensions because the robot moves forward standard motion equations are used to compute where the system is likely to end up if you look to the covariance matrix which entries are updated this actually holds for the uncertainty of the robot itself but also to the correlation between the robots pose and landmarks because while the robot moves the correlations between the robots poles and white mark poles actually changes so if we need to update this part of the state space what's the computational complexity that this operation will require any idea so how many entries need to be updated given this operation why do you think it's N squared here so we have n line marks so this is an N by n matrix that's absolutely right but yes I mean so we estimate this block these blocks over here these blocks stay unchanged here so it's linear in the net number of landmark operations of course all these guys are two by two blocks so we have two times four is eight so these are two by two blocks and then we have eight times number of landmarks and this is a three by three block which means to be updated so the prediction step is linear in the number of landmarks so whenever we update it whenever we execute in your motion command I need to do a number of operations that is linear in the in the number of landmarks that I have let's kind of efficient of course it would be better if it would be just constant time operation the courser dimensionality of the state of the robot is constant but given to the correlations between the robots posting landmarks posts we need to update linear number of blocks in our covariance matrix and this leads to linear complexity of this operation okay so the next thing we do is the Robert seems to be here this is the light mark which is already stored in its map so what it does it needs to make a compute the predicted observation so it takes the current state the best estimate it has and says given that the robot is here what kind of observation should I obtain and the observation says hey you should obtain this landmark over here so we can compute a predicted measurement which looks like this this dashed line and this covariance matrix for the predicted observation that's what the rogue assumes to measure given the best of its current knowledge okay okay so the next step we do is we need to consider the real measurement that we obtained in this case the real measurement looks like this so it's little bit shifted to the left hand side this is dotted line over here this is what the robot actually perceived in reality the word the robot now needs to do it needs to make a data Association by saying - which landmark does this observation actually belong to so far we had only one night mark so that's trivial and it needs to compute the difference between those landmarks what is exactly this step over here let's what they predicted observation this was the obtained observation and this line here it's a difference between both of them and based on this difference we can then update our mean vector and it leads to an update via the common gain of the covariance matrix in this case the overall matrix changes and possibly all estimates of the robots posters and the landmarks change so what's the complexity of this operation yeah so it's quadratic in the number of landmarks so a sensor update is a really expensive operation if my state space state space is large and we've met something which haven't shown in here about which we briefed or very shortly discussed last week the carbon filter has this when computing the common gain it needs to invert a matrix and this matrix has a dimensionality of my observations so the the state space of the erm of the of the observation the dimensionally of the observation vector and so it's actually cubic complexity in in this dimensionality but typically the robot observer has only a very local field of view so it observes whatever whatever is in with its 100 meter range and therefore the complexity of the that results from the measurement from taking to account dimensionality of the measurement vector is small compared to the number of landmarks s as let's say a large scale mapping problem in reality has if it would have a sensor that it can observe the whole state space can observe all landmarks except at the same time and also the correlations between those landmarks that we kind of need to treat the observation at the observing every single link mark at the same time then this operation will be more costly due to this inversion but if we can neglect this inversion absolutely right this is a quadratic operation so the worst case it's quadratic what happens here okay this was kind of the abstract example now let's get more concrete and let's realize an implementation of the extended Kalman filter for addressing the slime problem with all its details so we assume with a robot that moves in the 2d plane so as before X Y and theta that describes the pose of the robot and we have point landmarks that we can observe so we have an XY position for every landmark and we assume to have a velocity based motion model so we assume we don't have a dhama tree we take pure velocity commands translational velocity rotational velocity which we sent to the robot's motors and we have a range bearing sensor something like a laser rangefinder so it tells us the bearing information and the distance to the landmark that we can in some way extract from our laser range scanner or from our sensor information and we also assume we have known data associations so whenever we see a landmark we know to which landmark this corresponds in our map and we assume to know also the number of landmarks which are in the environment so n is known before it okay how do we initialize our problem house line problem given this information how would initialize your mean vector ano covariance matrix um that's not perfectly true so if we perfectly know the state covariance matrix should be zero matrix course we there's no uncertainty perfectly now it's kind of Dirac distribution in the state of the robot you said if we know the position of the robot so the nice thing about a mapping problem typically is that it's a relative problem so the robot observes has relative observations two landmarks so we can actually define our own reference frame and the easiest way to do that is whenever we switch on the robot we say let's zero zero zero that's the kind of volt reference frame for the robot and we just start from that so the mean first three dimensions of the mean vector would be zero zero zero and the three by three covariance matrix describing the uncertainty about the robot state should be a zero matrix that's really right what about landmarks so we don't know anything about that Mars yeah yeah we assume we have already so we need to build up our matrix and we say we know we have n landmarks so we need to somehow want to add them to our states by state vector directly that's exactly true so we can choose any position to pick D ro 0 0 0 as well and then we simply add an infinite uncertainty on this diagonal so we are bassoon no they are not correlated in the beginning because we don't have any information so these blocks these blocks here are 0 but there's an infinite uncertainty on the day so this would be one possible initialization for the slam problem yes please yes or no so the thing is that in reality you would do that differently you would kind of grow your matrix on the fly and start with a small matrix the problem is that then writing this down in an algorithm is a little bit more difficult and it's already not trivial to present that here on the slides without doing a lot of derivation therefore I kind of left that out but in practice it's absolutely right he would start with just the 3x3 covariance matrix and whenever you see a landmark you would grow in your state space that's the way you would typically do that but kind of just in writing down the formulas it gets a little bit more tricky and the other thing is is this really limitation from what you can compute it is not really limitation because you because you could simply send ads to very very large number like an upper bound and so they simply will in the end be landmarks which still have the initial state of an infinite uncertainty and you could simply say ok these are simply landmarks I have not seen of course this adds computational complexity and maybe some numerical issues that's also true which I will discuss later on because these infinite values here lead to some issues but in general if you don't care about numerical issues and you don't care about complexity or time complexity space complexity it gives should give you the same solution any further questions okay so that's what our initial estimate looks like now simply let's simply go through the individual steps of the Kalman filter algorithm and kind of expect them in detail how all this looks like so we start with the the first flower the line number two here and you say okay our new mean estimate is some function G which takes as input the the old mean and the control/command so we are now with the step kind of T equals one so this would be T equals zero so that's exactly our vector full of zeros which goes in there and the first odometry command that we put in and we want to estimate the new mean in here we said we want to do the velocity based motion model so we talked about velocity based motion model in the second lecture that's one way to describe that so the new pose X prime Y prime theta prime is the old pose plus this expression over here and the assumption was here that we have we think the velocity command which is a translation velocity and rotational velocity to the robot and it executes these velocities for a short amount of time and until we send a new motion command so during this motion that would actually moves on a circular arc if the rotational velocity is not zero this is the case for the rotation velocity Omega is not equal to zero and then this all the thing is simply our nonlinear function G only for the first three dimensions taking to account the odometry command UT which is a translation velocity and the rotational velocity and the old state X Y theta so why did I put these in indices X Y theta here to our function G if we go back to our algorithm the input should be the full state vector and the output should also be the full state vector this guy here takes into account only the first three therefore it has this index to distinguish that so if we if we did this we defined this function how do we map that into this 2n plus 3 dimensional space of course we kind of we have our 3 by 3 or our operation which D which updates a 3 by 3 vector it's exactly what happens here but now we have to do this in this high dimensional space and we can simply take a huge matrix which mainly consists of zeros and a few and small at 3 by 3 identity matrix to map this update step of the state space so that it only affects the first three dimensions and they're all the other dimensions are 0 this can be done quite easily we just define this matrix F X T so this has is has here three add two and plus three dimensions and here three dimensions this is the identity matrix and these are all zeros so this is the two n columns of zeros if you multiply this matrix transposed with this matrix we will get we obtain a matrix vector where the first three dimensions are exactly these dimensions that all others are 0 and then this guy here is our nonlinear function G so but this function G does it takes the current state space updates the fruit three dimensions and leave all the other dimensions untouched okay and this is exactly what the function G should do it's a non linear update step of the current pose of the robot given in an elementary command all only the pose is updated of the robot the landmark locations are not updated ok ok so the first line is done perfect complete let's look to the next one we have this so we know this guy we know this guy this is just the kind of the uncertainty of our motion model which is C we assume to be given so we need to compute the Jacobian of this matrix okay so we need to compute the Jacobian of our small func function small G and the thing is this update only effects we said before the position of the robot so the function G does not update the the the position of the landmarks so if we go here to a function this is XY theta like mark 1 or 2 like Mark 3 Network for if we derive this vector according to the individual nth marks we'll just get a 1 for the corresponding dimension in zero anywhere else so this will lead to an identity matrix in this lower matrix here because simply we don't change the position of the landmark in the update step so they're completely untouched the only thing that changes is this block over here which is a 3 by 3 matrix which tells me how does V so I need to derive the nonlinear function which Maps my odometry information into a state update this was exactly our audiometry motion model so just looking now to this 3 by 3 matrix over here which looks exactly like this so it's a partial derivative of this 3 dimensional function whose write back to X Y and theta so ok derive this vector the building the partial derivatives and this is the same explanation before gives me an identity matrix curses just simple linear function with X Y P the identity so if I derive that I will have this identity matrix over here and I need to derive this block over here and if I derive these equations here with respect to x and y actually see that there is no dependency of x and y in there so the the to call the first two columns of this block part of the Jacobian will be zeros because there's no dependency in egg and no dependency in Y in this update equation right okay so this leads to these two blocks of zeros here but there is the heading theta in here and if you derive this with respect to theta we'll end up this equation if you derive this one with respect to Sita we end up with this equation and there's we drive this one with respect to theta it gives us zero again so this is my Jacobian my function G so if I write it down it looks like this I have the identity so the identity plus this block so an identity matrix here along this line and though the only two elements which are not the identity matrix are these two blocks over here okay so now I have computed G X T and if I put that together so I have this plot so I kind of have an identity matrix all through this matrix and then two elements which are nonzero this will be takes to account linearizes the dependency of the sine and cosine functions of the heading therefore these blocks these are nonlinear functions and therefore these along these two elements are nonzero if they would be linear functions that should just be an identity matrix okay so now we we have we know G we know the previous uncertainty we know R so we can actually expand that and write that down so this is G TX so this is what's the matrix just computed identity matrix here zero zero times the previous estimate at time zero times this matrix transpose so this mate here this is the transpose only this block needs to be transposed because the identity identity transposed is the identity and plus RT then we end up with this equation and in this equation you can perfectly see which parts of the matrix are updated what I've shown you in the beginning way we had an update of the robots the entries of the covariance matrix that represent the robots pose and the correlations between the robots pose and the landmarks so this first element needs to be updated so this the original uncertainty in X X needs to have the has these matrix multiplications here so this is an update in here and these two blocks also get updated so GT times the croatians between the the quarians entries relating the robots pose and the landmarks and this block here and this is a really large block in reality in slam this is not touched so if you do this operations if you do that in an implementation you wouldn't do it exactly in this way because then you would add a high computational complexity you would update these blocks individually in your matrix because the large block is untouched right okay just as kind of an implementational okay so then friction step completed we know how to do it we are all happy now let's summarize the prediction step of the Kalman filter in an algorithm what do we do we first define our matrix which maps us from the low dimensional space to the high dimensional space it was FX just these identity the rest all zeros and then I can write the update of the mean as then the predicted mean is the old mean plus this function f times the this part of the nonlinear function which updates my my state given the translation velocity and rotation velocity that's exactly what we had before and then we need to compute our Jacobian G which is the identity plus this matrix F plus times this exactly this block of the elements which are changed which are nonzero in this in this in the derivative of nonlinear function and then we can update our state vector the only thing which I kind of haven't explained before is to compute RT RT is again the co where the change in the covariance for the whole state space but if the robot moves it only changes the uncertainty in the three by three blocks relating the pose and doesn't change the uncertainty of the landmarks so we can take the three by three matrix and again with this thunking map it to this high dimensional space where only the first three by three block contains your matrix R and the rest is all zero okay so that's the full implementation of the friction step which you can write down an octave MATLAB or whatever you want checked in this way so we just need to follow exactly this algorithm we can apply all the steps we're done like to make a short break here and ask you is there anything which is unclear about the prediction step then let me know now anything else I should explain again which wasn't clear he would like to know everything find who things who can now sit down implement this algorithm okay at least some of you that's good especially as I said the on the exercise sheet which will give out next week that will be the the exercise implement a full EKF based slam system so really the individual steps which happens here just to summarize them is writing down your state update function G which is just not only our function which describes how do we end up in the next state give me no the current state and our motion command the first thing we did the next thing we did is we just computed the first derivative of this function Jacobian so there's a partial derivatives of the individual vector valued functions with respect to the individual variables it's kind of the generalization of the first derivative you all know from 1d to high dimensional space and then we just followed exactly these update equations in this weird function f or you may found weird here just it's just used here because we are most of the operations done in a low dimensional space we need to map it to a high dimensional space but this kind of is just kind of taking this matrix has the three by three blocks and filling everything else with zero so there's no black magic behind this function f yes please that is true so if you want if you implement that with MATLAB octave you can do that much smarter than because you can individual you can explicitly modify blocks of matrices but that's kind of if you if you want to write if you want to use exactly the equations which are in the algorithms you have to write in this way to mapping this to the high dimensional space so why this isn't here this function of this matrix F is used in here to show you the direct correspondences between the row common filter equations and what we do here it means it's exactly the same thing with the update down here in reality you wouldn't do it in this way you would update those blocks individually by just modifying a sub block of this matrix because this is more efficient but kind of in order to see the course pundants between what we do and the original equations of the Kalman filter I found quite helpful to introduce this function so don't get don't get scared of this function f it's really just to take into account the different dimensions of our state space any further questions about the prediction step ok so then let's move to the update step over here the first thing we need to do which is kind of the problem specific thing is the observation function H and then it's Jacobian H over here so we need to compute these elements and we have to make a few clarify a few things before how do our observations look like how is the data Association given so we assume as I said before we have no data Association so for every landmark we observe we note which landmark does this one correspond in our map so obviously landmark we say this length this this feature I see in my observations correspond to landmark number whatever J and we do that with this correspondence variable so the eyes measure measurement taken at time T corresponds to the landmark J so I say what is the landmark that I see at time T let's say beam I of the laser scan says oh that's tree so that's kind of that's what we assume to be given how we obtain that it's a completely different problem there's actually a non trivial problem because there's a large number of possible data associations that you can do in there the data Association tree grows very very quickly going to track all your data associations that the reason why most people just do a so called nearest neighbor data Association they just say what do I see what's the best fit in my map if this fit is very close I accept the fit otherwise I just say let's ignore that mark it's kind of the standard strategy that the 80% of all slam systems do just nearest neighbor data Association so we assume here this problem is solved because it's kind of adds another complexity to our problem we say ok data Association is given that's what we assume so the first thing we need to do is initialize the lent market the net rent mug has never been observed we need to compute the expected observation you to know what we should measure we need to compute the Jacobian of H of our measurement function and we need to proceed with computing the Kalman gain and then the update step F in the algorithm so these are the next things we want to do let's first look into how does an observation look like so we said we have a range bearing observation so an observation that is a distance to the landmark and the bearing to the landmark so if I'm here and my landmarks are tripods with cameras this guy over here I would say okay this camera there is three meters away from me and I see that at an orientation or whatever minus twenty degrees that's what this block looked like so before every landmark that we observe we get this tuple of a distance information and an orientation in which we see landmark relative to the headings of the robot okay how can we now compute the location of the landmark that's actually quite easy we say the the positions where we observe the landmark where we predict the landmark to be is simply the position of the robot and then the distance in which I see it times cosine in which in the direction of the the heading plus the heading of the robot so the robot is that whatever 90 degrees if this is heading zero this is 90 degrees it's the X would be cosine of 90 degrees plus the heading so let's say three degrees in which I see the tripod over here and then times three meters and in this way I can compute the location of the landmark given the single observation and given the crude estimate where the robot is okay so given this this tuple you all now can compute the predicted location of a landmark so the location of the landmark given the measurement was correct and given that the robot was at the right position okay okay now let's look into the expected observation the expected observation was given we know the state of the world we know where the landmarks are where the robot is we need to compute a predicted observation and then what we what the common filter does it compares the predicted observations what I should observe give them a Crone estimate between what I observe to compute an update on my state so we need to compute the expected observation so two variables Delta and Q which I use here so Delta X Delta Y it's just a difference in X between the robots pose and the landmark so let's just whatever this is my x-axis y-axis I'm standing at 0 0 0 and observe the landmark over there it's just the different than in my X location the first dimension then the difference in Y is the second dimension so this Delta is just the difference in the x and y position in the difference between the so it's a difference between the position of the robot and the position of the landmark in x and y so if I compute Delta transpose Delta i exactly obtain the Euclidean this every squared Euclidean distance squared distance between the robot and a landmark that clear so if I take this elements that's dot product with the same elements it's these guys squared plus these guys squared so Cu squared distance ok so as a result of that the predicted observation that head is simply the square root of Q this was kind of first about the range the distance between the robots pose and the landmark so simply square root of Q and the orientation is simply the the aten faction taking account the difference in Y and the difference in X so it's kind of the what's the angle between the coordinate system where the robot currently is to the landmark and then I have to subtract the heading of the robot beakers if this is the orientation 0 in my coordinate system that will be kind of whatever 80 degrees something like this and it but if the robot is already looking in this direction I have to subtract this let's say 45 degrees so I end up with 35 degrees in this case so I obtain a distance and an orientation in which I should observe my landmark it's exactly what this is and this is exactly my function age given the current predicted pose of the robot so this function here is this magic measurement function which Maps the state to an predicted observation essentially not that complicated ok so we have to find our function H that's great now all we have to compute the Jacobian so this is just what everything which was written on the slide before and now we need to compute the Jacobian which is the partial derivative of this function with respect to the individual dimensions of our state vector now the state vector here is again 3 + 2 + 3 + 2 n dimensions so this will turn into a ver very huge matrix where basically everything is 0 except the pose of the robot the elements related to the pose of the robot and the elements related to the position of that landmark therefore I call this H low over here which just refers to the nonzero dimension so this low dimensional space in this only effects are the elements which where the derivative versus towards X Y and theta so the position of the robot and towards the location of the landmark that I'm taking into account because everything else I know is zero okay so just keep that in mind so so this guy here will be a two by five dimensional vector - because the function H which is this function between its two dimensions and data drive it's with respect to five different parameters so I take for the my matrix here the first element so the first line is always the first element of this function towards a drive towards X towards Y theta and the positions of the landmarks and down here it's a second line of this vector over here after X Y theta and so on let's look just as an example into this first block over here the other C will experience in the exercise sheet which are flying over here and see how do we compute this guy over here again that's actually not too complicated because it's just the application of the chain rule for computing a derivative so we need to derive square root of Q with respect to X so Q square root of Q this guy and it's just the the individual elements as the scalar product Obed so if you have just as a reminder in case that it's been a long time ago if you have the square root over function whatever square root of x and you want to derive this with respect to X you can write this as d - DX of X to the power 1/2 and so this 1/2 times 1 divided by square of X that's the first derivative just as a short reminder this is what we will use now so this is exactly the first block over here point 5 times 1 divided by square root of Q and then this is kind of the outer part of the first derivative and then we have to look into the inner part of the first derivative which is if you write down something like we have is a minus x squared plus b minus y square to this a B adjust two constant values you derive this with respect to X this term is zero and this will turn into two times a minus x times minus one for the game the inner first derivative so we have two times a minus x times minus 1 and this is 2 times a minus X is just the first dimension of this guy so it's Delta x times minus 1 so it's just twice applying the chain rule for computing the first derivative and then you can just rewrite this also in this form it's just multiplying by so this two can be eliminated here the 1/2 times 2 and then we have 1 divided by Q and then multiplied with Q here gives minus square root of Q DX and you can do this for all the individual elements this matrix and then you will end up with the Jacobian which looks like this so we computed this now for this block over here all the others are exactly the same elements derived with respect to Y you can see the symmetry in here let's respect to theta because there's no orientation coming up here so at 0 and respect to the landmark positions that's just what we obtained just by deriving this function with respect to X Y theta and the position of the landmarks and we can do the same block down here but that's something you will do in the exercise so we now have our kabuto Jacobian in this only in the dimension which are nonzero so just kind of only the dimensions that matter here turn on 0 now we need to kind of map that back to the high dimensional space that we again do with one of those mapping functions F which is now slightly different as was before so it has the identity here in the 3 by 3 block so it Maps the first three dimensions to our - this higher up to the three-dimensional space and then it takes the observed landmark which is just kind of the eye that the J's landmark over here has also won one in here so these elements are mapped into the these blocks over here and all the rest is zero so these are all the landmarks one - J minus one let's let my JB observe so at once here and then all the other networks again have zeros in here and then we can again use our Jacobian H that we compute a times this matrix and this will simply map these low dimensional matrix into our high dimensional space which is the complete three plus two n dimensional space and that's all the magic for computing H so we now have our predicted covariance it was exactly the guy we computed already H become just computed it H again again again and plus Q which is just a matrix which tells me how certain my sensors so something which depends on the specification of my sensor nothing I'm currently taking to account here so perfect done I can completely common gain once I can compute the common gain I can exactly compute this step over here because I know H we just just told you how to compute it we have our obtained observation the common gain so everything is straightforward so everything can just apply the basic formulas and we're done again you can now do the now great big form the correction step so we define our matrix Q which is the uncertainty of the sensor so we assume we have an uncertainty in the range reading and uncertainty in the orientation and then for all observed features that we observe at one point in time we say okay J is exactly so J is kind of the index of the feature we have observed if we have never seen a landmark that's kind of one special case we need to initialize this landmark otherwise computing the predicted observation is completely flawed again we have this uncertainty that the uncertainty is infinite for the effect of this predicted observation we'll be basically not taking into account but the problem that I have in here is when I compute the linearization this this H the first derivative you may remember last we have shown that the bigger the uncertainty of the matrix you fit through the up through the linear approximation of the function the bigger the mistake is and if you infinite covariance it's kind of something suboptimal therefore you treat the initial the initialization typically differently and you just initialize landmark with the first observation that's exactly what happens here so the landmark has never been seen I initialize the landmark based on the position of the robot and my forward equations for my former observations so mapping the observation into the state space and then I continue just as we said before we compute this Delta we compute Q we compute the predicted observation and then we have this big book this F matrix we compute our jacobians age exactly the way we did that use this F matrix to map it to the high dimensional space and then we just out of the box apply our our the common filter update equations and that's it so if I observed a landmark the first time I said I obviously I put it I initialize landmark with my with my physical observation of the physical observation that I got as a result the difference between the predicted observation the observation obtained reality is is zero because I initialized it that way and therefore whatever the Kalman gain is I stick with my predicted belief it makes sense so if I have a predicted belief I observe the landmark for first time this landmark observation if I don't have any information about the landmark doesn't help me to improve my post estimate but exactly what happens if I initialize the landmark with the predicted observation there is no update its kind of the kind of the trick that is done in here because then this value is zero and then I stick with my old predicted belief there is no update in the mean estimate if the predicted observation is exactly the obtained observation okay so that's done we're done a few notes if you want to implement that so if you perform the measurement update in a single step so this is kind of here we do this operation here with a for loop so for all lengths for all observed features we carry out this loop over here you can also do this in just one shot just take all the observations at the same point and compute one H matrix which has then more elements which are nonzero you can do that in exactly the same way and then you can do this everything was just one update step you don't have to iterate over the landmarks it has some advantages and some disadvantages the disadvantage is that your dimensionality changes of the four inverting this matrix it can be suboptimal and the other thing which changes is you can additionally take into account all the correlations between those landmarks in the same observation if the observation relates your landmarks in a certain way but it's kind of just as a side note the other important thing is you should always normalize your angular components if you're dealing with angles and your angles accumulate the be away of the wraparound especially if you compare the expected observation and the obtained observation there's a wraparound and run along the angle so and if you're around PI the wraparound actually say ah this observation is whatever 359 degrees away but in reality it's just one degree because of the wraparound so make sure you always normalize your angles otherwise you get really really weird effects due to this non-linearity two days just wrap around in the angular component and as discussed before it was raised before if you use something like octave or MATLAB you typically don't need to create your F matrices you just change the corresponding blocks in your matrix in place and you don't and this makes it much more efficient and also easier to implement so that's basically it that's kind of EKF slam from the basics to running algorithm the only thing is zoomed here is you have known data associations and that's it here we go that's how the simplest version vkf baseline works yes please no no I I so the so after the update step you have the uncertainty of the of the sense observation q plus the uncertainty of the robots pose but this comes automatically out of these equations which the update comes into the game here in the year in this update so the thing is really that the thing was that initially we said it as an infinite uncertainty if an infinite uncertainty and you take one observation all you knows the information from this single observation so after that it will have the certainty of the robot head when observing the white mark plus the uncertainty of the sensor so the kind of that that's that's true so it's a combination of two things the first thing is your linearization point but if you initialize it already at the best possible position then you have the good linearization point and then only the uncertainty of the robot itself matters so the problem is if you have it depends how far you are away from Europe so the thing was the the more nonlinear function is and the further you are away from the linearization point the birth the approximation that you have and if you initially the landmark at the zero zero zero position and if you put in here the right position that's kind of one way to get rid of this effect okay anything else so far okay perfect so then I would like to continue to tell you a little bit of about the properties of EKF slam and some things which may not be may not be directly obvious the first thing I would like to talk about is loop closing what is loop closing it's something which is pretty important thing in slam loop closing means that after let's say a long traversal through unknown part of the environment the robot revisits a known area and recognizes this for the robot comes back to a place that has been before and said here see those features which are exactly those features I have seen one hour ago and this this is kind of a data Association step and are very high uncertainty and one has to be careful with ambiguities in the environment or symmetric environments so if he for example will drive through this floor over here this room over here and I would observe this room though may say perfect i perfectly recognize this room at the roomba have been an hour ago but it may be that the robot has been in the neighboring room which looks exactly the same or in the room downstairs which looks exactly the same so loop closing always had the issue that there may be the robot revisits has revisited the previous place but has been or the environment is symmetric so it's actually a non-trivial decision to say the robot call close the loop or not but in our implementation here we assume we have known data Association and this also handles the loop closing for us but in practice at the non-trivial problem and one thing to observe is that typically if you close the loop the uncertainties collapse so they get dramatically smaller in an update step because the reason for this is that typically when the robot drive through the environment let's say takes a very very long tool through the environment so it's uncertainty grows and grows and grows and grows over time due to the motion uncertainty when it revisits a place at this place the motion the uncertainty was typically much smaller because a long time ago in history and then through the through the correlations between the previous pose and the light marks down here which then have also been accurately estimated and the the loop closing this uncertainty reduction can be propagated kind of through the other side of the loop as well you can see this may be an in small example so there what was starting over here or just going down down down driving along this path and these crosses here are landmarks and the ellipses here are the uncertainty the 2d uncertainty of that landmark and this before the loop close the road started here was moving down here here going up here is now here and the next step closes the loop so then it reabsorbed this landmark this lent my mother's perfectly known and so this reduction uncertainty will be propagated through the loop in this way and as a result you will see that the light marks which are down here which are kind of furthest away from the loop closing point will have the highest uncertainty but the uncertainty here is dramatically reduced so if you update that step it will actually look like this you can see here that you here they are certainly all shrunk now we have the larger sense certainties down here because just because we are knowing those that Max position that well and the robots pose and the landmark locations are correlated and if there will then observe the landmarks this leads to an reduction of uncertainty along that loop so whatever you calls a loop this is a great chance to reduce your uncertainty and the uncertainty reduces in this way through the correlations between the robots post and the landmarks okay so um as I said before the loop closing reduces the uncertainty in the robots post as well as in the landmark locations and the main thing one needs to take care of is once one doubt the wrong loop closure that means a wrong data cessation that typically leads to filter divergence because the estimate will be dramatically wrong the mean estimate is likely to be wrong the uncertainty estimate is likely to be wrong because one simply kind of deform of the environment because there are what things it is a certain place but in reality it's somewhere else so adding a wrong loop closure or accepting a wrong loop closure something very critical especially in these kinds of slam algorithms they are the algorithms which we will learn around Christmas or after Christmas which are more robust to this problem of having wrong data sensations but here that's kind of that kind of kills your filter so accepting a wrong loop closure is something that you really really want to avoid and kind of the the fact that loop closing reduces uncertainties sometimes actively used in for example exploration approaches or active approaches where the system tries tries to explicitly find loop closures to reduce the uncertainty in its belief but as we address it here at the moment we just say someone steers the robot just get the incoming data and just make our decisions this is a loop closure yes or no and then update the filter so loop closing is great we should try to find loop closures dramatically helps us to reduce uncertainty but we need to be sure that this is the right loop closure it's kind of a take-home message from that the next thing is that in the limits of the robot drives over and over through this environment all the landmarks would be fully correlated if all the correlations robot drive through the environment and will add new correlations between the robots pose and the landmarks you see here this is the position of the robot these are all the landmarks and the kind of is a graph showing the correlation so you see some landmarks are stronger correlated than others but in the limit all of those landmarks become correlated you can actually see that in an example so this is this is a map so these are landmark locations over here the blue dots and this is the end we associated to the landmark that's robots and this is the initial correlation matrix and so you have kind of these are all the unobserved landmarks all the rest is zero so this kind of the darker the well you'll be higher the entries in the covariance matrix it's just normalized over here the robot drives around maps environment you can see there are a couple of landmarks which have not been observed this corresponds to this wide area over here and the others have been observed you kind of feed start seeing certain patterns here in this correlation matrix and if you continue all the landmarks get correlated you can see that here but you see this kind of a little bit weird checkerboard pattern what does this checkerboard pattern mean any ideas yeah yes so there's a strong correlation between the expositions of the landmarks and between the Y positions of the landmark but not between x and y it's kind of if I fix X if I fix the X location of one landmark so if someone helped me hi this landmark here is at X location x equals 10 meters I perfectly know where all the expectations of all other landmarks but it doesn't tell me anything about the Y locations of these landmarks that's exactly the case and one important thing is that it's quite known since since quite a while actually since 97 that the correlations between the robots pose and the landmarks are important and cannot be ignored if you ignore them you can show that quite quickly you get a two optimistic estimate about the uncertainties of your filter so it will be overly confident about what you what you know about the world so any approach which ignores the correlations between the robots pose and the let mark locations doesn't handle them properly it's very likely to fail the next thing is that you can see that the uncertainty of the light mark locations decreases over time the more measurements you get so it has the maximum value that it has is edits initialization and then you start observing the landmark observing observing observing so you get more certain so the uncertainty decreases monotonically this is just an example here for a couple of landmarks so this is when you see them the first time so this is kind of the initialization of the landmark and you see two additional observations the the all the uncertainties decrease over here so it's just a plotted the uncertainty over time and it's just just an example so the maximum uncertainty allowed mark has a landmark in the beginning when you initialize it and then more the more the more of new measured landmark the more certain you get about that Denmark and next important relevant thing is that in the limit so if you continue driving and driving and driving and driving you cannot get infinitively a certain the lower bound of the uncertainty estimate that you may that you can reach is the initial uncertainty of the vehicle so if you if you start somewhere and when the robot took the first of the vation it had let's say is an initial uncertainty doing for the first observation that it takes all future observations that it takes of landmarks and estimating the position of the landmarks can never be smaller than its initial uncertainty so the robot starts with zero uncertainty because we fix a coordinate frame there that's fine but if let's say we fix it here there what drives for a few meters has certain uncertainty accumulated takes the first observation we can actually show that none of the estimates of the uncertainty will actually go below that initial uncertainty of course you can't just by observing something you can't get more certain than the initial uncertainty that the system had if there's no additional information so no nothing which relates your previous poses or gives you with with new observations or no GPS which kind of gives you a ground on an estimate right to an external frame which considers as an additional observation but as soon as you observe landmarks you can't get more certain than your initial uncertainty and finally I would like to show a few very let's say famous data sets which are used for slamming EKF slam so what you see here is a vehicle which has a laser rangefinder he has installed in front of the car that's at the Victoria park at University of Sydney and this was one of the first kind of large-scale data sets that have been recorded and used in the community and basically I don't say every slime paper which does landmark slam shows the Victoria Park dataset but really substantial amount so this is kind of a video on top of the vehicle the people driving here through Victoria Park and they only observe those trees so they have a a detector on them in delay the range scan which basically fits a circle into endpoints and then say okay that's a landmark and you may do data stations based on the width of the trunk or the radius of the trunk of the tree and so they just drive through Victoria Park and map those trees and so the final map is the location of those trees you can see the trajectory these observations these are kind of tree estimates and this is a typical EKF estimate that you get and you can actually overlay that with a satellite image and you can see here these are the locations of the estimator of the tree this directory some trees some estimates here are far off that maybe that there was some something standing there which was identified as a tree or his new tree has been planted between the time when the satellite and or aerial image was taking and the location was taken there are some which are here simply off so these are probably the trunks of these two trees so you can see actually they are errors in the estimate but that's kind of one of those very very famous slam data sets other people tried to get kind of ground truth mat so that's always the challenging task how good is my math how well can I evaluate it because I'm doing mapping algorithm and I want to know how I could do the mapping algorithm so people tried to build up kind of ground truths or ground truths like data set so where you know where the light marks are located and so some people use motion capture systems but they're typically quite limited in the in the size of the environment and one interesting thing are kind of nice nice idea was done by don't let Matthew Walter at MIT they they have these tennis courts which are extremely accurately measured for these for this tournaments and they just used these things they're used for Hooten laughs I don't know have no idea what the English term for that is and so they also kind of normalized very quite accurately fabricated and they put it at special occasions that there was lines at the tennis courts at several tennis courts and then drove wrong with the robot so you get those features no quite accurately where they are it's definitely not ground truth but it's closer than most other data sets that one has and this is an example so that's the original trajectory that the robot took this is actually the estimate of the system here estimating these poles on the tennis courts it's a kind of tool to be later said especially the Victoria data set is really one you find in a large number of papers on EKF base slam okay a few words about the complexity set last week in generally the cubic complexity but the cubic complexity only depends on the dimensionality of the measurement so this is to begin not the limiting factor the limiting factor is the number of landmarks that this system can handle the prediction step can be done in linear time but the update step is quadratic in the in the number of landmarks and also the memory consumption is quadratic because I need to store my matrices and therefore I typically end up with a quadratic complexity in memory and in computational requirements and this is a problem for the EKF for building really large scale maps because if your the number of landmark grows and you building at a really large scale Maps with millions maybe millions of features this really becomes a limiting factor and the approach is actually not applicable anymore in practice so to summarize EKF time so it was kind of the first solution to the slam problem in the robotics community started in the in the 90s beginning of the 90s approximatively they have been conversion proofs for the gaussian linear case and it also has been shown that the filter is quite likely to diverge if the nonlinearities are large or substantial and the real world the real goal is nonlinear it always depends on your exact sensor setup on the motion of the vehicle how bad the effect is but yeah the world is simply nonlinear and B the linearization of the function may help but can be critical especially if you're the uncertainty is very large so the larger the uncertainty the bigger the effects of the linearization and the fact that the resulting distribution is actually not Gaussian any more but the EKF assumes them to be Gaussian one of the limitations is that the system cannot handle well ambiguities because the Gaussian distribution is just a single mode so it's not that we have a bimodal distribution we say the robot is either here or here and we are pretty certain it's either here or here in the world it's something you cannot model with the Kalman filter due to its limitations of handling only a single mode nevertheless the system has been successfully applied to let's say medium scale environments or environments where you can place landmarks certain landmarks which can perfectly be identified and you have certain guarantees on the curity of the system so for example the Sydney heart not of the Sydney Harbor but a harbor in Australia don't know exactly which harbour it is it's completely automated by these huge trends unloading ships and they all running the Kalman filter for localization it's highly engineered so they are splittin let marks placed in the environment but actually these these trucks and cranes are really really huge and they are operated completely without any operator and from Sydney so it's kind of four or five hundred kilometers away from the actual harbour and ever things done remotely by just monitoring whatever the common filter it's much more than a common filter but the localization system itself works on that Kalman filter so it has been used for a lot of relevant applications has been used in industrial setups it's a very successful tool and especially if you can control the environment in the sense that you can place landmarks you can guarantee a certain density of landmarks and things like this it becomes a really powerful technique to use and if you look to the research community kind of they are not that many EKF based system which are published now over the last years because there are other techniques but which will learn the course as well which seemed to offer advantages in a lot of a lot of problems but a big of for a long period of time one research problem for EKF basis then was how do we find good approximation so that I don't lose much and have a whatever linear time complexity or linear memory complexity or whatever n log n or their different variants on how we can actually do that and one of the approaches actually to not maintain a big huge map but maintain multiple local maps and then kind of only correct those local maps and then build attach those local map source to source local maps together so there's a large number of so-called sub mapping techniques which have been proposed all with the goal of reducing the computational problems that the EKF has for large-scale maps so whatever I presented here today actually find in chapter 10 of the progressive robotics book the notation is more or less exactly the same the only difference is that they introduced for landmarks not a two-dimensional end mark but a three-dimensional end mark where the three dimension the third dimension is a data Association so it kind of index but it kind of blows up these matrices even more so I kind of skip that so if you look into the book don't get confused that they let mark they have an additional dimension which is kind of the ID which kind of simplifies things maybe a little bit in the implementation but yeah what's email but what's even harder or more advanced to present here on the slides on the blackboard therefore I decided to kind of leave that out but the rest of the notation should be done in the same way as it was done in the probabilistic robotics book okay that's it from myself for today are there any questions so you should all feel comfortable with what you have seen here by next week you will get the new homework sheets and building EKF based system more parts of that so we will provide some infrastructure but implementing the key part of the algorithm here will be part of the homework assignment next week okay that's it for my shot thank you very much and we see each other next Monday thanks

Channel: Cyrill Stachniss

Views: 61,262

Keywords: robotics, Simultaneous Localization And Mapping, EKF

Id: XeWG5D71gC0

Length: 84min 36sec (5076 seconds)

Published: Mon Nov 04 2013