M12-T2 — The Error-State Kalman Filter

Learning objectives

After this lesson, you should be able to:

• Describe the nominal, error and true states, and the cycle predict – correct – inject – reset.
• Derive the error-state dynamics and measurement Jacobian for attitude and gyro bias.
• Explain why the error state is small, and why that justifies the linearisation.
• Read the observability of the problem from the filter: what converges, what does not, and when the covariance is dishonest.

The intuition

An orientation filter has two difficulties. The state is a rotation, which lives on a curved manifold, and the gyro that drives it has a bias that slowly corrupts it. The ESKF separates the two.

1. The nominal state (a rotation matrix or quaternion, and a bias estimate) is integrated by the gyro on the manifold: $R\leftarrow R\,\mathrm{Exp}((\omega_m-\hat b)\Delta t)$. This is the big, nonlinear motion, done exactly and cheaply.
2. The error state $\delta x=(\delta\boldsymbol\theta,\delta\mathbf{b})$ (six numbers) is the small difference between the truth and the nominal state, in the tangent space. It is zero-mean, small, and obeys nearly linear dynamics, so a Kalman filter fits.
3. When a reading arrives, the filter estimates the error state, injects it into the nominal state ($R\leftarrow R\,\mathrm{Exp}(\delta\hat\theta)$, $\hat b\leftarrow\hat b+\delta\hat b$) and resets the error state to zero, adjusting the covariance for the change of reference.

The gyro has a bias of (0.02, −0.01, 0.015) rad/s, which would drift the attitude by 0.4 rad in 20 s. A gravity reading every 0.1 s corrects the tilt: after 20 s the tilt error is 0.006 rad (0.3°), against 0.42 rad for the gyro alone (initial attitude uncertainty 0.3 rad) and 0.29 rad (0.03 rad). The estimated bias converges to (0.0195, −0.0101, 0.0143), all three components correct to within 0.0007 rad/s, although gravity says nothing directly about the vertical component of the bias: the body rotates, so the bias appears in the horizontal axes at different times.

What the filter cannot do is correct the yaw, the rotation about the vertical axis: gravity points the same way whatever the heading. The yaw error stays at its initial level (0.289 rad for an initial uncertainty of 0.3; the gyro alone has 0.344 after drift). Worse, in the left case the filter claims a yaw uncertainty of only 0.157 rad: it is overconfident about the one thing it cannot measure. In the second case, with an initial uncertainty of only 0.03 rad, the claim is 0.088 (the real error 0.038) and the filter is consistent: a mean NEES of 1.8 against 3 expected (and 6.8 with the large initial error).

The ESKF is only as honest as its linearisation. It keeps the error small, which is exactly when linearisation is accurate; with large initial errors it can invent information about directions that cannot be observed.

Definitions

Nominal state $\hat x=(\hat R,\hat b)$

The filter’s best estimate, propagated by the nonlinear model on the manifold.

Error state $\delta x=(\delta\boldsymbol\theta,\delta\mathbf{b})$

The small difference between true and nominal: $R=\hat R\,\mathrm{Exp}(\delta\boldsymbol\theta)$, $b=\hat b+\delta\mathbf{b}$. It has the covariance $P$.

Injection

Adding the estimated error into the nominal state: $\hat R\leftarrow\hat R\,\mathrm{Exp}(\delta\hat{\boldsymbol\theta})$, $\hat b\leftarrow\hat b+\delta\hat{\mathbf{b}}$.

Reset

Setting the error state to zero after injection, and transforming the covariance: $P\leftarrow G\,P\,G^\top$ with $G=\mathrm{blockdiag}(I-\tfrac12[\delta\hat{\boldsymbol\theta}]_\times,\,I)$.

Gyro model

$\omega_m=\omega+b+\mathbf{n}_g$: the true rate, a slowly varying bias and white noise.

Derivation: the error dynamics, the measurement Jacobian, and why yaw is lost

Error dynamics

The true rotation evolves as $R_{k+1}=R_k\mathrm{Exp}(\omega\Delta t)$ with $\omega=\omega_m-b-\mathbf{n}_g$, the nominal as $\hat R_{k+1}=\hat R_k\mathrm{Exp}(u_k)$. The error is defined by $R=\hat R\,\mathrm{Exp}(\delta\boldsymbol\theta)$.

