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.
🎯 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 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:
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:
# 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.
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:
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.
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
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).
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)⁻¹
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.
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:
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.
Three limiting cases reveal the filter's logic:
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.
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.
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.
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:
- 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.
- 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.
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:
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:
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.
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.
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.
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."
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
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.
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.
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.