Kalman filtering: from sequential fitting to vehicle tracking

A least-squares fit uses all observations at once. A Kalman filter updates a state estimate as each observation arrives. We connect the two approaches, then track a moving vehicle and compare filtering, smoothing and a batch fit.

Prerequisites: Gaussian errors, covariance matrices and Python/NumPy. Install projects/requirements.txt. Run from the repository root or this notebook directory; the setup cell locates the shared projects/src code. All observations are synthetic and reproducible.

Kalman Filters

A Kalman filter is a recursive mathematical algorithm used to estimate the state of a dynamic system from a series of noisy and potentially incomplete measurements. It operates in two main steps: prediction and update. In the prediction step, the filter uses a mathematical model of the system to forecast its next state. In the update step, it incorporates new measurements, weighing them according to their estimated reliability, to refine its estimate. This process repeats in real time, allowing the filter to continuously improve its estimate as new data arrives.

One of the key elements of the Kalman filter is the need of mathematical model to allow prediction and updates to the state. This model can be learned from data if one has an appropriate data sample.

The Kalman filter is especially valuable because it can estimate variables that are not directly observable, and it does so in a way that minimizes the mean squared error of the estimates. It is most effective for linear systems with Gaussian noise, but extensions such as the Extended Kalman Filter (EKF), Gaussian Sum Filter (GSF) and Unscented Kalman Filter (UKF) exist for nonlinear problems.

A simple example

To fit a polynomial using a Kalman filter, we will set up a system that estimates the parameters of the polynomial in real-time. Given a polynomial has the form:

$$ y = a_0 + a_1 \cdot x + a_2 \cdot x^2+ \cdots + a_n \cdot x^n $$

Here, $a_0$, $a_1, \cdots a_n$ are the parameters to estimate. We'll construct a Kalman filter to estimate these coefficients based on a series of noisy observations.

State-Space Representation

We need to represent the problem in a state-space form suitable for applying the Kalman filter.

1. State Vector $\mathbf{x} \in \mathbb{R}^{p} $: The state vector will consist of the polynomial coefficients:

   $$ \mathbf{x} = \begin{bmatrix} a_0 \\ a_1 \\ \vdots \\a_n \end{bmatrix} $$

2. State Transition Model: The state transition equation is defined to be: $$ x_{k+1} = F x_k + N( 0, Q_{k+1}) $$ where $x_k$ is the state vector at time $k$, $F$ is the transition matrix and $Q_k$ is the process noise covariance which defines the expected variability or uncertainty in the models dynamics.

   Since we are only estimating the coefficients of a polynomial that are not changing over time (static parameters), the state transition model can be considered as an identity (i.e., the state does not change):

   $$ \mathbf{x}_{k+1} = \mathbf{x}_k $$

   Therefore, the state transition matrix $\mathbf{F}$ is:

   $$ \mathbf{F} = \mathbf{I} $$

3. Observation Model: The observed data $y_k$ at a point $x_k$ can be modeled as:

   $$ y_k = a_0 + a_1 \cdot x_k + a_2 \cdot x_k^2+ \cdots + a_n \cdot x_k^n + v_k $$

   where $v_k$ is the measurement noise.

   This can be written in matrix form as:

   $$ y_k = \begin{bmatrix} 1 & x_k & \cdots & x_k^n \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ \vdots \\ a_n \end{bmatrix} + v_k $$

   Therefore, the observation matrix $\mathbf{H}_k$ is:

   $$ \mathbf{H}_k = \begin{bmatrix} 1 & x_k & \cdots & x_k^n \end{bmatrix} $$

Kalman Filter Equations

The Kalman filter consists of two main steps: prediction and update.

1. Prediction Step

$$ \mathbf{x}_{k|k-1} = \mathbf{F} \cdot \mathbf{x}_{k-1|k-1} $$

$$ \mathbf{P}_{k|k-1} = \mathbf{F} \cdot \mathbf{P}_{k-1|k-1} \cdot \mathbf{F}^\top + \mathbf{Q} $$

where:

• $\mathbf{x}_{k|k-1} \in \mathbb{R}^{p}$ is the predicted state.
• $\mathbf{P}_{k|k-1} \in \mathbb{R}^{p \times p}$ is the predicted state covariance.
• $\mathbf{Q} \in \mathbb{R}^{p \times p}$ is the process noise covariance.

2. Update Step

$$ \mathbf{r}_k = z_k - \mathbf{H}_k \cdot \mathbf{x}_{k|k-1} $$

$$ \mathbf{S}_k = \mathbf{H}_k \cdot \mathbf{P}_{k|k-1} \cdot \mathbf{H}_k^\top + \mathbf{R_k} $$

$$ \mathbf{K}_k = \mathbf{P}_{k|k-1} \cdot \mathbf{H}_k^\top \cdot \mathbf{S}_k^{-1} $$

$$ \mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + \mathbf{K}_k \cdot \mathbf{r}_k $$

$$ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \cdot \mathbf{H}_k) \cdot \mathbf{P}_{k|k-1} $$