1. $$\mathrm{Exp}(\delta\boldsymbol\theta_{k+1})=\hat R_{k+1}^\top R_{k+1}=\mathrm{Exp}(-u_k)\,\mathrm{Exp}(\delta\boldsymbol\theta_k)\,\mathrm{Exp}(\omega\Delta t)$$

   Multiply out with the two propagation equations; $\hat R_k^\top R_k=\mathrm{Exp}(\delta\boldsymbol\theta_k)$.

2. $$=\mathrm{Exp}\bigl(\mathrm{Exp}(-u_k)\delta\boldsymbol\theta_k\bigr);\mathrm{Exp}(-u_k)\mathrm{Exp}(\omega\Delta t)$$

   Move $\mathrm{Exp}(-u_k)$ through the error rotation by conjugation (the identity of the last lesson), which rotates its axis.

3. $$\mathrm{Exp}(-u_k)\mathrm{Exp}(\omega\Delta t)\approx\mathrm{Exp}\bigl(\omega\Delta t-u_k\bigr)=\mathrm{Exp}\bigl(-\delta\mathbf{b}\Delta t-\mathbf{n}_g\Delta t\bigr)$$

   The two small rotations combine to first order; their difference is the bias error (with $\delta\mathbf{b}=b-\hat b$) and the gyro noise.

4. $$\delta\boldsymbol\theta_{k+1}\approx\mathrm{Exp}(-u_k)\,\delta\boldsymbol\theta_k-\Delta t\,\delta\mathbf{b}-\Delta t\,\mathbf{n}_g$$

   The error-state transition; it needs no rotation matrices beyond the small $\mathrm{Exp}(-u_k)$.

The gravity measurement

1. $$\mathbf{z}=R^\top\mathbf{g}=\mathrm{Exp}(-\delta\boldsymbol\theta)\hat R^\top\mathbf{g}\approx(I-[\delta\boldsymbol\theta]_\times)\hat R^\top\mathbf{g}$$

   Insert $R=\hat R\mathrm{Exp}(\delta\boldsymbol\theta)$ and linearise.

2. $$\mathbf{z}-\hat R^\top\mathbf{g}=-[\delta\boldsymbol\theta]_\times\hat R^\top\mathbf{g}=[\hat R^\top\mathbf{g}]_\times\,\delta\boldsymbol\theta$$

   Since $-\mathbf{a}\times\mathbf{b}=\mathbf{b}\times\mathbf{a}$. This is $H\,\delta x$ with $H_\theta=[\hat R^\top\mathbf{g}]_\times$.

3. $$H_\theta\,(\hat R^\top\mathbf{g})=\bigl[\hat R^\top\mathbf{g}\bigr]_\times\hat R^\top\mathbf{g}=0$$

   The direction of gravity in the body frame is in the null space of H. A rotation error about the vertical axis changes no reading.

What follows from this

• Tilt (rotation about the horizontal axes) is observed at every update: its covariance falls to the level set by the accelerometer noise and the gyro noise between readings (0.006 rad here).
• Yaw (about the vertical) is unobservable: the null direction $\hat R^\top\mathbf{g}$ is carried along by the dynamics (it is mapped by $\mathrm{Exp}(-u)$ to the next null direction). The yaw covariance can only grow, with the gyro noise and with the bias error.
• Bias is observable if the body rotates: $\delta\mathbf{b}$ enters the horizontal tilt rate as $R\,\delta\mathbf{b}$, whose horizontal part varies with the attitude. A body that does not turn (the “no rotation” case) leaves the bias component along gravity unobservable. In the experiment the three bias components converge since the test rotates about all axes.
• Honesty. With large initial errors, the linearisation points (the nominal states at successive updates) differ by more than the first-order model assumes; the filter then appears to learn about yaw (a spurious gain of information) and its yaw covariance falls below the real error. The cure is a filter whose Jacobians respect the unobservable direction (observability-constrained EKF), or simply a better initial attitude or a heading source.

Run the Kalman filter on the small error, not on the big state. The nominal state keeps the geometry exact; the error state keeps the statistics linear. And read the observability from H: its null space is what the filter will never learn.

A worked example

An attitude filter in pure Python: a rotation matrix and a gyro bias as the nominal state, a 6 × 6 error covariance, a gyro at 50 Hz (noise 0.005 rad/s per sample, bias random walk 0.0005 rad/s), a gravity reading at 10 Hz (noise 0.02). The body turns with angular velocity (0.3 sin 0.5t, 0.2 cos 0.4t, 0.1) rad/s for 20 s. The gyro bias is (0.02, −0.01, 0.015) rad/s. The nominal state starts with an error drawn from N(0, σ₀² I), with σ₀ = 0.3 and 0.03; the filter is told σ₀. 40 runs each.

