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

The Kalman Filter: Derivation and Geometric Intuition

The Kalman gain is not a magic formula — it is the minimum-variance solution to a linear regression problem, and deriving it makes every subsequent filter transparent.

⏱ ~25 min read 📐 Derivation + widget 🔑 Stage 4 core

🎯 By the end you'll understand

Why the Kalman gain is the minimum-mean-squared-error solution to the observation fusion problem, not an arbitrary weighting parameter. The five KF equations and exactly what each one does to the belief distribution. The geometric picture: the corrected mean always lies on the line between the predicted mean and the measurement, weighted by relative covariances. Why the covariance matrix P is just as important as the state estimate x̂ — and what happens to a filter initialized with P = 0. How to tune Q and R and read K's response to understand your system's trust structure.

KEY INSIGHTK is not a knob — it is a derived answer

Most engineers treat the Kalman gain as a parameter to tune. It is not. Given your claimed process noise Q and your claimed sensor noise R, K is uniquely determined by mathematics. If K looks wrong, your noise models are wrong — and the filter is telling you so.

The Kalman filter is the oldest and most widely deployed state estimator in engineering. It runs in GPS receivers, IMU fusion chips, aircraft autopilots, spacecraft attitude controllers, and — relevantly for us — in FollowBot's EKF localization stack. The reason it has survived sixty years of algorithmic competition is that it is the provably optimal linear estimator for Gaussian systems. Not "pretty good." Optimal, in a precise mathematical sense.

🔑 The claim to hold onto

The Kalman filter minimizes the expected squared error of the state estimate at every time step, given linear dynamics and Gaussian noise. The Kalman gain K is the exact weighting that achieves this minimum — derived, not chosen. Deriving it once replaces the "black box" mental model with a transparent mechanism: K is just the ratio of prediction uncertainty to total uncertainty. When prediction uncertainty is large relative to measurement noise, trust the measurement more (K → 1). When measurement noise is large relative to prediction uncertainty, trust the prediction more (K → 0). Everything else follows from this single fact.

CORE CONCEPTThe linear Gaussian system model

The Kalman filter is defined for a specific class of systems. Getting the class wrong is the most common reason a KF fails in practice. The class is characterized by two equations:

PROCESS MODEL OBSERVATION MODEL x k = F x k-1 + w k w k ~ N(0, Q) F = state transition matrix Q = process noise covariance z k = H x k + v k v k ~ N(0, R) H = observation matrix R = measurement noise covariance
The two model equations that define the linear Gaussian system. Process noise w acts on state transitions; measurement noise v acts on observations. Both are Gaussian with zero mean. F and H are constant matrices (for the basic KF).

Both noise terms are zero-mean Gaussian, and they are assumed independent of each other and of the state. This is the Gaussianity assumption — the bedrock that makes the posterior analytically tractable. If your noise is heavy-tailed (e.g., occasional IMU spikes), or bimodal (e.g., GPS multipath), the classical KF's posterior is wrong, and you need a particle filter or robust extension.

For concreteness, consider the 1D constant-velocity system used in the widget below. The state vector is x = [position, velocity]ᵀ. With time step Δt:

1D constant-velocity KF modelmath → code
# State: x = [position, velocity]^T  (2x1)
# Transition: position advances by velocity * dt
F = [[1, dt],   # x_{k} = x_{k-1} + v_{k-1}*dt + noise
     [0,  1]]   # v_{k} = v_{k-1}            + noise

# Observation: we measure position only (not velocity)
H = [[1, 0]]   # z_k = position + v_k,  v_k ~ N(0, R)

# Q encodes: how much does the model drift per step?
# R encodes: how noisy is the position sensor?
Q = [[q*dt**3/3, q*dt**2/2],   # continuous-time white accel noise
     [q*dt**2/2, q*dt       ]]   # discretized with power spectral density q
R = [[r]]                          # scalar sensor variance

Notice that H = [1, 0] — we observe position but not velocity. The filter must infer velocity from the sequence of position measurements. This is a central pattern in robotics: sensors measure some components of state directly, and the filter infers the rest through the dynamics model. An IMU gives you acceleration; the KF integrates it to get velocity and position. A LIDAR gives you range; the EKF uses the robot's motion model to maintain heading.

💡 When can you use a basic KF vs. an EKF?

