Skip to main content
Contents1 / 6 ▾
  1. ~Cover & contents
  2. IThe problem, before any algorithm
  3. IIThe Kalman filter: the exact case
  4. IIIThe EKF: pretend it is a straight line
  5. IVThe UKF: do not differentiate, sample
  6. VFactor graphs: keep everything, solve it all
  7. VIThe three compared, and unified
I

Chapter one The problem, before any algorithm

You have a robot. It carries sensors, and every one of them lies a little. The wheel encoders slip, the IMU drifts, the GPS wanders by a few metres, the camera sees blurry corners. You also have a model of how the robot moves, and that model lies too, because you never know the wind or the friction exactly.

The question is not "which sensor is right." None of them is. The question is: given everything I have measured up to now, what is the probability distribution over where I actually am? Every method in this book is an answer to that one question. They differ only in what they are willing to approximate in order to compute the answer fast enough.

1.1 · The state, and why it is enough

The state \(x_k \in \mathbb{R}^n\) is the smallest vector that captures everything about the system's past that matters for its future. For a ground robot that might be \([\,p_x, p_y, \theta\,]^\top\). For a drone with an IMU it is more like position, velocity, attitude quaternion, and the three-axis gyroscope and accelerometer biases — fifteen or sixteen numbers.

We assume the state evolves as a Markov process: the next state depends only on the current one, not the whole history. And we assume each measurement depends only on the state at the instant it was taken. Those two assumptions are what make everything tractable.

\[ x_k = f(x_{k-1},\, u_k) + w_k, \qquad w_k \sim \mathcal{N}(0, Q_k) \]
(1.1)
\[ z_k = h(x_k) + v_k, \qquad v_k \sim \mathcal{N}(0, R_k) \]
(1.2)

Equation (1.1) is the process model — physics, kinematics, whatever you know about how the thing moves, driven by a control input \(u_k\). Equation (1.2) is the measurement model — what a perfect sensor would read if the state were \(x_k\). The noises \(w_k\) and \(v_k\) are what you admit you do not know, and their covariances \(Q_k\) and \(R_k\) are how loudly you admit it.

Symbols used throughout

xk
state at step k, dimension n
zk
measurement at step k, dimension m
Pk
state covariance — how uncertain we are, and in which directions
Q, R
process and measurement noise covariance
f, h
process and measurement functions (possibly nonlinear)
F, H
Jacobians of f and h

1.2 · The recursive Bayes filter

What we actually want is the belief: the posterior distribution \(p(x_k \mid z_{1:k})\). It can be computed recursively in two half-steps. First predict, which pushes the old belief forward through the motion model and always makes us less certain:

\[ p(x_k \mid z_{1:k-1}) = \int p(x_k \mid x_{k-1})\; p(x_{k-1} \mid z_{1:k-1})\; dx_{k-1} \]
(1.3)

Then update, which multiplies in the new evidence and always makes us more certain:

\[ p(x_k \mid z_{1:k}) \;\propto\; \underbrace{p(z_k \mid x_k)}_{\text{likelihood}} \; \underbrace{p(x_k \mid z_{1:k-1})}_{\text{prior}} \]
(1.4)

That is the whole theory. Everything that follows is engineering, because that integral is intractable for a general distribution. There are exactly two escape routes, and they define the two families in this book.

Route one: assume the belief is a Gaussian. Then you never store a distribution, only a mean \(\hat{x}\) and a covariance \(P\), and (1.3) and (1.4) collapse into matrix algebra. This gives you the Kalman filter, and its nonlinear cousins the EKF and UKF. Route two: stop tracking a belief at all. Instead keep every measurement ever taken and, whenever you need an answer, solve for the whole trajectory that best explains all of them at once. This gives you the factor graph.

1