1
$\begingroup$

I am implementing a particle filter in Java. The problem with my particle filter implementation is that the particles suddenly go away from the robot i.e the resampling process is choosing particles which are away from robot more than those which are near.It is like particles chase the robot, but always remain behind it. I am trying to find the root cause, but to no luck. Can anyone please help me where I am going wrong?

I am adding all the imp. code snippets and also some screenshots in consecutive order to make it more clear.

1 2 3 4 5

Details:

I am using a range sensor which only works in one direction i.e. its fixed and tells the distance from the obstacle in front. If there is no obstacle in its line of vision, then it tells the distance to boundary wall.

Code: Calculating Range

/*
 * This method returns the range reading from the sensor mounted on top of robot. 
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */

private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}   /*
 * This method returns the range reading from the sensor mounted on top of robot. 
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */
private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}

Calculating Weights

/*
 * This method calculates the importance weights for the particles based on the robot_range which is 
 * the reading of the range sensor for the robot.
 */
private double measurementProbability(int index)
{
    double probability=1;
    double particle_x_position=particleListX.get(index);
    double particle_y_position=particleListY.get(index);
    double particle_Vx=particle_x_position;
    double particle_Vy=particle_y_position;
    int range_counter=0;
    int loop_counter=0;

    int distance = calculateRange(particle_x_position, particle_x_position, particle_Vx, particle_Vy ,range_counter, loop_counter); 
    probability *= calculateGaussianDistance(distance, senseNoise, robot_range);
    //System.out.println(probability);          
    return probability;
}

private double calculateGaussianDistance(double mu, double sigma, double x )
{
    double gDistance=Math.exp(-(((Math.pow((mu - x),2))/(Math.pow(sigma,2)) / 2.0) / (Math.sqrt(2.0 * Math.PI * (Math.pow(sigma,2))))));
    return gDistance;
}

Resampling

/*
 * This method provides a resampled particle back to the list. It chooses a particle randomly 
 * from the list based on the weights with replacement.
 */
private int giveResampledParticle()
{
    int index = randomInteger(0, n-1);
    double sample =0.0;
    double maxWeight = maximumWeight();

    sample += randomDouble(0, maxWeight);
    while(sample > particleListProbability.get(index))
    {
        sample -= particleListProbability.get(index);
        index = (index +1) % n;
    }
    return index;
}
$\endgroup$
  • $\begingroup$ 1. We can't see the screenshots. 2. Instead of posting code, it would be much more helpful if you explain your logic/algorithm concisely. If there is a bug in the code you posted, I'm afraid it will be quite difficult for us to figure that out just by looking at it. It is, however, quite possible for us to understand the logic of your implementation of the particle filter and figure out if there's anything else that needs to be done with the algo. $\endgroup$ – metsburg Sep 22 '14 at 7:32
2
$\begingroup$

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 calculateRange(double x, double y, double robot_orientation)
{
    double Vx = x, double Vy = y;
    int pace = 2;
    int counter = 0;
    while(robotIsWithinBoundary(Vx, Vy))
    {
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            break;
        }
        if(line1.intersects(obst2))
        {
            break;
        }

    }
    return counter*pace;
}
$\endgroup$
  • $\begingroup$ Thanks, yes i was using robot orientation for both particles and robot to calculate range. I will pass orientation as parameter now. $\endgroup$ – Ambidextrous Sep 25 '14 at 20:51

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.