The basic KF requires F and H to be constant matrices — i.e., the dynamics and observation function must be linear. A differential-drive robot's odometry kinematics are nonlinear (heading appears in a cosine). The EKF handles this by linearizing F and H at the current state estimate. For the widget in this lesson we use the 1D constant-velocity system where the model is genuinely linear, so no approximation is needed. Lesson 4.4 covers the EKF extension.

HOW IT WORKSDeriving the Kalman gain from the MMSE criterion

Here is the problem: after the predict step, we have a Gaussian prior over the state, N(x̂⁻, P⁻). We receive a measurement z, drawn from N(Hx, R). We want to fuse them. What is the optimal fusion weight?

Define the corrected estimate as a linear combination of the prior and the measurement:

📖 The update ansatz

x̂ = x̂⁻ + K(z − Hx̂⁻)

where (z − Hx̂⁻) is the innovation — the surprise. K is a matrix to be determined. The innovation is what the measurement tells us that our prediction didn't already know. When the innovation is zero (measurement matches prediction exactly), no correction is made regardless of K.

Now apply the MMSE criterion: choose K to minimize the trace of the posterior error covariance P = E[(x − x̂)(x − x̂)ᵀ]. This is the minimum-variance criterion — we want the posterior distribution to be as tight as possible around the true state.

1

Write the posterior error

e = x − x̂ = x − x̂⁻ − K(z − Hx̂⁻)

Substitute z = Hx + v (measurement model): e = (I − KH)(x − x̂⁻) − Kv

Let e⁻ = x − x̂⁻ (prior error). Then: e = (I − KH)e⁻ − Kv

2

Compute posterior covariance

P = E[eeᵀ] = (I − KH)P⁻(I − KH)ᵀ + KRKᵀ

This is the Joseph form — numerically stable because it keeps P symmetric even with floating-point errors. Two terms: uncertainty from the prior (reduced by I − KH) and uncertainty added by the measurement noise (scaled by K).

3

Minimize trace(P) with respect to K

Differentiate trace(P) with respect to K and set to zero. After expanding and applying the matrix differentiation identity d/dK trace(AKᵀBKC) = ..., the first-order condition gives:

−P⁻Hᵀ + K(HP⁻Hᵀ + R) = 0

Solving: K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹

4

Read the formula physically

HP⁻Hᵀ is the prediction uncertainty projected into measurement space (how uncertain are we about what z should be?). R is the measurement noise in measurement space. Their sum is total uncertainty in measurement space. K weights the innovation by (prediction uncertainty) / (total uncertainty). When P⁻ is large relative to R: K is large — trust the measurement. When R is large relative to P⁻: K is small — trust the prediction.

In the 1D scalar case with H = 1, this simplifies beautifully: K = P⁻ / (P⁻ + R). If prior variance P⁻ = 4 and measurement variance R = 1, then K = 4/5 = 0.8 — we're 80% trusting the measurement because our prediction was 4× more uncertain. This is the entire logic of the Kalman filter, exposed.

🔑 Why this is a linear regression

The update x̂ = x̂⁻ + K(z − Hx̂⁻) is literally a weighted average of two estimates: the prior x̂⁻ and the measurement z (after back-projecting through H). The Kalman gain is the optimal weight in the MMSE sense — exactly analogous to the normal equations in ordinary least squares. This is not a coincidence: the KF can be derived as the recursive solution to a batch least-squares problem. Deriving the gain once makes this connection obvious; treating K as a "tuning parameter" hides it forever.

DEFINITIONThe five Kalman filter equations

The complete KF consists of two steps — predict and update — described by five equations. Write them down and internalize what each one does:

Step Equation What it does Physical meaning
Predict 1 x̂⁻ = F x̂ Propagates state estimate forward Where do we think the robot is, given physics and our last estimate?
Predict 2 P⁻ = F P Fᵀ + Q Propagates + inflates covariance Our certainty degrades over time because the world is noisy (Q). The farther we predict without measuring, the more uncertain we become.
Update 1 K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹ Computes the Kalman gain How much should we trust the measurement vs. our prediction? Derived, not chosen.
Update 2 x̂ = x̂⁻ + K(z − Hx̂⁻) Corrects the state estimate Move the estimate toward the measurement by a fraction K of the innovation (z − Hx̂⁻).
Update 3 P = (I − KH)P⁻ Shrinks covariance after update Receiving a measurement reduces our uncertainty. How much? By the factor (I − KH).

