ProjekinAja. Robotics Stage 4 · State Estimation Lesson 4.1
● Stage 4 · Lesson 4.1

Probability for Robotics: Belief, Bayes, and Why Point Estimates Fail

A robot's state is a probability distribution, not a number — and every algorithm that pretends otherwise will fail exactly when precision matters most.

⏱ ~22 min read 🧠 Foundational theory 📐 Math with intuition

🎯 By the end you'll understand

Why a robot's position is a probability distribution over pose, not a single number. How Bayes theorem acts as the canonical belief-update rule. The difference between MAP and MMSE estimators — and when they disagree. Why a point estimate with no covariance is dangerous: the planner cannot be conservative when it doesn't know how uncertain the estimate is. And why sensor noise models (built in Lesson 1.6) are the exact input to the Bayesian update equation.

KEY INSIGHTA robot's state is a distribution, not a number

If your state estimator returns a single number, it is lying — it just isn't telling you what kind of lie it is.

Every sensor FollowBot carries returns a noisy measurement of the world. The LIDAR sees the wall at 2.03 m — but the true distance might be 2.01 m or 2.07 m, depending on surface reflectance, temperature, and the laser's jitter. The wheel encoders say the robot moved 0.45 m — but wheel slip and encoder quantization mean the true displacement is somewhere in a range. Stack these uncertainties across every sensor over time, and the honest representation of "where the robot is" is not a pose (x, y, θ) — it is a probability distribution over all possible poses.

🔑 The belief

In probabilistic robotics, the robot's internal representation of its own state is called the belief: a probability distribution bel(xt) over all possible states xt at time t. Every state-estimation algorithm in this stage — the Bayes filter, Kalman filter, EKF, particle filter — is fundamentally an algorithm for computing and maintaining that belief. The filter does not find the answer; it tracks the full distribution of plausible answers, weighted by evidence.

CORE CONCEPTRandom variables, PDFs, and conditional probability

Before Bayes, you need three concepts nailed down with physical intuition.

1

Random variable and PDF

A random variable X takes on values drawn from some sample space. For a 1D robot position on a line, X ∈ ℝ. The probability density function p(x) tells you the relative plausibility of each value: p(x) ≥ 0 for all x, and ∫p(x)dx = 1. The integral of p over any interval gives the probability that X falls in that interval. For a Gaussian (the workhorse distribution in robotics):

p(x) = (1 / (σ√(2π))) · exp(−(x − μ)² / (2σ²))

where μ is the mean (the most probable value) and σ² is the variance (how spread out the belief is). Physically: μ is the best estimate, σ is how much you trust it. A tight σ means confident; a wide σ means uncertain.

2

Conditional probability and likelihood

Conditional probability p(A | B) is the probability of A given that B is known to be true. In robotics, p(z | x) is the sensor model (also called the likelihood): given that the robot is truly at position x, what is the probability density of observing measurement z? This is exactly the noise model you built for each sensor in Lesson 1.6 — the LIDAR's range measurement noise is modeled as a Gaussian centered on the true range with standard deviation σlidar ≈ 0.02 m. The likelihood function answers: "if I'm at x, how likely is this measurement?"

3

Prior and posterior

The prior p(x) is what you believed about the state before receiving a new measurement. The posterior p(x | z) is what you believe after incorporating measurement z. The posterior is always sharper than the prior (you know more), but only if your sensor model is correct. If the model is wrong, the posterior can be confidently and systematically wrong — the filter concentrates belief around an incorrect mode.

robot position x (m) 0 1 2 3 4 Prior bel(x) — wide uncertainty before measurement Likelihood p(z|x) — sensor measurement at ~2.4 m Posterior bel(x|z) — sharper belief after fusing both
The Bayesian update in one picture. The prior (gold, wide) is what we believed before the measurement. The likelihood (blue, stage 4 color) encodes the sensor model: how plausible is this measurement at each x? The posterior (green) is the product, normalized — it is narrower (more confident) and pulled toward the measurement.

CORE CONCEPTBayes theorem as the robot's belief-update rule

Bayes theorem in its simplest form:

p(x | z) = p(z | x) · p(x) / p(z)

Read it left to right as the robot's reasoning process:

