EECE 5550 Mobile Robotics

Lecture 6

Sensors, Fusion, and Localization

Dr. Xian Li

Department of Electrical and Computer Engineering

Northeastern University

Back to the question

Ten minutes of driving. Which estimate would you trust?

/odomsmooth, but drifts without bound

Lidar + mapbounded, but noisy and sometimes ambiguous

Fusedsmooth and bounded; this is what Nav2 runs

Today

Every estimate is only as good as the models behind it.

The Bayes filter, revisited. Belief, and the two models it needs.
Choosing a filter. KF, EKF, UKF, and particles, step by step on small problems.
The motion model. Odometry predicts the pose, as a distribution.
Measurement models. What each TurtleBot 4 sensor reports, to correct the prediction: p(z | x).
Localization. The EKF and Monte Carlo localization on the TurtleBot pose.
Fusion and AMCL in practice. Count each sensor once; map → odom, in simulation and on the robot.

Reading

Thrun, Burgard, and Fox, Probabilistic Robotics, Ch. 3 to 6, and 8
Moore and Stouch, robot_localization, IAS 2014
Fox, KLD-sampling, IJRR 2003
Nav2 documentation: AMCL

Lecture 5 derived the Bayes and Kalman filters. Today: the filters that cope when models are nonlinear or non-Gaussian, the models the TurtleBot 4 needs, and AMCL, which puts them together.

The Bayes filter, revisited

From Lecture 5. Every filter today runs these two steps; they differ only in how they store the belief.

Predict

\overset{―}{bel} (x_{t}) = \int p (x_{t} | u_{t}, x_{t - 1}) bel (x_{t - 1}) d x_{t - 1}

Motion modelodometry, Part 3 of today

Correct

bel (x_{t}) = η p (z_{t} | x_{t}) \overset{―}{bel} (x_{t})

Measurement modeleach sensor, Part 4 of today

\overset{―}{bel}

is the belief before the measurement; η normalizes the product so it integrates to one.

Histograma probability for each cellgrid filter: the next Try-it

Gaussian N(μ, Σ)a mean and a covarianceKF, EKF, UKF

Weighted samplesM particles, each with a weightparticle filter, AMCL

Three ways to store the same belief

Try it: a Bayes filter in a hallway

The robot has the map (three doors) but not its position. Alternate Sense and Move, and watch the belief.

KF, EKF, UKF, and PF: what each filter assumes

Same predict and correct steps. Choose by two questions: are the models linear, and is the noise Gaussian?

Filter	Linear models?	Gaussian noise?	Belief stored as	How it handles nonlinearity	On the TurtleBot 4, ROS 2 Jazzy
Kalman filter (KF)	required	required	mean and covariance	not needed: exact, and optimal, for linear-Gaussian systems	Not in the default stack. Lecture 5's `wall_kf.py`; Lab 3's complementary filter is a fixed-gain KF.
Extended KF (EKF)	not required	required	mean and covariance	first-order : Jacobians G and H at the mean	Not launched by default. Available as robot_localization's `ekf_node`.
Unscented KF (UKF)	not required	required	mean and covariance	2n + 1 through the exact model; no Jacobians	Not launched by default. Available as robot_localization's `ukf_node`.
Particle filter (PF)	not required	not required	M , any shape	every sample goes through the exact model; cost grows with M	Yes: AMCL (`nav2_amcl`), from turtlebot4_navigation; it publishes map → odom.

Dotted terms with an i open a short explanation; underlined names with ↗ open the documentation.

The Create 3 fuses its wheel encoders, IMU, and optical floor sensor internally into /odom; iRobot does not document the algorithm. slam_toolbox (Lab 3) is not a filter: it optimizes a pose graph.

The KF, step by step: distance to a wall

Lecture 5's wall: the robot drives toward it and the lidar measures the distance d. Both models are linear.

