In robot state estimation, the Bayes filter is a probabilistic approach that estimates the state from a sequence of controls and measurements by recursively performing prediction with a motion model and correction with a measurement model. In practice, some common instantiations of the Bayes framework are the Kalman filter, information filter, and particle filter.

Mathematical Formulation of Bayes Filter

Given a sequence of measurements ${\mathrm{z}}_{1:t}={\mathbf{z}}_1,\dots ,{\mathbf{z}}_t$ and controls ${\mathbf{u}}_{1:t}={\mathbf{u}}_1,\dots ,{\mathbf{u}}_t$, and a measurement model $p({\mathbf{z}}_t\mid {\mathbf{x}}_t)$ and a motion model $p({\mathbf{x}}_t\mid {\mathbf{x}}_{t-1},{\mathbf{u}}_t)$. The Bayes filter provides a recursive framework for estimating the state ${\mathbf{x}}_t$ in terms of a probability distribution function, known as the belief $\mathrm{bel}({\mathbf{x}}_t)\triangleq p({\mathbf{x}}_t\mid {\mathbf{z}}_{1:t},{\mathbf{u}}_{1,t})$, or posterior.

The filtering algorithm is built upon two assumptions of conditional independence:

• Motion model is (1st-order) Markov:

• The measurement model is memoryless:

Given these two assumptions, we can propagate the belief over time:

We can define the predicted belief of the state at time $t$ to be the probability distribution before knowing the current measurement, i.e., $\overline{\mathrm{bel}}({\mathbf{x}}_t)\triangleq p({\mathbf{x}}_t\mid {\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})$. Then, the above equation becomes

where $\eta$ is the normalization coefficient, and

which is essentially a correction applied to the belief after observing the current measurement.

What remains to be done, obviously, is to predict the belief $\overline{\mathrm{bel}}({\mathbf{x}}_t)$ from the previous state:

Then, recognizing $p({\mathbf{x}}_{t-1}\mid {\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})=\mathrm{bel}({\mathbf{x}}_{t-1})$, we have

In summary, the Bayes filter provides a two-step framework for updating the belief of the current state ${\mathbf{x}}_t$ from the belief of the previous state ${\mathbf{x}}_{t-1}$:

• Prediction using the motion model (Equation 1);

• Correction using the measurement model (Equation 2).

These two steps can be performed recursively to estimate the state over time.

Kalman Filter

The Kalman filter is an instantiation of the Bayes filter with the assumption of linear Gaussian models. This means the motion model and the measurement model should be

• linear with respect to their arguments:

  Specifically, suppose $\mathbf{x}\in {\mathbb{R}}^m$ , $\mathbf{u}\in {\mathbb{R}}^n$, and $\mathbf{z}\in {\mathbb{R}}^p$. Let $\mathbf{F}\in {\mathbb{R}}^{m\times m}$, $\mathbf{G}\in {\mathbb{R}}^{m\times n}$, and $\mathbf{H}\in {\mathbb{R}}^{p\times m}$ be matrices (linear maps).

  • Motion model:

  $$\begin{array}{rlr}{\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{G}}_t{\mathbf{u}}_t+{\mathbf{w}}_t,& & {\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)\end{array}$$

  • Measurement model:

  $$\begin{array}{rlr}{\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t,& & {\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)\end{array}$$

  where $\mathbf{w}\in {\mathbb{R}}^m$ and ${\mathbf{v}}_t\in {\mathbb{R}}^p$ are white noise with variance $\mathbf{Q}\in {\mathbb{R}}^{m\times m}$ and $\mathbf{R}\in {\mathbb{R}}^{p\times p}$ , respectively.

  White noise

  A discrete-time random process (a sequence of random vectors), ${\mathbf{w}}_k$, is called white noise if

  $$\begin{array}{rlr}\mathbb{E}[{\mathbf{w}}_k{\mathbf{w}}_j^{\top }]={\mathbf{Q}}_k{\delta }_{kj},& & {\delta }_{kj}=\{\begin{array}{cc}1& k=j\\ 0& k\ne j\end{array}\end{array}$$

• following a multivariate Gaussian distribution:

  Specifically,

  • Motion model: $p({\mathbf{x}}_t\mid {\mathbf{x}}_{t-1},{\mathbf{u}}_t)=\mathcal{N}({\mathbf{x}}_t;{\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{G}}_t{\mathbf{u}}_t,{\mathbf{Q}}_t)$;
  • Measurement model: $p({\mathbf{z}}_t\mid {\mathbf{x}}_t)=\mathcal{N}({\mathbf{z}}_t;{\mathbf{H}}_t{\mathbf{x}}_t,{\mathbf{R}}_t)$.

  Note: here we need to interpret ${\mathbf{x}}_{t-1}$as a realization rather than a random variable.

  Multivariate Gaussian PDF

  $\mathcal{N}(\mathbf{x};\mathbf{\mu },\mathbf{\Sigma })$ is the probability distribution function of a random variable $\mathbf{x}\in {\mathbb{R}}^m$ that follows a Gaussian distribution with mean $\mathbf{\mu }\in {\mathbb{R}}^m$ and covariance matrix $\mathbf{\Sigma }\in {\mathbb{R}}^{m\times m}$,

  $$\mathcal{N}(\mathbf{x};\mathbf{\mu },\mathbf{\Sigma })=p(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^m\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}{\left(\mathbf{x}-\mathbf{\mu }\right)}^{\top }{\mathbf{\Sigma }}^{-1}\left(\mathbf{x}-\mathbf{\mu }\right)\right)$$

Before deriving the algorithm for the Kalman filter, there are two important properties of the multivariate Gaussian distribution that are useful here.

1. Affine transformation:

   Let $\mathbf{x}$ be a random vector following a Gaussian distribution $\mathbf{x}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$, then the random vector $\mathbf{y}=\mathbf{Ax}+\mathbf{b}$ follows a Gaussian distribution $\mathbf{y}\sim \mathcal{N}(\mathbf{A}\mathbf{\mu }+\mathbf{b},\mathbf{A\Sigma }{\mathbf{A}}^{\top })$.

   Proof

   Will come later

2. Conditional distribution:

   Let $\mathbf{y}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$ be a random vector of dimension $m+p$. Partition $\mathbf{y}$ into a vector of dimension $m$, $\mathbf{x}\in {\mathbb{R}}^m$ and a vector of dimension $p$, $\mathbf{z}\in {\mathbb{R}}^p$, and partition $\mathbf{\mu }$ and $\mathbf{\Sigma }$ respectively,

   $$\begin{array}{c}\left[\begin{array}{c}\mathbf{x}\\ \mathbf{z}\end{array}\right]\sim \mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_z\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xz}\\ {\mathbf{\Sigma }}_{zx}& {\mathbf{\Sigma }}_{zz}\end{array}\right]\right)\end{array}$$

   then, the conditional distribution $p(\mathbf{x}\mid \mathbf{z}=z)=\mathcal{N}(\mathbf{x};{\mathbf{\mu }}_{\mathbf{x}\mid \mathbf{z}},{\mathbf{\Sigma }}_{\mathbf{x}\mid \mathbf{z}})$ , where

   $$\begin{array}{rlr}{\mathbf{\mu }}_{\mathbf{x}\mid \mathbf{z}}={\mathbf{\mu }}_x+{\mathbf{\Sigma }}_{xz}{\mathbf{\Sigma }}_{zz}^{-1}\left(z-{\mu }_z\right),& & {\mathbf{\Sigma }}_{\mathbf{x}\mid \mathbf{z}}={\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xz}{\mathbf{\Sigma }}_{zz}^{-1}{\mathbf{\Sigma }}_{zx}\end{array}$$

   Proof

   Will come later