TermNamePhysical meaning
p(x | z)PosteriorUpdated belief after seeing measurement z. This is what we want.
p(z | x)LikelihoodSensor model: how probable is measurement z if the true state is x?
p(x)PriorBelief before the measurement. Our best guess before new data arrives.
p(z)Normalizer∫p(z|x)p(x)dx — ensures the posterior integrates to 1. Often written η.

The key insight for robotics: Bayes is an update rule, not a single computation. You carry a belief bel(xt−1) from the last time step, incorporate the prediction from your motion model (the predict step, Stage 4.2), then fold in the sensor likelihood (the update step). Repeat at sensor frequency. This recursive application of Bayes is the Bayes filter — the parent algorithm from which the Kalman filter, EKF, and particle filter are all derived as special cases.

📖 The normalizer p(z) is the hardest part

p(z) = ∫p(z|x)p(x)dx is the marginal likelihood of the observation, integrating out all possible states. For continuous distributions in high-dimensional spaces, this integral is analytically intractable in general — which is exactly why the Kalman filter (Gaussian assumption → closed-form integral) and particle filter (Monte Carlo approximation) exist. They are different ways of making the normalizer computationally tractable.

For a Gaussian prior with mean μ0 and variance σ0², and a Gaussian likelihood with measurement z and sensor noise σz², the posterior is also Gaussian:

μpost = (σz² · μ0 + σ0² · z) / (σ0² + σz²)
1/σpost² = 1/σ0² + 1/σz²

The posterior mean is a weighted average of the prior mean and the measurement, weighted inversely by their variances. The posterior precision (1/variance) is the sum of the prior and measurement precisions — certainties add. More data always sharpens the belief, as long as the noise models are correct.

SEE ITBayesian belief updater — watch the posterior sharpen

FollowBot is at an unknown position on a 1D corridor. You have a prior belief — a Gaussian with some mean and variance. A distance sensor fires and returns a reading. Apply the Bayes update and watch the posterior. Then toggle between MAP and MMSE estimators and observe them diverge when the posterior is asymmetric (artificially skewed by an obstacle prior).

⚙ Interactive · 1D Bayesian Belief Updater

Adjust the prior and sensor, then apply the measurement. The worst choice: set high sensor noise and observe how little the measurement moves the posterior — a weak sensor barely updates the belief.

Prior Belief
2.5
0.80
Sensor Measurement
3.2
0.30
Posterior shape:
Skewed mode: wall on one side truncates the posterior — MAP ≠ MMSE
Show estimator:
Posterior
μpost = — m
σpost = — m
MAP estimator
x̂MAP = — m
MMSE estimator
x̂MMSE = — m
Divergence |MAP − MMSE|
Δ = — m

Adjust the sliders — the posterior updates instantly. Try setting σ₀ very large (uninformed prior) and σ_z very small (precise sensor) to see the posterior collapse onto the measurement. The "Skewed" mode uses an asymmetric exponential approximation to illustrate shape; it is not a proper distribution derived from the observation model.

The divergence between MAP and MMSE is zero for a symmetric Gaussian — both point to the mode/mean, which coincide. Switch to "Skewed" mode to introduce an asymmetric posterior (as if the prior were truncated by a wall on one side). Now the mode (MAP) and the mean (MMSE) no longer coincide. This is the practical case in robotics: near walls, doorways, or corners, the robot's belief is often not symmetric, and the choice of estimator matters for the downstream planner.

DEFINITIONMAP vs. MMSE: two answers from one distribution

Given a posterior distribution, you often need to report a single number to the planner or display to the operator. Two principled choices:

Estimator Definition Minimizes For Gaussian posteriors When they differ
MAP (Maximum A Posteriori) x̂ = argmax p(x|z) 0–1 loss (mode) = mean = median (symmetric) Asymmetric or multimodal posterior
MMSE (Minimum Mean Squared Error) x̂ = E[x|z] = ∫x·p(x|z)dx Squared error (mean) = mode = median (symmetric) Asymmetric or multimodal posterior

For a unimodal symmetric posterior (Gaussian), MAP and MMSE are identical. They diverge for skewed or multimodal posteriors. Concretely in robotics:

💡 The Kalman filter is the MMSE estimator for linear Gaussian systems

This is the formal justification for the Kalman filter: under linear dynamics and Gaussian noise, the posterior is Gaussian, and the Kalman filter's state estimate is provably the MMSE estimator — it minimizes the expected squared error over all possible linear estimators. This is the theorem that makes the Kalman filter optimal, not merely practical. The EKF (Lesson 4.5) approximates this optimality by linearizing; the particle filter (Lesson 4.7) approximates the full distribution and can handle multimodal beliefs.