Belief at t − 1. $𝒩 (μ, σ^{2}) = 𝒩 (3.00, {0.30}^{2})$
Predict. $A = 1, B = - 1, u = 1.00$ u: odometry, 1 m toward the wall $\bar{μ} = A μ + B u = 2.00, {\bar{σ}}^{2} = A σ^{2} A + R = {0.36}^{2}$
Measurement model. $z = C d + δ, C = 1, Q = {0.10}^{2}$ Linear already: the EKF's Jacobian H is just C.
Measure. $z = 1.88, C \bar{μ} = 2.00$ innovation z − Cμ̄ = −0.12 m
Gain. $K = \frac{{\bar{σ}}^{2} C}{C^{2} {\bar{σ}}^{2} + Q} = 0.93$ The lidar is the better source here, so K is near 1.
Correct. $μ = \bar{μ} + K (z - C \bar{μ}) = 1.89$
$σ^{2} = (1 - K C) {\bar{σ}}^{2}, σ = 0.096$
Check. Lecture 5's precision-weighted mean, 1/σ² = 1/σ̄² + 1/Q, gives the same σ = 0.096 m. Linear and Gaussian, so this is the exact posterior, not an approximation.

The EKF, step by step: range to a pillar

The same robot, now measuring its range to a pillar at (4, 1) m. The motion is still linear; the range is not.

Belief at t − 1. $𝒩 (μ, σ^{2}) = 𝒩 (1.00, {0.30}^{2})$
Predict. $g = x + u, G = 1$
$\bar{μ} = μ + u = 2.00, {\bar{σ}}^{2} = σ^{2} + R = {0.36}^{2}$
Linearize h at μ̄. $H = {\frac{\partial h}{\partial x} |}_{\bar{μ}} = \frac{- (4 - \bar{μ})}{h (\bar{μ})} = - 0.89$
Measure. $z = 1.99, h (\bar{μ}) = 2.24$ innovation z − h(μ̄) = −0.25 m
Gain. $K = \frac{{\bar{σ}}^{2} H}{H^{2} {\bar{σ}}^{2} + Q} = - 1.02$ Q = 0.10². K < 0 because a larger x gives a shorter range.
Correct. $μ = \bar{μ} + K (z - h (\bar{μ})) = 2.25$
$σ^{2} = (1 - K H) {\bar{σ}}^{2}, σ = 0.11$
Check. The exact posterior is 2.26 ± 0.11 m. Over ±σ̄ the tangent fits h well, so the EKF is accurate here.

The UKF: sigma points instead of a tangent

When the belief is wide, the tangent is a poor fit. The unscented transform pushes a few chosen points through h instead.

The question. A wide belief near the pillar: what is the distribution of the range y = h(x)?
Ground truth. 10⁵ samples through h: skewed, never below 1 m. Mean 1.44, std 0.44 m.
EKF. Tangent at μ: N(1.28, 0.50²). The mean is too low, and 29% lies below 1 m, an impossible range.
Sigma points. $χ^{0} = μ, χ^{\pm} = μ \pm \sqrt{(n + λ) σ^{2}}$ n = 1, λ = 2: 1.81, 3.20, 4.59 m; weights 1/6, 2/3, 1/6.
Propagate. Through the exact h: Yⁱ = 2.40, 1.28, 1.16 m.
Recombine. $\bar{y} = \sum w^{i} Y^{i} = 1.45, σ_{y}^{2} = \sum w^{i} {(Y^{i} - \bar{y})}^{2}$ σ_y = 0.43 m: close to the truth, and no Jacobian.
The trade. Cost like the EKF (2n + 1 evaluations), accurate to second order. Still one Gaussian: the 1 m floor and multiple peaks need particles.

The particle filter, step by step: the hallway again

Same hallway, same three doors. Fifty particles replace the histogram; each click runs one line of the algorithm.