1. Tilt. With σ₀ = 0.3 the rms tilt error (about the horizontal axes) is 0.011 rad at 1 s, 0.0055 at 8 s and 0.0058 at 20 s. The gyro alone has 0.304, 0.339 and 0.421. With σ₀ = 0.03: 0.011, 0.0055, 0.0058 against 0.035, 0.149 and 0.291.
2. Bias. The mean estimated bias at 20 s is (0.0195, −0.0101, 0.0143) for both initial uncertainties, against the truth (0.02, −0.01, 0.015).
3. Yaw. With σ₀ = 0.3, the rms yaw error is 0.309, 0.291 and 0.289 rad at 1, 8 and 20 s: nothing is learnt, and the filter claims 0.170, 0.151 and 0.157. The NEES of the full attitude error at 20 s is 6.77 (3 expected): overconfident. With σ₀ = 0.03: yaw error 0.034, 0.039, 0.038 and claimed 0.058, 0.065, 0.088; the NEES is 1.78.

import math
from robotprob import Rng, mean, matmul, transpose, matvec, madd, msub, mscale, identity, inverse, solve

def hat(w): return [[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]]
def Exp(w):
    th = math.sqrt(sum(a * a for a in w))
    if th < 1e-12: return identity(3)
    K = hat(w); K2 = matmul(K, K)
    a, b = math.sin(th) / th, (1 - math.cos(th)) / th ** 2
    return [[(1.0 if i == j else 0.0) + a * K[i][j] + b * K2[i][j] for j in range(3)] for i in range(3)]
def Log(R):
    c = max(-1.0, min(1.0, (R[0][0] + R[1][1] + R[2][2] - 1) / 2)); th = math.acos(c)
    if th < 1e-9: return [0.0, 0.0, 0.0]
    k = th / (2 * math.sin(th))
    return [k * (R[2][1] - R[1][2]), k * (R[0][2] - R[2][0]), k * (R[1][0] - R[0][1])]
def block(P, r0, c0, M):
    for i in range(len(M)):
        for j in range(len(M[0])): P[r0 + i][c0 + j] = M[i][j]

DT, STEPS, EVERY = 0.02, 1000, 5                           # 50 Hz gyro, a gravity reading at 10 Hz, 20 s
SG, SB, SA = 0.005, 0.0005, 0.02                           # gyro noise, bias random walk (per step), accelerometer noise
B_TRUE = [0.02, -0.01, 0.015]
G = [0.0, 0.0, 1.0]

