6-State Kalman Filter — Observability of P, V, A

6-State Kalman Filter — Observability of P, V, A#

This demo extends the 4-state filter from one block earlier with an explicit acceleration channel, modeled as a first-order Gauss-Markov process (recall Block 2). The resulting state has six components: 2D position, 2D velocity, 2D acceleration. The headline lesson here is observability — which states each measurement type can pin down, and which it cannot. With three sensor toggles (position, velocity, acceleration) you can compose any subset and watch the filter behave accordingly:

  • Position fixes everything “downhill”. A position update teaches the filter about velocity (through the same cross-covariance mechanism as the 4D demo) and about acceleration (through the second integration). All six states stay bounded.

  • Velocity fixes only velocity and acceleration. No matter how good the velocity sensor is, the filter has no way to determine absolute position from a velocity-only stream. Position drifts and its covariance grows without bound.

  • Acceleration fixes only acceleration. The integration chain only runs one direction; an acceleration update has nothing to feed back into the velocity or position channels. Both drift.

The state and dynamics#

The state vector is

\[ \mathbf{x} = \begin{bmatrix} p_x & p_y & v_x & v_y & a_x & a_y \end{bmatrix}^{\top}. \]

Per axis, the dynamics are constant-acceleration kinematics with a Gauss-Markov decay on the acceleration:

\[\begin{split} \begin{aligned} p_{k+1} &= p_k + v_k\,\Delta t + \tfrac{1}{2}\,a_k\,\Delta t^2, \\ v_{k+1} &= v_k + a_k\,\Delta t, \\ a_{k+1} &= \rho\,a_k + w_k, \qquad \rho = e^{-\Delta t / \tau_a}. \end{aligned} \end{split}\]

Stacked across both axes, \(\mathbf{F}\) is a 6×6 block matrix. Process noise enters only on the two acceleration channels (the kinematic chain itself is treated as exact at this scale), so \(\mathbf{Q}_d\) is essentially \(\mathrm{diag}(0,0,0,0, \sigma_a^2(1-\rho^2), \sigma_a^2(1-\rho^2))\). The off-diagonal coupling that lets a position measurement teach the filter about velocity and acceleration is carried entirely by \(\mathbf{F}\) and the way it propagates \(\mathbf{P}\) forward.

The three available measurements are linear in the state:

\[ \mathbf{H}_\text{pos} = \begin{bmatrix} \mathbf{I}_2 & \mathbf{0} & \mathbf{0} \end{bmatrix}, \quad \mathbf{H}_\text{vel} = \begin{bmatrix} \mathbf{0} & \mathbf{I}_2 & \mathbf{0} \end{bmatrix}, \quad \mathbf{H}_\text{acc} = \begin{bmatrix} \mathbf{0} & \mathbf{0} & \mathbf{I}_2 \end{bmatrix}. \]

Interactive demo#

Open in full screen

The layout matches the 4D demo: live 2D top-down trajectory at the top with an auto-scrolled ±200 m window and a 95% position-covariance ellipse, three grouped error panels below (position, velocity, acceleration — both x and y channels overlaid in blue and orange), and a synchronized “now” cursor across all panels. Three sensor toggles on the right let you switch between sensor combinations to expose the observability story.

The two \(\sigma_a\) sliders are deliberately separate — \(\sigma_a\) (process) on the truth side and \(\sigma_a\) (model Q) on the filter side — same Q-tuning split as the 4D demo. The shared \(\tau_a\) slider controls the acceleration correlation time for both truth and filter together.

Walkthrough#

