
Title:
Final Solution  Artificial Intelligence for Robotics

Description:

So to implement the full particle filter,

the only thing is really missing is the measurement_prob function.

And that's a little bit more involved because I have to

really compare what the exact measurement would be for any ove, overt particle.

And what I sensed and compute the probability correspondence between

the correct measurements, and what I sensed over here.

To do this, I calculate predicted measurements using the sense function.

Here comes a little flag that I defined.

If I set it to 0, than the sense function acts noise free.

Which is what I want, it could be the measurement model.

But even we you left this out, you're going to get a fine answer on my opinion.

But that makes it a little bit more accurate.

So that allows me to compute the exact bearings of the landmarks for

my particle.

And then I can compare these correct bearings called predicted measurements

with the ones that I received.

Now do this, down here, in the compute errors routine.

Where I go through each measurement and in two steps,

I calculated the error in bearing.

First, it's the absolute difference between my measurement that I observed,

minus the predicted measurement, and there's an i at the end over here.

Let's see if you can see this.

Right there.

And this difference might fall outside minus pi plus pi.

So this line over here just brings it back

to the smallest possible value in this cyclic space of 0 to 2 pi.

So adding pi, adding more load 2 times pi and I subtract pi again.

So this gives me a value between minus pi and plus pi.

I then pluck this error_bearing into a Gaussian.

And here is my Gaussian where I squared it,

I divide it by my bearingnoise squared, complete the exponential, and

use my normalizer to strictly speaking of, don't really need for

the implementation, I can safely omit it because weights are selfnormalized.

But I left it in, so it's actually really a Gaussian.

And I take this Gaussian value and multiply it up into my error function.

So for each of the measurements, I multiply in one Gaussian.

And the final Gaussian is my importance whether I return in this

function over here.

So this is not easy to implement.

I hope you got it right.

Scrolling further down in my code,

I now implement the particle field as follows.

It uses a thousand particles.

And this is exactly the same routine we had before,

where we generate our initial particles.

Here, I set the noise for

these particles, to be bearing_noise, steering_noise and distance_noise.

I don't comment out the measurement generation step, I just take the input.

And then as I go further down, I just run my particle theta.

This is the exact same code you're familiar with.

There's a motion update,

there's a measurement update, and there's a resampling step over here.

All those are the same as before.

And at the very end I just print the result of get_position.

So if I do this for my example, here is the position I get.

And I guess for, I forgot to uncommon the Robot coordinate over here.

But if you look at the values over here, 7.0 is about the same as 8,

49 is about the same as 48, and 4.31 is about the same as 4.35.

So this particle filter,

clearly does a pretty job in estimating the forward position