Skip to main content
Open In Colab In the theory section, we derived the prediction and correction equations for the joint distribution of the robot pose and the landmark positions. We will now use those equations to map a room with a simulated TurtleBot. The robot starts with an empty map and receives range and bearing measurements of pillars, without being told which pillar produced each measurement. We will run the same estimator twice: first with a scanning lidar and then with a simplified D435i depth-camera model. The difference between the two runs lies in the observations available to the filter and their assumed noise covariances.

The algorithm

The function ekf_slam implements the following cycle, adapted from Table 10.2 of Thrun, Burgard and Fox (2005).
  1. Predict the robot pose using gg and propagate the covariance using GtG_t, VtV_t and MtM_t. The landmark means remain unchanged.
  2. For each observation ztiz_t^i, compute the predicted observation, innovation, Jacobian and innovation covariance for each landmark already in the map. Use the squared Mahalanobis distance to compare candidates.
  3. If no candidate passes the gate α\alpha, initialize a landmark using add_landmark. This enlarges the state and covariance without correcting the existing pose estimate.
  4. 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.
Observations are processed sequentially. Each one uses the belief left by the preceding observation. This implementation uses nearest-neighbor Mahalanobis association. It omits the log-determinant of the innovation covariance, which the full Gaussian likelihood includes. A rejected observation is treated as a new landmark, a simplification that we will examine in the experiments.

The robot model

We reuse the differential-drive model from EKF localization. The input is a pair of wheel angular velocities. The function g 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 [−π,π)[-\pi,\pi) 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 (I−KtHtk)Σˉt(I-K_tH_t^k)\bar\Sigma_t and symmetrizes the result. In exact arithmetic this is equivalent to the Joseph form for the Kalman gain used here: Σt=(I−KtHtk)Σˉt(I−KtHtk)⊤+KtRtKt⊤.\Sigma_t=(I-K_tH_t^k)\bar\Sigma_t(I-K_tH_t^k)^\top+K_tR_tK_t^\top. 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.
Output from cell 5 The green dot marks the starting position; the robot initially faces upward, along the positive yy axis. We use the blue pose, 30° along the path, to illustrate both sensors. The dashed circle marks the maximum accepted landmark range, and the wedge shows the camera’s approximately 55° horizontal field of view.

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 provides segment_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.
Output from cell 7 The left panel shows the beam endpoints in the room and the estimated pillar centers (crosses). The right panel shows the same scan as a function of bearing. Locate the three nearby pillars in both panels: each produces a dip in the range profile. The front end rejects the more distant pillars. Notice that a pillar occupies several beams, but contributes only one observation to the filter.

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: fx=617f_x=617 px, cx=321c_x=321 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 0.01δ2+0.0020.01\delta^2+0.002 m, where δ\delta 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 uu and depth δ\delta, the bearing and center range are ϕ=arctan⁡cx−ufx,r=δcos⁡ϕ+rpillar.\phi=\arctan\frac{c_x-u}{f_x},\qquad r=\frac{\delta}{\cos\phi}+r_{\text{pillar}}. 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.
Output from cell 9 At the example pose, only pillar 1 falls inside the camera field of view and accepted range. Its projected width is about 80 pixels. The simulated front end supplies its center column and near-surface depth. During this drive the camera sees at most one pillar at a time, whereas the lidar generally sees two or three. This difference in visibility will matter when the robot revisits previously mapped pillars.

Choosing the measurement covariance

The EKF requires a covariance RtR_t 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.
The lidar range estimates are short by 7 mm on average, with standard deviations of 8 mm in range and 0.2° in bearing. Selecting the minimum range accounts for much of the negative bias: when we repeat the calculation without beam noise, the mean error becomes +0.4 mm. The camera has a larger range standard deviation (3.1 cm) but a smaller bearing standard deviation (0.08°). For the initial SLAM runs we deliberately use larger standard deviations in RtR_t: 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 RtR_t 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.
Output from cell 12 Dead reckoning spirals inward and finishes 1.21 m from the true position. Both EKF runs stay much closer to the circular path. Each produces eight landmarks, with no wrong associations according to the simulator’s nearest-ground-truth scoring rule. The lidar landmark ellipses are smaller in this run. The lidar observes pillars for longer and from more viewpoints, and the filter assigns it lower range noise. Differences in pose uncertainty at landmark initialization also contribute. This experiment changes several sensor characteristics together; it does not isolate the effect of field of view.

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. Output from cell 15

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 2Σxx+Σyy2\sqrt{\Sigma_{xx}+\Sigma_{yy}}, 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. Output from cell 16

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 +1+1 or −1-1. 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.
The largest eigenvector of the landmark covariance is closely aligned with a small rotation about the starting position. The projected rotation uncertainty is large compared with the uncertainty in distances between successive mapped landmarks. In these runs those distance standard deviations are a few millimeters for lidar and one or two centimeters for the camera. Thus the filter knows the shape of the map much better than its orientation in the chosen world frame. Relative observations cannot by themselves determine the global translation and rotation of an unanchored map. The initial pose prior supplies a reference here, but the first prediction introduces heading uncertainty before any pillar is mapped. The heading standard deviation at that point is approximately 0.75°. The rotation uncertainty computed for the lidar map is close to that value, and the camera’s is larger. This illustrates the connection with the convergence results of Dissanayake et al. (Dissanayake et al., 2001). Those results rely on model assumptions; they do not guarantee that a nonlinear EKF will preserve this apparent uncertainty floor or that every future lap will improve the map consistently. Linearization can introduce spurious information in globally unobservable directions. Output from cell 18

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: ϵt=(μt,s−st)⊤Σt,ss−1(μt,s−st),\epsilon_t=(\mu_{t,s}-s_t)^\top\Sigma_{t,ss}^{-1}(\mu_{t,s}-s_t), 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 M=10M=10 independent noise seeds per sensor and at each time average the NEES across runs. Under the same Gaussian assumptions this average has distribution χ3M2/M\chi^2_{3M}/M, with central 95% interval approximately [1.68,4.70][1.68,4.70]. 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 RtR_t 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 RtR_t 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.
With the smaller measurement covariance, the lidar runs produce 16 to 18 landmarks and the camera runs 9 to 12. The room still contains only eight pillars. Under the scoring rule none of the measurements is assigned to a different physical pillar, but several pillars acquire duplicate map entries. The narrower innovation gate can reject a measurement from an existing pillar; the algorithm then initializes a second estimate of it. The lidar mean NEES rises to 38 to 53, indicating severe overconfidence in these runs. The camera values remain between 3 and 5. Bias, state-dependent noise, linearization and gating can all affect these results; the experiment does not establish a single cause for the camera duplicates. The practical lesson is that a landmark front end and a data-association rule must be evaluated together with the filter. A covariance measured from isolated sensor detections need not yield a well-calibrated estimator when used in a simplified observation model. Increasing RtR_t can help, but modeling systematic errors and handling rejected observations explicitly are preferable to relying on inflation alone. Key references: (Thrun et al., 2005; Dissanayake et al., 2001; Julier & Uhlmann, 2002; Huang et al., 2010)

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.