EKF-SLAM (Cyrill Stachniss)

good morning today i want to discuss the ekf slam so we want to tackle the problem of estimating the location of a platform as well as the map of the environment in an online approach and we want to use for that the extended kalman filter so we want to solve the simultaneous localization mapping problem um using a common filter and seeing how we can actually do this so this was kind of the first approach to slam which was out there developed in the late 80s and trying to use the tools from gaussian recursive estimation in order to solve the simultaneous localization mapping problem again as we have discussed before localization means building a map of the environment with a mobile sensor platform and at the same time localize a platform within the map built so far and if you do this together we refer to this as slam you can do this in an online or offline fashion so online slam or full slam and we will be looking into online slam here and look into again how to solve that with the recursive base filter namely the extended kalman filter that is relevant for large number of different application ranging from indoor application through navigation in urban environments with autonomous cars to undersea or underwater robotics underground exploration or space exploration and a lot of systems for which perform simultaneous localization and mapping on those more intelligent platforms use an extended common filter or an information filter which is a variant of the kalman filter or also a lot of approaches working in under c use a common filter based approach um common filter based approaches are not very popular today anymore for solving the slam problem because they have some disadvantages disadvantages especially with respect to computational complexity but they are still often used for if you only have a shorter map of the environment for example for performing um for building a map only from the last few hundred time steps and localized within this map in terms of visual odometry and it's one of the first approaches to slam and therefore it bursts understanding it diving it deeper deeper but also then understanding the limitations of those approaches and see how other approaches may be beneficially overcoming those limitations in terms of what we want to solve in slam we assume to have our control commands available so how was the platform moving through the environment based on controls such as go meter per second forward rotate by 30 degrees things like this our controls and we have our observations this can be can be a laser scanner observing the environment can be a camera then your the models for your observations will look different we are looking here just assuming a range bearing sensor so some sensor that tells me something about in which direction is an object and how far it is away so it could be a stereo camera or but could also be a laser range scanner where you extract that information from your sensor data if you want to want to have the map of the environment and the trajectory of my platform so the path so the map of the environment m could be a set of 3d points for example of those landmark locations or in the past could be xy that your pitch roll just as an example and we want to use a recursive base filter especially the extended common filter in order to solve this so we are interested in the online slam problem that means we don't want to estimate the full trajectory only the last pose the map of the environment and given the observations and my control commands and the kalman filter is a state estimation system assuming the linear gaussian world so assume all my models are linear all my uncertainties are gaussian and then it's an optimal estimator actually providing you with an equivalent solution to a least squares approach in reality however we don't live in a linear world we're living in a non-linear world therefore we need to linearize and that's actually what the extended kalman filter then is doing but still assuming um gaussian noise and gaussian uncertainties in here so in terms of our graphical model so we have this graphical model over here we always want to estimate the current location where we are and the map of the environment and we are not interested in the past observations anymore that means we are marginalizing them out we are forgetting about where we have been before of course we can just store the information on this but we are not updating our estimates anymore so this can for example be relevant if you drive through the environment and some point in time come back to a previously known location something that we refer to as a loop closure then the the loop closure will actually reduce the uncertainty also of all past poses where you've been before but that's something that the common filter is ignoring because it says i don't care that i need to can update where i was in the past i'm only interested where i am right now i forget about the past locations and therefore the system the ekf would not update the previous poses because it's removed them from its state vector although it could and this is something that is typically an open for online slam systems because they are only interested where i am right now because this impacts the decisions that a mobile robot does it may impact the navigation decision where you are but it doesn't impact where you have been 10 minutes ago and therefore that's something that is typically ignored okay so we're going to implement this using an extended comment filter so let's start look into the extended common filter formulation again this is one iteration of the extended kalman filter so we put in the estimate where we have been before in terms of a mean estimate and a covariance matrix this is those two quantities as well as the new control command and the new observation and what we want to have we want to estimate we want to update the mean and the covariance matrix so that the index advances from t minus one to t it has two parts one thing which is takes into account the motion which is called the prediction step those two lines over here as the updating the belief into the so-called predicted belief we predict how the belief looks like given our control commands not taking into account the observations yet this is a prediction that i'm doing and then this three lines down here are the correction step so correcting potential mistakes that we have done in our prediction based on our observations here taking into account my observation and what we are doing here basically we are computing a weighted sum and down here uh i'm using the kalman gain weighing the prediction as well as the correction so this is the prediction and this is the update of the correction and the kalman gain basically tells how strong will that update be if we take into account our observations and then we are updating my the covariance matrix over here so what we need to do now what we'll do in the next hour basically we go through those five lines of code and explain how to realize this in order to build an ekf based slam system so how does it work the first thing we need to do we need to specify how does our state vector actually looks like right so of course there are models in there that we need to fill such as my motion transition model and my observation function but and the gradients involved in this but the first thing we need to do we need to specify our state vector what do we want to estimate this is basically the dimensionality here of my mean so let's assume for now we're living in a 2d world so we're living on a plane so the only thing we're estimating is the x y location of the platform and it's heading the yaw angle in the plane we're not taking 3d into account but the same would actually transport to 3d but the mass is a little bit easier in 2d therefore we stick with 2d so that means the position of my platform should be a three-dimensional vector an x location a y location and an angle it's called theta that encodes the orientation of the platform on the 2d plane but the state vector also contains about in takes into account the landmark location so where are the the landmarks that i'm actually observing and the landmarks are points in the world and if we are living here in a two-dimensional world this is an x and a y-coordinate for every landmark so the state vector has basically three dimensions for the pose three dimensions for the pose and then two dimensions for every landmark that i'm seeing so if i have 100 landmarks this would be 3 plus 2 times 100 so a 203 dimensional state vector that i wanted to estimate okay so the state vector for ekf slam looks like this we have our robot's pose x y theta landmark 1 x location y location landmark n x location by location and this vector basically grows with the number of landmarks that we are actually seeing and again for now to make our life easy we assume non-correspondency that means this index one two one two three four until n is something which is known and if i get an observation i know which landmark which index does this observation correspond to in my map or in my state vector where this is basically the map and this is the pose estimate that i want to do so this again assumes known data cessation over here if you don't know the data station we need to estimate it for example through a nearest neighbor approach or some distance in my space or feature descriptions but then that gets a bit more tricky but what most of the approaches do they just guess the data station and stick with this data association because if you want to do take into account multiple data stations simultaneously it gets very complicated and therefore most of the approaches stick or assume no data station have a strategy to pick it and then don't take the uncertainty in the data stations into account anymore okay we can also visualize this graphically so this our mean vector has this kind of x t over here and then the landmarks from one to landmark n and the corresponding covariance matrices so this is the uncertainty about the pose this is the uncertainty about my landmark locations and this kind of links the um links the landmark locations with the position of our platform or we can even write it more compactly we also use this notation pose map and the corresponding uncertainties involved in this okay so now we have defined our state representation the next thing we need to do we need to actually cycle through our extended kalman filter and explain the individual steps how this vector that we have now defined will get updated over time so the kalman filter has several steps it has basically the state prediction it predicts a measurement it takes a real measurement it does this data station and then performs the update so it's slightly more advanced in steps so the prediction step and the update step but of course in the prediction step you make a computer predicted measurement um and you make the data association and then perform the update so it's slightly more expanded the steps because now we can visualize the steps and can also see what gets updated in this belief so let's look how we can visualize what's going on here so what you see here is basically a scene the current state what's going on and what you see down here is your state vector and your covariance matrix and we want to always indicate what will be updated so if we um move through the environment we perform a prediction step so we advance the platform without taking any observation um so we have this platform and we move it forward that means the uncertainty of our motion [Music] of our platform location increases so we will be more uncertain where we are with respect to our pose estimate because by moving for the environment we increase our uncertainty about the platform's pose so if we move forward we kind of increase the uncertainty about our position so what will get updated in the state vector is the landmark locations will be sorry the robot location will be updated but the landmark locations will not be updated because just by moving i'm not changing the position of my landmarks in the covariance matrix all those entries gets updated of course the uncertainty about my platform location increases this is this red ellipse shown over here it increased from here to there so this gets updated but also the correlation so the links between the platform location and the the landmark locations gets updated because it basically means um now the pose provides me less information about my landmarks because i got more uncertain about my post so it conveys a smaller amount of information for about the uncertainty of my landmark locations then what we're doing we need to perform a predicted measurement we need to predict what we see and the only thing we need to do we don't need to change anything in here the only thing we basically need to look up is where we are right now and where the landmark and then we can compute our predicted measurement but no updates in those quantities are actually performed then i'm actually taking my real observation so the sensor reports me okay the landmark is here so this is basically landmarks that i'm actually seeing of course no update has been done and then i need to compute the discrepancy between what i actually observed and what i was expecting to observe so between my actual observations that and my predicted observation h of x which is basically the mapping from the state space into the space of observations and we basically have this discrepancy over here between those two and then based on this the update step is performed and in the updates there we can see an update of all the quantities so the robots will get more certain and will get corrected the landmark locations will get corrected and also the uncertainty changes so this is kind of the more expensive update in the sense that all entries in that matrix may get updated so it's a quadratic number of entries and they potentially need to be or will get updated in my update step okay so let's make a more concrete example and then this abstracted example and then really go through every step of the extended kalman filter and explain how it looks like how do the models look like and how we can perform the state estimation so that you in the end can actually sit down and implement an ekf baseline system on your own so that's my goal for the lecture of today okay so the setup what we're assuming to have we assume as i said before we have a platform that moves on the 2d plane sort of an x y coordinate and orientation we assume to have a velocity-based motion model that means we have a translation velocity and a rotational velocity that controls how the platform moves so basically i can say the the the speed of the rotation and the speed of the um translation so something that um we have is a rather simple or general model that can be used on a lot of platforms um we assume to have point landmarks in the environment so individual points that we can observe um that could be let's say corners if you move through the environment could be door frames whatever it is but we assume to have basically point landmarks so not lines or large extended objects we just measure points this is our assumption here and we assume to have a range bearing sensor so something which tells us whatever 30 degrees to the right and five meters away there is that point that you want to observe that's by what our sensor tells me note that this is not a monocular camera because the monocular camera would prevent you would provide your bearing information but not range information right the camera doesn't tell you how far the object is away but if you have a stereo camera where you know the baseline then this would quantify qualify as a range bearing sensor that we could use over here we assumed to have known data sensations so whatever we see we know what index this landmark is in the map and we assume to have a known number of landmarks so the known number of landmarks is not really it constrained it's just otherwise we need to check make a lot of checks in the algorithm is our state vector already full do we need to extend it and do some bookkeepings which can be painful to implement and but i believe it will drag away your attention from the actual things which are important to the kalman filter itself and just saying we assume they're 100 marks and we just deal with 100 landmarks for example is something that is perfectly fine to be assumed over here so we assume to know we know the maximum number of landmarks so we can initialize our state vector and our covariance matrix accordingly which brings me to the first step we need to do we need to start with initial belief we need to initialize our platform so how do we initialize our system um that can be easily done um if everything is completely unknown um we only say where we start we define our coordinate system so this basically means i define everything up to start where i started so i say i start in 0 0 0 x 0 y 0 theta equals 0 and um i have zero uncertainty in this because i that's the way i define my coordinate system but well all the other landmarks i don't know anything because i haven't seen anything um so i can initialize them always zero but give them an infinite uncertainty over here right that's kind of the easiest initialization that i can do from the state destination point of view if you have an idea or reference to an external frame where you know okay the robot starts at a different location of course you can then change those three numbers and put um your the information about where you start in there um these infinity values over here um this may see this more from a conceptual point of view in practice you probably wouldn't do infinity in here because this always leads to numerical issues but you basically want to have a bookkeeping in some way which landmarks had to be initialized because later on we see if you have an infinite uncertainty here or we have never seen landmark before there's a smarter way for initializing a landmark than just putting uncertainty in here and assuming um zeros for the poses so this is more as a kind of conceptual initialization than what you would actually do in your implementation so you can put it whatever it's not a number and whenever it's not a number you know it hasn't been initialized you should initialize it on your own that would be probably the right way to do that or you have a bookkeeping which tells you how many landmarks you have seen initialized and then increasing this count until you reach the maximum number of landmarks that's the other alternative and then only use the state vector or and the covariance matrix up to this index of already seen landmarks okay we will now go through the excellent common filter really line by line the arrow always tells me where we are and we say okay how to do this step what needs to be done in here done we know how it works let's go to the next step and we will iterate over those steps over here and then do our computations some of the things to the end gets easy if all the com if all the quantities are actually computed so if i define how g the g looks like how the jacobian looks like how h looks like over here how the jacobian looks like then everything becomes automatically and easy but i actually need to provide those quantities and they are kind of the meat the user-defined thing that you need to put into your system so in the first step we need to know how does this function g look like so the function g was my prediction step integrating the motion of my platform basically taking this ut in order to advance those two quantities into the predicted belief and that's what exactly going to happen so we need to specify how a platform moved through the environment how does the platform move through the environment it can be easily done i can say okay i know i'm right now at x y theta and x prime y prime theta prime should be my new pose after i executed my motion command and then this is an additive term so this is my way where i start where i am right now and then make an update to that and the update just comes from a rotation velocity and a translation velocity so and these are the standard update motion equations for a differential drive robot which basically tells you if you set a translation velocity and rotational velocity where you will end up and these are basically movements on a circular arc that tells you where you're going to end up if you drive on that circular arc this is now the equation here put in for a rotation velocity unequal to zero otherwise this ratio wouldn't be defined if you just go straight forward then this even becomes easier you just have one sine one cosine operation in there and then you know where your update is because you're moving on a straight line and not changing your orientation but if you change your orientation um then it becomes slightly more complex complicated but it's still a very simple trigonometric trigonometric function uh or operation using trigonometry function which tells you where you are going to end up in the next point in time and this motion step just only changes the three dimensions where about the pose of the platform for where the system is my x y and theta coordinate which makes sense because when you move for the environment you're not changing landmark locations but we need to update our state vector which is 2n plus 3 dimensional and not just 3 dimensional so how do we actually map that into our state vector so in practice we would only update the first three dimensions so we can actually manually compute this so this is update for the first dimension for the second dimension and for the third dimension but for writing things down it's sometimes easier to actually write this in matrix form and basically introduce a mapping function which maps the three-dimensional update into a three plus a two n-dimensional update and this can be easily done by a function which we call f x in this case transpose which is a function which has um here two um two n plus three dimensions and here uh three dimensions um we where there is a three by three diagonal matrix with ones on the main diagonal here and all the other quantities are zero that basically means if i'm if i multiply this this update over here to this matrix and transpose this matrix multiply this it will actually take that map those rings be the identity matrix into the first three dimensions and then add zeros everywhere else that means the update only affects the first three dimensions and all the other dimensions will end up having zero so it's kind of a slightly more complicated form so that we can actually put our full state vectors over here and then have this mapping which basically sells yeah just update the first three dimensions and the rest is not updated so it's it's a very easy step so with this standard motion model taking into account the velocity command translation velocity and rotational velocity we described how the system would evolve from time t minus one to t so we have a new idea where we are for our mean estimate right because it's basically we are standing here we're going with a meter per second forward that means our new mean has shifted by a meter and if we rotate it we may have a rotation in our movement and that's the only thing which has actually happened here and i've described how to update that state vector so the first up thing is done so we know how to update our state vector right that was easy the next thing we need to do we need to update our covariance matrix and for update our covariance matrix we need to know our previous covariance matrix we need to know the uncertainty of the motion this r this is basically how how uncertain is my motion and we had those g functions over here and those g's were the jacobians of my small g so of my update function the function that i just used and again this function g the small g only affects the locations of the platform it doesn't affect the landmarks right so there's no update of the landmarks and so the only thing i need to compute in order to compute this function g is that i have the jacobian of the affecting the robot's pose which need to be taken to account so a three by three matrix over here because um this is my the the my my update but down here i have a two n by by two and um identity matrix because nothing has changed right so it's a it's a direct mapping i've just kind of reused what you had before so no update on this function it needs to be done there is no nonlinear part in here i'm just copying over the uncertainties of my landmarks i'm only affecting the um the jacobian here of the of the pose and as you see through the operation we will then get an update in all the quantities that need to be updated in our matrix okay so we can look into how to compute this um three-dimensional this jacobian which is a three by three matrix y3 because it basically i take my function and derive it with respect to the change in parameters that i have so i take my my function g and derivative with respect to x with respect to y and with respect to theta so i'm computing the first derivative with respect to x y theta of this update function that i had before so it's a jacobian of my motion model okay so how does that actually look like i have my quantities over here and now i want to take this i just need to sit down and actually compute my jacobian over here so what do i need to do i say okay compute the jacobian of this function x y that with respect to x y that which is easy which gives me the identity matrix for this part over here and then the second part is computing the derivative of x y red based on those quantities over here right so that's what i would now need to do so this boils down to deriving this with respect to x this with respect to y and this rest one with respect to theta and by just computing those first derivatives with respect to x y and theta i can see that there is no x actually popping up in here there is no y popping up in here there's only theta popping up here so as a result of that the only nonlinear component which i actually get in here is my orientation the orientation is the part of my of my update which has the is involved in this non-linear function over here so the sinus co and cosine function depend on the variable theta and not on other variables as a result of this the first derivative of those elements with respect to x and y will just lead to zeros and with respect to theta will lead to this update over here so basically the sine turns into a cosine function the cosine function will turn into a minus sine function over here and those constants those of three vectors are are unchanged and this element also doesn't appear any theta also turns into zero so only these two elements over here so my up my my function derived with respect to theta um in the in two components will be non-zero so these are kind of the non-linear elements that i have in here so if i combine this matrix with the identity matrix i actually get the matrix which is one on the main diagonal and the two uh kind of more complex expressions over here which actually shows you that you are um that you have these non-linear components in there otherwise you would have just the diagonal matrix and so this is my g x t so this is kind of the three by three blocks in this large matrix but we know already the rest of this matrix was just my identity matrix because i don't want to change any of my landmark locations just based on this jacobian so i can just use this jacobian and so this matrix over here turns into this matrix which i just computed which is has ones on all the diagonals and just kind of two elements in here which are non-zero elements so i take this matrix multiplied with my covariance matrix and the jacobian transposed plus my r and again my matrix r tells me which uncertainty do i add based on the motion and this is basically the edit uncertainty um through the x y z coordinate of my um of my of my role platform so this matrix r will have will only affect the this block over here the three by three matrix block and increase the uncertainty because it doesn't change any uncertainty of the landmark so just by moving through the environment i'm not changing the uncertainty about my landmarks only uncertainty about my post and these are the two elements that we see in here which are the off-diagonal um elements so this is basically the impact of the uncertainty update on my landmark on of the robot's pose on the uncertainty of the landmark so the correlations between pose and landmarks they also get updated and these were kind of the the first row and basically the first column of the block row block column of my covariance matrix which gets updated this part is not updated at all it will stay the same so this part i can fully copy over so the majority of elements this basically two n by two n dimensional elements of this can be copied over and are not updated at all okay so now i can just multiply those matrices with each other and i'm basically done nothing needs to be added to this so this step is now also done that means at this point in time we have performed our prediction step so let's look how that would look like into in some sort of code just executing those two steps over here so if you would specify the ekf slam let's say the prediction step the first thing we would need to do is we specify this matrix f which performed this mapping from this three-dimensional space into these um into this three plus two n-dimensional space if we transpose the matrix f and multiply it with um with a three-dimensional vector it will actually take the three-dimensional vector and move it up into this um three plus two n dimensional space and this exactly happens here for the for the mean update we say our predicted mean is our previous mean plus this f transposed and the update that i'm performing based on my motion model g and just kind of a copy paste from the previous slide the only thing the previous location x the previous location here is which was theta is now the mean of the the input mean so from this mean i'm taking the orientation so the orientation the best estimate of the rotation that i have and perform this update so this is kind of the first line of the ekm kf algorithm then i built my jacobian g is the identity matrix plus this small basically these two elements here which are mapped and into the two off-diagonal elements which are non-zero in this jacobian and then i perform this update and again if i specify r only in this xy theta space and also kind of need to make that mapping up to this um higher dimensional space or otherwise i can specify rt directly as a high dimensional matrix where everything is zero except the first three by three block which is non-zero and so this is basically a soda code implementation that you would need to write down in order to perform the prediction step of the kalman filter and so with this we are completely done we can just apply it and here we go everything is done we know how to perform the prediction step in our kalman filter and estimate where the system will be after executing the control command and the next thing we now need to do is to take the correction step into account so for then for the correction step we need to compute the kalman gain and the only quantity we don't know so this is this guy we know this basically specifies how certain i mean about my observation so it's a user defined quantity the only thing which is interesting here is this h and this h is the jacobian of my observation function which computes me my predicted observations of this function small h so capital h is the jacobian so what i need to do now i need to look into my observation function how do i compute a predicted observation take that function derive that function in order to get my jacobian over here okay so let's go and see how that works looking into the correction step again in the correction step just a reminder we assume we have no data association that means nothing else that i know for every observation i can basically know to which have a correspondence matrix c which tells me which landmark i should index in order to um to map that observation to the landmark in my state vector so basically the mapping from if i see that landmark in my image i know it is dimension whatever 153 where my landmark is stored so this is basically done with this index we initialize all landmarks as unobserved so we have a way for saying all this landmark has been observed yes or no because if it hasn't been observed i kind of want to initialize it in a kind of smart way using my observation and then i need to compute my function h or specify my function h and i need to compute is jacobian and then can compute my kalman gain and perform the remaining updates in the correction step of my kalman filter so range bearing observation how does that work so range bearing observation is a two-dimensional quantity which gives me a range and a bearing so i say okay that point is three meters away and 30 degrees to my right-hand side for example that would be an observation that i get here three meters away 30 degrees to my right hand side or 10 meters away 1 degree to my left hand side these are examples for range bearing observations and i know the point that i'm seeing again i have no data station i know to which index this corresponds to in my state vector so if i have a landmark has never been seen i can actually initialize a landmark and the best estimate i can have if i have if i don't know anything before i can say okay i just assume i take my pose where i am right now so my predicted pose the best thing i can do is my best prediction of what i have right now this is the uncertainty of my platform uncertainty of my platform and then i take the relative measurement into account basically takes okay what's the update in x and y is basically how far is the point away and that the cosine and the sine of the orientation of my platform and my measurement so i think i'm standing here i'm looking into this direction so this is this quantity over here and this quantity and then i say um okay and then the point is 5 meters away and 30 degrees to my right hand side so this would be the 30 degrees to my right hand side this would be the 5 meters and then this function with the cosine tells me what how do i need to update my x-coordinate and my y-coordinate in order to get the position of the point and this would be the kind of predicted location of that landmark of that mark j where it should be so if i have no information about landmark the best thing i can do i just take my observation to initialize the landmark and then um this night market this has an uncertainty um the position of that landmark which is based the uncertainty that i had about my post and the uncertainty of my observation in a combined fashion right so i can have a direct way for initializing the landmark and don't need to put this infinite uncertainty and zero zero zeros everywhere which is numerically not the best thing to do so you would initialize the landmark in that way and then what i need to do is i need to if the landmark has been then i'm basically done with this step and otherwise i need to compute if i if i know where the line markers i've seen it already i have already an estimate i need to estimate what i'm going to see right i need to compute my predicted observations so i need to predict a range and i have to predict a bearing from that observation given that i've i know where that light mark is okay um so if i know so if i know where my landmark is then i have an estimate of my landmark so i have an estimated x location and estimated y location of my landmark and then i can say okay how far so what should i observe so i know where i am i have an estimate of where i am and have an estimate where that landmark is so basically the euclidean distance tells me something about the range so how can i compute it i compute the vector of the differences in x and the y coordinates so this is basically delta x and delta y between my location and the landmark location which i can compute over here and then i have a q which is delta transpose delta which is basically the square distance that i want to observe right it's the the this the square distance so if i take the square root of q um this is my euclidean distance just expressed here in in form of vectors so i have this two-dimensional vector the the difference in x between the landmark and the robot the del the difference in y between the row and landmark and if i square that and take the square root then later on out of that over here it is my expected observation so i take this q square root of q is the distance between um the position of the robot and the position of the landmark given the current estimate and that's what i'm predicting to observe based on what i know that's what i should get and for the orientation i basically need to take into account again this delta x delta y and um so in order to compute the the angle and then also need to subtract on here where the robot is looking to because depending on where the robot is if the landmark is over there i'll of course linearly update the um the angle of angular part and so it's two components the eight on two which gives me the um the orientational offset in the uh based on the difference between in the x and the y coordinate of the landmark and the robot and the location of landmark itself and this is then gives me my function h so my observation function the function which computes my predicted observation is this h which is a function that takes the mean vectors and input and gives me a two dimensional output the first one is square root of q and the second one is a time two of this difference minus the um best estimate that i have about where the robot is right now in terms of its orientation and that's it again you can see there's a nonlinear function so this is non-linear and this is non-linear so this needs to be appropriately taken into account i specified h now the next thing is how to compute jacobian for of this h because i needed the jacobian of this function um to be uh taken into account in my for computing the common gain so what i need to do is i need to take this function and compute the jacobian so what are the quantities which are which matter over here so the quantities which i need to derive it is basically with respect to the x location the y location and the orientation of the platform and with respect to the x and y location of my landmark so for the jacobian i basically have five quantities which are interesting for me which are non-zero over here okay so i i still use those quantities delta q to compute my that so this is my was my function my function h and now i compute the uh first derivative so the jacobian which is all the partial derivative of this function so this function and of this function with respect to my mean vector but i'm not interested in the whole mean vector only five dimensions five dimensions matter which are non-zero which is x y theta of the platform and x and y of the landmarks that i'm observing right because these are only the ones which matter and this is how what i compute this kind of low dimensional if i write low here this low dimensional five dimensional jacobian because the rest of the entries don't matter and i will take this f matrices again you know to map it to this higher dimensional space but what i wanted to do now i need to take those functions and derive them with respect to x y zeta m j x m j y the same for those two what i will do now is i will sit down and make those derivations so first uh component of the function is this one derived with respect to x this function derived with respect to y this function derived with respect to theta and so on and so forth and then take the second row this one derived with respect to x this one derived with respect to y and so on and so forth so those are the derivatives that i now need to compute manually here i will just do it as an example for just one of those and you can do it on your own for the other dimensions so we'll just take this first partial derivative here the first component and compute it by hand how does it actually look like so i need to take this function over here square root of q and derive it with respect to x so i have the d square root of q with respect to the x and of course in this function q this is this delta delta transpose but inside delta there is the x because the x sits in here so the location of the robot so this component is the component where where the x this is basically this x over here so i need to apply the chain rule a few times in order to compute the first derivative so square root of q is basically q to the power of 0.5 so if it could be the first derivative of a function x to the power of 0.5 it is 0.5 times 1 divided by the square root of that function so it's 0.5 times 1 divided by the square root of this function because this is q to the power of minus 0.5 right so this is kind of the the the derivative of this of this outer function now i need to go to the inside and i basically have a f squared function over here so this is basically x squared so the first derivative of x squared with respect to x is 2 times x so i get 2 times my delta x over here and then i need to go inside the delta to this and say okay what's the first derivative of this expression with respect to this variable this is a constant there's a minus minus one times this variable the variable goes away just the minus one remains so i have the minus one over here so as the result of this the first derivative of the derivative of this first component with respect to x is 1 divided by q minus square root of q times delta x maybe not the most beautiful form but we know all quantities we know we know delta x this is just the square distance this is uh the the euclidean distance there's a squared distance this is a minus one all the quantities we can easily compute so nothing tricky in here and now we can repeat this process and do this for the other dimension and also for the other function and if we repeat this we get this as a result so the results over here the one divided by q pops up everywhere and then all the individual elements of this um of this jacobian are rather straightforward either q's deltas or a square root of those elements and that's quite easy to compute and gives me the first derivative of my observation function with respect to the five parameters which matter though the basically tells me how a change in the robot location or change in that multiplication would be actually mapped to so it describes the snapping it's still again i'm still did this in my low dimensional space so the my jacobian here has two two rows and five columns although in theory it should have three plus two n columns but we were only interested in one n so the other n minus one landmarks simply were not relevant for us and the three location three dimensions of the robot state space so i can define now again my a function f very similar to before which is basically has it's a one uh um an identity three by three identity matrix over here because the first three dimensions are the robot's pulse they're just mapped to the corresponding locations and then again everything is filled with zeros except two columns two columns need to be special there are two columns that are those two over here need to have one in here so remember this was the um fourth and fifth dimension here of my low dimensional jacobian those elements over here where those respect the the first derivatives with respect to my j's landmark so i need to put in j minus 1 times two zeros because these were all the landmarks up from landmark one to landmark j minus one nothing happens there all zeroes then for the landmark j i need to put a one one over here because those quantities need to be mapped to the corresponding part of the of the update where i updated my observation and then from j plus one to n everything is filled with zeros because all other landmarks don't matter at all and with this i actually get my high dimensional jacobian defined in the original space and so with this actually everything is done right so q is user defined h i just computed this is the covariance matrix i computed before given given given so i only need to take this expression invert it and multiply it with those two elements and then i get k done perfect but also down here h i just know how to compute it we just specified it before that is the observation that i have just need to compute the difference multiplied with k which i just computed and add it to a vector oh perfect all done and now here this quantity i know this was my predicted uncertainty this is my jacobin which i computed this is a common gain which i just computed this is an identity matrix perfect i can just compute it so i'm actually done with my algorithm i just need to perform those steps and execute those steps and i'm completely done with my ekf algorithm and realized ekf slam again let's look into the solder code so expand this a little bit more as we did for the prediction step now for the correction step so the first thing i need to do i need to specify the uncertainty that i have for my observation so i have an uncertainty in the range and uncertainty in my bearings this basically comes from the specification of my sensor and then i iterate over all observed features so i'm basically iterating over landmarks i have my data association which i assume for example in doing a nearest neighbor data association wherever it comes from and then i need to have my have my if then else statement did i ever observe the landmark j again if i don't if i haven't never observed it i should actually compute my um i can initialize it directly and then i'm done and then otherwise i compute my predicted observation so i can actually also compute the predicted observation but and do the same steps it's not really needed here because i already initialized the landmark but i can actually use it because the difference between the predicted observation computed in that way and the actual observation will be zero so there will be no update being performed so i'm not using this information twice but i get the update for the for the jacobian of course done in the right way then i specify my my big function f which is just the mapping from this five dimensional space to the three plus to n dimensional space i compute my jacobian which i done by hand map it into the high dimensional space and then i just write down the three lines copy paste the three lines of the kalman filter and i'm doing this for all the landmarks and then i'm done got my update nothing else needs to be done for that and i'm ready right so it was actually not too complicated to actually do those individual steps a few notes maybe about if you want to implement it on your own so if you do this measurement update in the way it was done here you do every measurement update independently this actually needs to requires a full update of your belief at every point in time so you may want to combine those observations and can kind of combine three observations three sensor observations at once and execute this um because you only need to iterate over your n square elements only once um then also one thing to take into account um if you haven't you have an angular component there and the angular component is only defined from minus pi to pi right so you want to be careful with certain wraparounds because this will actually can screw up your jacobians so you want to make sure that your angular component is always normalized so that means even if you through some computation you're put out of the minus pi to pi range you want to normalize it so it's again back between minus pi and pi otherwise this can can lead to very weird behaviors but something you it's not specific to the kalman filter you basically have to do that for nearly all state estimation approaches and the other thing is these matrix f you may not need to x compute this explicitly so if you have whatever if you use matlab or some other math libraries in python that you're using you can access the elements of your matrix directly and you would basically just fill those matrix directly we use this matrix f because then all the expressions can be written down in very easy form on those slides so that you can see the direct relation between what's written on the slide but and and what the core kalman filter as we introduced it looks like but if you implement it on your own there may be better ways for for just building up this huge matrix you can actually manipulate the entrance directly which which can be more it can be faster because you explicitly know what is zero and don't need to touch the elements which are zero but that's just kind of a small side note over here and with this we're done we implemented our kalman filter and we are ready to go what i want to do now in the next couple of minutes i want to introduce a few of the things that you see and explain you some of the concepts what you will observe if you actually run slam such an ekf based slam system so the first thing which is an important thing to note are loop closures so what is a loop closure a look or something which is not specific to ekf baseline it's something that is relevant in all slam algorithms a loop closer is a situation where the robot drives through unknown environments for quite a while and then re-enters a known part of the environment and this typically leads to dramatic reduction uncertainty because robot can make a data association between what it sees right now with respect to a landmark that has seen far back in the past where it potentially had a much smaller uncertainty in its estimate because while moving through this unknown space it's uncertainty increased so by re-observing a landmark with a small uncertainty you will reduce the uncertainty of all the landmarks you had on that way up to the point in time where you have been previously and therefore this is a great thing in order to reduce your uncertainty but of course again this is a very prominent example if you add a wrong loop closure you can completely screw up your map because you're getting overly confident because you make the wrong loop closure and reduced your uncertainty dramatically although that wasn't justified so you have to be careful with your data association there's a kind of key message in here i also have an example over here so what you see the system started over here initially and then drove around through this environment here here here here and is now over here and you can see how the uncertainty is increased because everything is specified with respect to the initial coordinate frame so you could see very small uncertainties because it's basically just the measurement uncertainty initially and then while the mobile drives the robot drives around it gets more uncertain so also the uncertainty about the landmark locations gets larger and larger and larger because robot doesn't know exactly where it is the sensor information adds some additional uncertainty to it and so you're getting estimates of those landmarks but the kind of the the ellipses increase but now the key thing is robot comes back and re-observes those landmarks those landmarks over here we had see it it had seen before and now he says okay given i know that just such a small uncertainty of this landmark and now i can see it i can reduce this uncertainty dramatically if i reduce this uncertainty dramatically this will actually propagate back to this one to this one to this one to this one so you can reduce the uncertainty of all those landmarks on the way until until back here and as a result of this those will be really small and then they will be slightly larger so those will also reduce but these will where you're kind of in the in the middle on the way into the unknown space and back they will then later on have the largest uncertainties but still much smaller than they are right now so if you make this update now after the next uh observation step robert moved here you could see all those poles are well known here the uncertainties also shrunk but they are kind of the biggest ones and then are kind of smaller over here so this is the area with the highest uncertainty now and that's the effect of a loop closure and so loop closures dramatically reduce the uncertainty and therefore are key in slam systems but the thing to keep in mind is that a wrong loop closure can dramatically lead to a wrong estimate or to what you call filter divergence where your estimate has nothing to do with the real world anymore because you're adding an observation where you're very very certain about because you have this small uncertainty observation um and update your belief and if this was wrong you will and when you give such a high weight to this observation but it was wrong you will of course lead to um a very you will you will lead to a wrong estimate because this one observation actually ruins your whole filter estimate if you also run your filter over extended periods of time and you draw a graph between the landmark locations and the robot's pose and the the the size of the um of the edges in this graph represent to the correlations that you have between the involved variables you can see that you actually get a fully connected graph some of those edges are smaller like this one over here between the robot's pose and the individual landmarks but there's a larger estimate a larger connection between those landmarks that means in the long run the estimate all those estimates get fully correlated so all the landmark locations are fully correlated with each other so is an effect that you can see and you can also see is if you draw your your state estimate and a correlation matrix which tells you how strongly correlated different variables are with each other and then you can actually see an interesting effect so you would start here this is the robot's pose it's it's first observation that is done so the robot has some uncertainty and this landmark has some uncertainty and this is basic correlation matrix that you would get so the first three dimensions refer to the x y and theta location of the robot and the rest of the blocks are basically the um all the landmarks so all the landmarks haven't have a kind of an initial uncertainty and then while you're moving through the environment you're getting your estimate and you're updating this so basically all those here haven't been initialized yet and you will see the system will actually grow and initialize those matrices um and um and therefore kind of can can fill this matrix so the raw would actually move around through the environment observes all those landmarks gets an estimate of those landmarks and they actually get this pattern these patterns emerging over here and you can go further and further and further and map the environment until you kind of have a fully filled kind of correlation matrix here which reveals this kind of checkerboard pattern so we can see here we have this x and y location over here of the platform and then this white line here is from the orientation and then we have this checkerboard pattern over here over all landmark locations and what you can see here that it's kind of a checkerboard pattern which kind of can appear a little bit weird why would you get up this checkerboard pattern but if you think about more closely about it actually makes sense so it basically says so you've been driving around um you're all those landmarks after vm driving around and seeing those landmarks from different locations will always have roughly circular um covariances so all the individual elements over here you say we have an uncertainty about x uncertainty of y but roughly a circular shaped covariance matrix for this landmark so the two of diagonal elements should be zero so you basically have this as for the individual two by two blocks sitting here on the main diagonal but or what about all the other elements so what happens you will drive around you have some uncertainty that that you have in your system out about your landmark locations and now you can say if i would be able to fix let's say the x location of one landmark so if god would tell me the exploitation of that landmark then i could dramatically reduce the uncertainty about the ex location of all the other landmarks as well because basically through the estimate i've rigidly owned rigidly but quite of rigidly connected those landmarks so if i shift if i know one landmark it gets shifted with a high uncertainty or with zero uncertainty then all the map will shift as well and this means if i could boil down saying okay this landmark i know the precise execution of this landmark i will know basically all x locations of all other landmarks as well but i will not learn anything about y but the same happens with y if i would know the y location of this landmark it would know the y locations of all other landmarks and this is exactly this checkerboard pattern that you can see here so you basically by saying by fixing one landmark i wouldn't the x location i would know all other landmarks in the same halls if you would know the x and y location of one landmark you could pin it down perfectly you would basically shift the map a bit fix it and also dramatically reduce the uncertainty of all other landmarks and this is basically what you can see from this checkerboard pattern this also means that the correlations of the estimates of the of the landmarks between the poles of the landmark cannot be ignored so if you ignore or would ignore all those correspondences or these correlations over here you would lead to two optic estimate of the uncertainty because your uncertainty would be too optimistic if you would because it would basically mean you're setting all the other uncertainty values to zero and this would not be the right way to go so in this you can't ignore the correlations that exist between your landmark locations another interesting thing to see is also the um uncertainty of your landmark estimates so how does the uncertainty about my landmark estimates develops so what happens what you see here is whenever the landmark occurs for the first time it's initialized and then it kind of you see how it decreases over time based on the observation so your time steps over here you basically have the uncertainty of the landmarks and you can see how the landmarks evolve and what you can see is that it's a monotonically decreasing function which have an initial uncertainty they get at the initial sorry the initialization so if i start and they were close to my starting location they uncertainty smaller if i'm further away they are initialized larger because they have a larger initial uncertainty because it's the combination of the landmarks uncertainty and the robot's uncertainty when observing it but then the more you observe those elements will go down it's a monotonically decreasing function and it goes down and down and down and down and down and the key question is will it ever get to zero and we can say ah it depends it depends basically on the initial estimate so even if your landmark while you initialize it you can reduce the uncertainty of those landmarks but you will reduce the uncertainty of those landmarks never under the uncertainty of the initial uncertainty of your platform so when the platform observed the first landmark that's initial uncertainty none of the landmarks uncertainty will go below this uncertainty right because just through the relatives as long as you have relative observations of those landmarks in the robot's pose if you have some other same thing like you add a gps or some other external sensor then of course this may change but if this is not the case then you will never be able to decrease your uncertainty of a landmark be no below the initial uncertainty because while the robot moves its uncertainty will increase and it can't decrease below its initial uncertainty it's kind of the uncertainty of the of the coordinate frame that you actually gave to the system and you will always have this initial uncertainty nav can never go below it unless kind of you fix your external reference frame with some external additional sensors but as soon as you have only these relative sensors the lower bound will be the um the robot's the vehicle uncertainty when you initialize your first landmark you can't go below that okay um just a few examples now so this was one of the kind of a very famous example called victoria park data set recorded in sydney in the victoria park with the vehicle driving around the laser scanner sitting here in front of the vehicle and basically scanning a 2d plane in this height and basically as features the trees were used basically extracting the trunk of the trees and these generated actually features and then by driving around with this vehicle through the environment you could actually build a map of the trees in the environment and this is an example of the trajectory so you have the trajectory of the vehicle driving around and then the different observations that have been made on the way and the uncertainty of those observations and you can actually build a map of the environment based on the based on the tree locations and where the vehicle was and so you then you have the trajectory that the vehicle has been taken and for the ground truth trajectory and then the estimates where the system estimated landmark locations overlaid with an aerial image some of them fit quite well others are not the best possible estimate that you could get but this was an example of this so-called victoria park dataset which was for years one of those standard data sets um used in the slam community or in the feature-based slam community in order to evaluate the locations or the slam algorithms by estimating the locations of the tree locations there are other data sets used for benchmarking for example this one over here where you put those very well precisely defined sticks on a badminton court so the back or tennis court badminton core data tennis court data set where you actually drive around um and place those landmarks on the precisely measured locations that you use uh for these courts and then drive around and if you drive around you can see the trajectory of the platform which roughly looked like this and if you perform your slime estimate you get the proper locations and can actually see the chords being precisely lined out here while having mapped that and that's kind of kind of useful for estimating where locations are in reality because if you know the precise measurements of those chords which are typically rather precisely done at least compared to the sensor which was used in the robot which had an uncertainty of roughly let's say five centimeters in the range readings um then you can use this as kind of a ground truth system or for in order to evaluate your slam algorithms okay a few final remarks on the kalman filter for slam in terms of complexity so the common filter in general you have a cubic complexity in there you have your matrices and matrix inversions involved but it's only a cubic complexity in here with respect to the measurement dimensionality um so no not with respect to the whole state um this also depends on the fact that you're only observing a small number of landmarks it will never observe the whole scene at once and then the cost step is dominated by the number of landmarks because you need to update your matrix um your your n by n matrix so that you have n squared elements you need to update and this is the cost is dominated as well as the memory consumption is dominated by that so it's basically a quadratic complexity that all the ekf based slam systems have and if your number of landmarks gets large this can start to be a computational limitation um so if you build wrap up the environment this is something where it can the updates can take longer and longer and longer because it will grow quadratically with the number of elements that you store therefore ekf based slam is not always in place anymore today especially want to build maps of larger environments but it's still quite popular if you use for example for visual odometry estimates where you say okay i restrict myself let's say to 30 or 40 landmarks and i'm just forgetting about the old landmarks and so whenever a new landmark comes in i forgot if i get the oldest one it's the one which i haven't seen for the longest period of time and this way update my estimate then this is still a standard way or standard toolbox that you can actually use but for really large scale map this can become computationally intractable okay this brings me to the end of ekf based slam so ekf based slam basically means one estimate the map of the environment and the position of my landmark using an extended kalman filter so for the linear gaussian case there's convergence proofs and you can also say that or for the linear gaussian case the ekf based solution is actually the solution that you would get also with the least squares approach a statistically optimal solution um but again only holds for the uh for the gaussian for the linear case um if you are living in a non-linear world which you are always doing in practice then you have the problem that you're linearizing in the ekf and the ekf only linearizes once compared to the least squares approach so as long as you're in a nonlinear world you're actually leaving the direct correspondency to the least squares approach and then your performance will degrade decrease with the ekf because you're not re-linearizing um but it's still it's a standard toolbox especially if you're living in with gaussian uncertainties and don't have too strong non-linearities it's a standard toolbox that he can actually use it also is better with smaller uncertainties this is something that we've seen when we discussed the kalman filter itself the kalman filter makes larger approximation errors if your uncertainty is larger and then you want to kind of keep your uncertainty as small as possible because then your linearization does a better job you may also say this is one of the reasons why there are smarter ways than initializing a landmark with infinite uncertainty one of the limitations of the ekf based slam approach but this also holds for least squares approach is that you only have a unimodal estimate about the where the platform is and where the landmarks are multi-modal estimates are not possible we say either the landmark is here or there i don't exactly know this is something that the ekf cannot do which other filters such as the particle filter would be able to do but it's still a standard toolbox it's kind of the first slam algorithm that was out there in the robotics community in order to solve the online slam problem so um it is kind of a key building block to know how to perform this recursive state estimation and estimate where the vehicle is in the environment and what the map of the environment actually looks like with this i'm coming to the end if you want to go through that again i recommend the probabilistic robotics book i basically took the notation from it chapter 10 will tell you more about ekf based slam so with this thank you very much for your attention and i hope you learned this basic slam algorithm i have now seen simultaneous localization and mapping using the ekf and also have an idea how that works for example and how this relates for example with respect to bundle adjustment or other state estimation techniques which would use a standard least squares approach so with this thank you very much for your attention

Channel: Cyrill Stachniss

Views: 6,435

Rating: 4.9135137 out of 5

Keywords: robotics, photogrammetry

Id: X30sEgIws0g

Length: 67min 3sec (4023 seconds)

Published: Fri Oct 02 2020