Initialize $x_{t}^{[m]} \sim uniform over the map, w = 1 / M$
Predict $x_{t}^{[m]} \sim p (x_{t} | u_{t}, x_{t - 1}^{[m]})$
Weight $w_{t}^{[m]} = p (z_{t} | x_{t}^{[m]})$
Resample draw M particles with probability ∝ $w_{t}^{[m]}$

Initialize. No idea where the robot is: 50 particles spread uniformly, with equal weights.

Weight. The robot sees a door. Each particle gets w = p(door | x): particles in front of any door gain weight.

Resample. Draw 50 new particles, each with probability proportional to its weight. Three clusters, one per door: three peaks.

Predict. The robot drives 1.5 m. Move every particle by u plus sampled noise: the clusters shift together and spread.

Weight. A door again. Only the cluster that is now in front of a door keeps its weight.

Resample. 70% of the particles now sit within 0.5 m of the robot at x = 3.25 m. One or two more readings remove the rest.

The odometry motion model

Thrun et al., Table 5.6, and nav2_amcl: each increment is a rotation, a translation, and a second rotation.

\begin{matrix} δ_{rot1} & = & atan2 (\bar{y}' - \bar{y}, \bar{x}' - \bar{x}) - \bar{θ} \\ δ_{trans} & = & \sqrt{{(\bar{x}' - \bar{x})}^{2} + {(\bar{y}' - \bar{y})}^{2}} \\ δ_{rot2} & = & \bar{θ}' - \bar{θ} - δ_{rot1} \end{matrix}

1. Decompose the odometry increment

\begin{matrix} σ_{rot1}^{2} & = & α_{1} δ_{rot1}^{2} + α_{2} δ_{trans}^{2} \\ σ_{trans}^{2} & = & α_{3} δ_{trans}^{2} + α_{4} (δ_{rot1}^{2} + δ_{rot2}^{2}) \\ σ_{rot2}^{2} & = & α_{1} δ_{rot2}^{2} + α_{2} δ_{trans}^{2} \end{matrix}

2. Its noise grows with each component

\begin{matrix} {\hat{δ}}_{j} & = & δ_{j} - ε_{j}, ε_{j} \sim 𝒩 (0, σ_{j}^{2}) \\ x', y' & = & x + {\hat{δ}}_{trans} \cos (θ + {\hat{δ}}_{rot1}), y + {\hat{δ}}_{trans} \sin (θ + {\hat{δ}}_{rot1}) \\ θ' & = & θ + {\hat{δ}}_{rot1} + {\hat{δ}}_{rot2} \end{matrix}

3. Sample a new pose, once per particle

α₁ rotation noise from rotationα₂ rotation noise from translationα₃ translation noise from translationα₄ translation noise from rotation

Nav2 parameters alpha1 to alpha4, default 0.2 each.

nav2_amcl uses min(|δ_rot|, |π − δ_rot|) in these variances, so driving backward is not treated as a half turn.

Try it: where could the robot be?

Sample the motion model 800 times and compare the cloud with the EKF's linearized 95% ellipse.

· 800 sampled poses◯ EKF 95% ellipse▬ the reported pathgrid: 0.5 m

The odometry increment, repeated

distance (m)turn (°)moves

Noise (Nav2 alpha1 to alpha4)

α₁ rot ← rotα₂ rot ← transα₃ trans ← transα₄ trans ← rot

The TurtleBot 4 sensors and their models

Each sensor needs a model p(z | x): the reading it should give at pose x, and how real readings scatter around it.

RPLIDAR A1M8/scan: range per beam, one plane, 360°

z_{k} = r_{k}^{*} (x, m) + ε, ε \sim 𝒩 (0, σ_{hit}^{2})

glass, black surfaces, objects below the plane

OAK-D Prostereo depth from disparity d, baseline b

Z = f b / d, σ_{Z} \approx Z^{2} σ_{d} / (f b)

far, blank, or shiny surfaces