WEAKNESSWhy a point estimate is dangerous: the planner needs the covariance

Suppose FollowBot's state estimator reports its position as (x=2.41, y=1.83, θ=0.12) with no further information. The path planner receives this pose and computes the shortest path to the target. It finds a route that passes 0.15 m from a wall — within the robot's footprint margin of 0.12 m, but seemingly safe. The robot executes the path and clips the wall.

What went wrong? The state estimate had an uncertainty of σ ≈ 0.18 m in the x direction — nearly one and a half times the safety margin. A planner that knew this would inflate the obstacle by σ (or some multiple thereof) before planning, producing a more conservative path. A planner given only the point estimate has no way to be appropriately conservative. It can't be conservative about something it doesn't know.

Point estimate only — no uncertainty wall robot target clips wall Planner has no reason to be conservative With covariance — obstacle inflated by σ σ-inflated zone uncertainty ellipse Planner uses covariance to stay conservative
Left: planning with a point estimate. The shortest path passes within the footprint margin of the wall — the planner has no information that the position estimate might be 0.18 m off. Right: planning with the full covariance. The inflation zone extends the obstacle boundary by σ, forcing a wider path. Same algorithm, different information → different safety outcome.
🔑 The covariance matrix is not optional metadata — it is a first-class output

In the ROS 2 ecosystem, every pose-carrying message that matters includes a covariance matrix: geometry_msgs/PoseWithCovarianceStamped carries a 6×6 covariance (x, y, z, roll, pitch, yaw). The nav2 costmap layers, the AMCL localizer, and the EKF localization node all publish this form. If your estimator publishes a bare geometry_msgs/Pose — no covariance — you are discarding the single most important output of the filter. Downstream planners cannot be appropriately conservative, and you have made a design error that will surface as an unpredictable collision in the field.

pose_with_cov_example.pyrclpy · publishing pose with covariance
from geometry_msgs.msg import PoseWithCovarianceStamped

def make_pose_msg(x, y, theta, cov_xx, cov_yy, cov_tt):
    # Build a pose with full 6x6 covariance for ROS 2 nav stack
    msg = PoseWithCovarianceStamped()
    msg.header.frame_id = 'map'
    msg.pose.pose.position.x = x
    msg.pose.pose.position.y = y
    # Covariance is row-major 6x6: [x, y, z, roll, pitch, yaw]
    # For 2D robot, set xx=cov_xx, yy=cov_yy, yaw-yaw=cov_tt
    c = [0.0] * 36
    c[0]  = cov_xx   # x variance (m²)
    c[7]  = cov_yy   # y variance (m²)
    c[35] = cov_tt   # yaw variance (rad²)
    msg.pose.covariance = c
    return msg

FOLLOWBOTConnecting probability to FollowBot's EKF

Stage 4's milestone is a full EKF operational on FollowBot with state vector [x, y, θ, v, ω], fusing wheel odometry, IMU, and LIDAR scan-matching. Every concept in this lesson maps directly to that implementation.

1

The state vector is the belief's support

FollowBot's EKF state vector x = [x, y, θ, v, ω]ᵀ defines what the belief is distributed over. At each time step, the EKF maintains a mean vector μ (the best estimate) and a 5×5 covariance matrix Σ (the uncertainty). Together, μ and Σ fully parameterize the Gaussian belief bel(xt) = N(μ, Σ). Publishing just μ to nav2 and discarding Σ is exactly the point-estimate failure described above.

2

Each sensor contributes a likelihood

Wheel odometry provides p(z_odom | x): if FollowBot is at pose [x, y, θ] with velocity v, what odometry reading do we expect? The noise model from Lesson 1.6 gives this as a Gaussian centered on the kinematic prediction with variance proportional to distance traveled. The IMU provides p(z_imu | x): angular rate measurement with bias and white noise. LIDAR scan-matching provides p(z_scan | x): a Gaussian-approximated likelihood from the scan correlation score. The EKF update step applies Bayes theorem for each sensor measurement, one at a time.

3

Filter consistency is a measurable property