where:

• $\mathbf{r}_k \in \mathbb{R}^{m}:$ is the innovation or measurement residual.
• $\mathbf{S}_k \in \mathbb{R}^{m \times m}:$ is the innovation covariance.
• $\mathbf{K}_k \in \mathbb{R}^{p \times m}:$ is the Kalman gain.
• $\mathbf{R_k} \in \mathbb{R}^{m \times m}:$ is the measurement noise covariance.

It should be noted that the Joseph form of the covariance matrix can be used for better numerical stability: $$ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \cdot \mathbf{H}_k) \cdot \mathbf{P}_{k|k-1} \cdot (\mathbf{I} - \mathbf{K}_k \cdot \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T $$

which can be shown to be equivalent to the normal form by first expanding the first term:

$$ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} - (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T $$ Substituting in $\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}$: $$ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} - (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1} \mathbf{R}_k \mathbf{S}_k^{-1} \mathbf{H}_k \mathbf{P}_{k|k-1} $$

• The term $(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} \mathbf{H}_k^T$ simplifies to $\mathbf{K}_k \mathbf{R}_k$.
• The cross terms involving $\mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T$ and $\mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T$ cancel out due to the optimal gain definition.

After cancellation, the Joseph form reduces to:

$$ \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}, $$

matching the compact covariance update in exact arithmetic. The implementation uses the Joseph form

Python Implementation

Here is a simple Python implementation of a Kalman filter to fit a polynomial:

Explanation of the Code

1. Initialization:

   • For fixed polynomial coefficients we use zero process noise; the measurement covariance comes from the supplied uncertainties.
   • A zero initial mean with a large finite covariance expresses a diffuse prior over the coefficients, rather than an absence of prior information.

2. Prediction and Update Steps:

   • For each observed data point $(x_k, y_k)$, we perform the prediction step to estimate the next state.
   • We then construct the observation matrix $H_k$ based on the current input $x_k$ and perform the update step using the noisy measurement $y_k$.

3. Output:

   • The filter updates its estimate of the polynomial coefficients with each new data point. The estimates should converge towards the true coefficients as more data is processed.

This approach demonstrates how the Kalman filter can be used for online, real-time estimation of parameters in a polynomial model, which is particularly useful in applications where data is arriving sequentially, and we want to update our estimates dynamically.

Prediction and update equations

For a linear model, the predicted state and covariance are $$x^-_k=F x^+_{k-1},\qquad P^-_k=F P^+_{k-1}F^T+Q.$$ With innovation $r_k=z_k-H_kx^-_k$ and covariance $S_k=H_kP^-_kH_k^T+R_k$, the gain and update are $$K_k=P^-_kH_k^TS_k^{-1},\qquad x^+_k=x^-_k+K_kr_k.$$ The shared implementation uses the Joseph covariance update $P^+=(I-KH)P^-(I-KH)^T+KRK^T$ to limit numerical asymmetry. The state classes define the observation matrix; the filter handles the update.

A small exact-line check

With zero process noise, the polynomial coefficients are fixed. Repeated measurements should recover a known line. We also check the RTS smoother, which uses future observations to refine earlier states without counting any measurement twice. The filter is configured for ordered, fixed-step observations.

The last smoothed estimate equals the last filtered estimate; earlier estimates can use the remaining observations.

Run Kalman Filter

Plotting the results with Plotly

Tracking position and velocity

Now the state is $x=[s,v]^T$, not a pair of polynomial coefficients. At fixed interval $\Delta t$, $F=\begin{pmatrix}1&\Delta t\0&1\end{pmatrix}$ and $H=[1,0]$: only position is observed. We use integrated white acceleration with spectral density $q$, $$Q=q\begin{pmatrix}\Delta t^3/3&\Delta t^2/2\\Delta t^2/2&\Delta t\end{pmatrix}.$$ The same $F,Q$ generate the truth and drive the filter, so any differences are estimation errors rather than an inconsistent simulation. The prior describes the state at the first observation time, before seeing that observation.

Reading the vehicle model step by step

Here the state components have physical meanings: $s$ is position and $v$ is velocity. Over an interval $\Delta t$, the mean prediction is $$\begin{pmatrix}s_{k+1}^-\\v_{k+1}^-\end{pmatrix} =\begin{pmatrix}1&\Delta t\\0&1\end{pmatrix} \begin{pmatrix}s_k^+\\v_k^+\end{pmatrix}.$$ Thus the predicted position advances by $v_k\Delta t$, while the predicted mean velocity stays the same. Random acceleration produces departures from that mean; its integrated covariance is the $Q$ given above. Position and velocity process errors are correlated, which is why $Q$ has off-diagonal entries.

Only position is measured in this example: $$z_k=H x_k+\eta_k,\qquad H=[1\;0],\qquad \eta_k\sim\mathcal N(0,R).$$ Velocity is inferred from successive positions and their dynamical relationship, rather than from a separate velocity sensor. The prediction carries the whole state covariance forward, including its position–velocity correlation: $$P_k^-=F P_{k-1}^+F^T+Q.$$

