UltimateKalman: Flexible Kalman Filtering and Smoothing Using Orthogonal Transformations

TL;DR

The article describes the algorithm and its implementation as well as a test suite of examples and tests and describes the programming interface of UltimateKalman, a flexible linear Kalman filter and smoother implemented in three popular programming languages.

Abstract

UltimateKalman is a flexible linear Kalman filter and smoother implemented in three popular programming languages: MATLAB, C, and Java. UltimateKalman is a slight simplification and slight generalization of an elegant Kalman filter and smoother that was proposed in 1977 by Paige and Saunders. Their algorithm appears to be numerically superior and more flexible than other Kalman filters and smoothers, but curiously has never been implemented or used before. UltimateKalman is flexible: it can easily handle time-dependent problems, problems with state vectors whose dimensions vary from step to step, problems with varying number of observations in different steps (or no observations at all in some steps), and problems in which the expectation of the initial state is unknown. The programming interface of UltimateKalman is broken into simple building blocks that can be used to construct filters, single or multi-step predictors, multi-step or whole-track smoothers, and combinations. The paper describes the algorithm and its implementation as well as with a test suite of examples and tests.

Figures (5)

• Figure 1: Kalman filtering, prediction, and smoothing of the trajectory of a rotating point in the plane. On the left $G=I$ and observations are indicated by red dots. The yellow curve represents a simulated system; the blue curve represents filtered estimates, the cyan whole-trajectory smoothed estimates, and the green predictions from one observation. On the right $G=10$; we cannot predict the state from one such observation, and we cannot produce a filtered estimate of the first state.
• Figure 2: Kalman filtering and smoothing of a scalar trajectory. The yellow lines show the actual trajectory, the red dots show the observations, the blue lines show the filtered estimates (left column), the cyan lines show smoothed estimates (right column), and the gray areas represent values that are $\pm3\hat{\sigma}_{i}$ from the estimate $\hat{u}_{i}$, where $\hat{\sigma}_{i}$ is the estimated standard deviation of $\hat{u}_{i}$. In the top row, the standard deviation of $\delta_{i}$ is always $10$; in the bottom row, it drops down to $0.25$ at iteration $50$ and then goes back up to $10$.
• Figure 3: Kalman filtering and smoothing of the trajectory of a projectile affected by gravity and drag. Again, the yellow line show the actual trajectory, the red dots show the observations, the blue line shows the filtered estimates of the trajectory, and the cyan line shows the smoothed estimates. Smoothing was performed on the entire trajectory, to estimate the point of departure and the point where the projectile hits the ground. The graph on the left shows the entire trajectory and the graph on the right only the part in which observations are available.
• Figure 4: Kalman tracking of the relative offsets of three clocks from times of arrival of radio packets. The graphs show the relative offsets relative to the offset of the first clock. The yellow lines are the simulated offsets and the blue lines are the filtered estimates.
• Figure 5: Performance testing of the three UltimateKalman implementations. The graphs show the time per step (window averages of nonoverlapping groups of steps) when the algorithm filtered systems with random square unitary matrices $F_{i}$ and $G_{i}$.