Chapter 11 · Estimate robot state, localize honestly, and measure uncertainty
Today in the field story
One problem, then the next
The IMU now reports a turn that wheel odometry does not support. Use a scalar Kalman example before configuring the simulated EKF: prediction variance, measurement variance, innovation, gain, and posterior each answer a different question. Fuse only non-duplicated variables in valid frames, then replay the outlier. A smooth estimate that ignores timestamp or covariance mistakes is still a failed night-shift result.
- Why now
The Bayes cycle can now be specialized into a practical estimator used by ROS systems.
- Ignore today
Ignore optimal filter design and real sensor tuning; inspect one bounded fusion configuration.
- Unlocks next
A timed odometry-IMU belief with innovations and covariance that can be challenged.
Understand
Build the physical picture first
A Kalman-style estimator moves a prediction toward a measurement by an uncertainty-derived amount, but only after frames, timestamps, variables, and noise claims make the two comparable.
A linear Kalman filter represents belief with a mean and covariance. Prediction applies a linear motion model and adds process noise Q, so uncertainty normally grows. The measurement update computes innovation—the difference between observed and predicted measurement—and innovation uncertainty. Kalman gain then decides how strongly that innovation changes each state variable. The gain is calculated from modeled uncertainty; it is not a hand-chosen percentage for making a plot look smooth.
In one dimension with direct measurement, K = P⁻/(P⁻ + R). Larger predicted variance P⁻ or smaller measurement variance R pulls the estimate farther toward the measurement. Updated covariance becomes smaller in the ideal consistent case because new information was added. These equations assume compatible units, frame, timestamp, approximately Gaussian error, and correct models. A biased sensor can make a mathematically neat filter confidently wrong.
Mobile-robot motion and sensor relationships are nonlinear because heading changes how forward travel affects x and y. An Extended Kalman Filter, or EKF, evaluates a local linear approximation using Jacobians around the current estimate, then performs a Kalman-style correction. Linearization does not repair a bad initial state, severe nonlinearity, wrong transform, or unmodeled bias. Innovation and consistency plots remain necessary.
robot_localization provides ROS state-estimation nodes for practical fusion. For a planar rover, a sensible first local estimator may fuse wheel-derived forward and yaw velocity with IMU yaw rate while publishing a smooth odom→base_link estimate. The per-sensor configuration is expressed in each input's frame. Do not fuse wheel pose, wheel velocity, and wheel-derived heading as if they were independent evidence, and convert unsupported IMU conventions before fusion.
Sensor fusion should add complementary information. Wheels measure motion against the floor but drift during slip; a gyro responds quickly to rotation but accumulates bias when integrated. Correct timestamps, static sensor transforms, axes, ENU convention where required, calibrated bias, credible covariances, timeouts, and outlier policy matter more than adding more topics. During a timeout, prediction-only output may continue, so uncertainty and diagnostics must expose the lost correction.
Words you need
Name each idea precisely
- Innovation
The difference between an actual measurement and the measurement predicted from current state.
Physical example:A landmark says 1.20 m while the prediction says 1.00 m, giving a 0.20 m innovation.
- Kalman gain
The uncertainty-derived matrix or scalar that converts innovation into state correction.
Physical example:Gain 0.692 applies about 69.2% of a direct one-dimensional measurement surprise.
- Process noise Q
Modeled uncertainty added during prediction for unrepresented changes in robot motion.
Physical example:A larger Q during fast turns represents uncertain tire slip and acceleration.
- Measurement noise R
The covariance assigned to observation noise in the measurement model.
Physical example:A vibrating gyro receives a larger angular-rate R than it had on a rigid bench.
- Extended Kalman Filter
A Kalman-style filter that locally linearizes nonlinear process and measurement models around the current estimate.
Physical example:A rover EKF linearizes how heading and forward velocity change x-y pose during the next small interval.
- Bias
A repeatable offset that shifts measurements and is not captured by zero-mean random noise alone.
Physical example:A stationary gyro reports 0.01 rad/s, causing integrated heading to drift steadily.
Math, one line at a time
Work through today’s relationship
Prerequisite rescue · optionalProbability, variance, and Kalman weighting
State estimation combines predictions and measurements according to uncertainty.
- μ
- best current estimateUnit: state unit
- σ²
- variance, or squared uncertainty spreadUnit: state unit squared
- K
- Kalman gain, the measurement weightUnit: unitless
Prediction is 10 m. Measurement is 12 m. Let K = 0.25.
Innovation is 12 − 10 = 2 m.
Updated estimate = 10 + 0.25×2 = 10.5 m; the lower-trust measurement only shifts the estimate partway.
It is a weighted merge like resolving two data sources, but the weights come from modeled uncertainty.
Prediction 5 m, measurement 7 m, K = 0.5. What is the update?
5 + 0.5×(7−5) = 6 m.
For this scalar update,
Calculate a scalar Kalman correction and uncertainty
A direct position prediction is x⁻ = 1.00 m with predicted variance P⁻ = 0.09 m². A synchronized measurement is z = 1.20 m with variance R = 0.04 m².
Check compatibility: both values are position in the same frame, at the same time, in metres; P⁻ and R are in m².
Compute innovation: y = z - x⁻ = 1.20 - 1.00 = 0.20 m.
Compute gain: K = P⁻ / (P⁻ + R) = 0.09 / 0.13 ≈ 0.6923.
Apply the correction: x⁺ = x⁻ + Ky = 1.00 + 0.6923 × 0.20 ≈ 1.1385 m.
Update variance for this scalar direct-measurement case: P⁺ = (1 - K)P⁻ ≈ 0.3077 × 0.09 ≈ 0.0277 m².
Interpret cautiously: the posterior moved nearer the lower-variance measurement, but only independent truth and repeated consistency tests can show whether P⁺ is credible.
The corrected position is about 1.138 m with modeled variance about 0.0277 m²; neither value proves physical accuracy without validation.
Kalman weighting follows uncertainty math after data contracts pass; it cannot rescue incompatible or biased inputs.
Physical examples
Where this appears in real life
Ruler reading versus step count
A hidden cart is predicted from counted wheel turns, then a ruler gives a slightly different visible position with a known reading spread.
The correction moves toward the ruler by an amount set by relative uncertainty, not by declaring either source perfect.
Suitcase wheels and phone gyro
An unpowered wheeled suitcase follows floor distance from wheel marks while a securely attached phone records turn rate during a gentle hand-pushed curve.
Wheel marks expose slip-sensitive distance, while gyro samples expose fast rotation plus bias; their timestamps and mounted axes must agree before combination.
Hands-on exercise
Make the idea observable
Use the existing simulated differential-drive rover, wheel odometry, IMU, Gazebo truth, and a short rosbag2 recording. Keep truth out of the EKF inputs.
Inspect wheel and IMU message timestamps,
frame_id, odometrychild_frame_id, units, static transforms, mounted axes, covariance fields, rates, and stationary gyro bias.Create a minimal
robot_localizationEKF configuration in 2D mode that fuses only justified, non-duplicated wheel velocity and IMU angular-rate variables and ownsodom→base_link.Replay a nominal straight-turn-straight bag, record raw inputs,
odometry/filtered,/tf,/diagnostics, and separate ground truth, then compare pose and heading over aligned time.Increase IMU R by a documented factor and rerun; compare innovation response, estimate smoothness, lag, and actual error rather than choosing the prettier curve.
Inject one IMU sign or 100 ms timestamp error, reproduce its signature, restore the correct transform or time, and rerun the unchanged nominal bag.
Set a bounded sensor timeout, remove IMU updates for a fixed interval, and verify prediction-only behavior, growing uncertainty or visible limitation, diagnostics, and recovery evidence.
Credible inputs produce a smooth local estimate; covariance changes alter weighting, while a sign or timing fault creates systematic innovations that smoothing cannot legitimately hide.
One command replays nominal and faulty cases, exact fused variables are documented, transform ownership is single, truth is evaluation-only, and every configuration comparison retains error plus uncertainty evidence.
Build today
Use the frozen Week 8 artifacts to fuse odometry and IMU, explain scan matching, loop closure, and AMCL, then rerun the unchanged scenarios and compare error, uncertainty coverage, transform age, dropout recovery, and navigation success.
Evidence to save
DONE when the integrated “Kalman/EKF intuition and odometry-IMU fusion” path is observable, cancelable, and leaves the prior baseline reproducible.
Common mistakes
Catch the wrong mental model
Setting a sensor covariance nearly to zero to force the filter to trust it.
Estimate covariance from representative residuals and validate consistency; tiny R turns noise, bias, frame, and timing faults into overconfident state errors.
Fusing wheel-derived pose, heading, and velocity as independent measurements.
Trace each field to its physical source and fuse a non-duplicated subset unless cross-correlation is modeled explicitly.
Treating IMU axes and timestamps as tuning problems.
Correct the mounted transform, convention, units, bias handling, and source time before changing Q, R, gain, or rejection thresholds.
Job connection
How this becomes employable evidence
Integrate and release-gate a wheel-and-IMU robot_localization EKF for an AMR, including input-contract checks, transform ownership, covariance tuning evidence, fault replay, and dropout diagnostics.
Relevant target roles
- Robotics Application / ROS 2 Integration Engineer
- Robotics Software Engineer — ROS 2 / AMR
- Robotics Deployment, Integration & Validation Engineer
Chapter 11 interview drill
Interview questions: Kalman/EKF intuition and odometry-IMU fusion
Practise a 60–90 second answer: define the idea, connect it to a physical robot, state assumptions, frames, and units when relevant, then finish with the failure signal or evidence you would inspect.
Primary interview scenario
An EKF output looks smooth but turns lag and the reported covariance is tiny. Explain innovation, Q, R, bias, duplicate wheel information, frames, timestamps, and the experiment order you would use.
Answer shape: clarify the situation → trace the physical and software path → test the most likely boundaries → name the evidence that would confirm the result.
Technical follow-up questions
Q1What happens to scalar Kalman gain when R grows while P⁻ stays fixed?
K decreases, so the correction moves less toward that measurement.
Q2Why can a smooth EKF output still be wrong?
Bias, duplicate evidence, incorrect models, frames, timestamps, or covariances can produce smooth but inaccurate and overconfident estimates.
Q3What should own `odom→base_link` in this exercise?
The one configured local robot_localization estimator; competing publishers must be disabled.