In Python with NumPy, this is a straightforward translation:

kalman_filter.pyrclpy / numpy
import numpy as np

class KalmanFilter1D:
    """1D constant-velocity KF: state = [position, velocity]."""

    def __init__(self, dt, Q_scale, R_var):
        self.dt = dt
        self.F = np.array([[1, dt], [0, 1]])       # state transition
        self.H = np.array([[1, 0]])               # observe position only
        q = Q_scale
        self.Q = np.array([                         # process noise (discrete)
            [q*dt**3/3, q*dt**2/2],
            [q*dt**2/2, q*dt      ]])
        self.R = np.array([[R_var]])               # measurement noise variance
        self.x = np.zeros((2, 1))               # initial state estimate
        self.P = np.eye(2) * 1.0               # initial covariance

    def predict(self):
        self.x = self.F @ self.x               # x^- = F x
        self.P = self.F @ self.P @ self.F.T + self.Q  # P^- = F P F^T + Q

    def update(self, z):
        S = self.H @ self.P @ self.H.T + self.R   # innovation covariance
        K = self.P @ self.H.T @ np.linalg.inv(S) # Kalman gain
        inn = z - self.H @ self.x                  # innovation (surprise)
        self.x = self.x + K @ inn                  # state update
        self.P = (np.eye(2) - K @ self.H) @ self.P  # covariance update
        return K[0, 0]                            # scalar gain for position

Two implementation notes that matter in practice. First, for the covariance update at Update 3, the textbook form P = (I − KH)P⁻ can lose symmetry due to floating-point errors over thousands of iterations. The Joseph form P = (I − KH)P⁻(I − KH)ᵀ + KRKᵀ is algebraically equivalent but numerically more stable — use it in production. Second, the matrix inversion (HP⁻Hᵀ + R)⁻¹ is fine for small state vectors (2–5 dimensions). For large systems, replace it with a linear solve to avoid explicit inversion.

CORE CONCEPTGeometric interpretation: the posterior lies on the line

The update equation x̂ = x̂⁻ + K(z − Hx̂⁻) has an elegant geometric reading in 1D. Rearrange it:

x̂ = (1 − K)x̂⁻ + K·z

This is a weighted average: the posterior mean lies on the line segment connecting the prior mean x̂⁻ and the measurement z, at fractional distance K from the prior toward the measurement.

x̂⁻ (prior mean) ±2σ prior z (measurement) ±2σ meas x̂ (posterior) K ≈ 0.76 K × (z − x̂⁻)
Geometric interpretation of the KF update in 1D. The posterior mean (green) always lies between the prior mean (blue) and the measurement (gold). The Kalman gain K determines the fraction: K ≈ 0.76 means we move 76% of the distance from prior to measurement. Wide prior → high K. Tight prior → low K.

Three limiting cases reveal the filter's logic:

K→0

Perfect prediction, noisy sensor

When R ≫ P⁻, the sensor is so noisy relative to our prediction that we barely update: x̂ ≈ x̂⁻. The posterior hugs the prior. This is the right behavior — an extremely noisy measurement carries almost no useful information. But be careful: if this persists because you set R too high, the filter will never track reality even when the sensors are clean. An honest R matters.

K→1

Perfect sensor, uncertain prediction

When P⁻ ≫ R, the prediction is so uncertain that we almost entirely trust the measurement: x̂ ≈ z. The posterior collapses onto the measurement. This is also right — if we truly know almost nothing about where the robot is, a clean measurement should dominate. The covariance after the update is P ≈ 0 (very certain), reflecting that we now know the state from the measurement.

K=0.5

Equal uncertainty — split the difference

When P⁻ = R, the posterior mean lands exactly halfway between prediction and measurement, and the posterior variance is half the prior variance. This is the Bayesian optimal averaging of two equally reliable Gaussian estimates. No weighting scheme outside this produces a lower expected squared error.

SEE ITStep the Kalman filter — 1D position tracking

This widget runs a 1D constant-velocity Kalman filter one step at a time. At each step, the filter predicts (uncertainty grows), then fuses a noisy measurement (uncertainty shrinks). Adjust Q (process noise — how much the model drifts per step) and R (measurement noise — how noisy the sensor is), then step through time and watch K adapt. The worst choice: set Q = 0 and click "Reset P to zero" — the filter will ignore all future measurements forever.