Wheel encodersticks Δn per wheel, inside /odom

\begin{matrix} Δ \tilde{s} = (2 π r / N) Δ n \\ Δ \tilde{s} = Δ s + ε, ε \sim 𝒩 (0, k | Δ s |) \end{matrix}

slip, wrong r or wheel base

Optical floor sensorfloor displacement Δp, inside /odom

Δ \tilde{p} = s Δ p + ε, ε \sim 𝒩 (0, σ^{2} I)

glossy or featureless floors: no reading

Gyro, in the Create 3/imu: angular rate ω about z

\tilde{ω} = ω + b + n, \dot{b} = n_{b}

bias b drifts with temperature

What the readings look like

Illustrative shapes of p(z | x): for a known true state, where the readings land and how likely each value is.

Gaussian is the default, not a law.Short readings, missing returns, slip, and dropouts are not Gaussian. A model that leaves them out makes the filter overconfident exactly when the sensor misleads it.

Try it: the beam model for one lidar beam

One beam, with the map predicting range z*. The reading comes from one of four causes; the weights sum to one.

p (z | z^{*}) = z_{hit} p_{hit} + z_{short} p_{short} + z_{\max} p_{\max} + z_{rand} p_{rand}

p_{hit} = 𝒩 (z; z^{*}, σ_{hit}^{2}), p_{short} = \frac{λ e^{- λ z}}{1 - e^{- λ z^{*}}} for z < z^{*}

p_{\max} : point mass at z_{\max}, p_{rand} = \frac{1}{z_{\max}}, z_{hit} + z_{short} + z_{\max} + z_{rand} = 1

The world

expected range z* (m)z_short: unexpected objects

The sensor

σ_hit (m)z_rand: random returns

Try it: the likelihood field, as AMCL scores a scan

Given an occupancy grid (Lecture 7 builds one), AMCL precomputes each cell's distance d to the nearest wall and scores each beam endpoint by its d.

p (z | x, m) = \prod_{k} [z_{hit} 𝒩 (d_{k}; 0, σ_{hit}^{2}) + \frac{z_{rand}}{z_{\max}}]

● candidate pose○ true pose● endpoint within σ_hit of a wall● farthershading: N(d; 0, σ²_hit)

The model

σ_hit (m)

The candidate pose (one particle)

position error Δx (cm)heading error Δθ (°)

nav2_amcl sums p_k³ over the beams instead of multiplying, a heuristic that keeps a few bad beams from eliminating a good particle.

The EKF on the TurtleBot pose

The same steps on the full pose (x, y, θ): predict with the odometry model, correct with the lidar.

Predict, with the odometry motion model

{\bar{μ}}_{t} = g (u_{t}, μ_{t - 1}), Σ_{t} = G_{t} Σ_{t - 1} G_{t}^{T} + V_{t} M_{t} V_{t}^{T}

G_{t} = [\begin{matrix} 1 & 0 & - δ_{trans} \sin (θ + δ_{rot1}) \\ 0 & 1 & δ_{trans} \cos (θ + δ_{rot1}) \\ 0 & 0 & 1 \end{matrix}]

M_{t} = diag (σ_{rot1}^{2}, σ_{trans}^{2}, σ_{rot2}^{2}), V_{t} = \frac{\partial g}{\partial (δ_{rot1}, δ_{trans}, δ_{rot2})}

Correct, with a lidar measurement

K_{t} = Σ_{t} H_{t}^{T} {(H_{t} Σ_{t} H_{t}^{T} + Q_{t})}^{- 1}

μ_{t} = {\bar{μ}}_{t} + K_{t} (z_{t} - h ({\bar{μ}}_{t})), Σ_{t} = (I - K_{t} H_{t}) Σ_{t}

Lecture 5's Kalman filter, with Aμ + Bu replaced by g(u, μ), and A and C by the Jacobians G and H, evaluated at each step. The red ellipse in the motion-model Try-it is exactly GΣG^T + VMV^T.

