19

The issue you're running into is often referred to as sample impoverishment. We can see why your approach suffers from it with a fairly simple example. Let's say you have 3 particles and their normalized weights are 0.1, 0.1, 0.8. Then multiplying by each weight by the 3 yields 0.3, 0.3, and 2.4. Then rounding yields 0, 0, 2. This means you would not pick ...


13

The Rao-Blackwellized Particle Filter (RBPF) as you say in your question performs a marginalization of the probability distribution of your state space. The particle filter uses sampling to represent the multivariate probability distribution of your state space. Using samples to represent a distribution is firstly only an approximation, and secondly not ...


9

As I guess you found out yourself, the resampling method you are proposing is slightly flawed, as it should not alter the number of particles (unless you want to). The principle is that the weight represents the relative probability with respect to the other particles. In the resampling step, you draw from the set of particles such that for each particle, ...


6

Your question addresses three very different problems, all of which are hard with complicated research-type algorithms. Localization: When you have a known map of the environment and an unknown robot position. The most common algorithm for this is Monte Carlo Localizataion. This is a particle filter exactly like what you're describing. Mapping: When the ...


5

For an example of python code that properly implements resampling, you might find this github project to be useful: https://github.com/mjl/particle_filter_demo Plus, it comes with its own visual representation of the resampling process, that should help you debug your own implementation. In this visualization, the green turtle shows the actual position, ...


5

Monte Carlo localization is just another name for a particle filter. Monte Carlo methods are a broader name for computational algorithms that rely on random sampling. A particle filter is a specific application of the general Monte Carlo method for localization, and so it is simply referred to sometimes as Monte Carlo localization. If you ask Lord Google, ...


4

Of course adding random particles will change the pose of the robot. That's the point: To add randomness to account for errors you can't compute yourself. You should add random particles every time the state of the robot may change, then adjust the weight of the particles based on which seem likely given sensor data. This is a very basic definition of ...


4

While this can be done it will be problematic. The issue comes from the fact that particle filters are a method for dealing with multi-modal probability distributions (PD) while Kalman filters assume your PD can be well represented with a Gaussian distribution. I can conceive of a few methods for doing so if you really must. The most naive would be to ...


4

Most particle filter implementations will use some kind of importance sampling, which does not require you to make an assumption on the underlying distribution. This is one of the main reasons for using a particle filter in the first place. Importance sampling does not sample from the estimated distribution, but from your set of weighted samples. This ...


4

Let $w_1 \dots w_n$ be the weights of $n$ particles, $p_i \triangleq \frac{w_i}{\sum\limits_{j=1}^{n}w_j}, \sum\limits_{j=1}^{n} p_j = 1$, then as you posted, the probability of the $i$th particle surviving in the resampling procedure for multinomial resampling is: $$ P(Survival_i) = 1 - (1 - p_i)^n $$ In systematic resampling, one concatenate $p_1 \dots ...


4

$p\left(x_t|u_t,x^{[m]}_{t-1}\right)$ should be created from your motion model. See chapter 5 in your book. Algorithms can be found in table 5.3 for a velocity model or table 5.6 for a odometry model. Roughly speaking: If your robot has a speed of 1m/s and it moves straight (which may be your $u_t$) and your update rate is 1Hz, then you could move each ...


4

I have used POMDP like models on top of a localization algorithm (Adaptive Monte Carlo Localization, from ROS), and a person detector [1][2] to find and follow a person with a humanoid robot. These two algorithms generate the input (observation) for the POMDP model in [1] and [2]. Also in [3] they used a POMDP model with similar input. As next step we used ...


3

A little background. You need to weight each particle by the liklihood of the particle being correct. The probability the particle is correct is given by the probability that it is correct given the measurements. Note that the "weight" (which is a terrible term) is simply the probability of the particle being correct. Therefore, each particle is really an ...


3

If you don't know the location of the obstacles and want to apply your filter, I see two solutions: Do a quick mapping using a simple algorithm easy to implement like the Occupancy Grid Algorithm (with this you localize the obstacles relatively to your robot) Apply your particles filter Move the robot Relocalize your robot using odometry Correct odometry ...


3

Once you have enough particles to resolve your position, the effect of adding more particles shrinks to zero. You are likely seeing the best possible results that your particle filter can achieve. It looks like the smallest adequate number of particles for your simulation is somewhere between 120 and 1200, and my guess is that if you plotted the Odom ...