⚙ Interactive · 1D Kalman Filter Step-Runner

Each "Step" button call runs one predict + update cycle. The chart shows the true position (dashed), predicted mean + ±2σ band, measurement (dot), and corrected mean + ±2σ band. Drag Q and R sliders to see K respond.

0.10
0.50
Step k
0
time steps
Kalman gain K
—
fraction trusting sensor
Predicted σ
—
before update (m)
Posterior σ
—
after update (m)
Innovation
—
z − Hx̂⁻ (m)
Kalman gain K — trust allocation:
← Prediction Sensor →
K=0 (ignore sensor) K=1 (ignore prediction)

Initialize P correctly — then step. Try Q ≫ R to see K → 1 (trust sensor). Try R ≫ Q to see K → 0 (trust prediction).

The key observation the widget reveals: K is not constant. When you first step (large P⁻ from initialization), K is close to 1 — we barely know anything, so trust the sensor. As the filter gains confidence (P shrinks), K drops and the prediction carries more weight. If you drag Q up, uncertainty accumulates faster, K rises again — the filter trusts the sensor more because the model drifts more. This self-adapting property is the core competency of the Kalman filter over simple averaging: it tracks uncertainty and adjusts weighting in real time.

CORE CONCEPTWhy P matters as much as x̂

Practitioners new to filtering fixate on x̂ — the state estimate. They monitor whether x̂ tracks the truth, tune until it does, and declare success. This is a mistake. P is equally fundamental, and a filter with a correct x̂ but wrong P is a liability.

P has two jobs:

  1. It determines K. Everything above established this. A wrong P leads to a wrong K, which produces a suboptimal or divergent update — even if x̂ is currently close to the truth.
  2. It is the filter's report of its own credibility. Downstream systems (the planner, the safety monitor, the person-tracker) need to know how reliable the state estimate is. A filter that claims σ = 0.01 m when the true error is 0.8 m is dangerous. The classic way to check this is the Normalized Estimation Error Squared (NEES) test — Lesson 4.4 covers it in detail, but the concept is simple: if (x − x̂)ᵀ P⁻¹ (x − x̂) is chi-squared distributed with the right degrees of freedom, the filter is consistent. If it's systematically too small, the filter is overconfident.
⚠ The zero-P initialization trap

What happens if you initialize P = 0 (a zero matrix) — meaning "I am completely certain of my initial state"? In the predict step, P⁻ = FPFᵀ + Q. If P = 0 initially: P⁻ = Q (only process noise). The Kalman gain then immediately jumps to the steady-state value K = Q/(Q+R) — all future updates use this fixed gain, losing any adaptive trajectory memory. This is not permanent measurement blindness, but it means the filter cannot exploit covariance information accumulated during the trajectory; it always behaves as if it just started. If your initial state estimate x̂₀ is wrong (it usually is), the filter recovers only at the rate set by Q/(Q+R) rather than adapting its trust based on trajectory history. The fix is simple: initialize P with a value that honestly reflects your initial uncertainty (e.g., P₀ = diag([σ_pos², σ_vel²]) with σ values chosen conservatively large).

This is why the "Reset P = 0" button in the widget above demonstrates a degenerate initialization — not a tuning option. Try it: after resetting with P = 0, the filter's posterior σ collapses immediately to the steady-state determined solely by Q and R, and K is fixed at Q/(Q+R) regardless of how much trajectory information has been accumulated. The filter continues processing measurements but without any memory of past covariance evolution.

FOLLOWBOTFrom scalar intuition to the FollowBot EKF

The scalar derivation above is not a toy — it is the same mathematics FollowBot's EKF executes, extended to a higher-dimensional state vector. FollowBot's full localization EKF uses the state:

📖 FollowBot EKF state vector

x = [x, y, θ, v, ω]ᵀ

where (x, y) is position in the map frame, θ is heading, v is forward velocity, and ω is angular velocity. The state vector is 5-dimensional. The process model uses the differential-drive kinematic equations (nonlinear in θ, hence "Extended" KF — Lesson 4.4). Sensor updates arrive from three sources: wheel odometry (updates v and ω), IMU (updates ω and Δθ), and LIDAR scan-matching (updates x, y, θ). Each sensor has its own H matrix (the Jacobian of the nonlinear observation function) and its own R covariance.

