# How to implement Bounded Angle Vision in Particle Filter?

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));
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.

• Welcome to robotics M Faisal Hameed, could you explain what coordinate frames you are using here? Is everything relative to the robot coordinate frame, if so you should be able to use basic trig to calculate the angle of each particle & gate according to the boundaries, as Ian suggests. If your particles are in a World coordinate frame, but your robot is viewing in its own coordinate frame, then the answer will be rather different (and your existing distance calculation will need work too). – Mark Booth Jul 2 '13 at 14:05
• Coordinates are relative to the robot and for distance calculation what you will suggest? Because I am using Euclidean Distance method and my Robot gets localized correctly. – M Faisal Hameed Jul 3 '13 at 7:59
• Thanks for the information M Faisal Hameed, but on stack exchange it is better to add information requested in comments by editing your question, that way the comments can be tidied up (deleted) and so they no longer distract people from the question. – Mark Booth Jul 3 '13 at 11:56

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.

• Yes my robot has a heading direction (orientation). What do dx,dy representing here. Is it robot coordinates or landmark coordinates? I modified your formula a little bit. I think it should be as ( dy = landmark.y – robot.y; dx = landmark.x – robot.x;point_angle = Math.atan2(dy, dx);) Please correct me if I am wrong. – M Faisal Hameed Jul 3 '13 at 9:36
• @MFaisalHameed This information doesn't seem to fit with your earlier comment that Coordinates are relative to the robot. Could you edit your question with some examples of x, lm.getX();, y, lm.getY(); for a two particles with the robot at two different positions? That should only be a table with 4 rows and 5 columns and will tell us if you have misunderstood what we mean by coordinate frame. – Mark Booth Jul 3 '13 at 12:03
• My dx and dy are taken from your code: double dx = x - lm.getX(); and double dy = y - lm.getY();. I assumed that x and y represent the robot position, is that not the case? – Ian Jul 3 '13 at 17:57