3

Particle filter According to the OP a robot with at least a distance sensor is available and a map too. That's a nice starting point for developing a hypothesis tracker aka particle filter. At first a game engine is needed which simulate the map and the position of particles. The game-engine calculates the expected sensor-information from the distance-...


3

I am going to give a 5 minutes answer because I am still wondering how do I got to this page.... =P Assuming you know Particle Filter concept. 1 - What is "Clustered Particle Filter"? The main idea of "Clustered Particle Filter" is to keep hypotheses alive! Imagine that in your environment you have two regions that are very similar. Almost the same. But ...


2

It looks like you're missing the angle that the robot is currently looking, e.g. robot_angle_radians. Once you know where the robot is looking, you just use the atan2 function (see also: example usage) on each point: point_angle = Math.atan2(dy, dx); // in radians Then you want to check whether the angle is not more than 30° off of the robot's angle: ...


2

It is not completely clear what your question is, but I assume you wanted to ask: Why does the output not change with the number of particles above a certain number of particles? By allowing amcl to use more samples, you decrease the errors (or inaccuracies) that are caused by the fact that the particle filter approximates your probability distributions ...


2

Since you have a 2D sensor which you can not rotate in a controlled way, you can only expect to perform SLAM in a 2D plane. Your best bet is to use the IMU for attitude estimation (roll and pitch only, since you do not have a compass) and correct each slightly rotated 2D laser scan accordingly. If your IMU does not provide you with an attitude estimate ...


2

I agree that the motion models in Probabilistic Robotics are badly suited for omnidirectional robots. I always interpreted the models presented there as examples only that should enable you to devise a custom model for your own robot. First of all you need to model and solve the forward kinematics for this kind of omnidirectional drive. I guess you already ...


2

To be clear, this is a single-point range sensor, correct? It is possible to do Monte Carlo localization with such a sensor, but it will take a very long time to converge if it does so at all, and will be easily confused. Each additional point you can add when you're doing localization will improve performance, so if you can get a few more of these sensors, ...


2

Your function calculateRange() uses a variable robot_orientation which I cannot see from your code snippets that it is set or changed. I would expect something like: double particle_orientation = particleListOrientation.get(index); inside your function measurementProbability() and then call calculateRange() with that additional parameter. private int ...


2

Your description of sample impoverishment and the way to fix it seems about right. Resampling only when the variance gets low is doing exactly what you are asking for when you say the measurements come in asynchronously. You can also improve matters by selecting the right resampling strategy. Using e.g. stratified resampling you can make sure that your ...


2

As said by Jacob, sample impoverishment is inherit to the Sampling-Importance-Resampling family of particle filters. An alternative solution which does require some extra effort is to switch to a Markov Chain Monte Carlo (MCMC) particle filter. MCMC relies on constructing a Markov chain that has a stationary distribution which is equal to the distribution ...


2

Grid based FastSlam relies on the same principle that Landmakr based FastSlam. The difference is that we are not working with each grid cell as a landmark, but the whole gridmap itself. For Grid based FastSlam, each particle updates its own grid-map using the data from the range sensor (Lidar, UltraSound, etc.) and its odometry. This is called "Mapping with ...


2

This is a good question, in that it's actually two good questions. The answer to which of those two options to use, is that they are the same. Your idea of "confusion" is intuitively correct, but is not a function of how you structure the state, but is instead about how you associate the measurements to part of the state (which robot was observed). ...


2

The third line comes from what it is called Markov Assumption and it is Stochastic Processes stuff. Basically, it says that a distribution is not altered by the insertion and/or remotion of variables that the distribution does not really depend on. It goes like this: Is assumed that $ z_t $ simply does not depend on the previous reading history, inputs $ u_{...


1

I'm not familiar with particle filtering, but if applying the weights between each sensor read is causing issues, why not accumulate weights to be applied between sensor calls and apply them in bulk after all sensors have been read? For example, it sounds like you're doing: [Read 1]-< apply weights >-[Read 2]-< apply weights >-[Read 3]-< apply ...


1

The new particle should be created from your motion model, basically you select one random possible outcome of the data you measured. This is an example for differential drive, the random.gauss calls are resoponsible for the sampling part: def update_sample(self, sample, left_ticks, right_ticks): """ Returns a new sample updated according to measured ...


Only top voted, non community-wiki answers of a minimum length are eligible