A consistent filter is one whose covariance correctly captures the true estimation error. The Normalized Estimation Error Squared (NEES) test, built in Lesson 4.4, checks whether the true error divided by the predicted covariance follows a chi-squared distribution. A filter that reports overconfident covariances (too small) is worse than a filter with large covariances: the planner trusts the wrong certainty and plans unsafely. NEES testing is how you verify that your Bayesian machinery is honest.

🔑 FOLLOWBOT milestone preview

By the end of Stage 4, FollowBot will maintain a full probabilistic belief over [x, y, θ, v, ω], fusing three sensor modalities in a validated EKF. Position error below 0.1 m RMS in a known map. Sensor dropout behavior characterized: during LIDAR blackout, the posterior widens (Σ grows) rather than staying falsely confident — exactly the behavior Bayesian theory demands. The concepts in this lesson are the mathematical foundation every one of those properties rests on.

WEAKNESSWhere probabilistic thinking bites in practice

✗ Trusting confidence when the noise model is wrong

The posterior is only as trustworthy as the likelihood p(z | x). If the LIDAR sensor model assumes σ = 0.02 m but the actual noise is 0.08 m on retroreflective surfaces, the Bayes update will overweight the measurement — the posterior will be narrower (more confident) than reality warrants. The filter is consistent with its model, not with reality. This is the most common source of EKF divergence in field robotics: not the algorithm, but the assumed noise parameters. The fix is calibration against ground truth, not algorithm tuning. Never accept default noise parameters from a driver package without measuring the actual sensor noise on your hardware.

✗ Collapsing a multimodal posterior to one point

In a symmetric corridor, under LIDAR only, the robot's belief may be genuinely bimodal: it could be in the left branch or the right branch. A Gaussian filter (Kalman, EKF) cannot represent this — it maintains a unimodal Gaussian and will average the two modes, placing the peak in the wall between them. The filter will appear confident and will be wrong. For any scenario where multimodal beliefs are geometrically plausible — symmetric environments, initialization without GPS, loop closure in SLAM — a Gaussian filter is architecturally incapable of correct behavior. This is why particle filters exist. Choosing a Gaussian filter for an environment with symmetric ambiguity is an architectural error, not a parameter-tuning problem.

✗ Discarding covariance at ROS 2 topic boundaries

In practice, the covariance is frequently lost at topic type conversions. A common pattern: the EKF localizer publishes PoseWithCovarianceStamped (correct), a downstream node reads it and republishes it as a bare Pose for a legacy visualizer (covariance lost), and the path planner subscribes to whichever topic it finds first. If it finds the bare Pose, all downstream planning is covariance-blind. Always enforce covariance-carrying message types at system boundaries. In ROS 2, this means using PoseWithCovarianceStamped for nav2 inputs, not PoseStamped.

KEY INSIGHTHow this connects

  • Sensors: Physics, Noise Models, and Failure Modes (1.6) — the noise models built there are the literal input to the likelihood function p(z | x) used in every Bayes update. The LIDAR Gaussian noise model with σ ≈ 0.02 m is the sensor model that feeds FollowBot's EKF update step.
  • 4.2 · The Bayes Filter and the Predict-Update Cycle — next lesson: builds the full recursive Bayes filter from this lesson's one-step update. Adds the motion model (predict step) to complete the two-step cycle. All Kalman variants follow directly.

This lesson establishes the foundation: everything in Stage 4 is a specific instantiation of Bayes theorem applied recursively to a state-space model. The Kalman filter is Bayes for linear Gaussians. The EKF is Bayes for linearized nonlinear systems. The particle filter is Monte Carlo Bayes for arbitrary distributions. MAP and MMSE are two ways to read a single number from the posterior the filter maintains. Understanding these relationships at the probabilistic level means you can reason about when each algorithm is appropriate — and when it will fail.

GLOSSARYKey terms introduced