For FollowBot, the Stage 4 milestone requires position error below 0.1 m RMS during person-following. Here is how the five equations map onto that requirement:

1

Predict: odometry + IMU rate ~100 Hz

The predict step runs at the IMU rate (200 Hz, downsampled to 100 Hz to match the motor controller). It propagates the kinematic model forward: x_{k} = f(x_{k-1}, u_k) + w_k where u_k = [v_cmd, ω_cmd]. The Jacobian F_k = ∂f/∂x at x̂_{k-1} is recomputed each step (this is the "extended" part). Q is set from bench-tested IMU drift rates and encoder slip characterization.

2

LIDAR scan-match update: ~10 Hz

The LIDAR update arrives at 10 Hz (one scan per revolution). The observation h(x) = [x, y, θ]ᵀ is a 3D projection of the full 5D state — H is the 3×5 Jacobian of this projection. R for LIDAR scan-matching is set from the ICP alignment covariance output (available from the slam_toolbox localization mode in ROS 2). The innovation (z − Hx̂⁻) is the discrepancy between the scan-matched pose and the predicted pose.

3

Sensor dropout characterization

The Stage 4 milestone explicitly requires characterizing sensor dropout behavior. When LIDAR scans are absent (e.g., going through a reflective corridor), only the predict step runs and P grows. The 0.1 m RMS requirement sets an implicit deadline on dropout duration: with a typical Q that inflates position uncertainty at ~0.02 m/s, a 5-second LIDAR dropout accumulates ~0.1 m of positional uncertainty. Document this in the field test report. If the corridor is longer, add a fallback to IMU-only dead-reckoning with a wider R tolerance.

🔑 FollowBot EKF tuning heuristic

Start with Q larger than you think (conservative overestimate of model drift) and R matching the sensor datasheet variance. Then run the filter on recorded data with a known-good ground truth (e.g., a motion capture reference). Compute NEES = (x_truth − x̂)ᵀ P⁻¹ (x_truth − x̂) and plot it over time. If NEES > n (state dimension = 5) systematically, the filter is overconfident: increase Q or decrease R. If NEES < n systematically, the filter is underconfident: decrease Q or increase R. This is the principled approach — not "try smaller Q until it tracks."

followbot_ekf_node.pyrclpy · EKF skeleton
import rclpy, numpy as np
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped

class EKFNode(Node):
    def __init__(self):
        super().__init__('followbot_ekf')
        # State: [x, y, theta, v, omega]
        self.x = np.zeros(5)
        self.P = np.diag([1.0, 1.0, 0.5, 0.2, 0.2])  # honest init!
        self.create_subscription(Odometry, '/odom', self.odom_cb, 10)
        self.create_subscription(
            PoseWithCovarianceStamped, '/lidar_pose',
            self.lidar_cb, 5)
        self.pub = self.create_publisher(
            PoseWithCovarianceStamped, '/ekf_pose', 10)

    def odom_cb(self, msg):
        # Predict step (high-rate, ~100 Hz)
        v = msg.twist.twist.linear.x
        w = msg.twist.twist.angular.z
        self._predict(v, w, dt=0.01)
        self._publish()

    def lidar_cb(self, msg):
        # Update step (lower-rate, ~10 Hz)
        z = np.array([msg.pose.pose.position.x,
                       msg.pose.pose.position.y,
                       self._quat_to_yaw(msg.pose.pose.orientation)])
        R_lidar = np.array(msg.pose.covariance).reshape(6,6)[:3,:3]
        self._update(z, R_lidar)
        self._publish()

WEAKNESSWhere the Kalman filter bites back

✗ Mistuned Q and R are the most common failure, not bugs

The Kalman filter is a mathematical optimum — given correct Q and R. In practice, Q and R are chosen by the engineer, and they are almost always wrong initially. A Q that is too small forces the filter to trust a model that drifts, causing systematic estimation error. An R that is too large causes the filter to ignore good data, limiting achievable accuracy. What makes this dangerous: mistuned noise models produce filter output that looks plausible but is subtly wrong. The filter continues to publish confident pose estimates. The robot continues to navigate — to the wrong places. Without a NEES consistency check, this can persist undetected through an entire field test session. Rule of thumb: tune Q and R on offline data with ground truth first, then deploy.

