The Kalman Filter is an optimal recursive estimator. It is designed to estimate the state of a dynamic system when the available measurements are noisy and the underlying physics are not perfectly predictable.
The filter treats every variable as a Gaussian Distribution (a bell curve).
The filter operates in two distinct ways on these Gaussians:
The filter operates in two distinct phases that repeat at every time step ($k$).
The filter uses its internal mathematical model to project the state into the future.
$$\hat{\mathbf{x}}_k^- = \mathbf{F}_{k-1} \hat{\mathbf{x}}_{k-1}^+ + \mathbf{G}_{k-1} \mathbf{u}_{k-1}$$
The transition matrix $\mathbf{F}_{k-1}$ defines how the state evolves from $k-1$ to $k$.
Uncertainty Growth:
$$\mathbf{P}_k^- = \mathbf{F}_{k-1} \mathbf{P}_{k-1}^+ \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1}$$
The uncertainty at $k-1$ is propagated through the dynamics and increased by the process noise $\mathbf{Q}_{k-1}$ encountered during the transition.
Key Insight: During prediction, uncertainty always increases because of the Process Noise ($\mathbf{Q}$). We are "guessing" what happened while we weren't looking.
This phase incorporates the new measurement $\mathbf{z}_k$ received at time $k$.
$$\mathbf{y}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-$$
The difference between the actual observation and the observation expected by the model $\mathbf{H}_k$.
$$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$$
The gain $\mathbf{K}_k$ is optimal for time $k$, balancing the current predicted uncertainty against the current sensor noise $\mathbf{R}_k$.
$$\hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \mathbf{y}_k$$
The final estimate for time $k$ is the weighted sum of the prediction and the measurement surprise.
$$\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-$$
The uncertainty is updated to reflect the gain in information from the measurement $\mathbf{z}_k$.
If the noise in your system follows a Gaussian (normal) distribution and your model is linear, the Kalman Filter is the Best Linear Unbiased Estimator (BLUE). It is mathematically impossible to do better with the information provided.
An estimator is unbiased if its expected error is zero — it does not systematically over- or under-estimate the true state. The Kalman Filter achieves this while also minimizing the variance of the estimation error, making it optimal in both accuracy and consistency.
This table summarizes the two-step recursive process used to estimate the state $\mathbf{x}$ at time $k$.
| Step | Phase | Mathematical Operation | Physical Intuition |
|---|---|---|---|
| 1 | State Predict | $\hat{\mathbf{x}}_k^- = \mathbf{F}_{k-1} \hat{\mathbf{x}}_{k-1}^+ + \mathbf{G}_{k-1} \mathbf{u}_{k-1}$ | Use physics to guess the new state based on the past. |
| 2 | Covariance Predict | $\mathbf{P}_k^- = \mathbf{F}_{k-1} \mathbf{P}_{k-1}^+ \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1}$ | Account for how uncertainty grows during movement. |
| 3 | Compute Gain | $\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$ | Determine the "trust ratio" between model and sensor. |
| 4 | State Update | $\hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-)$ | Correct the guess using the new measurement $\mathbf{z}_k$. |
| 5 | Covariance Update | $\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-$ | Shrink uncertainty now that we have more information. |
Mathematical Conventions Used: