Particles not behaving correctly in the implementation of particle filter

Question

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.

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;
}

Answer 1

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;
}