Extended Kalman filter

Deep dive

How it works

In: previous x, P, control u, measurement z, models f, h with Jacobians, noise Q, R · Out: updated x, P (and innovation for gating) · State: mean x and covariance P · Cost: O(N^3) per step for N states plus Jacobian evaluation

Model: x_k = f(x_{k-1}, u_k) + w, w ~ N(0, Q); z_k = h(x_k) + v, v ~ N(0, R). The linear filter's algebra (gain, Joseph form, NIS gate) is unchanged: see Kalman filter; only these steps differ.

Predict:

1. x- = f(x, u) (the full nonlinear function)
2. F = df/dx at the old x; P- = F P F^T + Q

Update:

1. y = z - h(x-), innovation (wrap angle components to (-pi, pi])
2. H = dh/dx at x-; S = H P- H^T + R; K = P- H^T S^-1
3. x = x- + K y; P = (I - K H) P- (I - K H)^T + K R K^T

Control noise enters through the input Jacobian: Q = G Su G^T, G = df/du, so uncertainty grows only in directions the controls can move the state.

Example: unicycle [x, y, theta] driven by (v, w) (differential drive), measuring range and bearing to known landmarks.

f = [x + v dt cos th,  y + v dt sin th,  th + w dt]     F = [[1,0,-v dt sin th],[0,1, v dt cos th],[0,0,1]]
h = [ sqrt(dx^2+dy^2),  atan2(dy,dx) - th ]   (dx,dy = landmark - position)

Code

pub type Mat<const R: usize, const C: usize> = [[f64; C]; R];

pub fn mul<const A: usize, const B: usize, const C: usize>(x: &Mat<A, B>, y: &Mat<B, C>) -> Mat<A, C> {
    let mut o = [[0.0; C]; A];
    for i in 0..A { for k in 0..B { for j in 0..C { o[i][j] += x[i][k] * y[k][j]; } } }
    o
}
pub fn t<const R: usize, const C: usize>(x: &Mat<R, C>) -> Mat<C, R> {
    let mut o = [[0.0; R]; C];
    for i in 0..R { for j in 0..C { o[j][i] = x[i][j]; } }
    o
}
pub fn add<const R: usize, const C: usize>(x: &Mat<R, C>, y: &Mat<R, C>) -> Mat<R, C> {
    let mut o = *x;
    for i in 0..R { for j in 0..C { o[i][j] += y[i][j]; } }
    o
}
/// Solve S X = B for symmetric positive-definite S (Cholesky, no explicit inverse).
fn chol_solve<const M: usize, const C: usize>(s: &Mat<M, M>, b: &Mat<M, C>) -> Mat<M, C> {
    let mut l = [[0.0; M]; M];
    for i in 0..M { for j in 0..=i {
        let mut v = s[i][j];
        for k in 0..j { v -= l[i][k] * l[j][k]; }
        l[i][j] = if i == j { v.sqrt() } else { v / l[j][j] };
    } }
    let mut x = *b;
    for c in 0..C {
        for i in 0..M { for k in 0..i { x[i][c] -= l[i][k] * x[k][c]; } x[i][c] /= l[i][i]; }
        for i in (0..M).rev() { for k in i + 1..M { x[i][c] -= l[k][i] * x[k][c]; } x[i][c] /= l[i][i]; }
    }
    x
}

/// EKF core: the caller supplies f(x), h(x) and their Jacobians, evaluated at the current estimate.
pub struct Ekf<const N: usize> { pub x: [f64; N], pub p: Mat<N, N> }

impl<const N: usize> Ekf<N> {
    /// x = f(x_old) (already computed by the caller), F = df/dx at x_old, Q in state space.
    pub fn predict(&mut self, fx: [f64; N], f: &Mat<N, N>, q: &Mat<N, N>) {
        self.x = fx;
        self.p = add(&mul(&mul(f, &self.p), &t(f)), q);
    }
    /// y = z - h(x) (wrap angles first), H = dh/dx at x. Returns NIS = y^T S^-1 y.
    pub fn update<const M: usize>(&mut self, y: [f64; M], h: &Mat<M, N>, r: &Mat<M, M>) -> f64 {
        let hp = mul(h, &self.p);
        let s = add(&mul(&hp, &t(h)), r);
        let sinv_hp = chol_solve(&s, &hp);                       // S^-1 H P
        let k = t(&sinv_hp);                                     // K = P H^T S^-1
        let mut yc = [[0.0; 1]; M];
        for i in 0..M { yc[i][0] = y[i]; }
        let sy = chol_solve(&s, &yc);
        for i in 0..N { for j in 0..M { self.x[i] += k[i][j] * y[j]; } }
        let kh = mul(&k, h);
        let mut a = [[0.0; N]; N];
        for i in 0..N { for j in 0..N { a[i][j] = (i == j) as u8 as f64 - kh[i][j]; } }
        self.p = add(&mul(&mul(&a, &self.p), &t(&a)), &mul(&mul(&k, r), &t(&k))); // Joseph
        (0..M).map(|i| y[i] * sy[i][0]).sum()
    }
}

// --- Model: unicycle [x, y, theta], control (v, w); range-bearing to a known landmark ---
pub fn wrap(a: f64) -> f64 { a - std::f64::consts::TAU * ((a + std::f64::consts::PI) / std::f64::consts::TAU).floor() }