Belief (bel(x))
The robot's probability distribution over its own state. A Gaussian belief is parameterized by mean μ and covariance Σ. The belief is maintained and updated by the state estimator at every time step — it is the fundamental output of a Bayes filter, not a single pose.
Prior
The probability distribution p(x) over state before incorporating a new measurement. In recursive Bayesian estimation, the prior at time t is the posterior from time t−1, propagated forward by the motion model (predict step).
Posterior
The updated belief p(x | z) after incorporating measurement z via Bayes theorem. The posterior is always at least as concentrated as the prior (given a valid likelihood), reflecting the information gained from the measurement.
Likelihood (sensor model)
p(z | x) — the probability of observing measurement z given that the true state is x. Encodes the sensor's noise characteristics. An incorrect noise model produces a likelihood that pulls the posterior toward the wrong location with false confidence.
MAP estimator
Maximum A Posteriori: x̂ = argmax p(x|z). The mode of the posterior distribution. Equals the MMSE estimator for symmetric unimodal posteriors; diverges for asymmetric or multimodal distributions. Minimizes 0–1 loss.
MMSE estimator
Minimum Mean Squared Error: x̂ = E[x|z]. The mean of the posterior distribution. The optimal estimator under squared-error loss. For linear Gaussian systems, the Kalman filter computes this analytically. For nonlinear or non-Gaussian systems, must be approximated.
Covariance matrix (Σ)
The uncertainty of a Gaussian belief. A 5×5 matrix for FollowBot's state [x, y, θ, v, ω]. Diagonal entries are variances; off-diagonal entries capture correlations (e.g., x and y uncertainty may be correlated after turning). The covariance is a first-class output of the filter; discarding it produces the point-estimate failure described in this lesson.
Filter consistency
A consistent filter's covariance correctly bounds the true estimation error. Measured via the Normalized Estimation Error Squared (NEES) statistic. An inconsistent filter with artificially small covariances produces overconfident beliefs that lead to unsafe planning decisions.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why is a robot's position better represented as a probability distribution than as a single (x, y, θ) triple?›

Every sensor measurement is noisy: the LIDAR range has quantization and reflectance-dependent error; the wheel encoders accumulate slip; the IMU drifts. No single measurement, and no finite accumulation of measurements, can pin down the true pose to a point. The probability distribution p(x, y, θ) captures the full state of knowledge: the most probable location (the mode), the spread of uncertainty (the variance/covariance), and the shape of the uncertainty (Gaussian, multimodal, truncated near walls). A point estimate implicitly claims zero uncertainty, which is never true and leads to unsafe planning. The distribution is the honest answer; the point estimate is a lossy summary that discards the most safety-relevant information.

2Write out Bayes theorem for a single sensor update and label each term with its robotics interpretation.›

p(x | z) = p(z | x) · p(x) / p(z)

  • p(x | z) — the posterior: updated belief over robot state x after receiving measurement z. This is what the filter computes.
  • p(z | x) — the likelihood (sensor model): how probable is this measurement if the robot is truly at state x? Encodes sensor noise characteristics (Lesson 1.6).
  • p(x) — the prior: belief before this measurement, typically the prediction from the motion model.
  • p(z) — the normalizing constant: ∫p(z|x)p(x)dx, ensuring the posterior integrates to 1. Often written η⁻¹. Computationally hard for high-dimensional continuous state spaces — the key motivator for Kalman/particle filter approximations.
3Under what conditions do MAP and MMSE estimators give different answers, and which failure does each produce in a path planner?›

MAP and MMSE agree when the posterior is unimodal and symmetric (e.g., a Gaussian). They diverge for asymmetric (e.g., truncated near a wall) or multimodal (e.g., a robot in a symmetric corridor with two equally plausible locations) posteriors.

MAP failure: near a wall, the mode is pulled toward the wall side; a planner using MAP may plan a path too close to the obstacle because the most likely position is near (but not at) the wall.

MMSE failure: for a bimodal posterior with two peaks on opposite sides of a wall, the MMSE (mean) lies between the peaks — inside the wall. A planner using MMSE receives a position estimate that is geometrically impossible and will plan into the wall. In multimodal scenarios, neither single-point estimator is safe — the full distribution must be communicated to the planner.

4FollowBot's EKF localizer publishes a PoseWithCovarianceStamped. A teammate's middleware node converts it to PoseStamped to feed a legacy visualizer. Why is this a potential safety issue, not just an API convenience issue?›

PoseStamped carries only position and orientation — no covariance. If the path planner subscribes to the PoseStamped topic (instead of the original PoseWithCovarianceStamped), it receives a point estimate with no uncertainty. The planner then plans at the nominal clearance (e.g., 0.15 m from a wall) without knowing that the position uncertainty is σ = 0.18 m — larger than the clearance itself. Paths that look geometrically safe are not safe given the actual uncertainty. The covariance is not metadata for visualization; it is the primary safety input for conservative planning. Any topic conversion that strips the covariance is a correctness bug, not a cosmetic issue.

Next: the Bayes filter and the predict-update cycle →