Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M2 Mathematical Foundations / M2-T6

Localization & SLAM · Theory · M2

M2-T6 — Motion and Observation Models

Two functions, written once, carry the whole of EKF and graph SLAM: how the robot moves, and what it sees.

  • Lesson 6 of 7
  • Module 2: Mathematical Foundations
  • Theory
  • About 9 minutes to read

The Bayes filter needs a motion model and a measurement model. The EKF needs them in a particular shape: a function plus its Jacobian. This lesson writes both for the most common 2D case, a wheeled robot with odometry and a sensor that returns range and bearing to landmarks, and shows how to check the derivatives with finite differences before they cost you a week.

Learning objectives

After this lesson, you should be able to:

  • Write the odometry motion model f(x, u) and identify its noise.
  • Write the range-bearing observation model h(x, m) and compute its prediction for a given pose and landmark.
  • Derive the Jacobians of both models with respect to pose and landmark and evaluate them numerically.
  • Verify any Jacobian with a finite-difference test.

The intuition

The robot must answer two “what if” questions. If I was here and drove like this, where would I be? That is the motion model: it predicts the next pose from the previous one and the odometry. If I were here and the landmark were there, what would my sensor read? That is the measurement model: it predicts the reading from the pose and the map. The filter compares prediction with reality and corrects.

Both are noisy, and the noise is part of the model: a noise-free model would claim the prediction is exactly right and that no measurement can disagree.

Range and bearing to a landmarkA robot at (1,2) facing 30 degrees and a landmark at (4,6). The vector from robot to landmark has length 5 at 53.1 degrees in the world; relative to the robot heading the bearing is 23.1 degrees. robot (1, 2, 30°) landmark (4, 6) r = 5 b = 23.1° dx = 3, dy = 4 r = √(dx² + dy²) = 5 b = atan2(dy, dx) − θ = 53.1° − 30° = 23.1° The sensor sees only (r, b); it does not see x, y, θ. The measurement model h(x, m) predicts (r, b).
The observation model: from a known pose and a known landmark it predicts the range r and bearing b the sensor should return.

The mathematics

Motion model

Odometry summarises a step as a relative motion u = (δtrans, δrot): a forward distance and a turn. With the pose x = (x, y, θ) and noise w ~ N(0, Q):

Odometry motion model (midpoint form)
xt=f(xt−1, ut)+wtx_{t}=f(x_{t-1},\,u_{t})+w_{t}
xt=xt−1+δtrans cos⁡(θt−1+δrot2)x_{t}=x_{t-1}+\delta _{\text{trans}}\,\cos (\theta _{t-1}+\frac{\delta _{\text{rot}}}{2})
yt=yt−1+δtrans sin⁡(θt−1+δrot2)y_{t}=y_{t-1}+\delta _{\text{trans}}\,\sin (\theta _{t-1}+\frac{\delta _{\text{rot}}}{2})
θt=θt−1+δrot\theta _{t}=\theta _{t-1}+\delta _{\text{rot}}

The Jacobian with respect to the previous pose (F) and to the odometry (G) are, with φ = θ + δrot/2:

Motion Jacobians
F=∂f∂x=[10−δtranssin⁡φ01δtranscos⁡φ001]\mathbf F = \frac{\partial f}{\partial \mathbf x} = \begin{bmatrix}1 & 0 & -\delta_{\text{trans}}\sin\varphi\\ 0 & 1 & \delta_{\text{trans}}\cos\varphi\\ 0 & 0 & 1\end{bmatrix}
G=∂f∂u=[cos⁡φ−12 δtranssin⁡φsin⁡φ12 δtranscos⁡φ01]\mathbf G = \frac{\partial f}{\partial \mathbf u} = \begin{bmatrix}\cos\varphi & -\tfrac{1}{2}\,\delta_{\text{trans}}\sin\varphi\\ \sin\varphi & \tfrac{1}{2}\,\delta_{\text{trans}}\cos\varphi\\ 0 & 1\end{bmatrix}

The covariance of the odometry noise depends on the motion itself: more travel, more error, and rotation leaks into translation. A common model is Qu = diag( (α1 |δrot| + α2 δtrans)², (α3 δtrans + α4 |δrot|)² ), mapped to pose space by G Qu Gᵀ.