✗ Linearization error in the EKF is a structural flaw, not a tuning error

The EKF linearizes the nonlinear dynamics f(x) and observation h(x) at the current estimate. If the true nonlinearity is large — e.g., FollowBot is taking a sharp turn with high heading uncertainty — the first-order Taylor expansion is a poor approximation of the true posterior. The posterior is no longer Gaussian even if the prior and noise are. The EKF forces a Gaussian fit onto a non-Gaussian posterior, introducing systematic bias and underestimating covariance. For FollowBot's typical operating envelope (slow to moderate turns), this error is tolerable. For a fast-turning car or a robot navigating a tight switchback, the Unscented Kalman Filter (UKF) or particle filter may be necessary. The EKF's linearization error is not a tuning problem — it is an approximation whose magnitude grows with nonlinearity and uncertainty. Know when you've exceeded the regime.

✗ Non-Gaussian noise breaks the MMSE guarantee

The MMSE optimality proof assumes Gaussian process and measurement noise. Many real sensors violate this. A LIDAR returning specular reflections produces outliers that are not Gaussian — they're impulse noise at arbitrary ranges. IMU sensors have bias terms that grow over time (non-stationary, definitely not zero-mean Gaussian). GPS multipath produces bimodal position errors. When noise is non-Gaussian, the Kalman filter is still linear and unbiased, but it is no longer the minimum variance estimator. For heavy-tailed noise, consider robust variants (Huber loss in the update) or particle filters. For bimodal noise (multipath GPS during building entry), the particle filter with a sufficient number of particles is more appropriate than any Gaussian filter. Knowing when to leave the KF family is as important as knowing how to derive K.

KEY INSIGHTHow this connects

  • 4.2 · The Bayes Filter and the Predict-Update Cycle — the mathematical parent of the KF. The Kalman filter is the Bayes filter applied to linear Gaussian systems. The predict step corresponds to the Chapman-Kolmogorov equation; the update step to Bayes' theorem. The derivation in this lesson shows exactly which Gaussian-linearity assumptions make Bayes' update analytically tractable.
  • 4.4 · Kalman Filter Tuning: Q, R, and the Art of Honest Uncertainty — the practical follow-up. Given the derivation above, Lesson 4.4 develops systematic procedures for setting Q and R from physical sensor characterization data, using NEES and NIS tests to verify consistency, and diagnosing specific tuning failures from their filter signatures.

The Kalman filter is the analytical core of Stages 4 and 5. The EKF (Lesson 4.4) is the KF with linearization. SLAM (Stage 5) extends the KF to simultaneously estimate robot pose and map — the same five equations, with a state vector that grows as landmarks are observed. Understanding the derivation here means every subsequent extension is a recognizable variation on a familiar theme.

GLOSSARYKey terms introduced

Kalman gain (K)
The matrix weight applied to the innovation during the update step, derived by minimizing the trace of the posterior error covariance. In scalar form, K = P⁻/(P⁻ + R). It is uniquely determined by the noise models Q and R — not a free tuning parameter.
Innovation
The difference z − Hx̂⁻ between the actual measurement and the measurement predicted from the prior state estimate. Also called the "residual." When the innovation is zero, the measurement contains no new information. Persistently large innovations indicate model misfit or sensor bias.
Process noise covariance (Q)
The covariance matrix of the noise added to the state at each predict step. Encodes how much the dynamics model drifts from reality per time step. Increasing Q makes the filter trust measurements more (larger K). Must be set from physical characterization of model errors, not tuned arbitrarily.
Measurement noise covariance (R)
The covariance matrix of the sensor measurement noise. Encodes how noisy the sensor output is. Increasing R makes the filter trust the prediction more (smaller K). Should match the sensor's true noise statistics — over- or under-setting it breaks the MMSE optimality guarantee.
Posterior covariance (P)
The covariance matrix of the state estimation error after the update step. Represents the filter's claimed uncertainty. P is used to compute K and is published alongside the state estimate for downstream systems. An incorrect P is as problematic as an incorrect x̂.
Joseph form (of the covariance update)
P = (I − KH)P⁻(I − KH)ᵀ + KRKᵀ — the numerically stable form of the KF covariance update. Algebraically equivalent to P = (I − KH)P⁻ but preserves symmetry and positive semi-definiteness under floating-point arithmetic. Preferred in production implementations.
MMSE (Minimum Mean Squared Error)
The criterion used to derive the Kalman gain: find K such that the expected squared error E[‖x − x̂‖²] is minimized. The KF is the MMSE estimator for linear Gaussian systems — not approximately optimal, exactly optimal given the linearity and Gaussianity assumptions.
Filter consistency
The property that the filter's claimed covariance P accurately reflects the true estimation error distribution. A consistent filter has NEES ≈ n (state dimension) on average. Overconfident filters (NEES < n) underestimate uncertainty and assign too much weight to stale predictions; underconfident filters (NEES > n) are safe but waste available sensor information.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1What criterion is used to derive the Kalman gain K, and what does "minimizing the trace of the posterior covariance" mean physically?›

