So now we’ll learn all about Linear Graph Slam, and that’s quite a bit. And it’s really simple. Every time there’s a constraint, initial position, motion or measurement, we take this constraint and add something to omega and C. What we add is the constraint itself, but it’s up multiplied by a strength factor, there is nothing else but one over sigma, the uncertainty in motion or measurements. And then when we’re done with this adding, we simply calculate this guy and outcomes our best possible path and along with a map of all the landmarks. Isn’t that something? Isn’t that really cool? So let’s dive in and have you program your own real robot example. This is a fairly complicated generalization of what we just saw. I’m giving you an environment where you can specify the number of landmarks that exists, the number of time steps you want the world to run, the world size, the measurement range, that is the range at which a robot might be able to see a landmark of it’s further away than this response see it, emotion noise and measurement noise, and the distance parameter, the distance specifies how faster what moves in each step. Then I’m giving you a routine which makes the data. It takes all these parameters and outputs a data field that contains sequence of motions and a sequence of measurements, the code comments on exact format of what data looks like. Now I wanted to program the function Slam that inputs the data in various important parameters and then outputs my result, a sequence of estimated poses, the robot path, and estimated landmark positions. This is really challenging to program. It’s based on the math I just gave you. The vocal coordinates are now X and Y coordinates. The measurements are differences in X and Y, so you have to duplicate things for X and things for for Y. I myself put them all into one big matrix, but you can have them in two separate matrices if you so wish. You have to apply everything we’ve learned so far, including the the weights of one of our measurement noise. And then with the motion noise, these happen to be equivalent in this case, but they might be different. We need to run Slam and return back to me a result data structure. I’m also supplying you with the print results routine, so you can go in and see how the result has to look like. There’s an example routine that doesn’t work that outputs all the correct formats, but it tends not to implement the estimates that I want you to estimate. You have to bring this to life and turn this into an amazing Slam routine. So that when you run it, you get the same results that I do. For the examples here, where there’s an estimated path, an estimated landmark positions. There’s one last thing I want you to know is I assume the initial robot position is going to be the center of the world, so it’s the word size of 100 is going to be 50-50. Or here it’s printed as 49.999, but it’s the same as 50. So you have to put in a constraint that sets the initial robot pose to the center of the world