> ## Documentation Index
> Fetch the complete documentation index at: https://aegean.ai/llms.txt
> Use this file to discover all available pages before exploring further.

# An Overconfident EKF Prediction

> One EKF prediction step on a TurtleBot compared with sampled motion.

[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/rse/ekf-overconfident-prediction/index.ipynb)

The [EKF section](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md) pushed a Gaussian belief through a U-shaped function and found that the tangent line fails where the function is flat at the mean of the belief: the linearized variance drops to zero although the true output still spreads. This section shows the same failure on a robot. A TurtleBot drives straight ahead with its heading known only roughly, and one prediction step of the EKF is compared with samples pushed through the motion model itself.

The prediction uses the `EKF` class of the EKF section, built on filterpy's `ExtendedKalmanFilter`:

```python theme={null}
n_samples = 100_000  # samples for the reference prediction


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) and P the covariance (Sigma).
    Q is the motion noise and R the measurement noise, as above.
    """

    def __init__(self, dim_s, dim_z, g, G):
        # dim_s: size of the state; dim_z: size of a measurement
        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):
        """Prediction step for the action a: lines 1 and 2 of the algorithm."""
        # Evaluate the Jacobian at the previous mean, before the mean moves.
        G_t = self.G(a, self.x)  # Jacobian at mu_{t-1}
        # Line 1: push the mean through the full nonlinear motion model.
        self.x = self.g(a, self.x)
        # Line 2: push the covariance through the linearized model, then add
        # the motion noise.
        self.P = G_t @ self.P @ G_t.T + self.Q
        # Keep a copy of the predicted belief, as the parent class does.
        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.
```

## The TurtleBot motion model