The Kalman gain is derived by minimizing the trace of the posterior error covariance P, which equals the sum of the posterior variances in each state dimension — i.e., the total mean squared estimation error. Minimizing trace(P) means choosing the update weight K that makes the posterior distribution as tight as possible around the true state, in the sense of minimizing expected squared error. It is the MMSE (minimum mean squared error) criterion. Physically: we want the filter to be as confident as the data allows — not overconfident (P too small), not underconfident (P too large). The derivation sets d/dK trace(P) = 0, yielding K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹ as the unique solution.

2A Kalman filter consistently produces large innovations (z − Hx̂⁻). What does this indicate, and where should you look first?›

Large persistent innovations indicate that the measurement is consistently far from what the prediction expected. Three possible root causes: (1) Model mismatch — the process model F does not accurately describe the system's dynamics, so the predicted state x̂⁻ is consistently wrong. Check F and the physics. (2) Sensor bias — the measurement z has a systematic offset not captured in R (which models zero-mean noise). A biased IMU, for example, produces large innovations in the heading channel even when the dynamics model is correct. (3) Wrong H — the observation matrix maps the state to the wrong measurement space. In EKF terms, the Jacobian of h(x) was computed incorrectly. Start by checking the innovation time series in each dimension separately — a large innovation in a single channel narrows the search to that sensor's observation model and noise characterization.

3Explain geometrically why the posterior mean in a 1D Kalman filter update always lies between the prior mean and the measurement.›

The update equation rearranges to x̂ = (1 − K)x̂⁻ + K·z — a convex combination of the prior mean x̂⁻ and the measurement z. Since K ∈ [0, 1] (because it equals P⁻/(P⁻ + R) where both terms are positive), the weights (1 − K) and K sum to 1 and are both non-negative. A convex combination of two points always lies on the line segment between them — geometrically, the posterior is between the prior and measurement. K determines the position along that segment: K = 0 means the posterior is at the prior (sensor completely ignored), K = 1 means the posterior is at the measurement (prior completely ignored), K = 0.5 means exactly halfway. Crucially, the posterior can never be pushed past the measurement by a correctly functioning KF — only a negative K could do that, which would indicate a nonsensical noise model (e.g., negative variance).

4Why is initializing P = 0 catastrophic, and what is the correct procedure for choosing the initial P?›

Initializing P = 0 tells the filter "I am perfectly certain of the initial state." After the first predict step, P⁻ = FPFᵀ + Q = Q (only the process noise contributes, since P = 0). The Kalman gain then immediately jumps to the steady-state value K = Q/(Q+R) — all future updates use this fixed gain, losing any adaptive trajectory memory. This is not permanent measurement blindness, but it means the filter cannot exploit covariance information accumulated during the trajectory; it always behaves as if it just started. If the initial state estimate x̂₀ is wrong (which it almost always is), the filter recovers only at the rate set by Q/(Q+R) rather than adapting its trust based on trajectory history. The correct initialization procedure: set P₀ = diag([σ₁², σ₂², ..., σₙ²]) where each σᵢ reflects your honest uncertainty about that state dimension. Be conservative — err on the side of larger P₀. An oversized P₀ causes the filter to converge aggressively from measurements in the first few steps and then settle correctly. An undersized P₀ (or P₀ = 0) means all future updates use the steady-state gain Q/(Q+R), which may be far from the correct gain for your operating trajectory.

Next: KF tuning — Q, R, and the art of honest uncertainty →