Tracking a turning object: choosing the state model
Position measurements do not directly reveal velocity or acceleration. We compare three linear state models on a noisy circular path, then use an extended Kalman filter (EKF) whose state explicitly describes a circle. The point is to expose model assumptions, rather than find a universal winner.
Prerequisites: the prediction/update equations in KalmanFilter.ipynb, NumPy
and covariance matrices. Install projects/requirements.txt and run
all cells in order. No external data is required.
Show or hide code
from pathlib import Path
import sys
# Works from the repository root, projects/, or projects/notebooks/.
project_dir = next(p / "projects" for p in [Path.cwd(), *Path.cwd().parents]
if (p / "projects" / "src").is_dir())
sys.path.insert(0, str(project_dir / "src"))
import logging
import numpy as np
import plotly.graph_objects as go
import plotly.io as pio
from Measurement import Measurement, generate_data
from ModelState import ModelState, ModelStatePoly1
from KalmanFilter import KalmanFilter
logging.getLogger().setLevel(logging.WARNING)
pio.renderers.default = "notebook_connected"
np.random.seed(1234)
A reproducible circular trajectory
We observe two Cartesian coordinates every $\Delta t=0.1$ s with independent Gaussian noise of standard deviation $0.15$ m. The true orbit has radius 2 m and angular velocity 0.7 rad/s. These are simulated observations, not detector data.
Show or hide code
rng = np.random.default_rng(2026)
delta_t, nmeasurements = 0.1, 120
times = np.arange(nmeasurements) * delta_t
radius, omega = 2., 0.7
true_positions = radius * np.column_stack([np.cos(omega*times), np.sin(omega*times)])
true_velocities = radius*omega * np.column_stack([-np.sin(omega*times), np.cos(omega*times)])
true_accelerations = -omega**2 * true_positions
sigma = 0.15
R = np.eye(2)*sigma**2
observed = true_positions + rng.normal(0, sigma, true_positions.shape)
measurements = [Measurement(z[:, None], np.array([t]), R.copy()) for t, z in zip(times, observed)]
Position, velocity and acceleration models
Use state order $[x,y]$, $[x,y,v_x,v_y]$, or $[x,y,v_x,v_y,a_x,a_y]$. Only the first two entries are observed. The transition matrices apply constant position, velocity, or acceleration over one interval. None describes circular motion exactly: the process covariance allows departures from the approximation. We use integrated white driving noise in each model, with different physical units and fixed illustrative strengths. This is not an optimisation of each model's hyperparameters.
Show or hide code
class CartesianState(ModelState):
"""Observe the first two entries of a Cartesian state column."""
def calc_H(self):
return np.eye(2, len(self.x))
def cartesian_model(order, dt, spectral_density):
# One-dimensional transition and continuous-white-noise covariance.
if order == 0:
F1 = np.ones((1, 1)); Q1 = np.array([[dt]])
elif order == 1:
F1 = np.array([[1., dt], [0., 1.]])
Q1 = np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])
elif order == 2:
F1 = np.array([[1., dt, dt**2/2], [0., 1., dt], [0., 0., 1.]])
Q1 = np.array([[dt**5/20, dt**4/8, dt**3/6],
[dt**4/8, dt**3/3, dt**2/2], [dt**3/6, dt**2/2, dt]])
else:
raise ValueError('Supported orders: 0, 1, 2')
return np.kron(F1, np.eye(2)), spectral_density*np.kron(Q1, np.eye(2))
models = {}
for label, order, q in [('Position', 0, 1.), ('Velocity', 1, 0.5), ('Acceleration', 2, 0.5)]:
F, Q = cartesian_model(order, delta_t, q)
mean = np.zeros((len(F), 1)); mean[:2, 0] = [2., 0.]
initial = CartesianState(mean, np.array([0.]), np.eye(len(F))*4.)
kf = KalmanFilter(initial, Q, F)
states = kf.fit(measurements)[1:]
models[label] = states
for state in states:
np.testing.assert_allclose(state.x_cov, state.x_cov.T, atol=1e-10)
assert np.linalg.eigvalsh(state.x_cov).min() >= -1e-10
Compare trajectories and position uncertainty
The ellipse is a joint 95% region for a two-dimensional Gaussian state: its squared Mahalanobis radius is $\chi^2_2(0.95)$, not $2^2$. Measurement error bars and estimated-state ellipses describe different quantities.
Show or hide code
from scipy.stats import chi2
def covariance_ellipse(mean, covariance, probability=0.95):
values, vectors = np.linalg.eigh(covariance)
angle = np.linspace(0, 2*np.pi, 80)
unit = np.array([np.cos(angle), np.sin(angle)])
return (np.asarray(mean)[:, None] + vectors @ (np.sqrt(chi2.ppf(probability, 2)*np.maximum(values, 0))[:, None]*unit)).T
fig = go.Figure()
fig.add_scatter(x=true_positions[:, 0], y=true_positions[:, 1], name='Truth')
fig.add_scatter(x=observed[:, 0], y=observed[:, 1], mode='markers', marker=dict(size=4, opacity=0.4), name='Measurements')
for label, states in models.items():
means = np.array([s.x[:2, 0] for s in states])
fig.add_scatter(x=means[:, 0], y=means[:, 1], name=label)
ellipse = covariance_ellipse(means[-1], states[-1].x_cov[:2, :2])
fig.add_scatter(x=ellipse[:, 0], y=ellipse[:, 1], name=label+' final 95% region', showlegend=False)
fig.update_layout(title='The state model changes how a turning path is followed', xaxis_title='x [m]', yaxis_title='y [m]')
fig.update_yaxes(scaleanchor='x', scaleratio=1)
fig.show()
A nonlinear circular state
Let $c=[c_x,c_y,r,\phi,\omega]^T$. Prediction advances $\phi\leftarrow\phi+\omega\Delta t$, while the observation is $$h(c)=\begin{pmatrix}c_x+r\cos\phi\\c_y+r\sin\phi\end{pmatrix}.$$ The EKF linearises this observation at the predicted state. We initialise near the true orbit to illustrate local estimation; inferring a circle from arbitrary initial conditions is a separate problem. A linear observation matrix cannot represent the sine and cosine exactly.
Show or hide code
def circle_observation(c):
cx, cy, r, phi, w = c
value = np.array([cx+r*np.cos(phi), cy+r*np.sin(phi)])
H = np.array([[1., 0., np.cos(phi), -r*np.sin(phi), 0.],
[0., 1., np.sin(phi), r*np.cos(phi), 0.]])
return value, H
# Check the analytic Jacobian against finite differences before using it.
c_test = np.array([0.1, -0.2, 1.8, 0.4, 0.6])
eps = 1e-6
numerical = np.column_stack([(circle_observation(c_test+eps*e)[0]-circle_observation(c_test-eps*e)[0])/(2*eps)
for e in np.eye(5)])
np.testing.assert_allclose(circle_observation(c_test)[1], numerical, atol=1e-8)
c = np.array([0.1, -0.1, 1.8, 0.1, 0.6])
P = np.diag([0.25, 0.25, 0.25, 0.1, 0.04])
F = np.eye(5); F[3, 4] = delta_t
Q_circle = np.diag([1e-7, 1e-7, 1e-7, 1e-5, 1e-6])
circle_states, circle_positions, circle_covariances = [], [], []
for k, z in enumerate(observed):
if k:
c = F @ c; P = F @ P @ F.T + Q_circle
prediction, H = circle_observation(c)
K = np.linalg.solve(H @ P @ H.T + R, H @ P).T
c = c + K @ (z-prediction)
J = np.eye(5)-K @ H
P = J @ P @ J.T + K @ R @ K.T
P = (P+P.T)/2
assert c[2] > 0, 'This local parameterisation requires positive radius.'
assert np.linalg.eigvalsh(P).min() >= -1e-10
xy, H_post = circle_observation(c)
circle_states.append(c.copy()); circle_positions.append(xy)
circle_covariances.append(H_post @ P @ H_post.T)
circle_positions = np.array(circle_positions)
print('Final [cx, cy, radius, phase, angular velocity]:', np.round(c, 4))
Show or hide code
from plotly.subplots import make_subplots
fig_error = make_subplots(rows=2, cols=1, shared_xaxes=True,
subplot_titles=['Position error', 'Final comparison'])
for label, states in models.items():
xy = np.array([s.x[:2, 0] for s in states])
err = np.linalg.norm(xy-true_positions, axis=1)
fig_error.add_scatter(x=times, y=err, name=label, row=1, col=1)
# Skip burn-in for the summary; report the same range for every model.
print(f'{label:14s} RMSE after 2 s: {np.sqrt(np.mean((xy[20:]-true_positions[20:])**2)):.4f} m per coordinate')
fig_error.add_scatter(x=times, y=np.linalg.norm(circle_positions-true_positions, axis=1), name='Circular EKF', row=1, col=1)
fig_error.add_scatter(x=true_positions[:, 0], y=true_positions[:, 1], name='Truth', row=2, col=1)
fig_error.add_scatter(x=circle_positions[:, 0], y=circle_positions[:, 1], name='Circular EKF path', row=2, col=1)
fig_error.update_layout(height=800, title='Model comparison on one seeded circular trajectory')
fig_error.update_xaxes(title_text='Time [s]', row=1, col=1)
fig_error.update_yaxes(title_text='Euclidean error [m]', row=1, col=1)
fig_error.update_xaxes(title_text='x [m]', row=2, col=1)
fig_error.update_yaxes(title_text='y [m]', scaleanchor='x2', scaleratio=1, row=2, col=1)
fig_error.show()
print('Circular EKF RMSE after 2 s:', np.sqrt(np.mean((circle_positions[20:]-true_positions[20:])**2)))
Interpretation, limitations and exercise
A constant-velocity model tends to cut corners unless process noise allows the velocity to change. Acceleration can follow curvature over short intervals, but it still approximates the circular dynamics. The circular EKF can exploit the correct geometry; that advantage can be lost when the geometry is wrong. Initialisation and noise tuning affect this comparison, and one trajectory is not a statistical performance study. EKF uncertainty is a local approximation.
Exercise: double the assumed observation variance without changing the data. How do the final uncertainty ellipse and responsiveness change? Then change the truth to an ellipse and inspect which assumptions the circular EKF violates.
Show or hide code
# Answer scaffold: larger stated sensor noise gives less information.
F, Q = cartesian_model(1, delta_t, 0.5)
initial = CartesianState(np.array([[2.], [0.], [0.], [0.]]), np.array([0.]), np.eye(4)*4.)
wide_measurements = [Measurement(m.local_pos.copy(), m.global_pos.copy(), 2*m.local_cov) for m in measurements]
wide_states = KalmanFilter(initial, Q, F).fit(wide_measurements)
assert np.trace(wide_states[-1].x_cov[:2, :2]) > np.trace(models['Velocity'][-1].x_cov[:2, :2])
print('Larger assumed sensor variance increases the final position uncertainty.')
Interactive experiment: choose a tracking model
Compare position, velocity and acceleration models on the turning path, then vary the noise. Look for lag and corner cutting as well as smoothness. This experiment compares the Cartesian models; the circular EKF is demonstrated above.
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.
Show or hide code
from pathlib import Path
import sys
interactive_project = next(p / "projects" for p in [Path.cwd(), *Path.cwd().parents]
if (p / "projects" / "src").is_dir())
sys.path.insert(0, str(interactive_project / "src"))
from notebook_interactive import display_interactive
display_interactive("tracking")
Conclusion
Choosing a state model determines which motion the estimator can describe. For this seeded circular trajectory, the position-only, constant-velocity and constant-acceleration models have position RMSEs of $0.144$, $0.105$ and $0.100$ m per coordinate after the first two seconds. Adding velocity helps in this run; adding acceleration gives a smaller further improvement with the chosen noise settings. The process-noise strengths have different physical units and were chosen illustratively, so this is not a comparison of optimally tuned models.
The circular EKF reaches about $0.045$ m RMSE per coordinate over the same range. It benefits from a state that matches the simulated geometry, and its final radius and angular velocity are close to the generated values. That result also depends on starting near the true orbit. It does not show that a circular model will handle an arbitrary trajectory or recover from poor initialisation. The Jacobian check verifies the implemented derivative, while the EKF covariance remains a local approximation.
A smooth path and a small uncertainty ellipse are useful diagnostics, but neither establishes that the model is right. Compare errors over repeated simulations, check uncertainty coverage, and test motion that violates the assumed dynamics. The ellipse describes uncertainty in the estimated position, rather than the noise of a new sensor reading. For an application, choose the state and noise model together, then validate them on trajectories beyond the one used for tuning.