Notation follows Thrun et al. and the Lecture 5 summary: a bar marks the prediction, R_t = V_tM_tV_t^T is the motion noise, Q_t the measurement noise. robot_localization and many texts swap the letters Q and R.

Monte Carlo localization: the same loop in 2D

Fox, Burgard, Dellaert, and Thrun, 1999. Each particle is a pose (x, y, θ); the scan is scored with the likelihood field.

N_{eff} = \frac{1}{\sum w_{i}^{2}}

A common rule resamples only when N_eff falls below M/2; nav2_amcl instead resamples every resample_interval updates (default 1). Cost grows as particles × beams.

Try it: a particle filter in a room

Press Global localization: the cloud splits between look-alike places until the room's asymmetry resolves it.

The filter

particlessensor σ (m)beams

● the robot· particles○ weighted mean— lidar beams

A complementary filter, for heading

High-pass the integrated gyro, low-pass an absolute heading; the two filters sum to one. You build one in Lab 3.

{\hat{θ}}_{k} = α ({\hat{θ}}_{k - 1} + (ω_{k} - \hat{b}) Δ t) + (1 - α) z_{k}

τ = \frac{α Δ t}{1 - α}, K = 1 - α

Δt is the interval between blends: the gyro step in Lab 3. The form matches heading_filter.py and Problem Set 2.

τ sets the crossoverHeading changes faster than τ come from the gyro, slower ones from the absolute heading. One parameter (α, K, or τ) trades bias drift, about b·τ, against passed noise, about σ_z√(K / (2 − K)).

A Kalman filter with a fixed gainK is the steady-state Kalman gain for the gyro and heading variances (Lab 3, TODO 2).

Angles wrapBlend wrap(z − θ̂), the atan2(sin, cos) of the difference, never raw angles.

Try it: blending a gyro with an absolute heading

Sixty seconds of turning. The gyro has a bias; the absolute heading is noisy. Find the gain that outperforms both.

The gyro

bias (°/s)

The absolute heading

noise (°)update rate f

The blend

gain K = 1 − α

▬ gyro integrated alone▬ absolute heading alone▬ complementary filter- - predicted offset b·τ

Count each sensor once

A Kalman filter assumes independent measurement errors. Fusing /imu on top of /odom counts the same gyro twice.

On the TurtleBot 4the Create 3 already fuses /odom, and AMCL adds map → odom. The default stack needs no additional EKF.

Before fusing a topicWhat is already inside it? Is its covariance realistic? Is its timestamp correct?

AMCL, as Nav2 runs it

Each parameter maps to a model or a Try-it from this lecture.

Parameter	Nav2 default	Controls
`min_particles`, `max_particles`	500, 2000	how many hypotheses (KLD adapts between them)
`update_min_d`, `update_min_a`	0.25 m, 0.2 rad	how far to move before updating
`laser_model_type`, `max_beams`	likelihood_field, 60	the measurement model, and how many beams it scores
`sigma_hit`, `z_hit`, `z_rand`	0.2 m, 0.5, 0.5	the likelihood field's width and mixture weights
`alpha1` to `alpha4`	0.2	the odometry motion model's noise
`resample_interval`	1	how many updates between resampling steps
`recovery_alpha_slow`, `_fast`	0.0, 0.0	random particles injected when the fit drops: off

These are Nav2's defaults; turtlebot4_navigation's configuration can override them. Query what your robot runs: ros2 param get /amcl max_particles.

What AMCL publishes: map → odom

The answer to Lecture 2's question.

T_{map, odom} = T_{map, base} {T_{odom, base}}^{- 1}

Controllers run in odomthe pose never jumps

Planners run in mapthe pose never drifts

Also published/amcl_pose and /particle_cloud

TF allows one parent per frame, and odom is already base_link's parent. AMCL therefore publishes the correction map → odom, not map → base_link.

