
Title:
0638 Implementing Slam

Description:

So now we've learned all about Linear GraphSLAM,

and that's quite a bitand it's really simple.

Every time there's a constraint

Initial Position, Motion or Measurement

we take this constraint and add something to Omega, Xi.

And what we add is the constraint itself,

but it's up multiplied by a strength factor.

There's nothing else but 1 over sigma

the uncertainty in Motion or in Measurements.

And then when we're done with this adding

we simply calculate this guy

and out comes our best possible PATH

and along with the 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 exist,

the number of time steps you want the robot to run,

the world_size, the measurement_rangethat is

the range at which a robot might be able to see a landmark

if it's further away than thisit just won't see it;

a motion_noise, a measurement_noise,

and a distance parameter.

The distance specifies how fast a robot moves in each step.

And then I'm giving you a routine which makes the data.

It takes all these parameters and it outputs a data field

that contains a sequence of motions and a sequence of measurements.

The code comments on the exact format of what data looks like.

Now I want you to program the function, SLAM,

that inputs the data and various important parameters

and it outputs my resulta 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 robot 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 y.

I, myself, put them all into one big matrix,

but you could have them in 2 separate matrices, if you so wish.

You have to apply everything we learned so far,

including the weights of one with our measurerment_noise

and one with our motion_noise.

These happen to be equivalent, in this casebut they might be different.

And then you have to run SLAM

and return back to me a result data structure.

I'm also supplying you with the print_result routine

so you can go in and see how the result has to look like.

There's an example routinethat doesn't work

that outputs all the correct formats,

but it tries not to implement the estimate 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

and estimated landmark positions.

There's one last thing I wanted to know

is I assume the initial robot position

is going to be in the center of the world.

So it's the realworld set of 100

and it's going to be 50/50or here it's printed as 49.999,

but this is the same as 50.

So you have to put in a constraint

that sets the initial robot pose

to the center of the world.