Operational navigation systems must estimate position, velocity, attitude, sensor biases, and clock errors all at the same time. They are coupled — accelerometer bias affects velocity through the mechanization, attitude error mis-projects gravity into the horizontal plane — so they must be estimated jointly.
\(\mathbf{x}_k = [p_{x,k},\, p_{y,k},\, v_{x,k},\, v_{y,k}]^\top\). Two position components and two velocity components, all in 2D Cartesian coordinates.
\(\displaystyle \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\). The off-diagonal \(\Delta t\) entries couple velocity into position — that is what lets a position measurement update the velocity estimate through the cross-covariance.
Real motion is not perfectly constant-velocity. Unknown accelerations along each axis inject uncertainty into the state. Modeling them as zero-mean white noise gives \(\mathbf{Q}\) a structured form with \(\Delta t^4 / 4\) on the position diagonal, \(\Delta t^2\) on velocity, and \(\Delta t^3 / 2\) on the off-diagonals. The off-diagonal terms reflect the kinematic coupling: an acceleration affects position and velocity together.
\(\mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}\). The first two rows of the identity, picking out position. \(\mathbf{H}\) is \(2\times 4\); \(\mathbf{R}\) is \(2\times 2\).
\(\hat{\mathbf{x}}_{k+1}^- = \mathbf{F}\,\hat{\mathbf{x}}_k^+\) and \(\mathbf{P}_{k+1}^- = \mathbf{F}\,\mathbf{P}_k^+\,\mathbf{F}^\top + \mathbf{Q}\). Same predict step as the scalar filter, with matrices everywhere.
\(\mathbf{K}_k = \mathbf{P}_k^-\,\mathbf{H}^\top\,\left(\mathbf{H}\,\mathbf{P}_k^-\,\mathbf{H}^\top + \mathbf{R}\right)^{-1}\). The denominator \(\mathbf{S}_k = \mathbf{H}\,\mathbf{P}_k^-\,\mathbf{H}^\top + \mathbf{R}\) is the innovation covariance, the multivariate analog of the scalar \(P^- + R\).
\(\hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k^- + \mathbf{K}_k\,(\mathbf{z}_k - \mathbf{H}\,\hat{\mathbf{x}}_k^-)\) and \(\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k\,\mathbf{H})\,\mathbf{P}_k^-\). Innovation in measurement space gets weighted by \(\mathbf{K}_k\) and added to the state estimate; covariance shrinks accordingly.
\(\mathbf{K}_k\) is \(4\times 2\). It maps measurement-space corrections into state-space updates: a 2D innovation becomes a 4-element state correction. That is how a 2D measurement updates all four state components simultaneously.
Cross-covariance between state errors. \(P_{ij}\) says how strongly an error in state \(i\) correlates with an error in state \(j\). They are how an observation of one state corrects others — the structural reason a position fix simultaneously refines velocity, and the reason a GPS update on a 15-state INS/GPS filter pulls the accelerometer biases into agreement with truth.
Block diagonal. The \(x\)-axis states \((p_x, v_x)\) form a \(2\times 2\) block; the \(y\)-axis states \((p_y, v_y)\) form another \(2\times 2\) block; the cross-axis terms are zero. Within each block the position and velocity covariance are coupled through \(\mathbf{F}\) and \(\mathbf{Q}\).
Two consecutive position fixes with \(\sigma_p = 5\) m give a differenced-position uncertainty of \(\sqrt{2}\,\sigma_p \approx 7.07\) m, which is the uncertainty on \(v\,\Delta t\). So \(\sigma_v \approx 7.07 / 10 \approx 0.71\) m/s. The actual filter does better because it averages many past measurements with optimal weights, but this is the right order of magnitude.
It adds 2D acceleration as a state, modeled as a first-order Gauss-Markov process: \(\dot{a} = -a/\tau_a + w\). The state becomes \([p_x, p_y, v_x, v_y, a_x, a_y]^\top\). Useful when the vehicle has slowly varying but non-zero acceleration: instead of letting the unknown accelerations sit inside \(\mathbf{Q}\), the filter explicitly tracks them as states with a known time constant, and gets a better estimate of position and velocity as a side benefit.
Position is not directly observable from velocity. A velocity measurement bounds the velocity error, but position is the integral of velocity, and the integration constant (initial position) is unobservable. Even with perfect velocity tracking, position uncertainty grows linearly with time. This is the canonical observability lesson: not every state is observable from every sensor.
\(\mathbf{x} = [\,x,y,z,\ v_x,v_y,v_z,\ \phi,\theta,\psi,\ b_{ax},b_{ay},b_{az},\ b_{gx},b_{gy},b_{gz}\,]^\top\): position, velocity, attitude, accelerometer biases, gyro biases. Each block is 3-dimensional, total 15 states. The filter equations are the same matrix equations as the 4-state filter, just with bigger matrices.
Dimensions only. Scalars become vectors, variances become covariance matrices, division becomes matrix inversion. The conceptual machinery is unchanged: predict via a motion model that grows uncertainty, update via inverse-covariance weighting against an aiding measurement, repeat. Every multi-state Kalman filter is the same five matrix equations.