Verify in simulation

Exercise 1: watch AMCL converge, then kidnap it

Run this on your Ubuntu PC: no robot needed

terminal 1

$ros2 launch turtlebot4_gz_bringup turtlebot4_gz.launch.py \$ nav2:=true slam:=false localization:=true rviz:=true

Press Play in Gazebo. In RViz, use 2D Pose Estimate to drop the robot roughly where it is, and watch the particle cloud. Then give it a deliberately wrong pose, about a meter off, and drive.

terminal 2

$ros2 topic echo /amcl_pose --once$ros2 run tf2_ros tf2_echo map odom$ros2 param get /amcl max_particles

Predict firstHow many particles after convergence? Does map → odom change while the robot sits still?

Then break itMove the robot in Gazebo while AMCL runs. Does it recover? Why not, with the defaults?

MeasureThe covariance in /amcl_pose before and after a turn near a wall.

Verify on your TurtleBot

Exercise 2: characterize your sensors before you trust a filter with them

Run this on your Ubuntu PC, with the robot on the network

terminal

$ros2 topic hz /scan$ros2 topic hz /imu$ros2 topic hz /odom

terminal

$ros2 topic echo /scan --once | head -n 12$ros2 interface show sensor_msgs/msg/LaserScan

If /imu is missing, it may only exist in the hidden namespace, as /wheel_ticks did in Lecture 4: ros2 topic list --include-hidden-topics | grep imu.

Quantity	Predict	Measured
`/scan` rate	Hz
`/imu` rate	Hz
`/odom` rate	Hz
`range_min`, `range_max`	m
beams per scan
gyro bias at rest	°/s

For the gyro bias: leave the robot still for 60 s, record ros2 bag record /imu, and average angular_velocity.z.

Verify on your TurtleBot

Exercise 3: localize on your own map

Run these on your Ubuntu PC, with the map from Lab 3

terminal 1

$ros2 launch turtlebot4_navigation localization.launch.py \$ map:=$HOME/maps/room122.yaml

terminal 2

$ros2 launch turtlebot4_viz view_navigation.launch.py

terminal 3

$ros2 run teleop_twist_keyboard teleop_twist_keyboard \$ --ros-args -p stamped:=true

Set the initial pose in RViz, then drive one lap of the room. Keep a fourth terminal on ros2 run tf2_ros tf2_echo map odom.

Watchmap → odom changes in small steps as AMCL corrects odometry's drift. Plot the steps: they are Lecture 4's error, measured.

CompareDrive the same lap with the room's cardboard walls moved. Where does the cloud spread?

Name the placeFind the spot in the room where AMCL is least certain. Why there?

When localization fails

Review these before Lab 4: every Nav2 plan depends on the pose AMCL reports.

Symmetrytwo poses produce the same scan

Glass and mirrorsthe wrong range, or no return

One scan planeobstacles below it are invisible

Dynamic obstaclespeople and objects absent from the map

Poor initial posewith recovery off, AMCL tracks but does not search

Overconfidencea tight cloud at the wrong pose

Each failure is a mismatch between a model and the world. Fix the model, the map, or the environment before retuning the filter.

What to take away

The one habitBefore you trust a filter, write down its models: how each sensor fails, and what is already inside each topic.

1The Bayes filter

Predict with the motion model, correct with the measurement model.

2Match the filter to the models

KF if linear-Gaussian; EKF or UKF if nonlinear but single-peaked; particles for many peaks.

3Ellipse or banana

Odometry noise is Gaussian in (δ_rot1, δ_trans, δ_rot2), not in x and y.

4Sensors are distributions

Beam model or likelihood field for the lidar, σ_Z ∝ Z² for stereo, bias plus noise for the gyro.

5Fuse with care

Complementary filter: τ sets the crossover. Count each sensor once.

6map → odom

AMCL publishes the correction. Check its parameters on your robot.