explainer
Extended Kalman filter: localize a robot with range and bearing
Predict a robot's nonlinear motion, linearize with Jacobians, and correct its pose from a known landmark. Inspect angle wrapping, covariance, and a poor initial heading that challenges the local approximation.
What you will learn
- Define a nonlinear motion and landmark measurement model for a planar robot.
- Calculate the Jacobians used to propagate and correct covariance.
- Wrap a bearing innovation before applying the Kalman gain.
- Interpret position and heading uncertainty with their correct units.
- Explain why a smaller covariance does not establish a more accurate estimate.
Before you start
An Extended Kalman Filter, or EKF, estimates state through a nonlinear model while tracking a local approximation of uncertainty. It predicts with the nonlinear equations, uses their Jacobians to update covariance, then corrects the estimate with a measurement.
For a robot, heading affects where forward motion ends. A landmark's range and bearing also depend nonlinearly on position. This lesson works through one complete pose update and exposes what changes when the initial estimate is poor.
Extend the Kalman filter to nonlinear models
The linear Kalman filter uses matrices to describe motion and measurements. An EKF starts with nonlinear functions f and h. Around the current estimate, their derivatives give the local matrices F and H.
For a small state error Δs, the measurement approximation is:
The mean prediction uses f(s), and the predicted measurement uses h(s). The Jacobians describe how nearby errors change. Harry Asada's MIT notes on the EKF explain this distinction and the risk of linearizing around an inaccurate estimate.
This example estimates a planar robot pose using a supplied landmark position and known correspondence. It does not estimate the landmark or combine wheel and IMU readings. Those would require additional states or measurement models.
Store pose and local uncertainty together
Use the state s = [x, y, θ]ᵀ. World x points right, world y points up, and θ increases counterclockwise from +x. Position uses meters; every angle and angular standard deviation uses radians.
The estimate consists of a mean m and a 3 × 3 covariance P. Each covariance entry has the product of its two state units:
| Entries | Meaning | Units |
|---|---|---|
| Pₓₓ, Pᵧᵧ | Position variances | m² |
| Pₓᵧ = Pᵧₓ | Position cross-covariance | m² |
| Pθθ | Heading-error variance | rad² |
| Pₓθ, Pᵧθ and their transposes | Position-heading cross-covariances | m·rad |
The default prior is m₀ = [1.2, 0.8, 0.1]ᵀ with P₀ = diag(0.25, 0.25, 0.04) in these units. Its standard deviations are 0.5 m, 0.5 m, and 0.2 rad. A covariance describes uncertainty around the mean; it is not an extra displacement to add to the robot.
The angular covariance describes a local heading error. Storing the mean between −π and π changes its representative, not its physical orientation. A large, multimodal uncertainty on the circle would exceed this single local Gaussian description.
Declare the robot's discrete motion
The robot translates by d along its starting heading, then turns by δ in place. That order defines the discrete model:
The experiment uses d = 1 m and δ = 0.2 rad. This is a straight segment followed by a turn. A simultaneous constant-speed turn follows an arc and needs a different motion equation, such as the one in differential-drive kinematics.
For the default mean, prediction gives:
The separate reference starts at [1, 1, 0]ᵀ and follows the declared motion exactly, ending at [2, 1, 0.2]ᵀ. We choose zero process error for this single reference move. The filter still allows uncertain motion through Q; this is a worked record, not a statistical validation dataset.
Propagate uncertainty through the motion Jacobian
Differentiate f with respect to the starting state:
Evaluate F at the prior mean. The heading derivatives show why heading uncertainty creates position uncertainty after translation. The local derivative of wrapped heading is one away from its coordinate seam.
The covariance prediction is:
Here Q = diag(0.01 m², 0.01 m², 0.0025 rad²) models independent, additive errors in the final world x, world y, and local heading coordinates. It is a declared state-noise model. It does not derive wheel-distance or steering uncertainty from hardware.
The process error has zero mean and is independent of the prior state error. This assumption lets the prediction add Q without extra cross-covariance terms.
At the default prior, P⁻ᵧθ = 0.04 cos(0.1) = 0.039800 m·rad. The prediction has created correlation even though the initial covariance was diagonal. A measurement can then correct position and heading together.
Predict range and bearing to a known landmark
The known landmark lies at ℓ = [5, 4]ᵀ m. Define Δx = ℓₓ − x, Δy = ℓᵧ − y, and q = Δx² + Δy². The expected sensor reading is:
Range is a distance in meters. Bearing is the landmark's direction relative to the robot's heading, in radians. The sensor sits at the pose origin and shares its heading; a displaced or rotated sensor would need an extrinsic transform.
The measurement Jacobian, evaluated at the predicted mean, is:
Moving toward the landmark reduces range, which explains the first row's signs. Increasing robot heading while position stays fixed reduces relative bearing, which explains H₂₃ = −1. At the landmark itself, bearing is undefined and this Jacobian cannot support an update.
The default predicted reading is [4.180794 m, 0.535342 rad]ᵀ. The fixed observation is z = [√18 + 0.1, π/4 − 0.2 − 0.03]ᵀ = [4.342641 m, 0.555398 rad]ᵀ. We constructed it from the reference endpoint with errors of +0.1 m and −0.03 rad.
Wrap the bearing innovation
The innovation compares the actual reading with the expected reading. Subtract range normally, but wrap the bearing difference to [−π, π):
Suppose predicted bearing is +179° and measured bearing is −179°. Ordinary subtraction gives −358°. The local angular difference is +2°, or 0.034907 rad.
For the default record, ν = [0.161847 m, 0.020056 rad]ᵀ. The poor-heading preset predicts bearing −2.958588 rad. Its raw difference of 3.513986 rad wraps to −2.769199 rad.
Wrapping prevents a coordinate seam from creating the wrong correction direction. It cannot make a large innovation small or make a poor linearization accurate. The poor preset remains a demanding update after the angle convention is correct.
Correct the mean and covariance
Assume independent, zero-mean local measurement errors with covariance R = diag(σᵣ², σᵦ²). Defaults are σᵣ = 0.2 m and σᵦ = 0.1 rad. Changing these assumptions leaves the same observation record in place.
The measurement error is also independent of the predicted state error. This independence supports the innovation covariance and Joseph update below.
Compute the innovation covariance and gain:
Then apply the innovation to the predicted mean:
K is 3 × 2: every state correction can use both range and bearing innovations. Its entries convert each measurement's units into the relevant state units. A gain column should not be read as three interchangeable dimensionless weights.
The lab uses the Joseph covariance update:
Asada's covariance derivation gives this form before simplifying with the optimal linear gain. Each term is symmetric and positive semidefinite in exact arithmetic. The form helps preserve those properties numerically; it does not repair an inaccurate physical or statistical model.
The default correction is [2.110974 m, 0.784788 m, 0.284001 rad]ᵀ. Its covariance diagonal is [0.109800 m², 0.092137 m², 0.012597 rad²], giving standard deviations [0.331361 m, 0.303540 m, 0.112238 rad].
Compare prior, prediction, and correction
Start with Near truth, then switch the stage between Prior, Prediction, and Correction. The position ellipse shows how uncertainty changes with translation and measurement. The heading diagram keeps the circular meaning visible when angles cross ±π.
The position contour has Mahalanobis radius 2 in the marginal x-y covariance. An exact 2D Gaussian puts 1 − exp(−2) ≈ 86.47% of its mass inside that contour. The separate heading arc shows ±2 heading standard deviations, a marginal local interval.
These are different uncertainty summaries. Neither is measured coverage over repeated trials. The covariance and uncertainty ellipse lesson explains why a two-dimensional radius-2 ellipse is not a 95% region.
The default position error grows from 0.219226 m after prediction to 0.242139 m after correction, even as covariance shrinks. With Exact initial mean, prediction has zero error, while the noisy observation moves the corrected position 0.094885 m from truth. Individual measurements need not improve realized error.
Try Poor heading, then increase the assumed bearing noise. Inspect how much the correction trusts this large angular discrepancy. Stage changes preserve the experiment; noise changes alter the filter's assumptions while truth and observations stay fixed.
Check what the local approximation cannot establish
The EKF tracks one mean and covariance. It does not maintain separate pose hypotheses when a scene supports several locations. Particle-filter localization shows how a different representation can preserve that ambiguity.
One range-bearing observation gives two measurements for a three-component pose. Without prior information, it does not uniquely determine x, y, and θ. Positions around the landmark can share the same range and relative bearing if heading changes with them.
The poor-heading preset illustrates a bad local approximation. After correction, its position error remains 1.614545 m, yet its reported heading standard deviation is only 0.123827 rad. A small covariance states confidence under the approximation; it does not establish that the chosen mean is near truth.
This example assumes a known landmark and a correct association between the observation and that landmark. Real failures can come from incorrect associations, bias, delayed readings, poor calibration, and unsuitable Q or R. Repeated observations and independent reference data are needed to evaluate those assumptions.
Known-map EKF localization keeps landmark coordinates fixed. EKF-SLAM would include uncertain landmark states and their correlations with the robot. Pose graph optimization addresses another estimation structure: jointly adjusting multiple poses linked by constraints.
Reproduce the landmark update in Python
The following standard-library script reproduces the default and poor-heading updates. It spells out the small matrix operations, uses radians throughout, and keeps the reference-derived observation fixed. It is one prediction and one correction, with no iterative relinearization.
from math import atan2, cos, hypot, pi, sin, sqrt
def wrap(angle):
return (angle + pi) % (2 * pi) - pi
def transpose(a):
return list(map(list, zip(*a)))
def multiply(a, b):
return [[sum(x * y for x, y in zip(row, col))
for col in zip(*b)] for row in a]
def add(a, b):
return [[x + y for x, y in zip(ar, br)] for ar, br in zip(a, b)]
def update(heading):
prior = [1.2, 0.8, heading]
p = [[0.25, 0, 0], [0, 0.25, 0], [0, 0, 0.04]]
f = [[1, 0, -sin(heading)], [0, 1, cos(heading)], [0, 0, 1]]
predicted = [prior[0] + cos(heading), prior[1] + sin(heading),
wrap(heading + 0.2)]
q = [[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.0025]]
p = add(multiply(multiply(f, p), transpose(f)), q)
dx, dy = 5 - predicted[0], 4 - predicted[1]
distance, square = hypot(dx, dy), dx * dx + dy * dy
expected = [distance, wrap(atan2(dy, dx) - predicted[2])]
h = [[-dx / distance, -dy / distance, 0],
[dy / square, -dx / square, -1]]
r = [[0.04, 0], [0, 0.01]]
pht = multiply(p, transpose(h))
s = add(multiply(h, pht), r)
det = s[0][0] * s[1][1] - s[0][1] * s[1][0]
inv_s = [[s[1][1] / det, -s[0][1] / det],
[-s[1][0] / det, s[0][0] / det]]
k = multiply(pht, inv_s)
observation = [sqrt(18) + 0.1, pi / 4 - 0.23]
raw_bearing = observation[1] - expected[1]
innovation = [observation[0] - expected[0], wrap(raw_bearing)]
corrected = [v + sum(g * e for g, e in zip(row, innovation))
for v, row in zip(predicted, k)]
corrected[2] = wrap(corrected[2])
kh = multiply(k, h)
a = [[int(i == j) - kh[i][j] for j in range(3)] for i in range(3)]
posterior = add(multiply(multiply(a, p), transpose(a)),
multiply(multiply(k, r), transpose(k)))
return predicted, innovation, corrected, posterior, raw_bearing
def values(xs):
return ', '.join(f'{x:.6f}' for x in xs)
predicted, innovation, corrected, p, raw = update(0.1)
print('Prediction:', values(predicted))
print('Innovation:', values(innovation))
print('Correction:', values(corrected))
print('Posterior variances:', values([p[i][i] for i in range(3)]))
print('Position errors:', values([hypot(s[0] - 2, s[1] - 1)
for s in (predicted, corrected)]))
_, innovation, corrected, _, raw = update(-2.9)
print('Poor bearing, raw and wrapped:', values([raw, innovation[1]]))
print('Poor correction:', values(corrected))
Expected output:
Prediction: 2.195004, 0.899833, 0.300000
Innovation: 0.161847, 0.020056
Correction: 2.110974, 0.784788, 0.284001
Posterior variances: 0.109800, 0.092137, 0.012597
Position errors: 0.219226, 0.242139
Poor bearing, raw and wrapped: 3.513986, -2.769199
Poor correction: 0.725893, 1.991668, -0.670417
The innovation uses the nonlinear h(m⁻), while its covariance uses the local H. Substituting Hm⁻ for h(m⁻) would omit the linearization's offset and change the model.
Check two updates yourself
Exercise 1. Set the prior heading to zero and keep P₀ = diag(0.25, 0.25, 0.04). Translate 1 m, then turn 0.2 rad, with the lesson's fixed Q. What are predicted y variance and y-heading covariance?
Show the covariance calculation
At θ = 0, the y row of F is [0, 1, 1]. Thus P⁻ᵧᵧ = 0.25 + 0.04 + 0.01 = 0.30 m². The heading uncertainty contributes through the translation derivative.
The heading row is [0, 0, 1], so P⁻ᵧθ = 0.04 m·rad. Q has no cross term. The turn changes the mean heading; its fixed value does not rotate the already completed translation.
Exercise 2. A predicted bearing is +179°, and a reading is −179°. What innovation should the update use in radians? If the corrected covariance shrinks, does that prove this single correction reduced actual pose error?
Show the angle and accuracy checks
Subtracting gives −358°, which wraps to +2° = π/90 ≈ 0.034907 rad. The gain must use that local difference.
A smaller covariance does not prove a smaller realized error. In the default experiment, position error increases from 0.219226 m to 0.242139 m while covariance decreases. Check an estimate against independent reference data and assess uncertainty over repeated records.
To configure an existing ROS estimator, continue with robot_localization in ROS 2. Its separate supplied odometry/IMU experiment checks field selection, frames, and the effect of accepting a biased gyro.
Sources and further study
- Harry Asada, MIT 2.160 lecture 8, Extended Kalman Filter and implementation issues: local linearization around the estimate, use of the nonlinear measurement function, initialization risks, and Joseph covariance form. Its continuous-time presentation supports these concepts; this lesson declares a separate discrete motion model.
- Harry Asada, MIT 2.160 lecture 6, Kalman gain and covariance update: derivation of the gain and covariance update under uncorrelated prior and measurement errors.
- Dellaert and Hutchinson, Introduction to Robotics and Perception, section 4.4: localization as inference over robot position, motion prediction, and measurement updates.
Before adapting this example to a robot, write down its motion timing, sensor frame, measurement units, and landmark association rule. Those choices determine the functions and Jacobians that the filter needs.