Extended Kalman Filters

In the one-dimensional Kalman filter the belief stays Gaussian at every step. The predict step adds two Gaussians and the update step multiplies two Gaussians, and both results are Gaussian again. That holds because the motion model and the measurement model are linear. However, most real systems are not linear. Both state transition model and measurement model must accommodate non-linearities: when a turn motion that moves the robot along an arc and when a range sensor measures distance through a non-linear function such as a square root.

A range sensor that sees a landmark at

(m_x, m_y)

from a robot at

(x, y)

returns the Euclidean distance

r = \sqrt{(m_x - x)^2 + (m_y - y)^2}

plus noise. This is the landmark measurement model of Probabilistic Robotics (Thrun, Burgard and Fox, Section 6.6.2, Equation 6.41). The square root makes

r

a nonlinear function of the robot’s position, so a Gaussian belief about the position does not give a Gaussian prediction of the range. The EKF localization section uses this model and its Jacobian. The beam model gets the expected range by ray casting through the map instead.

The extended Kalman filter (EKF) handles such models by linearizing them around the current estimate. Before we see this linearization step, lets first understand what happens to to Gaussian when it goes through a non-linearity.

A Gaussian through a U-shaped function

Take a belief

x \sim \mathcal{N}(\mu, \sigma^2)

and pass it through the obviously non-linear U-shaped function

y = g(x) = (x - \mu)^2,

whose lowest point sits exactly at the mean of the Gaussian. The two halves of the input fold onto the same output:

x = \mu + d

and

x = \mu - d

both give

y = d^2

. This function produces positive numbers and the majority of the probability mass is near zero, where the input is most likely. To find the distribution of

y

, draw many samples of

x

, apply

g

to each one, and look at the histogram of the results.

mu, sigma = 2.0, 1.0


def g(x):
    """U-shaped function whose lowest point is at the input mean."""
    return (x - mu) ** 2


n_samples = 100_000
x_samples = np.random.normal(mu, sigma, n_samples)
y_samples = g(x_samples)

y_mean, y_var = y_samples.mean(), y_samples.var()
print(f"mean of y: {y_mean:.3f}, variance of y: {y_var:.3f}")

mean of y: 1.005, variance of y: 2.020

The output looks nothing like a Gaussian. All of its mass sits at

y \ge 0

, and its density grows without bound as

y \to 0

. For this

g

the exact density is a scaled chi-squared distribution with one degree of freedom:

f_Y(y) = \frac{1}{\sqrt{2\pi\sigma^2 y}}\exp\left(-\frac{y}{2\sigma^2}\right), \qquad y > 0.

Its mean is

\sigma^2

and its variance is

2\sigma^4

. The dashed curve is the Gaussian with that mean and variance. It puts about 24% of its probability on negative values of

y

, which

g

can never produce, and it misses the peak at zero. A filter that forced the belief back into a Gaussian after this step would carry the dashed curve forward, with the wrong shape and much of its mass in the wrong place.

Linearizing the function

The EKF keeps the belief Gaussian by replacing the nonlinear function with its tangent line at the mean of the belief. A first-order Taylor expansion of

g

around a point

m

gives

g(x) \approx g(m) + g'(m)\,(x - m).

The right-hand side is linear in

x

. If the input is

x \sim \mathcal{N}(m, \sigma^2)

, the output of the tangent line is Gaussian again:

y \sim \mathcal{N}\left(g(m),\; g'(m)^2\,\sigma^2\right).

The slope

g'(m)

stretches or shrinks the spread of the belief. With a vector state the slope becomes a Jacobian matrix

G

, and the variance

g'(m)^2\sigma^2

becomes the covariance

G\,\Sigma\,G^\top

The Jacobian of a vector function

\mathbf{g}: \mathbb{R}^n \to \mathbb{R}^m

is the

m \times n

matrix of its first partial derivatives, with entries

G_{ij} = \partial g_i / \partial x_j

. Row

i

is the gradient of the output

g_i

, and the whole matrix is the best linear approximation of

\mathbf{g}

near the point where it is evaluated. We have seen Jacobians in backpropagation in deep networks and the references therein.

The code below compares this approximation with the sampled output in two cases. In the first, the belief is centered at

x = 6

, far enough up the right arm that it no longer reaches the bottom of the U. In the second, it is centered at the lowest point of

g

x = \mu = 2

, as before.

def dg(x):
    """Derivative of g."""
    return 2 * (x - mu)


def linearized_moments(m, s):
    """Mean and variance of g(x), x ~ N(m, s^2), with g replaced by its tangent line at m."""
    return g(m), dg(m) ** 2 * s**2


belief_means = [6.0, mu]
cases = {}
for m in belief_means:
    y_case = g(np.random.normal(m, sigma, n_samples))
    lin_mean, lin_var = linearized_moments(m, sigma)
    cases[m] = (y_case, lin_mean, lin_var)
    print(
        f"belief mean {m:g}: sampled mean {y_case.mean():.3f}, variance {y_case.var():.3f} | "
        f"linearized mean {lin_mean:.3f}, variance {lin_var:.3f}"
    )

belief mean 6: sampled mean 17.009, variance 66.150 | linearized mean 16.000, variance 64.000
belief mean 2: sampled mean 1.000, variance 1.973 | linearized mean 0.000, variance 0.000

On the right arm, across most of the belief,

g

is close to its tangent line, and the linearized Gaussian

\mathcal{N}(16, 64)

sits close to the sampled output, whose mean is about 17 and variance about 66. The small gap comes from the curvature of

g

, which the first-order expansion does not capture. Next to a large slope it is a small fraction of the total. At the lowest point the picture changes. The tangent line is flat. The slope is

g'(\mu) = 0

, so the linearized output has zero variance and puts all of its mass at

y = 0

. The true output has mean

\sigma^2

and variance

2\sigma^4

. The tangent line sees only the slope at one point, and here the slope says nothing about how quickly the function rises on either side. The first-order expansion does not capture the curvature of

g

, and here the curvature is all there is.

Write

x = m + e

with

e \sim \mathcal{N}(0, \sigma^2)

. For this

g

the expansion ends at the second-order term and is exact:

g(m + e) = g(m) + g'(m)\,e + \tfrac{1}{2}\,g''(m)\,e^2, \qquad g''(m) = 2.

Because

\mathbb{E}[e] = 0

and

\mathbb{E}[e^2] = \sigma^2

, the mean of

y

g(m) + \tfrac{1}{2}\,g''(m)\,\sigma^2 = g(m) + \sigma^2

. The linear and quadratic terms are uncorrelated, since

\mathbb{E}[e^3] = 0

, and a Gaussian has

\operatorname{Var}(e^2) = 2\sigma^4

, so

\operatorname{Var}(y) = g'(m)^2\,\sigma^2 + \tfrac{1}{4}\,g''(m)^2 \cdot 2\sigma^4 = g'(m)^2\,\sigma^2 + 2\sigma^4.

The tangent line keeps only

g(m)

and

g'(m)^2\sigma^2

, so it misses

\sigma^2

in the mean and

2\sigma^4

in the variance wherever the belief is centered. At

m = \mu

the slope is zero, so the curvature terms are the whole output: mean

\sigma^2

and variance

2\sigma^4

. At

m = 6

with

\sigma = 1

the slope is

g'(6) = 8

, and the same terms are small next to it: the exact moments are

17

and

64 + 2 = 66

, against the linearized

16

and

64

. For a general

g

the expansion goes on, and the third- and higher-order terms add further errors.

Inside a filter this error carries into the next step. Line 2 of the algorithm below predicts the covariance as

G_t \Sigma_{t-1} G_t^\top + R_t

. If the motion model is flat at the current estimate, then

G_t = 0

and the predicted covariance is just the motion noise

R_t

. The uncertainty in

\Sigma_{t-1}

is dropped, and the filter is more confident than it should be. The Kalman gain in line 3 grows with the predicted covariance, so a small covariance gives a small gain. The update then gives little weight to the measurement

z_t

, and the measurement is what would correct the estimate. The predicted mean is also wrong: the filter predicts

y = 0

, but the true mean is

\sigma^2

. Over the next steps the estimate moves away from the true state, and the covariance stays small. This is called filter divergence. An EKF whose covariance is smaller than its actual error is called inconsistent. The measurement model can fail the same way. If

h

is flat at the predicted mean, then

H_t = 0

, the gain

K_t

is zero, and the update ignores the measurement however informative it is.

In robots a Jacobian is rarely zero in every direction. More often it is flat in some directions of the state, or its slope is small compared with how much the model bends across the belief.Measurement models:

GPS. A GPS position fix does not depend on the heading, so the heading column of $H_t$ is zero. The heading becomes observable only through motion, by comparing successive fixes or through the GPS velocity $v(\cos\theta, \sin\theta)$ . That velocity has derivative $v(-\sin\theta, \cos\theta)$ with respect to $\theta$ , which vanishes when the robot stands still.
Terrain-aided navigation underwater. An autonomous underwater vehicle can fix its position by matching multibeam echosounder soundings to a stored depth map of the seabed. Over a flat ocean floor every nearby position predicts the same depths, so $h$ is flat in both horizontal directions and the soundings give no position information. The terrain navigation module of the Kongsberg HUGIN vehicles therefore needs “a certain degree of terrain variation” (Ånonsen et al.). A 2022 study of bathymetric SLAM on HUGIN 3000 survey data includes such a flat stretch of seabed (Torroba et al., 2022).
Range sensors (lidar, sonar, ultra-wideband). A single range does not change as the robot moves on a circle around the landmark, so that direction is flat.
Bearing sensors and cameras. Moving along the line of sight changes neither the bearing nor the pixel, so depth is flat. The bearing derivative also shrinks with distance, so far landmarks give a nearly flat $H_t$ .
Saturated readings. A lidar return at maximum range or a saturated pixel stays the same when the state changes a little.

Motion models:

Contact and limits. A robot pushed against a wall, a joint at its limit, or a saturated actuator. In the blocked direction the next state does not depend on the current one, so that part of $G_t$ is zero.
Wide heading uncertainty. After startup or after the robot is moved by hand, the heading can be almost anywhere. The terms $\cos\theta$ and $\sin\theta$ in the motion model bend strongly across the belief, and the tangent at the mean describes them poorly.

Linearization works when the function is close to a straight line over the region where the belief has most of its mass. A narrow belief, or a function that bends slowly, keeps the EKF accurate. A wide belief near a sharp bend breaks it.

Nonlinearity and uncertainty

Probabilistic Robotics (Thrun, Burgard and Fox, Section 3.3) names two factors that decide how good the EKF’s linearization is: the degree of local nonlinearity of the function, and the degree of uncertainty in the belief. The comparison above varied the first factor. It kept the width of the belief fixed and moved it from the arm, where

g

is nearly straight, to the bottom of the U, where

g

bends sharply. The comparison below varies the second factor. Both beliefs are centered at

x = 6

on the arm, where the tangent line worked well, and only their standard deviations differ:

\sigma = 0.5

and

\sigma = 2.5

m_arm = 6.0
widths = [0.5, 2.5]
width_cases = {}
for s in widths:
    y_case = g(np.random.normal(m_arm, s, n_samples))
    lin_mean, lin_var = linearized_moments(m_arm, s)
    width_cases[s] = (y_case, lin_mean, lin_var)
    print(
        f"belief std {s:g}: sampled mean {y_case.mean():.3f}, variance {y_case.var():.3f} | "
        f"linearized mean {lin_mean:.3f}, variance {lin_var:.3f}"
    )

belief std 0.5: sampled mean 16.234, variance 16.017 | linearized mean 16.000, variance 16.000
belief std 2.5: sampled mean 22.222, variance 476.621 | linearized mean 16.000, variance 400.000

The narrow belief covers a short, nearly straight piece of

g

, and its linearized Gaussian matches the sampled output. The wide belief reaches past the bottom of the U at

x = 2

. That part of the belief folds back, and the sampled output is skewed, with extra mass near

y = 0

and a long right tail. The tangent line cannot see the fold. Its mean is too low by

\sigma^2 = 6.25

, and its Gaussian puts probability on negative values of

y

that

g

never produces. The same point on the same function is easy to linearize for a confident belief and hard for an uncertain one. A robot that is less certain has a wider belief, and a wider belief covers more of the curvature of the motion and measurement models. This is why an EKF works best when the uncertainty of its estimate stays small, and why the overconfidence described above is so damaging: a filter that underestimates its uncertainty also stops noticing that its linearization has become poor.

The extended Kalman filter algorithm

Write the state as

s_t

, the action as

a_t

and the measurement as

z_t

. The motion model and the measurement model are now nonlinear functions with additive Gaussian noise:

s_t = g(a_t, s_{t-1}) + \varepsilon_t, \qquad \varepsilon_t \sim \mathcal{N}(0, R_t),

and

z_t = h(s_t) + \delta_t, \qquad \delta_t \sim \mathcal{N}(0, Q_t).

From here on,

g

is the motion model and no longer the U-shaped function above. The EKF linearizes

g

around the previous mean

\mu_{t-1}

and

h

around the predicted mean

\bar\mu_t

. The two Jacobians are

G_t = \left.\frac{\partial g(a_t, s_{t-1})}{\partial s_{t-1}}\right|_{s_{t-1} = \mu_{t-1}}, \qquad H_t = \left.\frac{\partial h(s_t)}{\partial s_t}\right|_{s_t = \bar\mu_t}.

One step of the filter takes the previous belief

(\mu_{t-1}, \Sigma_{t-1})

, the action

a_t

and the measurement

z_t

, and returns the new belief

(\mu_t, \Sigma_t)

\begin{array}{lll} 1: & \bar\mu_t = g(a_t, \mu_{t-1}) & \text{predict the mean} \\ 2: & \bar\Sigma_t = G_t\, \Sigma_{t-1}\, G_t^\top + R_t & \text{predict the covariance} \\ 3: & K_t = \bar\Sigma_t H_t^\top \left(H_t \bar\Sigma_t H_t^\top + Q_t\right)^{-1} & \text{Kalman gain} \\ 4: & \mu_t = \bar\mu_t + K_t \left(z_t - h(\bar\mu_t)\right) & \text{update the mean} \\ 5: & \Sigma_t = (I - K_t H_t)\, \bar\Sigma_t & \text{update the covariance} \end{array}

In this notation

R_t

is the motion noise covariance and

Q_t

is the measurement noise covariance. Compared with the Kalman filter:

The means pass through the nonlinear functions themselves, in lines 1 and 4.
The covariances pass through the Jacobians $G_t$ and $H_t$ , which take the place of the fixed matrices of the linear model.

from filterpy.kalman import ExtendedKalmanFilter


class EKF(ExtendedKalmanFilter):
    """Extended Kalman filter with a nonlinear motion model.

    Naming in the parent class: x is the mean (mu), P the covariance (Sigma),
    Q the motion noise (R_t above) and R the measurement noise (Q_t above).
    """

    def __init__(self, dim_s, dim_z, g, G):
        super().__init__(dim_x=dim_s, dim_z=dim_z)
        self.g, self.G = g, G  # motion model g(a, s) and its Jacobian G(a, s)

    def predict(self, a):
        G_t = self.G(a, self.x)  # Jacobian at mu_{t-1}
        self.x = self.g(a, self.x)  # line 1
        self.P = G_t @ self.P @ G_t.T + self.Q  # line 2
        self.x_prior, self.P_prior = self.x.copy(), self.P.copy()


# Lines 3 to 5 are the parent's update(z, HJacobian, Hx), where HJacobian(s)
# returns H_t and Hx(s) returns h(s), both evaluated at the predicted mean.

A differential-drive robot with an uncertain heading

The U-shaped function we used for the state transition was synthetic and was used to illustrate the EKF but it was not accidental. The same fold appears in the motion model of a differential-drive robot such as the TurtleBot. As in wheeled robots, two wheels of radius

r

sit at distance

d

on either side of the chassis origin. The action is the pair of wheel speeds

a_t = (u_L, u_R)

, which set the forward speed and the turning rate:

v = \frac{r}{2}(u_L + u_R), \qquad \omega = \frac{r}{2d}(u_R - u_L).

With the state

s = (x, y, \theta)

and a time step

\Delta t

, the motion model and its Jacobian are

g(a_t, s_{t-1}) = \begin{bmatrix} x + v\,\Delta t\cos\theta \\ y + v\,\Delta t\sin\theta \\ \theta + \omega\,\Delta t \end{bmatrix}, \qquad G_t = \begin{bmatrix} 1 & 0 & -v\,\Delta t\sin\theta \\ 0 & 1 & v\,\Delta t\cos\theta \\ 0 & 0 & 1 \end{bmatrix}.

Let the robot start at the origin and drive straight down the page,

u_L = u_R

, with its heading known only roughly. Write the heading as

\theta = -90° + \psi

, where

\psi

is the heading error. After the drive the robot is at

x = v\,\Delta t\sin\psi

and

y = -v\,\Delta t\cos\psi \approx -v\,\Delta t + \frac{v\,\Delta t}{2}\psi^2

. The forward coordinate

y

is the U-shaped function from the start of this section, with its lowest point at

\psi = 0

: a heading error of

+\delta

-\delta

shortens the progress by the same amount, and the two halves of the heading belief fold together. The Jacobian entry

\partial y / \partial\theta = v\,\Delta t\cos\theta

is zero at

\theta = -90°

, so the EKF passes none of the heading uncertainty into

y

. Sideways,

\sin\psi \approx \psi

is close to linear, and the EKF handles

x

well. The code below uses the geometry of a TurtleBot3 Waffle and drives it straight down the page at 0.25 m/s for 4 s, starting from a belief that knows the position to 2 cm but the heading only to

30°

. It predicts the new pose twice: by pushing samples of the starting pose through

g

, and with one prediction step of the EKF class above. To isolate the effect of the heading, the motion noise

R_t

is left out.

r, d = 0.033, 0.1435  # TurtleBot3 Waffle wheel radius and half wheel separation, in meters
dt = 4.0  # seconds


def g_dd(a, s):
    """Differential-drive motion model. s has shape (3, n): one pose (x, y, theta) per column."""
    u_L, u_R = a
    v, w = r / 2 * (u_L + u_R), r / (2 * d) * (u_R - u_L)
    x, y, th = s
    return np.vstack([x + v * dt * np.cos(th), y + v * dt * np.sin(th), th + w * dt])


def G_dd(a, s):
    """Jacobian of g_dd with respect to the state, at the pose s of shape (3, 1)."""
    u_L, u_R = a
    v, th = r / 2 * (u_L + u_R), s[2, 0]
    return np.array([[1, 0, -v * dt * np.sin(th)], [0, 1, v * dt * np.cos(th)], [0, 0, 1]])


u = 0.25 / r  # equal wheel speeds for 0.25 m/s straight ahead, in rad/s
a = (u, u)
mu0 = np.array([[0.0], [0.0], [-np.pi / 2]])  # heading straight down the page
Sigma0 = np.diag([0.02**2, 0.02**2, np.deg2rad(30) ** 2])

# samples of the starting pose, pushed through the motion model
s_samples = g_dd(a, np.random.multivariate_normal(mu0.ravel(), Sigma0, n_samples).T)

# one EKF prediction step
ekf = EKF(dim_s=3, dim_z=1, g=g_dd, G=G_dd)
ekf.x, ekf.P, ekf.Q = mu0.copy(), Sigma0.copy(), np.zeros((3, 3))
ekf.predict(a)

print("sampled mean x, y:", s_samples[:2].mean(axis=1).round(3), "std x, y:", s_samples[:2].std(axis=1).round(3))
print("EKF     mean x, y:", ekf.x[:2, 0].round(3), "std x, y:", np.sqrt(np.diag(ekf.P)[:2]).round(3))

sampled mean x, y: [ 0.    -0.872] std x, y: [0.461 0.17 ]
EKF     mean x, y: [ 0. -1.] std x, y: [0.524 0.02 ]

The dashed ellipse is the EKF’s predicted belief about the position. Write

\bar\mu

and

\bar\Sigma

for the

x, y

parts of the predicted mean and covariance. The ellipse is the set of positions

p

whose Mahalanobis distance

\sqrt{(p - \bar\mu)^\top \bar\Sigma^{-1} (p - \bar\mu)}

equals 2. Its axes point along the eigenvectors of

\bar\Sigma

, each half-axis is two standard deviations long, and it encloses about 86% of the probability of the Gaussian. The samples form a curved band, the shape often called a banana. It opens upward like the U, because every heading error shortens the forward progress: no sample gets below

y = -1

m, and the sampled mean stops short of it. The EKF puts its mean at exactly

y = -1

m. Its ellipse is wide in

x

, where

\sin\psi

is nearly linear, but only as thick in

y

as the 2 cm starting uncertainty, because the Jacobian passes no heading uncertainty into

y

. The histogram on the right shows the fold directly: the forward position has a hard limit at

-1

m and a long tail toward shorter progress, while the EKF predicts a narrow spike at the limit. This is the U-shaped example from the start of the section, now in a real motion model. The EKF is confidently wrong about how far the robot went. The localization section continues this example: the robot measures the range and bearing to a landmark, and the EKF uses the measurement to correct its belief.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Extended Kalman Filters

A Gaussian through a U-shaped function

Linearizing the function

Nonlinearity and uncertainty

The extended Kalman filter algorithm

A differential-drive robot with an uncertain heading

​A Gaussian through a U-shaped function

​Linearizing the function

​Nonlinearity and uncertainty

​The extended Kalman filter algorithm

​A differential-drive robot with an uncertain heading

A Gaussian through a U-shaped function

Linearizing the function

Nonlinearity and uncertainty

The extended Kalman filter algorithm

A differential-drive robot with an uncertain heading