Observation model

A landmark at m = (lx, ly) seen from pose (x, y, θ), with measurement noise v ~ N(0, R):

Range-bearing observation model
z=h(x, m)+v=[rb]+vz=h(x,\,m)+v=\begin{bmatrix}r \\ b\end{bmatrix}+v
dx=lx−x\mathrm{dx}=l_{x}-x
dy=ly−y\mathrm{dy}=l_{y}-y
r=dx2+dy2r=\sqrt{\mathrm{dx}^{2}+\mathrm{dy}^{2}}
b=wrap⁡(atan2(dy, dx)−θ)b=\operatorname{wrap}(\mathrm{atan}2\text{(dy, dx)}-\theta )
z = (r, b)
the measurement: range in metres and bearing in radians, relative to the robot’s heading.
h(x, m)
the prediction of z for a given pose and landmark.
R
measurement noise covariance, for example diag(σr², σb²).
Hx, Hm
Jacobians of h with respect to the pose and with respect to the landmark.
Observation Jacobians
Hx=[−dx/r−dyr 0dyr2−dxr2−1]H_{x}=\begin{bmatrix}-\mathrm{dx}/r & -\frac{\mathrm{dy}}{r\,0} \\ \frac{\mathrm{dy}}{r^{2}}-\frac{\mathrm{dx}}{r^{2}}-1\end{bmatrix}
Hm=[dx/rdy/r−dyr2dxr2]H_{m}=\begin{bmatrix}\mathrm{dx}/r & \mathrm{dy}/r \\ -\frac{\mathrm{dy}}{r^{2}} & \frac{\mathrm{dx}}{r^{2}}\end{bmatrix}

The innovation, the difference between the measured and predicted values, must wrap its bearing component: ν = (r − r̂, wrap(b − b̂)).

Jacobian rule of thumb: the signs of Hx and Hm must be opposite in the position part. Moving the robot toward a landmark and moving the landmark toward the robot are the same change of range with opposite signs.

A worked example: robot (1, 2, 30°), landmark (4, 6)

  1. Predict the measurement. dx = 3, dy = 4, so r = 5. atan2(4, 3) = 53.13°, minus θ = 30° gives b = 23.13° = 0.4037 rad.
  2. Pose Jacobian. dx/r = 0.6, dy/r = 0.8, dx/r² = 0.12, dy/r² = 0.16. Hx = [[−0.6, −0.8, 0], [0.16, −0.12, −1]]. The first row says: moving the robot 1 m in x toward the landmark reduces the range by 0.6 m; turning does not change the range at all (third entry 0). The second row says: turning the robot by +1 rad changes the bearing by −1 rad.
  3. Landmark Jacobian. Hm = [[0.6, 0.8], [−0.16, 0.12]]: the negative of the position part of Hx, as the rule of thumb predicts.
  4. Finite-difference check. Move the robot x by ε = 1e-6: dx becomes 3 − 1e-6, r becomes 5 − 0.6e-6, so ∂r/∂x ≈ −0.6 ✓. Move y by 1e-6: ∂r/∂y ≈ −0.8 ✓. Bearing: ∂b/∂x ≈ +0.16 ✓.
  5. The effect on uncertainty. With σr = 0.05 m and σb = 0.01 rad (R = diag(0.0025, 0.0001)), a bearing error of 0.01 rad at 5 m corresponds to a sideways error of 5 cm, comparable to the range error. The two noise terms have different units, which is why a Mahalanobis distance, not Euclidean distance, is used to compare measurements.

Writing the Jacobian by hand is the most common source of bugs in EKF-SLAM implementations. The finite-difference check takes five lines and should be in every unit test.

Engineering implications and limitations

