# 4-State Kalman Filter — 2D Constant-Velocity Tracking
This demo runs the canonical 4-state Kalman filter on a 2D constant-velocity scenario — the cleanest non-trivial example of multi-state estimation. A vehicle moves in 2D under random acceleration disturbances; a noisy 2D position sensor delivers fixes every few seconds; an optional velocity sensor can be turned on to compare observability behavior. The filter carries position and velocity together as one state vector, propagates the full $4 \times 4$ covariance, and the trajectory view above the time-series panels shows the 95% covariance ellipse moving and reshaping live.
## The state, dynamics, and measurement model
The state vector and constant-velocity dynamics:
$$
\mathbf{x} = \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}, \qquad
\mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \qquad
\mathbf{Q} = \sigma_a^2
\begin{bmatrix}
\Delta t^4/4 & 0 & \Delta t^3/2 & 0 \\
0 & \Delta t^4/4 & 0 & \Delta t^3/2 \\
\Delta t^3/2 & 0 & \Delta t^2 & 0 \\
0 & \Delta t^3/2 & 0 & \Delta t^2
\end{bmatrix}.
$$
The off-diagonal $\Delta t^3/2$ terms in $\mathbf{Q}$ are what couple position and velocity errors. They are why a position-only measurement still produces a useful velocity estimate — the cross-covariance carries the correction across.
The position sensor measures the first two states directly:
$$
\mathbf{H}_\text{pos} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}, \qquad
\mathbf{R}_\text{pos} = \sigma_\text{pos}^2 \mathbf{I}_2.
$$
The optional velocity sensor measures the last two:
$$
\mathbf{H}_\text{vel} = \begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \qquad
\mathbf{R}_\text{vel} = \sigma_\text{vel}^2 \mathbf{I}_2.
$$
## Interactive demo
Open in full screen
The demo simulates the full scenario deterministically (given parameters and seed) and animates a "now" cursor through the run at the speed you select. The **live trajectory panel** at the top is a 2D top-down view auto-scrolled to keep the estimate centered. Cyan path = truth, navy path = KF estimate; the navy diamond is the current estimate position; the red dashed ellipse is the current 95% position-covariance ellipse. Update flashes in the corner indicate when a position or velocity measurement arrives. The four error panels below carry a synchronized vertical "now" cursor matched to the trajectory view.
The two $\sigma_a$ sliders are deliberately separate: $\sigma_a$ (process) controls the *truth-side* random acceleration, while $\sigma_a$ (model Q) is the *filter's tuning* of $\mathbf{Q}$. With them equal, the filter is matched to the world; setting them unequal exposes the classic Q-tuning lesson.
## Walkthrough
The demo opens at the canonical Block 5 scenario and starts playing automatically: $v_{x,0} = 5$ m/s, $v_{y,0} = 3$ m/s, both $\sigma_a$ sliders at 2 m/s² (matched), $\sigma_\text{pos} = 10$ m at 10 s intervals, velocity sensor off, $T = 300$ s. Try the following:
1. **Watch the first ~30 seconds.** The covariance ellipse starts roughly circular (initial uncertainty $\sigma_p = 10$ m, identical in $x$ and $y$). After a few position updates the ellipse shrinks and elongates along the velocity direction — that elongation is the cross-covariance: the filter's position uncertainty in the *direction of motion* is highest because that's where past velocity error accumulates fastest.
2. **Read the cross-correlation stat.** The "ρ(p, v)" values in the bottom-right card are the position-velocity correlations. They climb from 0 to roughly 0.7+ during the first few updates. **That correlation is the structural reason a position-only sensor teaches velocity** — when the position update corrects $p_x$, the cross-covariance pulls $v_x$ along with it.
3. **Toggle the velocity sensor on.** Velocity bounds tighten dramatically; the ρ values shift because both channels are now directly observed; the steady-state ellipse on the trajectory shrinks. With both sensors active, the filter has redundant information.
4. **Disable position, keep velocity.** Velocity remains well-tracked but the position channel has no absolute reference — it integrates velocity-noise-corrected estimates indefinitely. The covariance ellipse on the trajectory panel grows continuously, and the position error in the per-state panel walks off the bounds. **This is the canonical observability lesson:** velocity-only updates cannot pin position down absolutely.
5. **Q-tuning experiment, conservative side.** Re-enable position, and slide $\sigma_a$ (model Q) up to 5 m/s² while leaving $\sigma_a$ (process) at 2 m/s². The filter now believes the truth is more turbulent than it actually is. **Bounds widen but the error doesn't grow** — the filter is over-conservative, leaving sensor information on the table.
6. **Q-tuning experiment, dangerous side.** Slide $\sigma_a$ (model Q) down to 0.5 m/s² while $\sigma_a$ (process) stays at 2 m/s². The filter now believes the truth is smoother than it really is. The bounds visibly tighten, but the actual error escapes them: **a tight 95% bound that doesn't contain the truth 95% of the time** is the operationally most dangerous Kalman tuning failure. *Confidence interval ≠ accuracy* — the filter is overconfident.
7. **Pause and scrub.** Drag the time slider back to a moment of interest. The cov ellipse, all four error-panel cursors, and the stats card all snap to that time so you can inspect filter state at any point.
## Key observations
- **A 4×2 Kalman gain maps measurement-space corrections back into the full state space.** The position innovation is a 2-vector, the gain matrix is 4×2, and the result is a 4-vector update — the *velocity* row of the gain is non-zero precisely because $\mathbf{P}^-$ has a non-zero $p$-$v$ off-diagonal.
- **Steady-state ellipse orientation tracks velocity.** Once the filter is in steady state, the ellipse's major axis aligns with the direction of motion. Try setting $v_{x,0}$ negative or zero and watch the orientation rotate. Cross-covariance is anisotropic; observability isn't.
- **Filter consistency is checked by bound coverage.** When $\sigma_a$ (model) = $\sigma_a$ (process), the actual error stays inside the 95% bounds about 95% of the time. Any persistent escape means the filter is overconfident; any persistent under-coverage means it's over-conservative. Both are tuning failures, but only the first is dangerous.
- **The bounds at any single update aren't the whole story.** The first few updates do most of the covariance shrinking; after that the filter is at steady state and the ellipse essentially keeps the same shape between updates (growing slightly under $\mathbf{Q}\Delta t$ each step, snapping back at each fix).
- **Same recursion as the scalar filter.** Compare the predict-update equations here with Block 4. Every quantity has gained dimensions, but the structure is identical: gain is inverse-variance weighting, innovation is the new information, posterior covariance is smaller than prior.
## Source
MATLAB · code/KalmanFilter4D.m↓
With the 2D covariance ellipse helper in `code/navutils/Draw2DErrorBounds.m`.