4-State Kalman Filter — 2D Constant-Velocity Tracking

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:

\[\begin{split} \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}. \end{split}\]

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:

\[\begin{split} \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. \end{split}\]

The optional velocity sensor measures the last two:

\[\begin{split} \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. \end{split}\]

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.

Contents