pub fn motion(x: &[f64; 3], v: f64, w: f64, dt: f64) -> ([f64; 3], Mat<3, 3>, Mat<3, 2>) {
    let (s, c) = x[2].sin_cos();
    let fx = [x[0] + v * dt * c, x[1] + v * dt * s, wrap(x[2] + w * dt)];
    let f = [[1.0, 0.0, -v * dt * s], [0.0, 1.0, v * dt * c], [0.0, 0.0, 1.0]];
    let g = [[dt * c, 0.0], [dt * s, 0.0], [0.0, dt]];           // df/du: lets control noise enter via G Su G^T
    (fx, f, g)
}

pub fn range_bearing(x: &[f64; 3], l: [f64; 2]) -> ([f64; 2], Mat<2, 3>) {
    let (dx, dy) = (l[0] - x[0], l[1] - x[1]);
    let q = dx * dx + dy * dy;
    let r = q.sqrt();
    ([r, wrap(dy.atan2(dx) - x[2])],
     [[-dx / r, -dy / r, 0.0], [dy / q, -dx / q, -1.0]])
}

Usage per step: motion then predict with Q = G Su G^T, then range_bearing and update when a measurement arrives. Checked by test: both Jacobians match central finite differences to 1e-5. Over 2000 steps (dt 0.1, v=1, w=0.3, two landmarks, one measurement per 5 steps, wrong initial pose by 0.5 m / 0.2 rad, noisy controls) position RMS error was 0.049 m against 0.83 m for dead reckoning; mean NIS 2.05 (chi-square, 2 dof: mean 2), so the covariance was honest.

Complexity and constants

• Same as the linear filter: order of 4 N^3 flops per step dense, P is N^2 x 8 bytes (15 states: 1.8 KB). Jacobians add O(nnz) flops plus the sin/cos/atan2 calls, which dominate at small N.
• Measured on an Apple M4 Pro, one core, code above (f64, N=3, M=2, no BLAS): predict 15 ns; predict plus one range-bearing update 79 ns including sin/cos/atan2.
• No syscalls, no allocation, no I/O. In SLAM with N landmarks the state is O(N) but each update touches all of P: O(N^2) per measurement.

Efficiency

Bound: compute for one filter; memory bandwidth once P leaves cache (a few hundred states).

• Exploit sparsity of F and H: in SLAM only the robot pose moves, F = diag(F_robot, I), so predict touches only the robot rows and columns of P: O(N) not O(N^3). H is non-zero only in pose and observed-landmark columns.
• Closed-form Jacobians (hand-derived or code-generated): finite differences cost N extra evaluations of f; keep them only as a test.
• Sequential scalar updates for diagonal R: no M x M factorization (re-evaluate h, H between components).
• Gate with NIS before updating: outliers cost nothing and do no damage.
• Update only when data arrives (predict at IMU rate, update at sensor rate). A fixed-gain complementary filter is the cheaper alternative when one scalar gain suffices.

Implementation notes

• Evaluate F at the previous posterior, H at the prior; never at the truth, even in simulation (it hides inconsistency).
• Orientation: do not put four quaternion components into P (rank 3 on a 4-vector). Carry a nominal quaternion plus a 3-vector error state in P, inject delta after each update (q = q * exp(delta)) and reset; see quaternion rotations, rigid-body transforms and, for the full derivation, Sola's report. Same for any state on a manifold.
• Cholesky solve (LU/Cholesky) and the Joseph form, as in the linear filter.
• Iterated EKF: re-linearize h at the updated x and repeat the update from the same prior; fixes badly linearized measurements at a few times the update cost.
• Overconfident initial P plus a bad linearization point is the typical divergence; initialize P to the real initial error.

Pitfalls

• Wrong Jacobian (sign, missing term, radians vs degrees): the filter still runs and just performs badly. Always test against finite differences.
• Unwrapped bearing innovations: +3.1 vs -3.1 rad gives a 6.2 rad residual and a huge wrong correction.
• Inconsistency from linearization: the EKF can claim information in unobservable directions (yaw in SLAM and visual-inertial navigation), shrinking covariance that should stay large. Observability-constrained EKFs (Hesch et al.) choose linearization points that keep those directions unobservable.
• Multimodal posterior (symmetric corridor, kidnapped robot): one Gaussian collapses onto one mode.

Variants and alternatives

In production

Flight controllers (PX4 EKF2, ArduPilot EKF3) fusing IMU, GNSS, barometer and magnetometer; ROS robot_localization; EKF-SLAM; MSCKF-style visual-inertial odometry; attitude determination. Fields: aerial vehicles (PX4, ArduPilot), ground robots (ROS robot_localization), target tracking and state estimation research (Stone Soup, from the UK Dstl), and GNSS/IMU navigation in general.

Implementations

Sources

• R. Smith, M. Self, P. Cheeseman, Estimating Uncertain Spatial Relationships in Robotics, in Autonomous Robot Vehicles, Springer 1990. paper
• Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, MIT Press 2005 (EKF localization and EKF-SLAM chapters).
• Joan Sola, Quaternion kinematics for the error-state Kalman filter, 2017. paper
• B. M. Bell, F. W. Cathey, The Iterated Kalman Filter Update as a Gauss-Newton Method, IEEE Trans. Automatic Control 38(2), 1993. paper
• S. J. Julier, J. K. Uhlmann, Unscented Filtering and Nonlinear Estimation, Proc. IEEE 92(3), 2004. paper
• S. Huang, G. Dissanayake, Convergence and Consistency Analysis for Extended Kalman Filter Based SLAM, IEEE Trans. Robotics 23(5), 2007. paper
• A. I. Mourikis, S. I. Roumeliotis, A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation, ICRA 2007. paper
• J. A. Hesch, D. G. Kottas, S. L. Bowman, S. I. Roumeliotis, Consistency Analysis and Improvement of Vision-aided Inertial Navigation, IEEE Trans. Robotics 30(1), 2014. paper