Now, if we assume the initial belief to be Gaussian, i.e., $\mathrm{bel}({\mathbf{x}}_0)=p({\mathbf{x}}_0)=\mathcal{N}({\mathbf{x}}_0;{\mathbf{\mu }}_0,{\mathbf{\Sigma }}_0)$. Then, from equations (1) and (2), we can see that all subsequent beliefs will be Gaussian. Thus,

Then, we only need to track ${\mathbf{\mu }}_t^{-},{\mathbf{\Sigma }}_t^{-}$ in the prediction step and ${\mathbf{\mu }}_t,{\mathbf{\Sigma }}_t$ in the correction step.

For the prediction step, use the joint distribution $p({\mathbf{x}}_t,{\mathbf{x}}_{t-1}\mid {\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})$ and realize

Then $\overline{\mathrm{bel}}({\mathbf{x}}_t)=p({\mathbf{x}}_t\mid {\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})$ can be obtained by marginalize ${\mathbf{x}}_{t-1}$,

For the correction step, form a joint distribution $p({\mathbf{x}}_t,{\mathbf{z}}_t\mid {\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})$ and realize

Thus, $\mathrm{bel}({\mathbf{x}}_t)=p({\mathbf{x}}_t\mid {\mathbf{z}}_t,{\mathbf{u}}_{1:t},{\mathbf{z}}_{1:t-1})$, where

Finally, normalize for numerical stability

Extended Kalman Filter (EKF)

One way to handle the case where motion model and measurement model are not linear is to approximate the functions via local linearization (Taylor expansion). Specifically, we extend the Kalman filter algorithm to cases where models are non-linear:

• Motion model

• Measurement model

Let

The EKF algorithm is

Unscented Kalman Filter (UKF)

Another non-linear Kalman filter is the unscented Kalman filter which uses deterministic sampling to approximate the distribution.

Let a function $f:{\mathbb{R}}^n\to {\mathbb{R}}^m$ be a non-linear transformation. Suppose $\mathbf{x}\in {\mathbb{R}}^n$ and $\mathbf{x}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$. We draw a set of $n$ sigmapoints, $\mathcal{X}=\{{\mathbf{x}}_i{\}}_{i=1}^n$, and compute a weight $w_i$ for each sigma point by executing

Further let $\mathbf{y}=f(\mathbf{x})$, then $\mathbf{y}\sim \mathcal{N}({\mathbf{\mu }}^{\prime },{\mathbf{\Sigma }}^{\prime })$, where

The UKF algorithm is

Reference

[1] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Mit Press, 2010.
[2] M. Ghaffari, ROB530 Lecture Slides — Kalman Filtering. University of Michigan, 2026.