The straight drive at the start of the [EKF section](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md#nonlinear-motion) is a special case of the differential-drive motion model of the TurtleBot. As in [nonholonomic robots](https://aegean.ai/aiml-common/lectures/kinematics/nonholonomic-robots/index.md), 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, as [derived there](https://aegean.ai/aiml-common/lectures/kinematics/nonholonomic-robots/index.md#wheel-speeds-of-a-differential-drive) from rolling without slipping:

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

This model moves the robot $v\,\Delta t$ along its current heading and then turns it by $\omega\,\Delta t$. It is the first-order approximation of the motion model in [EKF localization](https://aegean.ai/aiml-common/lectures/rse/ekf-localization/index.md#motion-model), which holds the body twist constant over the step and moves the robot along the exact arc, $T_t = T_{t-1}\exp([\mathcal{V}_b]\,\Delta t)$. Expanding that arc in powers of $\Delta t$ gives

$$
\begin{aligned}
x_t &= x + v\,\Delta t\cos\theta - \tfrac{1}{2}\, v\,\omega\,\Delta t^2 \sin\theta + O(\Delta t^3), \\
y_t &= y + v\,\Delta t\sin\theta + \tfrac{1}{2}\, v\,\omega\,\Delta t^2 \cos\theta + O(\Delta t^3).
\end{aligned}
$$

The model here keeps the first-order terms and leaves out the $\Delta t^2$ terms, a sideways drift that the robot builds up by turning during the step. The two models agree exactly when $\omega = 0$. The drive below is straight, so its results are the same under either model.

The action $a_t = (u_L, u_R)$ is in $\mathbb R^2$ and the state is in $\mathbb R^3$, so $g$ maps $\mathbb R^3 \times \mathbb R^2$ to $\mathbb R^3$. The EKF holds the action at its given value and differentiates with respect to the state only. In the notation of the [Jacobian note](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md#linearizing-the-function) of the EKF section, the map is $s_{t-1} \mapsto g(a_t, s_{t-1})$ from $\mathbb R^3$ to $\mathbb R^3$, so $n = m = 3$ and $G_t$ is $3 \times 3$. The derivative with respect to the two wheel speeds is a $3 \times 2$ matrix. The [EKF localization](https://aegean.ai/aiml-common/lectures/rse/ekf-localization/index.md) section uses it to turn wheel-speed noise into motion noise.

Let the robot start at the origin and drive straight down the page, $u_L = u_R$, with its heading known only roughly. The heading is uncertain even when every sensor works as specified: wheel slip and small errors in the wheel radii add heading error at every step of odometry, the [gyro's bias](https://aegean.ai/aiml-common/lectures/sensor-models/imu/index.md#noise-and-bias) integrates into a heading drift, and after startup, or after the robot is carried to a new spot, it has no heading reference until it sees a landmark. Write the heading as $\theta = -90° + \psi$, where $\psi$ is the heading error. The figure shows where the robot ends up.

![Top: a right triangle formed by a 1 m drive from the start, rotated by a heading error psi from the intended straight-down direction. The drive is the hypotenuse v dt, the forward leg is v dt cos psi, the sideways leg is v dt sin psi, and the forward shortfall v dt (1 - cos psi) is marked, with the circle of all end points dotted. Bottom: the sideways offset and the shortfall as functions of psi from -60 to 60 degrees, with their first- and second-order approximations dashed.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/ekf-overconfident-prediction/images/cell_3_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=601035528864eb923cc510c7716e1b27)

The heading is $\theta = -90° + \psi$, so the drive of length $v\,\Delta t$ points $\psi$ away from straight down. The drive is the hypotenuse of a right triangle, and its two legs are the sideways offset and the forward progress. Its components are $x = v\,\Delta t\cos\theta$ and $y = v\,\Delta t\sin\theta$. With $\cos(-90° + \psi) = \sin\psi$ and $\sin(-90° + \psi) = -\cos\psi$ they become

$$
x = v\,\Delta t\sin\psi, \qquad y = -v\,\Delta t\cos\psi .
$$

Whatever the heading error, the end point lies on the dotted circle of radius $v\,\Delta t$ around the start. For small $\psi$, $\sin\psi \approx \psi$ and $\cos\psi \approx 1 - \psi^2/2$, so

$$
x \approx v\,\Delta t\,\psi, \qquad y \approx -v\,\Delta t + \frac{v\,\Delta t}{2}\,\psi^2 .
$$

The sideways offset (green) is first order in the heading error. It grows in proportion to $\psi$, and in the lower panel it stays on its straight line across the $\pm 30°$ band. The shortfall in forward progress (pink) is second order. It is flat at $\psi = 0$ and grows like $\psi^2$, and it is the same for $+\psi$ and $-\psi$, so the robot always stops short of the intended 1 m and never goes past it. A small heading error moves the robot sideways much more than it shortens the drive: at $\psi = 10°$ the offset is 17 cm and the shortfall 1.5 cm.

## Sampled and EKF predictions

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 $Q_t$ is left out.

Before looking at the result, evaluate $G_t$ at $\theta = -90°$ and decide how much of the heading uncertainty the EKF passes into the sideways coordinate $x$ and how much into the forward coordinate $y$.

```python theme={null}
# Robot geometry, in meters: TurtleBot3 Waffle wheel radius r and half the
# distance between the wheels d.
r, d = 0.033, 0.1435
dt = 4.0  # length of the time step, in 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  # wheel speeds, in rad/s
    # forward speed v and turning rate w from the wheel speeds
    v, w = r / 2 * (u_L + u_R), r / (2 * d) * (u_R - u_L)
    x, y, th = s
    # Move forward v * dt along the current heading, then turn by w * dt.
    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]
    # Only the heading enters nonlinearly, so only the third column differs
    # from the identity: it says how x and y change when theta changes.
    return np.array([[1, 0, -v * dt * np.sin(th)], [0, 1, v * dt * np.cos(th)], [0, 0, 1]])


# The action: both wheels at the same speed, so the robot drives straight
# at 0.25 m/s. Over dt = 4 s it covers 1 m.
u = 0.25 / r  # wheel speed in rad/s: with u_L = u_R = u, v = r u, so u = v / r
a = (u, u)

# The starting belief: the robot is at the origin, facing straight down the
# page (theta = -90 degrees). Its position is known to 2 cm, its heading only
# to 30 degrees.
mu0 = np.array([[0.0], [0.0], [-np.pi / 2]])
# Sigma0 is 3 x 3, in the order of the state (x, y, theta). Its diagonal holds
# the variances, the squared standard deviations: (2 cm)^2 for x and y, in m^2,
# and (30 degrees)^2 for theta, in rad^2. The off-diagonal entries are the
# covariances between pairs of coordinates; zero says that an error in one
# coordinate tells nothing about the others.
Sigma0 = np.diag([0.02**2, 0.02**2, np.deg2rad(30) ** 2])

# Reference answer: draw starting poses from the belief and push each one
# through the motion model. Each column of s_samples is one predicted pose.
s_samples = g_dd(a, np.random.multivariate_normal(mu0.ravel(), Sigma0, n_samples).T)

# The EKF's answer: one prediction step from the same belief. The motion
# noise Q is zero so that only the heading uncertainty spreads the belief.
ekf = EKF(dim_s=3, dim_z=1, g=g_dd, G=G_dd)
# filterpy's names: x is the mean mu, P the covariance Sigma, Q the motion noise.
ekf.x, ekf.P, ekf.Q = mu0.copy(), Sigma0.copy(), np.zeros((3, 3))
ekf.predict(a)

# Compare the two predictions of the position.
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))
```

```output theme={null}
sampled mean x, y: [ 0.001 -0.872] std x, y: [0.46  0.171]
EKF     mean x, y: [ 0. -1.] std x, y: [0.524 0.02 ]
```

![Left: the forward position after a 1 m drive as a function of the heading error, a U-shaped curve with its flat tangent at zero error and the plus or minus two sigma heading belief shaded. Right: histogram of the sampled forward positions, ending at -1 m and spread toward shorter progress, with the EKF's narrow Gaussian spike at -1 m.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/ekf-overconfident-prediction/images/cell_5_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=38d7148c294e62232e097863f6e3b3d0)

The left panel is the motion model along $y$, drawn the same way as the U-shaped function $g$ in the [EKF section](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md#a-gaussian-through-a-u-shaped-function). The curve is the forward position after the drive as a function of the heading error, $y(\psi) = -v\,\Delta t\cos\psi$, and the shaded band is the heading belief out to two standard deviations, $\pm 60°$. It is the U again, with its lowest point at the mean of the belief, $\psi = 0$. The EKF replaces the curve with its tangent line at the mean. That line is flat, because the Jacobian entry $\partial y / \partial\theta = v\,\Delta t\cos\theta$ is zero at $\theta = -90°$. This is the case of the belief centered at the bottom of the U in [When linearization fails](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md#when-linearization-fails), where the linearized variance was zero.

The right panel shows the result. The sampled forward position stops at $-1$ m and spreads toward shorter progress, and its mean is $-0.87$ m. The EKF puts all of its probability in a narrow spike at $-1$ m, only as wide as the 2 cm starting uncertainty. It reports almost no uncertainty about how far the robot went, and it is wrong.

The same samples, drawn in the plane, show where the robot can end up.

![Sampled end positions of the drive lying on an arc of radius 1 m below the start, with the EKF's two-sigma ellipse, wide sideways and only 4 cm tall, centered at the full 1 m, and the sampled mean above it.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/ekf-overconfident-prediction/images/cell_6_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=128f1cf1d6d578172576820bde7cda5f)

The gray square marks the start at the origin, and the arrow shows the heading straight down the page. The dots are 3,000 of the sampled positions. They lie on an arc. Every sample gets the same command, both wheels at the same speed, so it drives straight for exactly 1 m and never turns. The samples differ only in where they point at the start: the belief knows the starting heading only to 30°, and each sample draws one heading from it and keeps it for the whole drive. A 1 m straight drive along each of those headings ends on a circle of radius 1 m around the start, and the 2 cm uncertainty in the starting position gives the arc its thickness. The dashed ellipse is the EKF's prediction, drawn two standard deviations out from its mean. The prediction is one step from the starting belief $(\mu_0, \Sigma_0)$ to $(\bar\mu_1, \bar\Sigma_1)$. With $\bar\mu_1$ and $\bar\Sigma_1$ restricted to their $x, y$ parts, the ellipse is the set of positions $p$ with $\sqrt{(p - \bar\mu_1)^\top \bar\Sigma_1^{-1} (p - \bar\mu_1)} = 2$, and it holds about 86% of the probability of the EKF's Gaussian.

The shape of the ellipse comes from the Jacobian. With the motion noise left out, the EKF's predicted covariance is $\bar\Sigma_1 = G_1 \Sigma_0 G_1^\top$, with $G_1$ evaluated at the starting mean $\mu_0$. Only the third column of $G_1$, the one for $\theta$, differs from the identity. At the starting heading $\theta = -90°$ that column is $(v\,\Delta t, 0, 1)^\top$, because $-v\,\Delta t\sin\theta = v\,\Delta t$ and $v\,\Delta t\cos\theta = 0$. All of the heading variance therefore goes into $x$ and none into $y$:

$$
(\bar\Sigma_1)_{xx} = \sigma_x^2 + (v\,\Delta t)^2\sigma_\theta^2, \qquad (\bar\Sigma_1)_{yy} = \sigma_y^2, \qquad (\bar\Sigma_1)_{xy} = 0.
$$

With $v\,\Delta t = 1$ m, $\sigma_\theta = 30° = 0.52$ rad and $\sigma_x = \sigma_y = 2$ cm, the EKF's standard deviation is 0.52 m in $x$ and 0.02 m in $y$. The cross term is zero, so the ellipse lines up with the axes.

In $x$ the EKF is close to the samples. Sideways, the robot ends at $x = v\,\Delta t\sin\psi$, and $\sin\psi \approx \psi$ for small heading errors, so the tangent line stays close to the curve. The samples spread 0.46 m in $x$, a little less than the EKF's 0.52 m, because $\sin\psi$ falls below $\psi$ for the larger heading errors in the tails of the belief. In $y$ the EKF is far off. The samples spread 0.17 m, more than eight times the EKF's 2 cm, because the tangent at the bottom of the U is flat and passes none of the heading uncertainty into the forward direction.

The samples show what actually happens. Each one is a starting pose that the belief allows, pushed through the motion model itself, so together they are the robot's distribution at $t = 1$: somewhere on the arc, on average 0.87 m forward, and about 1 m at most. The EKF describes the same moment with a Gaussian centered at the full meter that claims to know the forward position to 2 cm. That belief is overconfident twice over. Its mean puts the robot 13 cm farther forward than the samples' mean, and its spread in $y$ is less than an eighth of the real one. Nothing inside the filter shows the problem: $\bar\Sigma_1$ is small, so the belief looks trustworthy, and the next measurement update gives the measurement less weight than it deserves. This is the start of the filter divergence described in the EKF section under [When linearization fails](https://aegean.ai/aiml-common/lectures/rse/ekf/index.md#when-linearization-fails).

The [localization section](https://aegean.ai/aiml-common/lectures/rse/ekf-localization/index.md) 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](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/rse/ekf-overconfident-prediction).


This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.