fast slam

[chrisplyn]

Hi, I am using your code to solve a localisation problem, is there way I can contact you and ask you a couple of questions? Thanks!

[chrisplyn]

@nwang57

[nwang57]

Hi,

I am glad my little project can help with your localisation problem. You can send me emails about your questions and I will try to answer them.

On Sat, 18 Nov 2017 at 4:49 PM, chrisplyn [email protected] wrote:

@nwang57 https://github.com/nwang57

— You are receiving this because you were mentioned.

Reply to this email directly, view it on GitHub https://github.com/nwang57/FastSLAM/issues/1#issuecomment-345473853, or mute the thread https://github.com/notifications/unsubscribe-auth/AGvRjYdPA3Byj1oLruodY99FChFMsKQYks5s31DOgaJpZM4QhHDy .

[chrisplyn]

@nwang57 thanks a lot, can you give me your email address?

[chrisplyn]

@nwang57 Basically my problem is I was given a robot moving in a grid world with certain number of landmarks, I don't know where is the robot initially.I tried to use your code to find the robot's initial location based on measurements data, but the particle filter estimates was far away from the robot's actual location.

[nwang57]

For the FastSLAM algorithm, to my knowledge, it was used to estimate the robot path which means we have to know the initial position of the robot in the world. The algorithm is a way to reduce the path estimate error based on motion and measurement data.

In your case, using the particle filter to estimate the initial position which means we need to estimate the Pr(x|z_1, z_2, z_3) (x is the robot initial location, z_1, z_2, z_3 are the measurements) z_i given x follows a normal distribution with mean mu_i(x) and some variance. Based on Bayes Rule, Pr(x|z_1, z_2, z_3) is proportional to Pr(z_1|x)*Pr(z_2|x)*Pr(z_3|x)*Pr(x) (note the conditional distribution are independent). We could use particle filter to get a estimate of this distribution:

1. Initialize N particles randomly (set up the prior Pr(x))
2. Get the robot measurements (z_1, z_2, z_3)
3. For each particle, get their individual measurements (z_i1, z_i2, z_i3) for particle i
4. compute the posterior probability for each particle i by Pr(x)*Pr(z_i1|x)*Pr(z_i2|x)*Pr(z_i3|x)
5. Normalize the posterior probability and sample those particles with replacement.
6. Keep iterate.

Let me know how you approach the problem and we can discuss.

On 19 November 2017 at 11:49, chrisplyn [email protected] wrote:

@nwang57 https://github.com/nwang57 Basically my problem is I was given a robot moving in a grid world with certain number of landmarks, I don't know where is the robot initially.I tried to use your code to find the robot's initial location based on measurements data, but the particle filter estimates was far away from the robot's actual location.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/nwang57/FastSLAM/issues/1#issuecomment-345531063, or mute the thread https://github.com/notifications/unsubscribe-auth/AGvRjWufh4MIeShSaspOLO-2W1FbiFjSks5s4Fv8gaJpZM4QhHDy .

[chrisplyn]

warehouse = ['..#..','.....','..#..','.....','....@']
max_distance = 5.0
max_steering = PI/2.+0.01 
particle_size = 100
world_size = 10
state = State(warehouse, max_distance, max_steering)

measurements = state.generate_measurements(noise = True)

particles = [Particle(np.random.uniform(0,world_size), np.random.uniform(-world_size,0), np.random.uniform(-PI,PI)) for i in range(particle_size)]
robot = Particle(0, 0, 0, is_robot=True)

#
obs = robot.sense(measurements)
#
for p in particles:
    p.update(obs)

particles = resample_particles(particles,particle_size)

for p in particles:
    print(p)

@nwang57 Here is what I did, I just used your code get the initial location of the robot. in real case warehouse is unknown, the measurements are based on the robot's actual location, and we don't the correspondence between measurements and landmarks.

Just after one iteration, I only have one particle left.