def run(seed, init_sigma=0.3, use_updates=True):
    rng = Rng(seed)
    Rt = identity(3)                                       # the true attitude
    e0 = [rng.gauss(0.0, init_sigma) for _ in range(3)]    # the initial error is drawn from the prior the filter is given
    Rn = Exp([-a for a in e0])                             # the nominal attitude of the filter: wrong from the start
    bn = [0.0, 0.0, 0.0]
    P = [[0.0] * 6 for _ in range(6)]
    for i in range(3): P[i][i] = init_sigma ** 2; P[3 + i][3 + i] = 0.05 ** 2
    Rg = Exp([-a for a in e0])
    out = []
    for k in range(1, STEPS + 1):
        t = k * DT
        w = [0.3 * math.sin(0.5 * t), 0.2 * math.cos(0.4 * t), 0.1]
        Rt = matmul(Rt, Exp([a * DT for a in w]))
        wm = [w[i] + B_TRUE[i] + rng.gauss(0.0, SG) for i in range(3)]          # the gyro: a bias and noise
        # nominal state, propagated on the manifold
        u = [(wm[i] - bn[i]) * DT for i in range(3)]
        Rn = matmul(Rn, Exp(u)); Rg = matmul(Rg, Exp([wm[i] * DT for i in range(3)]))
        # error-state covariance: F = [[Exp(-u), -I dt], [0, I]]
        F = identity(6); block(F, 0, 0, transpose(Exp(u))); block(F, 0, 3, mscale(identity(3), -DT))
        Q = [[0.0] * 6 for _ in range(6)]
        for i in range(3): Q[i][i] = (SG * DT) ** 2; Q[3 + i][3 + i] = SB ** 2
        P = madd(matmul(matmul(F, P), transpose(F)), Q)
        if use_updates and k % EVERY == 0:
            z = [sum(Rt[j][i] * G[j] for j in range(3)) + rng.gauss(0.0, SA) for i in range(3)]   # gravity in the body frame
            zp = [sum(Rn[j][i] * G[j] for j in range(3)) for i in range(3)]
            Hm = [[0.0] * 6 for _ in range(3)]
            block(Hm, 0, 0, hat(zp))                                              # d z / d delta_theta = [R^T g]_x
            S_ = madd(matmul(matmul(Hm, P), transpose(Hm)), [[SA ** 2 if i == j else 0.0 for j in range(3)] for i in range(3)])
            K = transpose(solve(S_, transpose(matmul(P, transpose(Hm)))))
            y = [z[i] - zp[i] for i in range(3)]
            dx = matvec(K, y)
            I_KH = msub(identity(6), matmul(K, Hm))
            P = madd(matmul(matmul(I_KH, P), transpose(I_KH)), matmul(matmul(K, [[SA ** 2 if i == j else 0.0 for j in range(3)] for i in range(3)]), transpose(K)))
            Rn = matmul(Rn, Exp(dx[:3])); bn = [bn[i] + dx[3 + i] for i in range(3)]   # inject the error into the nominal state
            Gr = identity(6); block(Gr, 0, 0, msub(identity(3), mscale(hat(dx[:3]), 0.5)))       # reset: the error is zero again
            P = matmul(matmul(Gr, P), transpose(Gr))
        e = Log(matmul(transpose(Rn), Rt))                                         # the true attitude error, as a tangent vector
        eg = Log(matmul(transpose(Rg), Rt))
        ew, egw = matvec(Rn, e), matvec(Rg, eg)                                     # the same errors seen from the world frame: z is the vertical
        Pw = matmul(matmul(Rn, [[P[i][j] for j in range(3)] for i in range(3)]), transpose(Rn))   # and the claimed covariance in that frame
        out.append((t, ew, egw, [Pw[i][i] for i in range(3)], list(bn)))
        if k == STEPS:
            Pa = [[P[i][j] for j in range(3)] for i in range(3)]
            nees = sum(e[i] * v for i, v in enumerate(solve(Pa, e)))
    return out, nees

print('errors are measured in the world frame: tilt = about the horizontal axes, yaw = about the vertical axis')
print('the gyro has a bias of (0.02, -0.01, 0.015) rad/s and a gravity reading arrives at 10 Hz; 40 runs of 20 s')
print('initial std   time   ESKF rms tilt   ESKF rms yaw   claimed yaw std   gyro only: rms tilt  rms yaw')
for init in (0.3, 0.03):
    res = [run(seed, init) for seed in range(1, 41)]
    for t_idx in (49, 399, 999):
        r_ = [r[0][t_idx] for r in res]
        tilt = math.sqrt(mean([(r[1][0] ** 2 + r[1][1] ** 2) / 2 for r in r_]))
        yaw = math.sqrt(mean([r[1][2] ** 2 for r in r_]))
        gt = math.sqrt(mean([(r[2][0] ** 2 + r[2][1] ** 2) / 2 for r in r_])); gy = math.sqrt(mean([r[2][2] ** 2 for r in r_]))
        claim = math.sqrt(mean([r[3][2] for r in r_]))
        print(str(init).rjust(11), str(round(r_[0][0], 1)).rjust(6), str(round(tilt, 4)).rjust(13), str(round(yaw, 3)).rjust(14), str(round(claim, 3)).rjust(15), str(round(gt, 3)).rjust(20), str(round(gy, 3)).rjust(8))
    bn_ = [r[0][-1][4] for r in res]
    print('            mean NEES of the 3-D attitude error at 20 s (3 expected):', round(mean([r[1] for r in res]), 2), ' mean gyro bias estimate:', [round(mean([b[i] for b in bn_]), 4) for i in range(3)])

errors are measured in the world frame: tilt = about the horizontal axes, yaw = about the vertical axis
the gyro has a bias of (0.02, -0.01, 0.015) rad/s and a gravity reading arrives at 10 Hz; 40 runs of 20 s
initial std   time   ESKF rms tilt   ESKF rms yaw   claimed yaw std   gyro only: rms tilt  rms yaw
        0.3    1.0        0.0114          0.309            0.17                0.304    0.283
        0.3    8.0        0.0055          0.291           0.151                0.339    0.271
        0.3   20.0        0.0058          0.289           0.157                0.421    0.344
            mean NEES of the 3-D attitude error at 20 s (3 expected): 6.77  mean gyro bias estimate: [0.0195, -0.0101, 0.0143]
       0.03    1.0         0.011          0.034           0.058                0.035    0.033
       0.03    8.0        0.0055          0.039           0.065                0.149    0.027
       0.03   20.0        0.0058          0.038           0.088                0.291     0.18
            mean NEES of the 3-D attitude error at 20 s (3 expected): 1.78  mean gyro bias estimate: [0.0195, -0.0101, 0.0143]