The demo opens at a default scenario with the position sensor on, others off, \(\tau_a = 60\) s, \(\sigma_a = 1\) m/s² for both truth and filter, \(T = 180\) s, speed 4×. Try the following:

  1. Watch the default — position-only. All three error panels (position, velocity, acceleration) settle into bounded steady-state behavior. The covariance ellipse on the trajectory plot stabilizes into a small, slightly-elongated shape. Position is observed directly; velocity and acceleration are estimated from the cross-covariance that \(\mathbf{F}\) injects step by step. Position fixes everything downhill.

  2. Toggle position OFF, velocity ON. The position error panel drifts away from zero and its bounds widen continuously — there is no measurement that can absolutely pin down position. The trajectory view shows the cov ellipse balloon along the direction of motion. Meanwhile, the velocity panel stays bounded (directly measured) and the acceleration panel also stays bounded (caught by the cross-covariance from the velocity updates). Velocity fixes only velocity and acceleration.

  3. Toggle velocity OFF, acceleration ON. Now even velocity walks off, alongside position. Only the acceleration panel stays bounded. The trajectory view diverges fast — the cov ellipse balloons in all directions. Acceleration fixes only acceleration. The integration chain is one-way; observation at the bottom of the chain cannot constrain anything above.

  4. Try pos + vel together. Velocity bounds visibly tighten compared to position-only. Position bounds tighten slightly too. Same total information goes further when split across two complementary sensors.

  5. Try pos + acc. Now the acceleration estimate is locked down by direct measurements rather than inferred. Position and velocity bounds stay tight via the position fixes. This is closer to a real INS+GPS architecture, where the IMU measures specific force directly and GPS pins absolute position.

  6. Slide \(\tau_a\) up and down. Long \(\tau_a\) (300 s) means acceleration is essentially constant over the run; the filter’s \(\mathbf{Q}_d\) is small and bounds are tight. Short \(\tau_a\) (5 s) means acceleration looks like white noise; the filter has to inflate \(\mathbf{Q}_d\) and bounds are wider. Same \(\sigma_a\), different memory length.

  7. Q-tuning experiment. Slide \(\sigma_a\) (model Q) to 0.5 m/s² while \(\sigma_a\) (process) stays at 1 m/s² — the filter believes the world is smoother than it actually is. Bounds tighten visibly but the actual error escapes them. Same overconfidence pattern as the 4D demo, just one state-vector layer deeper.

  8. Pause and scrub. Drag the time slider; the cov ellipse, all three error-panel cursors, and the stats card update in lockstep so you can inspect filter state at any moment.

Key observations#

  • Observability runs downhill, not uphill. Position is the deepest integral in the chain. Information injected at any level reaches everything below it (downhill toward acceleration) but cannot reach anything above. This is a structural property of the constant-acceleration model, not of the Kalman filter — the filter just inherits whatever observability the dynamics allow.

  • The 95% bounds on the trajectory ellipse make divergence visible quickly. When you turn off the position sensor, the cov ellipse starts to grow on every prediction step and never gets re-tightened. That visible inflation is the structural failure of velocity-only or acceleration-only architectures for absolute positioning.

  • Bounds for x and y are identical by symmetry. The error panels overlay both channels so you can see the realizations differ (different random samples) but the dashed red bound is shared — Pxx = Pyy at every step because the model treats the two axes symmetrically.

  • Why operational systems look the way they do. A standalone IMU (acceleration-only, essentially) cannot navigate forever — drift grows in both velocity and position. A GPS+IMU pair pins absolute position with rare GPS fixes while letting the IMU fill in fast attitude/velocity dynamics between fixes. This demo is the reduced-dimension prototype of that architecture.

  • Same recursion as 4D. The predict-update equations are unchanged from the 4-state demo; only the matrices grew. Every Kalman lesson in this course works the same way at any state-vector size.

Source#

MATLAB · code/KalmanFilter6D.m↓

With the 2D covariance ellipse helper in code/navutils/Draw2DErrorBounds.m and the Van Loan discretization helper in code/navutils/CalculateQdVanLoan.m. The MATLAB version uses the continuous-time formulation \(\dot{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{G}\mathbf{w}\) with Van Loan to compute \(\mathbf{Q}_d\); the demo above takes the equivalent shortcut of writing the discrete-time matrices directly, which is exact for the Gauss-Markov component and an excellent approximation for the kinematic chain at \(\tau_a \gg \Delta t\).

Contents