In mobile robotics, one of the most important problems is localization: determining the robot's position and orientation within an environment. A robot must be able to estimate where it is, understand its surroundings, and update its position as it moves.
In this tutorial, we explore a simple robot localization system implemented in C++. The program simulates a robot moving in a two-dimensional world containing eight known landmarks. By measuring the distances to these landmarks and comparing them with the predictions of multiple particles, the algorithm estimates the robot's position.
The project introduces several fundamental concepts:
Robot motion and orientation.
Random numbers and Gaussian noise.
Distance measurements from landmarks.
Probability and measurement likelihood.
Particle filters and resampling.
Localization error evaluation.
The main goal is to understand how a particle filter can estimate the position of a robot even when motion and sensor measurements are affected by uncertainty.
1. Defining the Environment
The simulated environment is a square world measuring 100 × 100 meters.
The robot operates within this world, and eight landmarks are placed at known coordinates:
double landmarks[8][2] = {
{20.0, 20.0},
{20.0, 80.0},
{20.0, 50.0},
{50.0, 20.0},
{50.0, 80.0},
{80.0, 80.0},
{80.0, 20.0},
{80.0, 50.0}
};Each landmark is represented by its x and y coordinates.
The world size is defined as:
double world_size = 100.0;The robot's position is described by three variables:
x: horizontal position.y: vertical position.orient: orientation angle in radians.
The orientation determines the direction in which the robot moves. An angle of zero corresponds to movement along the positive x-axis, while an angle of π/2 corresponds to movement along the positive y-axis.
The environment uses cyclic boundaries. When the robot moves beyond one edge of the world, it reappears on the opposite side. This behavior is implemented using the mod() function.
2. The Robot Class
The Robot class represents the simulated robot and contains its position, orientation, noise parameters, and motion and sensing methods.
Initializing the robot
The constructor generates a random initial position and orientation:
Robot()
{
x = gen_real_random() * world_size;
y = gen_real_random() * world_size;
orient = gen_real_random() * 2.0 * M_PI;
forward_noise = 0.0;
turn_noise = 0.0;
sense_noise = 0.0;
}The initial coordinates are randomly selected within the world, and the orientation is selected between zero and 2π radians.
The robot initially has no motion or sensor noise. These parameters can be changed using set_noise().
Setting the robot's pose
The set() method assigns a specific position and orientation to the robot:
myrobot.set(30.0, 50.0, M_PI / 2.0);This places the robot at coordinates (30, 50), facing upward along the positive y-axis.
The method also checks whether the coordinates and orientation are within their permitted ranges. If an invalid value is supplied, the program throws an exception.
Configuring noise
Real robots do not move or measure distances perfectly. Their motors, wheels, and sensors introduce uncertainty.
The following method configures the noise parameters:
myrobot.set_noise(5.0, 0.1, 5.0);The three values represent:
Forward movement noise: 5.0.
Turning noise: 0.1 radians.
Sensor measurement noise: 5.0.
In this implementation, the parameters passed to the Gaussian random generator are standard deviations, despite the variable names used in the original code.
Noise allows the simulation to reproduce some of the uncertainty encountered in real robotic systems.
3. Simulating Robot Motion
The move() method changes the robot's orientation and position.
For example:
myrobot.move(-M_PI / 2.0, 15.0);The first parameter represents the turning angle, and the second represents the forward distance.
A negative turning angle rotates the robot clockwise. The movement direction is calculated using trigonometric functions:
x = x + cos(orient) * dist;
y = y + sin(orient) * dist;Here, dist is the commanded forward distance plus a random noise component.
The orientation is normalized using:
orient = mod(orient, 2 * M_PI);The same cyclic operation is applied to the x and y coordinates.
The method returns a new Robot object with the updated pose and the same noise parameters.
An important detail is that the original robot is modified before the updated object is returned. Therefore, the caller should assign the result when continuing the simulation:
myrobot = myrobot.move(0.1, 5.0);This simulates a rotation of 0.1 radians followed by a forward movement of 5 meters, with motion noise added.
4. Measuring Distances to Landmarks
The robot estimates its surroundings using the sense() method.
For each landmark, the distance is calculated using the Euclidean distance formula:
[
d_i=\sqrt{(x-x_i)^2+(y-y_i)^2}
]
where:
(x,y) are the robot's coordinates.
(x_i,y_i) are the coordinates of landmark (i).
(d_i) is the distance between the robot and the landmark.
Random sensor noise is then added to the calculated distance.
The resulting measurements are stored in a vector:
vector<double> z = myrobot.sense();The vector contains eight distance measurements, one for each landmark.
Because noise is added to every measurement, repeated calls to sense() can produce different results even when the robot remains at the same position.
The read_sensors() method converts these measurements into a string so they can be printed to the console.
5. Understanding Gaussian Probability
A particle filter requires a way to determine how well a particle's predicted measurements match the robot's actual sensor readings.
This is achieved using a Gaussian probability distribution.
The Gaussian function in the program is:
double gaussian(double mu, double sigma, double x)
{
return exp(-(pow((mu - x), 2)) /
(pow(sigma, 2)) / 2.0) /
sqrt(2.0 * M_PI * (pow(sigma, 2)));
}The mathematical expression is:
[
p(x)=\frac{1}{\sqrt{2\pi\sigma^2}}
e^{-\frac{(\mu-x)^2}{2\sigma^2}}
]
In this formula:
(\mu) is the predicted distance.
(x) is the measured distance.
(\sigma) is the sensor noise standard deviation.
If the predicted distance is close to the measured distance, the Gaussian probability is relatively high. If the difference is large, the probability decreases.
This provides a mathematical way to evaluate how compatible a particle's position is with the sensor measurements.
6. Calculating Particle Weights
A particle is a possible hypothesis about the robot's position and orientation.
The algorithm creates 1,000 particles:
int n = 1000;
Robot p[n];Each particle starts at a random position in the world and receives its own motion and sensing noise parameters.
After the robot moves and obtains a new set of measurements, every particle performs the same commanded movement:
p2[i] = p[i].move(0.1, 5.0);
p[i] = p2[i];The algorithm then calculates the weight of each particle:
double w[n];
for (int i = 0; i < n; i++) {
w[i] = p[i].measurement_prob(z);
}The measurement_prob() method compares the distances predicted by the particle's position with the eight distances measured by the actual simulated robot.
The individual Gaussian probabilities are multiplied together:
[
w_i=\prod_{j=1}^{8}p(z_j\mid x_i)
]
Here, (w_i) represents the likelihood of the measurements given particle (i).
A particle that predicts measurements similar to the robot's observations receives a higher weight. A particle whose predicted measurements disagree with the observations receives a lower weight.
These weights are the basis for selecting which particles should survive the resampling stage.
Why are the weights sometimes extremely small?
The program multiplies eight probability values together. Since each probability can be much smaller than one, the resulting product may become extremely small or even underflow to zero in floating-point arithmetic.
This is a numerical limitation rather than necessarily an error in the algorithm.
For more advanced implementations, logarithmic probabilities or normalized log weights can help prevent numerical underflow.
7. Normalizing Probability Weights
A separate example in the project demonstrates how to normalize a set of weights.
Consider the following values:
double w[] = {0.6, 1.2, 2.4, 0.6, 1.2};The total weight is:
[
S=0.6+1.2+2.4+0.6+1.2=6
]
Each normalized probability is calculated as:
[
P_i=\frac{w_i}{S}
]
The resulting probabilities are:
| Particle | Original weight | Normalized probability |
|---|---|---|
| P1 | 0.6 | 0.10 |
| P2 | 1.2 | 0.20 |
| P3 | 2.4 | 0.40 |
| P4 | 0.6 | 0.10 |
| P5 | 1.2 | 0.20 |
The probabilities sum to one.
This means that P3 has a 40% probability of being selected in a single independent draw from this discrete distribution.
The corresponding C++ function is:
void ComputeProb(double w[], int n)
{
double sum = 0.0;
for (int i = 0; i < n; i++) {
sum += w[i];
}
if (sum <= 0.0) {
throw invalid_argument("Total weight must be positive");
}
for (int i = 0; i < n; i++) {
w[i] /= sum;
cout << "P" << i + 1 << "=" << w[i] << endl;
}
}The normalized probabilities can be used directly for categorical sampling. However, the main particle-filter implementation uses a different approach: the resampling-wheel algorithm.
8. Particle Resampling
Resampling is a fundamental step in Monte Carlo Localization.
After the weights have been calculated, particles with higher weights should have a greater chance of being selected. Particles with low weights should be less likely to survive.
The program uses a resampling wheel based on the maximum particle weight:
int index = gen_real_random() * n;
double beta = 0.0;
double mw = max(w, n);
for (int i = 0; i < n; i++) {
beta += gen_real_random() * 2.0 * mw;
while (beta > w[index]) {
beta -= w[index];
index = mod((index + 1), n);
}
p3[i] = p[index];
}The algorithm works as follows:
Select a random starting particle.
Increase the variable
betaby a random amount between zero and twice the maximum weight.Compare
betawith the current particle's weight.Move to the next particle while
betaexceeds the current weight, subtracting each skipped weight.Select the particle where the wheel stops.
Repeat until the new population contains the required number of particles.
Particles with high weights occupy larger sections of the wheel and are consequently more likely to be selected.
The new particle population replaces the old one.
This process concentrates the particle population around positions that better explain the robot's measurements.
In a correctly functioning particle filter, the population gradually becomes more representative of the robot's actual position.
9. Evaluating Localization Accuracy
The evaluation() function measures the average positional error between the simulated robot and the particle population.
For each particle, the algorithm calculates the difference between its position and the robot's true position.
Because the world has cyclic boundaries, the shortest distance across the boundary must be considered.
The function calculates:
[
e_i=\sqrt{dx_i^2+dy_i^2}
]
The average error is:
[
E=\frac{1}{N}\sum_{i=1}^{N}e_i
]
where (N) is the number of particles.
The result is printed after each simulation step:
cout << "Step = " << t
<< ", Evaluation = "
<< evaluation(myrobot, p, n)
<< endl;For example, an output line might look like this:
Step = 10, Evaluation = 4.25This is an illustrative example, not a guaranteed output from a particular execution.
A lower evaluation value indicates that the particle positions are, on average, closer to the simulated robot's true position.
It is important to note that this function measures positional error only. It does not directly evaluate orientation accuracy.
10. Putting Everything Together
The complete Monte Carlo Localization process repeats the same sequence over multiple time steps:
Move the simulated robot.
Collect noisy sensor measurements.
Move all particles according to the motion model.
Calculate the measurement likelihood for every particle.
Resample the particle population according to the weights.
Evaluate the average positional error.
Repeat the process for the next movement.
In the main simulation, the process runs for 50 steps using 1,000 particles.
At each step, the robot's true pose is available to the simulator, while the particle filter estimates its position using motion predictions and landmark measurements.
The true pose is used for evaluation, but a real robot would not normally have direct access to its exact position.
11. Important Implementation Notes
There are several details worth considering when developing this project further.
Randomness: The program uses random_device and mt19937 to generate random values. Consequently, the particle trajectories and numerical results can differ between executions.
Noise parameters: The standard deviation used by normal_distribution must be nonnegative. A zero standard deviation produces no random variation.
Particle initialization: The particles initially cover the world uniformly. The filter must identify which regions are consistent with the sensor measurements.
Numerical stability: Multiplying many Gaussian probabilities can cause underflow. A log-likelihood implementation is a useful improvement.
C++ portability: The declarations Robot p[n], Robot p2[n], and Robot p3[n] use variable-length arrays. These are not standard C++ when n is a runtime variable. A portable implementation should use std::vector<Robot> instead.
Sensor model: The implementation assumes independent distance measurements and uses the same sensor noise standard deviation for all landmarks.
Resampling: If every particle weight becomes zero, the current resampling algorithm cannot work correctly. A robust implementation should detect this condition and recover, for example by reinitializing or redistributing particles.
This project demonstrates the essential principles of Monte Carlo Localization using C++.
By combining a robot motion model, noisy landmark measurements, Gaussian likelihoods, and particle resampling, the algorithm estimates the robot's position in an uncertain environment.
The most important idea is that the robot does not rely on a single position estimate. Instead, it maintains a population of possible states and progressively favors those that best explain the observed sensor data.
Particle filters are useful in mobile robotics because they can represent complex and multimodal position uncertainty. The same underlying concepts can be applied to autonomous navigation, mobile robot tracking, and other systems that must estimate their state from noisy observations.
This implementation provides a foundation for further experiments, including visualization of the particle population, improvements to numerical stability, and more advanced localization techniques.