In robotics

Everywhere an orientation is estimated

ESKFs run in flight controllers (PX4’s attitude and position estimators are EKFs on the same principle), in phone and VR headset tracking, in visual–inertial odometry and in legged-robot state estimation. The nominal state typically includes position, velocity, orientation and sensor biases (15 error states); the sensors are an IMU for the prediction, and GPS, a magnetometer, barometer, camera or leg kinematics for the corrections. The UAV course derives the estimators step by step in its state-estimation lessons.

Why ESKF rather than a plain EKF on the state

• Minimal error representation: three numbers for attitude (a quaternion has four and a unit constraint that an additive update violates).
• Small error: the error is near zero, so the linearisation is at its best and singularities are far away.
• Separation of rates: the nominal state can be integrated at the IMU rate (hundreds of Hz) and the covariance propagated and corrected at a lower rate.
• Clean sensor models: corrections are written in terms of the error, as in $H=[\hat R^\top\mathbf{g}]_\times$.

Fixing the unobservable

Add a measurement of yaw: a magnetometer (vulnerable to disturbance), a GPS-derived course while moving, a visual landmark or a map match. Until then, keep the yaw covariance honest: let it grow, do not let it shrink, and inflate it where a linearisation could invent information. Many implementations decouple the yaw from the tilt-and-bias update for this reason.

Checks to run

• NEES or the NIS of the gravity reading (module 7): rising values signal an overconfident filter.
• Accelerometer readings contain linear acceleration, not just gravity: gate the update by the norm of the reading (it must be near 1 g), or the tilt will be wrong during manoeuvres.
• Check the Jacobians by finite differences on the manifold: perturb with $\mathrm{Exp}(\epsilon)$, not by adding.
• Test the filter with initial errors drawn from the prior (as here), not a single hand-picked one.

Common misconceptions

Key takeaways

1. The ESKF keeps a nominal state on the manifold, integrated by the gyro, and runs a Kalman filter on the small tangent-space error.
2. Cycle: predict (nominal and covariance), correct (error state), inject (R ← R Exp(δθ), b ← b + δb), reset (P ← G P Gᵀ).
3. The gravity Jacobian [R̂ᵀg]× observes tilt and has the gravity direction as its null space: yaw is unobservable. Tilt error fell to 0.006 rad against 0.29–0.42 for the gyro alone.
4. The bias is learnt on all three axes if the body rotates (0.0195, −0.0101, 0.0143 against 0.02, −0.01, 0.015).
5. With a large initial error the filter invented information about yaw (claimed 0.157 for a real error of 0.289, NEES 6.8); with a small one it was consistent (NEES 1.8).

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• J. Solà, “Quaternion kinematics for the error-state Kalman filter”, arXiv:1711.02508, 2017 , the standard derivation of the ESKF for an IMU-driven state. Free to read; the arXiv licence was not checked.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , the chapters on rotations and on nonlinear filters on matrix Lie groups. Free to read as the author’s PDF; the book is copyrighted.
• F. L. Markley and J. L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, Springer, 2014, the chapters on the multiplicative extended Kalman filter. Paid book.
• J. A. Hesch, D. G. Kottas, S. L. Bowman and S. I. Roumeliotis, “Consistency analysis and improvement of vision-aided inertial navigation”, IEEE Transactions on Robotics 30 (1), 2014. Paid journal article; the observability-constrained EKF.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The filter above uses only past data. The next lesson lets the estimator use the future as well: smoothing, with the fixed-lag and batch forms, and the price in latency.

Related content

Nothing below is required to finish this lesson.

• Before
  M12-T1: Uncertainty on Manifolds: Angles and Rotations

  Explain why angles cannot be averaged or subtracted like numbers, and represent the uncertainty of a rotation in its tangent space.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T6: Error-State Kalman Filter
• UAV M6-T5: PX4 state estimation and EKF2
• Physical AI: Belief under uncertainty