The innovation compares the new reading with its predicted position, $$r_k=z_k-Hx_k^-,\qquad S_k=HP_k^-H^T+R.$$ The gain $K_k=P_k^-H^TS_k^{-1}$ has one entry for each state component, so a single position innovation can update both position and velocity: $$x_k^+=x_k^-+K_kr_k.$$ The velocity update is possible because prediction develops a correlation between the two state components. The Joseph covariance update then accounts for the information supplied by that measurement.

A larger $R$ expresses less trust in the sensor. A larger process-noise strength expresses less confidence in the constant-velocity prediction. These assumptions affect both the responsiveness and reported uncertainty. A finite initial covariance is also prior information, even when the initial mean is zero.

Batch fit with process constraints

For the same linear Gaussian model, filtering followed by RTS smoothing is another way to solve one global quadratic problem: $$\chi^2=\|x_0-m_0\|_{P_0^{-1}}^2+ \sum_k\|z_k-Hx_k\|_{R^{-1}}^2+ \sum_{k=1}^{N-1}\|x_k-Fx_{k-1}\|_{Q^{-1}}^2.$$ Here $\|u\|_W^2=u^TWu$. We whiten each residual block with a Cholesky solve, then solve the stacked least-squares system. This is a process-noise example; a detector scattering model would need its own propagation and covariance. The first observation is at the prior time, so no extra propagation is added.

Constructing the global least-squares system

Stack all states into one vector $$u=[x_0^T,x_1^T,\ldots,x_{N-1}^T]^T.$$ There are two unknowns per time in this example. The global fit combines three types of residual, each with its own covariance:

1. The prior row block selects $x_0$, with target $m_0$ and covariance $P_0$.
2. A measurement row places $H$ in the columns for $x_k$, with target $z_k$ and covariance $R$.
3. A process row block places $-F$ in the columns for $x_{k-1}$ and $I$ in those for $x_k$, with target zero and covariance $Q$.

For any block $Bu-b$ with covariance $C=LL^T$, whitening gives $$\widetilde B=L^{-1}B,\qquad\widetilde b=L^{-1}b.$$ Its squared Euclidean residual is exactly the corresponding covariance-weighted term in $\chi^2$. Stacking the whitened blocks produces one ordinary least-squares problem, $$\widehat u=\operatorname*{argmin}_u\|Au-b\|^2.$$ The implementation uses a least-squares solve for the mean. Under these linear Gaussian assumptions the full state covariance is $(A^TA)^{-1}$; its diagonal $2\times2$ blocks can be compared with the RTS marginal covariances.

The process residual $x_k-Fx_{k-1}$ represents the inferred random change between times. It is penalised according to $Q$, rather than forced to zero. A detector model with explicit scattering kicks would instead need its own kick variables, propagation matrices and constraints. Here the batch fit deliberately uses the same vehicle model as the filter and smoother, making their comparison meaningful.

Interpretation and exercise

The filter only uses observations available at that time. The smoother uses the whole sequence and improves earlier estimates; the final estimate is unchanged. The batch comparison checks both the means and uncertainties independently. A single simulated sequence does not guarantee a particular RMSE ordering.

Exercise: increase measurement variance by a factor of four and regenerate the observations with twice the noise standard deviation. Predict what will happen to the uncertainty band. Keep the simulated noise and filter covariance consistent.

Interactive experiment: tune the sequential estimate

Vary the assumed measurement noise and process noise to explore how the sequential estimate responds. Compare its responsiveness and uncertainty; the controls change the estimation assumptions as well as the sample size.

The controls use a separate, seeded synthetic example and run in your browser in the exported HTML article. Some notebook viewers do not execute embedded JavaScript; run scripts/check_notebooks.py --html and open the article preview to use the controls.

Conclusion

The static-coefficient example connects sequential estimation to batch fitting: with zero process noise and a diffuse prior, the final Kalman estimate closely matches WLS. For the dynamical example, the stronger check is that RTS smoothing and the global batch fit agree to numerical tolerance for both the state means and their marginal covariances, using the same prior, observations and process constraints.

On the simulated vehicle sequence, position RMSE falls from $0.778$ m for the measurements to $0.530$ m after filtering and $0.330$ m after smoothing. The filter uses observations available up to each time; the smoother also uses later observations to refine earlier states. Its final state is unchanged. The lower RMSE is a result for this seeded sequence, rather than a guarantee for every realisation.

The uncertainty calculation depends on the assumed dynamics and noise. Increasing the stated measurement variance widens the final position uncertainty in the exercise, but a covariance band is only as trustworthy as those assumptions. This example generates the truth with the same linear Gaussian process model used by the estimator. Before applying it to data, check innovations and their predicted covariance, and test sensitivity to the prior and noise choices. The shared implementation uses fixed $F,Q$ and ordered observation times; irregular sampling requires propagation and process covariance matched to each time interval.

For the original prediction/update formulation, see R. E. Kalman (1960), A New Approach to Linear Filtering and Prediction Problems.