The algorithm
The functionekf_slam implements the following cycle, adapted from Table 10.2 of Thrun, Burgard and Fox (2005).
- Predict the robot pose using and propagate the covariance using , and . The landmark means remain unchanged.
- For each observation , compute the predicted observation, innovation, Jacobian and innovation covariance for each landmark already in the map. Use the squared Mahalanobis distance to compare candidates.
- If no candidate passes the gate , initialize a landmark using
add_landmark. This enlarges the state and covariance without correcting the existing pose estimate. - Otherwise, use the candidate with the smallest squared Mahalanobis distance and correct the joint mean and covariance. The correction may affect landmarks that were not observed.
The robot model
We reuse the differential-drive model from EKF localization. The input is a pair of wheel angular velocities. The functiong integrates the resulting linear and angular velocities over one sampling interval; G and V differentiate this motion with respect to the pose and wheel velocities, respectively. The sampling interval is 0.2 s for both simulated sensors.
Implementing one filter step
The first two functions below compute the range and bearing to a landmark and the corresponding Jacobians.innov wraps the bearing residual into so that angles on opposite sides of the branch cut are compared correctly.
When a new landmark is added, add_landmark computes its world coordinates from the current pose and the observation. Notice that the new rows and columns of the covariance matrix are not initialized to zero: the new landmark inherits uncertainty and cross-covariances from the pose.
For a known landmark, ekf_slam uses the covariance update and symmetrizes the result. In exact arithmetic this is equivalent to the Joseph form for the Kalman gain used here:
The Joseph form is more robust to roundoff. Symmetrization alone does not guarantee positive definiteness; the final covariance eigenvalues in the repeated baseline runs are checked later. The code retains the shorter update for this demonstration.
The room and the drive
The room is 8 m by 8 m. Eight cylindrical pillars, each with a radius of 10 cm, lie on a circle of radius 2.8 m. The TurtleBot drives counterclockwise on a concentric circle of radius 2.4 m at 0.2 m/s. We use landmarks within 2.5 m and run the simulation for 603 intervals, approximately 1.6 laps. This gives the robot a chance to revisit landmarks from the beginning of the drive. The true robot follows the prescribed circle. We corrupt the reported wheel speeds with independent Gaussian noise, using a standard deviation of 5% of the magnitude of each commanded wheel speed plus 0.1 rad/s. Both dead reckoning and the EKF integrate these noisy readings. The simulator knows the pillar locations and true pose, but the estimator does not use them for association. Ground truth is used to generate measurements and evaluate the estimates.Extracting landmarks from a lidar scan
The lidar reports 360 ranges per scan, one per degree, with independent Gaussian range noise of standard deviation 1 cm. The filter needs a range and bearing to each pillar center. The scan gives only distances to surfaces. A pillar produces a short run of returns in front of the more distant wall. The code splits the scan at range jumps greater than 15 cm, rejects clusters wider than 25 cm and discards distant detections. It estimates the bearing from the cluster’s beam angles and estimates the center range by adding the known pillar radius to the shortest measured range. The minimum is convenient, but selecting the smallest of several noisy values introduces a bias that we will measure below. On a real robot, a fixed jump threshold is often inadequate because the separation between adjacent beam endpoints increases with distance. Circle fitting can estimate the center and radius of a pole; line fitting can extract walls and corners. laser_segmentation is one ROS 2 implementation of scan segmentation. For an unordered 3D point cloud, another common approach is to remove the ground plane and cluster the remaining points. Open3D providessegment_plane and cluster_dbscan; the Point Cloud Library provides SACSegmentation and EuclideanClusterExtraction.
Geometry and return intensity can help distinguish types of lidar landmarks. Shape descriptors such as FPFH and VFH are also useful in 3D. None of these guarantees a unique identity for identical pillars, however. In this experiment the observations carry no landmark identifiers.
Simulating observations from the D435i
Unlike the lidar, the camera sees only a narrow sector in front of the robot. We use the color-camera intrinsics from the course configuration: px, px and image width 640 px. This is an idealized feature-level simulation, not an RGB-D image-processing pipeline. The simulator projects the known pillar centers, adds Gaussian noise of standard deviation one pixel to the horizontal coordinate, and computes the depth of the near surface along the ray to the center. It does not perform image segmentation or resolve occlusions. We assume a depth standard deviation of m, where is depth in meters. This is an assumed model, motivated by stereo depth error, not a calibrated D435i specification. The camera measures depth along the optical axis. The range lies along the viewing ray. Given the image column and depth , the bearing and center range are The cosine converts optical-axis depth to distance along the ray. Adding the pillar radius then gives the center range in the ideal geometry. We place the camera at the robot origin, facing forward. For a physical TurtleBot, use the camera-to-base transform discussed in the D435i section before expressing the observation in the base frame.Choosing the measurement covariance
The EKF requires a covariance for the errors in the extracted range and bearing, not the raw sensor readings. To estimate the error statistics, we sample 400 random robot poses and compare detections with the corresponding simulated pillars using the known pose. Correspondences here are used for evaluation, not supplied to the SLAM filter.
These values are tuning choices, not new sensor calibration results. The filter uses a zero-mean additive observation-noise model and a local linearization. The measured range bias and range-dependent camera noise do not fit that model exactly. Increasing reduces the weight of each observation and widens the association gate, but does not remove the bias. We will compare this choice with the raw empirical standard deviations at the end.
Running the filter
We now run the estimator with each sensor using identical wheel-noise realizations. Both start with the same pose estimate and empty map. The simulator records the true, dead-reckoned and estimated trajectories, as well as covariance and association diagnostics.Watching the estimation process
The recording places the lidar run on the left and the camera run on the right. The green arrow denotes the true pose and the orange arrow the estimate. Blue dots and ellipses show the estimated landmarks. Gray traces show dead reckoning, gold lines show current measurements, and the camera wedge marks its field of view. The lower plots track position error and map size. The ellipses use a radius of two in Mahalanobis units. For a bivariate Gaussian this encloses about 86.5% of its probability mass, not 95%. Watch what happens to the pose and landmark ellipses when a previously mapped pillar becomes visible again.
Press play or drag the time marker. Scroll to zoom; double-click to reset the view. The panel control at the upper right opens a panel full size.
Interpreting the trajectory error
In the camera run the position error grows to about 0.34 m during the first lap. A pillar leaves the field of view soon after it is observed; as the robot continues, new landmarks inherit uncertainty from its estimated pose. At 67.8 s a landmark from early in the drive is reobserved after a long absence. The position error drops from 0.289 m to 0.149 m in that step and briefly falls below 1 cm in subsequent steps. This is an example of loop closure. The joint covariance allows the update to correct other landmark estimates as well, although the magnitude of each correction depends on its cross-covariances and the innovation. The final camera position error is 0.133 m. The lidar run closes the loop earlier, at 57.8 s; its position error falls from 0.085 m to 0.026 m. It has more simultaneous observations and stays below roughly 10 cm in this run. The shaded region is , twice the root-mean-square position uncertainty implied by the covariance. It is a useful scale for comparison with the Euclidean position error, but it is not a calibrated 95% confidence interval. A statistical consistency test follows below.What has the filter learned about the map?
The heatmap shows the final camera run’s correlation matrix. The first three rows and columns represent the robot pose; each subsequent pair corresponds to a landmark’s two coordinates. Many coefficients are near or . This tells us that coordinate errors tend to vary together, but it does not identify the geometric motion responsible. To find that motion we examine the dominant eigenvector of the landmark covariance block.Testing estimator consistency
A small position error is not enough to establish that the filter is working well. We also need to know whether its reported covariance is commensurate with its errors. Because this is a simulation, we can compute the normalized estimation error squared (NEES) of the pose: where the heading difference is wrapped. Under a correctly specified Gaussian error model, pose NEES follows a chi-squared distribution with three degrees of freedom and has expectation 3. The dashed lines are its central 95% interval. In the two example runs, 97% (lidar) and 99% (camera) of the values fall inside it; both have mean NEES about 3.1. These are not 603 independent statistical tests: errors at successive times are correlated. Instead, run the simulation with independent noise seeds per sensor and at each time average the NEES across runs. Under the same Gaussian assumptions this average has distribution , with central 95% interval approximately . The time series of this average remains correlated, so its fraction of excursions is descriptive. It is not an independent-sample hypothesis test.Results across noise realizations
There are no wrong associations under the scoring rule in the twenty baseline runs. Nineteen recover exactly eight landmarks; the camera run with seed 9 creates a duplicate of pillar 6. Under the assumed two-dimensional innovation model, a 99.9% gate rejects a correct association with probability 0.001 per candidate test. This does not predict the duplicate count exactly, but explains why occasional false rejections are possible even with a well-calibrated gate. All final position errors in these runs are below 0.24 m. The camera’s average NEES is 3.58, with 98% of time samples inside the pointwise bounds. The results are broadly compatible with the assumed uncertainty model for this experiment, not proof of consistency in general. For lidar the corresponding mean is 4.88, with 44% of time samples above the upper bound, indicating overconfidence. Two diagnostic experiments help investigate the lidar result. Seed 3 has an unusually large heading error after the first prediction (about 2.5°, compared with a predicted standard deviation of 0.75°). The initial map inherits that error, which relative observations cannot directly distinguish from a rotation of the global frame. Excluding this seed reduces the average NEES to 3.85. After also correcting the measured range bias in the remaining nine seeds, it falls to 1.71. These are exploratory comparisons: dropping an outlier is not a valid substitute for evaluating the full ensemble. The bias correction and conservative may both contribute to the low NEES. Over longer trajectories, ordinary EKF linearization can also create artificial information about the global heading (Julier & Uhlmann, 2002; Huang et al., 2010). This short, conservatively tuned experiment does not establish whether that effect is present or absent.Experiment: using the raw front-end noise
Finally, we replace with the diagonal covariance obtained from the empirical range and bearing standard deviations, without inflation, and repeat three seeds per sensor. The association rule is unchanged. Compare the number of mapped landmarks and the NEES with the baseline.References
- Dissanayake, M., Newman, P., Clark, S., Durrant-Whyte, H., Csorba, M. (2001). A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans. Rob. Autom.
- Huang, G., Mourikis, A., Roumeliotis, S. (2010). Observability-based rules for designing consistent EKF SLAM estimators. Int. J. Rob. Res.
- Julier, S., Uhlmann, J. (2002). A counter example to the theory of simultaneous localization and map building. Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).
- Thrun, S., Burgard, W., Fox, D. (2005). Probabilistic Robotics.
Found an error on this page? Report an issue.

