Bayesian Inference and the Kalman Filter

A robot never sees its state. It sees noisy sensors and knows a rough model of how it moves. State estimation is the problem of turning that stream into a probability distribution over the hidden state $x$, updated every time a new measurement arrives.

Recursive Bayesian estimation

Keep a belief $p(x_k \mid z_{1:k})$, the posterior over the state given all measurements so far. Each step does two things. First predict: push the belief through the dynamics with the Chapman-Kolmogorov integral,

$$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}.$$

Then update: multiply the predicted belief (now the prior) by the likelihood of the new measurement and renormalize,

$$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}}.$$

That is Bayes' rule applied once per timestep. Posterior becomes the next prior, forever. The integral is intractable in general, but for one family it closes in a few matrix operations.¹

The linear-Gaussian case is the Kalman filter

Assume linear dynamics and a linear measurement with Gaussian noise,

$$x_k = F x_{k-1} + w,\quad w \sim \mathcal{N}(0, Q), \qquad z_k = H x_k + v,\quad v \sim \mathcal{N}(0, R).$$

A Gaussian pushed through a linear map and multiplied by another Gaussian stays Gaussian, so the belief is fully described by its mean $\hat{x}$ and covariance $P$. Tracking those two through predict and update gives the Kalman filter.

Predict:

$$\hat{x}_k^- = F\,\hat{x}_{k-1}, \qquad P_k^- = F P_{k-1} F^\top + Q.$$

Update with innovation $y_k$, innovation covariance $S_k$, and gain $K_k$:

$$y_k = z_k - H\hat{x}_k^-, \qquad S_k = H P_k^- H^\top + R, \qquad K_k = P_k^- H^\top S_k^{-1},$$ $$\hat{x}_k = \hat{x}_k^- + K_k\, y_k, \qquad P_k = (I - K_k H)\, P_k^-.$$

The gain $K_k$ is the whole story. It is the ratio of what the model does not know, $P_k^- H^\top$, to the total uncertainty, $S_k = H P_k^- H^\top + R$. When the sensor is noisy ($R$ large) the gain shrinks and the estimate leans on the model. When the model is uncertain ($Q$ large, so $P_k^-$ large) the gain grows and the estimate leans on the measurement.²

A one-step worked example

Take the simplest case: a scalar constant position, $F = H = 1$, no process noise. Start with prior mean $\hat{x}^- = 0$ and variance $P^- = 1$, sensor variance $R = 1$. A measurement $z = 10$ arrives.

$$S = H P^- H^\top + R = 1 + 1 = 2, \qquad K = P^- H^\top S^{-1} = \frac{1}{2} = 0.5.$$ $$\hat{x} = 0 + 0.5\,(10 - 0) = 5, \qquad P = (1 - 0.5)\cdot 1 = 0.5.$$

Equal trust in model and sensor puts the estimate exactly halfway, at $5$, and halves the variance from $1$ to $0.5$ ($\sigma$ from $1$ to $0.707$). Two measurements with equal weight, one combined answer with less uncertainty than either alone.

Fusing a moving target

Now a 1D target with position and velocity, $x = [p, v]^\top$, constant-velocity model $F = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}$, measuring position only, $H = [1\ \ 0]$. The true track below is a slow sinusoid, so the constant-velocity model is deliberately wrong at the turns and $Q$ has to absorb the mismatch. Drop $R$ and the estimate chases the noisy dots; raise it and the band widens as the filter smooths. Starve $Q$ and the filter grows overconfident and lags the curve; feed it and the estimate stays nimble.

Kalman filter tracking a noisy 1D targetinteractive

meas. noise R = 0.04 (σ = 0.20)process noise q = 0.60

true position

measurement z

estimate ± √P₀₀

gain K₀

1.000

σ = √P₀₀

1.000

PauseReset

This is the core of how a robot fuses an IMU, wheel odometry, and GPS into one physical state estimate: each sensor is a linear-Gaussian measurement, and the filter weighs every one by its variance. When the dynamics or measurements turn nonlinear, the same predict-update skeleton runs on a local linearization (the extended Kalman filter) or on sampled sigma points (the unscented filter), but the Bayesian recursion underneath does not change.¹

Footnotes

1. Thrun, Burgard, Fox, Probabilistic Robotics (2005). ↩ ↩²

2. Kalman, A New Approach to Linear Filtering and Prediction Problems (1960). ↩