How to implement Bounded Angle Vision in Particle Filter?

Question

I have built a Particles Filter simulator and I wanted to add the following functionalities.

1. Limited Range Vision (Robot can see up to 50 meters)
2. Limited Angle Vision (Robot can see within a certain angle w.r.t its current orientation. e.g. If the current orientation is 30 degree then it can see in the range from 0 to 60 degree.)

I have managed to add the Limited Range Vision functionality but unable to add Limited Angle Vision.

Method to Sense the landmarks distance within the range

public double[] sense(boolean addNoise) {
    double[] z = new double[World.getLandmark().getLandmarks().size()];
    for (int i = 0; i < z.length; i++) {
        Point lm = World.getLandmark().getLandmarks().get(i);
        double dx = x - lm.getX();
        double dy = y - lm.getY();
        double dist = Math.sqrt(Math.pow(dx, 2) + Math.pow(dy, 2));
        if (addNoise) {
            dist += Util.nextGaussian(0, sense_noise);
        }
        if (isBoundedVision()) {
            // TODO Limited angle vision
            // if robot can see within 60 degree angle w.r.t its orientation
            if (dist <= laserRange) {
                z[i] = dist;
            }
        } else {
            z[i] = dist;
        }
    }
    return z;
}

Method to calculate the probability of this particle

@Override
public double measurement_prob(double[] measurements) {
    double prob = 1.0;
    int c = 0;
    double[] myMeasurements = sense(false);
    for (int j = 0; j < measurements.length; j++) {
        if (measurements[j] != 0) {
            prob *= Util.gaussian(myMeasurements[j], sense_noise, measurements[j]);
            c++;
        }
    }
    if (isBoundedVision()) {
        if (c > 0) {
            // increase the probability if this particle can see more landmarks
            prob = Math.pow(prob, 1.0 / c);
        } else {
            prob = 0.0;
        }
    }
    return prob;
}

Coordinates are relative to the robot and for distance calculation I am using the Euclidean Distance method and my Robot gets localized correctly.

Answer 1

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:

offset = Math.abs(robot_angle_radians - point_angle) % (2 * Math.PI); 
if (Math.toRadians(30) < offset) // then it's out of +/- 30 degree range

Make sure you are consistent between a polar coordinate system and a compass rose coordinate system.