Practical rules

  • Test every Jacobian numerically. Perturb each input by 1e-6, compare columns with the analytic matrix, and fail loudly on a relative error above 1e-4.
  • Wrap angles in innovations, not only in the state. An unwrapped bearing difference of 2π − 0.02 tells the filter its landmark is nearly a full turn away.
  • Choose noise from data. Drive known paths to estimate α terms; compare filter innovations against their predicted covariance (NIS test). A noise model borrowed from a paper is a starting point.
  • Model the dependencies. Heading noise couples into position through distance. A model that uses independent, constant noise in x, y and θ overestimates short moves and underestimates long ones.
  • Keep sensor and base frames straight. The measurement model above is in the sensor frame at the base; an offset sensor needs the mount transform inside h (Module 2 and the frames lab).

Limits

Both models are first-order descriptions. The odometry model ignores slip; the range-bearing model assumes a point landmark, a known identity and no outliers. For a 2D lidar scan the “landmark” becomes a point on a line, and the measurement model changes (point-to-line distance, scan matching: Module 5). The linearisation holds near the current estimate and degrades when the pose uncertainty is large.

Common misconceptions

“A motion model is just a kinematic equation.”

It is the equation plus its noise model. Without noise there is no uncertainty to propagate and no weighting against measurements.

“The Jacobian of the range with respect to heading is non-zero.”

Range does not depend on the robot’s heading. Only the bearing does, with derivative −1.

“If it compiles and converges, the Jacobian is right.”

A wrong Jacobian often still converges, slowly or to an inconsistent covariance. Check it numerically.

“Measurement noise is the same in all directions.”

Range and bearing noise have different units and different effect on position: bearing noise grows with distance.

Engineering takeaways

  1. The motion model f predicts the next pose from the previous pose and odometry; the observation model h predicts the sensor reading from pose and map.
  2. Both carry noise: Q for motion and R for measurement. These matter as much as the functions.
  3. For range and bearing: r = √(dx² + dy²), b = atan2(dy, dx) − θ, with the Jacobians above.
  4. Always wrap the bearing in the innovation.
  5. Verify Jacobians with finite differences; it is the cheapest bug insurance in SLAM.

References and further reading

Primary papers and authoritative documentation.

  • Thrun, S., Burgard, W. and Fox, D. (2005). Probabilistic Robotics. MIT Press, chapters 5 and 6 (motion and measurement models) and chapter 7 (EKF localization).
  • Siegwart, R., Nourbakhsh, I. and Scaramuzza, D. (2011). Introduction to Autonomous Mobile Robots. MIT Press, chapter 5: error propagation in odometry.
  • Barfoot, T. D. (2017). State Estimation for Robotics. Cambridge University Press, chapter 4.
  • Bailey, T., Nieto, J., Guivant, J., Stevens, M. and Nebot, E. (2006). Consistency of the EKF-SLAM algorithm. IEEE/RSJ IROS: why wrong Jacobians and noise cause overconfidence.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

Why does the second row of the pose Jacobian of the bearing contain −1 in the heading column but the range row contains 0?

Explanation

The bearing is measured relative to the robot’s heading, so turning the robot by +δ changes the bearing by −δ. The range is a distance between two points and does not depend on which way the robot faces, so its derivative with respect to heading is zero.

Question 2

An EKF-SLAM run works in simulation but its covariance is far too small on real data. Name two likely causes in the models.

Explanation

Noise too small in Q or R (taken from a data sheet rather than measured), and a wrong Jacobian or a missing angle wrap in the innovation. Either makes the filter overconfident, because updates appear more informative than they really are.

Question 3

You move a landmark by 1 cm toward the robot and, separately, move the robot by 1 cm toward the landmark. How should the changes in range compare, and what does that say about Hx and Hm?

Explanation

The change in range is the same in magnitude and the same in sign for both (the distance falls by 1 cm). Therefore the position part of Hm is the negative of that of Hx: moving the robot along +d and moving the landmark along −d have the same effect.

What’s next

One piece is missing before measurements can be used: knowing which landmark each measurement belongs to. That is data association.

Practice this lesson

Understand Pose Uncertainty

Propagate a pose covariance through odometry steps and a landmark update, and compare your ellipses with a Monte Carlo cloud.

Next lesson

M2-T7 — Data Association

Match observations to landmarks with nearest neighbour, Mahalanobis gating and joint compatibility, and see how wrong matches break estimates.

Nothing below is required to finish this lesson.