Reasoning Under Hidden States

Chapter 1: Belief Initialization

Before taking any action, we need a starting belief b₀. Two broad representations are available:

Choosing the initial belief matters enormously. A diffuse initial belief (e.g., uniform over all states) is robust — it makes few assumptions. A confident wrong initial belief can be catastrophic: it may take a very long time to correct, even with informative observations.

Two special starting cases:

In practice, initial belief construction is one of the most important engineering decisions for a POMDP system. Here is a concrete comparison of three initialization strategies for a robot localization problem in a 100×100 meter warehouse (state = (x, y) position):

Chapter 2: Discrete State Filter

For finite state and observation spaces, belief updates are exact. The belief b is a probability vector of length |S|. After taking action a and receiving observation o, the new belief b' is computed in two steps:

where η is a normalizing constant: η = 1 / ∑_s' O(o|a,s') b¨(s').

This is recursive Bayesian estimation on a discrete state space. The prediction step propagates the belief forward through the dynamics model. The update step incorporates the observation using Bayes' rule. The normalization ensures b' is a valid probability distribution.

The computational cost is O(|S|²) per update (the double sum over s and s'). For large state spaces, this becomes impractical, motivating the approximate methods in later chapters.

Here is a complete step-by-step worked example of one discrete filter update. The crying baby problem: states = {sated=0, hungry=1}, action = "ignore", observation = "crying". Starting belief b = [0.5, 0.5].

python
# Crying baby discrete filter: one step, action=ignore, obs=crying
# T[a][s][s'] = P(s'|s, a). Action: ignore=2
T_ignore = [[0.9, 0.1],  # from sated: 90% stay sated, 10% go hungry
             [0.0, 1.0]]  # from hungry: always stay hungry
# O[a][s'][o]: observation model, o=0 means crying
O_ignore = [0.1, 0.8]    # P(crying|sated)=0.1, P(crying|hungry)=0.8

b = [0.5, 0.5]              # prior belief: 50/50

# --- PREDICT STEP ---
# b_pred[s'] = sum_s T(s'|s,a) * b[s]
b_pred = [0, 0]
b_pred[0] = T_ignore[0][0]*b[0] + T_ignore[1][0]*b[1]   # 0.9*0.5 + 0.0*0.5 = 0.45
b_pred[1] = T_ignore[0][1]*b[0] + T_ignore[1][1]*b[1]   # 0.1*0.5 + 1.0*0.5 = 0.55
print(f"Predicted: {b_pred}")   # [0.45, 0.55]

# --- UPDATE STEP --- (observation = crying)
# b_new[s'] = eta * O(crying|s') * b_pred[s']
unnorm = [O_ignore[0] * b_pred[0],   # 0.1 * 0.45 = 0.045  (sated, but still crying)
          O_ignore[1] * b_pred[1]]   # 0.8 * 0.55 = 0.440  (hungry, expected crying)
eta = 1.0 / sum(unnorm)              # 1 / 0.485 = 2.062
b_new = [eta * u for u in unnorm]   # [0.093, 0.907]
print(f"Updated:   {[f'{v:.3f}' for v in b_new]}")

# Interpretation: P(hungry|ignore, crying) ≈ 90.7%
# The crying observation strongly confirms hunger.
# If we had observed "quiet" instead:
# unnorm_q = [(1-0.1)*0.45, (1-0.8)*0.55] = [0.405, 0.110] → P(hungry) ≈ 21%

Chapter 3: Kalman Filter

For continuous states with linear Gaussian dynamics, the belief is always a Gaussian N(μ,Σ). This is not an approximation — it's exact. The dynamics and observation models are:

The belief update has the famous two-step structure:

The matrix K is the Kalman gain. It is the weight given to the innovation (o − O_sμ_p), the difference between the actual observation and the predicted observation. Think of K as a trust slider:

• When Σ_o is small (observations are precise): K → large. Trust the observation more.
• When Σ_o is large (observations are noisy): K → small. Trust the prediction more.
• When Σ_p is large (lots of prediction uncertainty): K → large. Eager to correct.

In steady state, the variance Σ converges to a fixed-point determined entirely by the noise parameters — independent of the initial belief. This is called the steady-state Kalman filter. For a 1D random walk with process variance σ_s² and observation variance σ_o², the steady-state gain K* and variance Σ* satisfy:

With σ_s² = 0.25 and σ_o² = 1.0: Σ* = 0.125 + √(0.0156 + 0.25) ≈ 0.125 + 0.5063 = 0.631. K* = 0.631/(0.631+1) ≈ 0.387. After reaching steady state, every update sets K ≈ 0.387 — a fixed fraction of the innovation is incorporated regardless of history. This is why steady-state Kalman filters are so efficient: precompute K* offline, then run at O(n) per step online.

Here is a 5-step worked example tracing the 1D Kalman filter, starting from uncertainty σ²=4 and true position 0. Process noise σ_s²=0.25, observation noise σ_o²=1.0:

Notice that σ² starts large (4.25) and rapidly shrinks to a steady state around 0.40 — consistent with the analytic steady-state formula (Σ* ≈ 0.631 for this noise setting). The gain K also stabilizes around 0.40. The filter's mean tracks the true position, staying within ±2σ = ±2×0.63 of each observation. The estimate at step 5 (0.80) is close to the true position 0.9, within one sigma.

The three key properties of the Kalman filter that make it the gold standard for linear systems:

• Optimal (minimum variance): Among all linear estimators, the Kalman filter minimizes the mean squared error. No other linear filter can do better given the same data.
• Recursive: The belief at time t only depends on the previous belief — not the full history. This enables real-time streaming at O(n³) per step.
• Consistent: The estimated covariance accurately reflects the true estimation uncertainty (assuming correct noise models). A well-tuned Kalman filter's 95% confidence intervals actually contain the truth ~95% of the time.

Here is the complete 1D Kalman filter in Python. The belief is just two numbers — mean μ and variance σ². Everything else is algebra:

python
import numpy as np

class KalmanFilter1D:
    """1D Kalman filter: belief = (mu, sigma2)."""
    def __init__(self, mu0=0.0, sigma2_0=1.0):
        self.mu = mu0             # belief mean
        self.sigma2 = sigma2_0    # belief variance

    def predict(self, sigma2_process):
        # Propagate belief through random walk dynamics
        # mu_p = mu  (no deterministic drift in this example)
        # sigma2_p = sigma2 + sigma2_process
        self.sigma2 += sigma2_process
        # self.mu unchanged (identity transition)

    def update(self, obs, sigma2_obs):
        # Incorporate a noisy observation
        K = self.sigma2 / (self.sigma2 + sigma2_obs)   # Kalman gain
        self.mu = self.mu + K * (obs - self.mu)         # belief mean update
        self.sigma2 = (1 - K) * self.sigma2              # belief variance shrinks
        return K  # return K so we can inspect it

# Worked example matching the textbook's robot localization scenario
kf = KalmanFilter1D(mu0=0.0, sigma2_0=4.0)  # uncertain about initial position
true_pos = 0.0
for step in range(10):
    true_pos += np.random.randn() * 0.5       # true random walk
    obs = true_pos + np.random.randn() * 1.0  # noisy observation
    kf.predict(sigma2_process=0.25)           # process noise variance
    K = kf.update(obs, sigma2_obs=1.0)        # observation noise variance
    print(f"Step {step+1}: true={true_pos:.2f}, obs={obs:.2f}, "
          f"mu={kf.mu:.2f}, sigma={kf.sigma2**.5:.2f}, K={K:.3f}")

Chapter 4: Extended Kalman Filter

What if the dynamics or observation functions are nonlinear? The standard Kalman filter fails because a Gaussian pushed through a nonlinear function is no longer Gaussian. The Extended Kalman Filter (EKF) handles this by linearizing the nonlinear functions at the current belief mean using first-order Taylor expansions (Jacobians).

where J_T = ∂f_T/∂s |_μb and J_O = ∂f_O/∂s' |_μp are the Jacobian matrices evaluated at the current mean. The Kalman update equations then apply with J_T and J_O in place of T_s and O_s.

Here is a concrete worked EKF example: 2D robot tracking with a bearing-only sensor. State s = (x, y). Observation: the angle from the origin to the robot: f_O(x,y) = atan2(y,x). This is highly nonlinear.

python
import numpy as np

# Robot state: s = [x, y] = [3.0, 1.0]   (true position)
# Belief mean: mu = [3.0, 1.0]  sigma2 = [[0.5, 0], [0, 0.5]]
mu = np.array([3.0, 1.0])
Sigma = np.diag([0.5, 0.5])

# Nonlinear observation model: angle theta = atan2(y, x)
# EKF: linearize via Jacobian J_O = d/d(x,y) atan2(y,x)
# d(atan2(y,x))/dx = -y/(x^2+y^2)
# d(atan2(y,x))/dy =  x/(x^2+y^2)

x, y = mu
r2 = x**2 + y**2          # = 3^2+1^2 = 10
J_O = np.array([[-y/r2, x/r2]])   # = [[-0.1, 0.3]]  (1x2 Jacobian)
print(f"Jacobian J_O = {J_O}")   # [[-0.1, 0.3]]

# Observation noise: sigma_o = 0.1 radians
Sigma_o = np.array([[0.01]])    # (0.1 rad)^2

# Kalman gain: K = Sigma * J_O^T * (J_O * Sigma * J_O^T + Sigma_o)^{-1}
S_innov = J_O @ Sigma @ J_O.T + Sigma_o  # = [[-0.1,0.3]] [[0.5,0],[0,0.5]] [[-0.1],[0.3]] + 0.01
                                          # = [[-0.1,0.3]] [[-0.05],[0.15]] + 0.01 = [0.05] + 0.01 = [0.06]
K = Sigma @ J_O.T @ np.linalg.inv(S_innov)  # 2x1 gain vector
print(f"K = {K.flatten()}")    # approximately [-0.833, 2.5]

# Observe: true bearing = atan2(1,3) ≈ 0.3217 rad
# Noisy observation: theta_obs = 0.35 rad
theta_pred = np.arctan2(mu[1], mu[0])   # ≈ 0.3217
innovation = np.array([0.35]) - theta_pred  # ≈ 0.028 rad
mu_new = mu + K.flatten() * innovation[0]  # adjust position toward observation
Sigma_new = (np.eye(2) - K @ J_O) @ Sigma  # covariance shrinks
print(f"Updated mu: {mu_new}")
# mu changes: x decreases slightly (bearing too large implies y was underestimated)
# Key: the EKF exploits the local geometry (Jacobian) to map bearing residual
# into (x,y) correction. Works well near the true position; fails far from it.

Chapter 5: Unscented Kalman Filter

The EKF linearizes using a derivative. But what if computing Jacobians is hard (or the function isn't differentiable)? The Unscented Kalman Filter (UKF) takes a different approach: instead of linearizing the function, it passes a carefully chosen set of sigma points through the nonlinear function and fits a Gaussian to the output.

For a state of dimension n, the UKF computes 2n+1 sigma points from the current belief N(μ,Σ):

where (√(n+λ)Σ)_i denotes the i-th column of the matrix square root, and λ is a spread parameter. Each sigma point is propagated through the nonlinear function, and the output mean and covariance are reconstructed as weighted sums of the transformed points.

The weights are not all equal. The mean sigma point (s₀) gets a weight designed to absorb the remaining probability mass, while the off-center points get equal weights that together balance the mean:

where α controls the spread, β = 2 for Gaussian priors (optimal), and λ = α²(n+κ) − n. After propagating each sigma point through the nonlinear function f:

A concrete 1D example: n=1, λ=2. Belief: μ=0, σ²=1. Sigma points: s₀=0 (weight 2/3), s₁=+√3≈1.73 (weight 1/6), s₂=−√3≈−1.73 (weight 1/6). Propagate through f(x) = sin(x): f(0)=0, f(1.73)≈0.987, f(−1.73)≈−0.987. Reconstructed mean: (2/3)×0 + (1/6)×0.987 + (1/6)×(−0.987) = 0. Reconstructed variance: (1/6)(0.987−0)² + (1/6)(0.987−0)² = 0.325 + 0.325 ≈ 0.65. The UKF correctly estimates that sin(x) for x~N(0,1) has variance less than 1 (since |sin| ≤ 1 compresses the tails).

Chapter 6: Particle Filter

What if none of the Gaussian assumptions hold? What if the posterior is multimodal (two plausible locations), or the dynamics are highly nonlinear, or the state space is mixed discrete-continuous? Particle filters represent the belief as a set of weighted samples (particles) that can approximate any distribution.

The update algorithm for m particles {(s_i, w_i)}_i=1^m:

The key insight: resampling discards low-probability particles and duplicates high-probability ones. After many steps, particles concentrate near the true state. The belief estimate is approximated by the empirical distribution of particles.

There are three standard resampling algorithms. They all draw m new particles proportional to weights but differ in implementation:

Systematic resampling (also called "low-variance resampling") is strongly preferred in practice. With multinomial resampling, a particle with weight 3/m might be duplicated 0, 1, 2, 3, or more times due to random chance. With systematic resampling, it is always duplicated exactly 3 times. This determinism reduces the randomness in the particle set and improves accuracy for the same m.

python
import numpy as np

def systematic_resample(particles, weights):
    """Low-variance systematic resampling: O(m)."""
    m = len(particles)
    cumulative = np.cumsum(weights)
    u = np.random.uniform(0, 1.0/m)           # one random number
    positions = u + np.arange(m) / m            # m equally-spaced positions
    indices = np.searchsorted(cumulative, positions)  # O(m) binary searches
    return particles[indices]

def multinomial_resample(particles, weights):
    """Standard multinomial resampling: O(m log m), higher variance."""
    m = len(particles)
    indices = np.random.choice(m, size=m, replace=True, p=weights)
    return particles[indices]

# For comparison with m=5 particles, weights=[0.4, 0.3, 0.1, 0.1, 0.1]:
# Multinomial: may give [0,1,3,1,0] or [2,1,1,1,0] — high variance
# Systematic: always gives [2,1,1,1,0] or [2,2,1,0,0] — deterministic spread

Here is a miniature worked example with m = 5 particles (normally you'd use hundreds). 1D position tracking. True position = 0.6. Particles: {0.2, 0.4, 0.5, 0.7, 0.9}. Noisy observation: z = 0.65. Observation noise σ_o = 0.1.

After normalization, w = [0.002, 0.073, 0.370, 0.244, 0.057] (sum to 1). Weighted mean: 0.002×0.21 + 0.073×0.42 + 0.370×0.51 + 0.244×0.72 + 0.057×0.89 ≈ 0.60 — very close to the true position 0.6! After resampling with these weights, particles near 0.51 and 0.72 will be duplicated, while the particle at 0.21 will almost certainly be dropped.

The particle filter algorithm is remarkably concise to implement. Each "particle" is just a state sample with a weight:

python
import numpy as np

def particle_filter_update(particles, weights, action, obs,
                              transition_fn, obs_likelihood_fn, m):
    """
    particles: array of shape (m, state_dim) — current particle set
    weights:   array of shape (m,) — current particle weights
    action:    the action taken
    obs:       the observation received
    transition_fn(s, a): samples s' ~ T(s'|s,a)
    obs_likelihood_fn(o, s'): returns O(o|s') as a scalar
    Returns: new particles and uniform weights after resampling
    """
    # Step 1: Propagate — sample s_i' ~ T(s'|s_i, a)
    new_particles = np.array([transition_fn(s, action) for s in particles])

    # Step 2: Weight — w_i = O(obs | s_i')
    new_weights = np.array([obs_likelihood_fn(obs, s) for s in new_particles])

    # Step 3: Normalize
    total = new_weights.sum()
    if total < 1e-12:
        new_weights[:] = 1.0 / m  # degenerate case: reset to uniform
    else:
        new_weights /= total

    # Step 4: Resample — draw m particles with replacement
    indices = np.random.choice(m, size=m, replace=True, p=new_weights)
    resampled = new_particles[indices]
    uniform_weights = np.full(m, 1.0 / m)

    return resampled, uniform_weights

# Example: 2D position tracking with random walk dynamics
m = 500  # number of particles
particles = np.random.uniform(0, 1, (m, 2))   # uniform initial belief over [0,1]^2
weights = np.full(m, 1.0 / m)

def transition(s, a): return np.clip(s + np.random.randn(2) * 0.03, 0, 1)
def likelihood(obs, s): return np.exp(-np.sum((s - obs)**2) / (2 * 0.05**2))

# After each action + observation, call:
# particles, weights = particle_filter_update(particles, weights, a, obs,
#                                              transition, likelihood, m)
# Belief mean estimate:
# mu = particles.mean(axis=0)

Chapter 7: Particle Injection

Particle filters have a critical failure mode called particle deprivation: all particles can converge to a single region of state space, leaving no coverage of other regions. If the true state then jumps to a previously-uncovered region (due to a sudden large transition or a rare event), the filter has no particles there and cannot recover.

Particle injection addresses this by periodically adding a small fraction of randomly sampled particles (from the prior or from the observation model) to the particle set. This ensures global coverage is maintained even as most particles concentrate around the likely true state.

The mechanics of particle deprivation can be understood quantitatively. After k steps without resampling, the effective sample size N_eff = 1/(∑w_i²) decreases as particles drift. After resampling, N_eff resets to m (all uniform weights). However, with each predict–weight–resample cycle, some particles are duplicated and others are dropped. After many cycles in a smooth, well-observed system, the particle set collapses to a few distinct values — particle deprivation even without any sudden state jumps. Injection prevents this collapse by continuously replenishing diversity.

Two types of injected particles:

The injection rate ρ (fraction of particles replaced each step) is a crucial hyperparameter. Too low: the filter cannot recover from kidnapping. Too high: the filter loses precision even in normal operation. The optimal rate depends on the expected frequency of sudden state jumps:

An alternative to fixed-rate injection: adaptive injection. After each resampling step, compute the normalized probability of the current observation under the existing particles: p(o|particles) = ∑_i w_i O(o|s_i). If this probability is very low (the observation is poorly explained by current particles), inject at a higher rate. This adapts injection to the evidence — injecting more when the filter is likely wrong.

Chapter 8: Showcase — The Crying Baby Filter

The crying baby problem is the canonical 2-state POMDP from the textbook. Two hidden states: {sated, hungry}. Three actions: {feed, sing, ignore}. Two observations: {crying, quiet}. The agent must maintain a belief over the baby's state and decide how to act.

The belief is a single number b = P(hungry). Watch how each action-observation pair updates this belief. Note that feeding deterministically resets belief to P(hungry) = 0 (we know feeding always sates).

Chapter 9: Summary & Filter Comparison

Chapter 19 establishes the belief update machinery that underpins all POMDP planning. Every filter produces the same object (a belief distribution over states) but makes different assumptions about the structure of the problem.

All five filters covered in this chapter share a common structure: a prediction step that propagates the belief forward through the dynamics, and an update step that incorporates the observation. The filters differ in how they represent the belief and how they implement each step. Understanding this shared structure is key to adapting filters to new problems.

Here is a side-by-side comparison of computational costs for each filter, to guide your implementation choice:

The practical choice flowchart:

Concrete robotics example: an autonomous vehicle navigating indoors must track its (x, y, θ) pose. Here is how the choice plays out:

The industry standard for mobile robot localization is the Adaptive Monte Carlo Localization (AMCL) algorithm, which is a particle filter with particle injection and adaptive particle count (more particles when uncertain, fewer when confident). AMCL is implemented in ROS (Robot Operating System) and runs on hundreds of deployed robots worldwide. It uses the discrete map as an implicit state space and samples particles from the map grid rather than continuously.

Common implementation pitfalls. Even experienced practitioners make these mistakes with belief filters:

Numerical comparison: EKF vs UKF on a sine observation model. Suppose the state is x_t = x_t−1 + w_t (w∼N(0,0.1)) and the observation is z_t = sin(x_t) + v_t (v∼N(0,0.05)). Starting from x₀∼N(0,1), both filters are initialized at μ₀=0, Σ₀=1. After one step with z₁=0.7:

The key difference: EKF approximates the observation variance as 1.15 (treating sin as linear), while UKF captures the true nonlinear spread of sin(x) when x∼N(0,1.1), giving a smaller S of 0.51. This means the UKF trusts the measurement more (the observation function is more informative than the EKF thinks), and converges faster. In strongly nonlinear problems, this difference compounds over time and EKF can diverge while UKF remains stable.

When to upgrade your filter. In practice, engineers often start with the simplest adequate filter and upgrade only when it fails. Here is a principled upgrade path:

1. Start with Kalman. Check: does the normalized innovation squared (NIS) statistic χ²-test pass? NIS = (z−z&hat;)^TS⁻¹(z−z&hat;) should be χ²(|z|) distributed. If NIS is consistently too large, the filter is inconsistent.
2. If NIS fails due to nonlinearity: upgrade to EKF. Check: is the Jacobian stable and computable at every time step?
3. If EKF diverges or Jacobians are ill-conditioned: upgrade to UKF. Check: are sigma point weights all positive after the choice of κ and α?
4. If belief is multimodal (kidnapped robot, cycle ambiguity): particle filter is required. No Gaussian filter can represent multimodal posteriors.

This upgrade ladder reflects a fundamental tradeoff: Gaussian filters are fast and analytically clean, but restricted to unimodal, approximately normal posteriors. Particle filters are universal approximators of probability distributions, but require Θ(m) compute per step where m must grow with the dimension and complexity of the belief. Choose the simplest filter that passes consistency tests on your data.