KalmanFilter<T>

Models & Types · AiDotNet.Control

The recursive Kalman filter: the optimal state estimator for a linear system with Gaussian noise.

For Beginners

You have a model of how something moves and some noisy measurements of it. Neither is trustworthy alone. This blends them, weighting each by how much it deserves to be trusted, and keeps track of how uncertain the blend is. It is what lets a GPS give a smooth position from jumpy satellite fixes.

How It Works

Implements the filter of R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems”, Transactions of the ASME — Journal of Basic Engineering 82(D), 1960, pp. 35-45, for the system x[k+1] = A·x[k] + B·u[k] + w[k], y[k] = C·x[k] + v[k], with process noise covariance Q and measurement noise covariance R.

Why an estimator is needed at all. A regulator such as LinearQuadraticRegulator assumes the whole state is known. Almost never is it: sensors measure some combination of the state, corrupted by noise, and often fewer numbers than there are states. The filter reconstructs the rest. It is the exact mirror image of the regulator — the equation it solves is the regulator’s Riccati equation with the matrices transposed — which is the duality Kalman pointed out and the reason SteadyStateGain can be computed by the same solver.

Predict and update. Each step has two halves. Predict pushes the estimate forward through the dynamics, which always increases uncertainty — the model is not perfect. Update folds in a measurement, which always decreases it. The Kalman gain sets the balance, and it is not a tuning parameter: it is derived, and it is the value that minimizes the estimate’s variance given Q and R.

The covariance update is Joseph’s form,(I−KC)P(I−KC)ᵀ + KRKᵀ, rather than the shorter P − KCP. The two are algebraically equal, but only at the exactly optimal gain, and only in exact arithmetic. The short form is a difference of two positive matrices, so rounding can make it asymmetric or even indefinite, and once a covariance goes indefinite the filter diverges and never recovers. Joseph’s form is a sum of two positive terms and stays symmetric positive-semidefinite whatever the arithmetic does — at the cost of one extra matrix product per step, which is the cheapest insurance in filtering.

Example

// Track position and velocity from noisy position measurements alone.
var filter = new KalmanFilter<double>(
    transition: new Matrix<double>(new[,] { { 1.0, 1.0 }, { 0.0, 1.0 } }),
    observation: new Matrix<double>(new[,] { { 1.0, 0.0 } }),
    processNoise: new Matrix<double>(new[,] { { 0.01, 0.0 }, { 0.0, 0.01 } }),
    measurementNoise: new Matrix<double>(new[,] { { 1.0 } }));
            
filter.Initialize(new Vector<double>(new[] { 0.0, 0.0 }), Matrix<double>.CreateIdentity(2));
            
filter.Predict();
filter.Update(new Vector<double>(new[] { 1.02 }));
// filter.State now holds the best estimate of both position and velocity.

Constructors

Constructor Summary
KalmanFilter(Matrix<T> transition, Matrix<T> observation, Matrix<T> processNoise, Matrix<T> measurementNoise, Matrix<T> control) Creates a Kalman filter.

Properties

Property Summary
Matrix<T> Covariance { get; set; } Gets the current estimate covariance, whose diagonal holds the variance of each state.
Matrix<T> Gain { get; set; } Gets the Kalman gain from the most recent Update.
Vector<T> Innovation { get; set; } Gets the innovation from the most recent Update — the difference between the measurement received and the one expected.
Vector<T> State { get; set; } Gets the current state estimate.
int MeasurementCount { get; } Gets the number of measurements per step.
int StateCount { get; } Gets the number of states.

Methods

Method Summary
Matrix<T> SteadyStateGain(Matrix<T> transition, Matrix<T> observation, Matrix<T> processNoise, Matrix<T> measurementNoise, AlgebraicRiccatiSolverOptions options) Computes the steady-state Kalman gain, the value the recursive gain converges to.
void Initialize(Vector<T> state, Matrix<T> covariance) Sets the initial state estimate and its covariance.
void Predict() Advances the estimate through the dynamics with no control input.
void Predict(Vector<T> input) Advances the estimate through the dynamics, accounting for a known control input.
void Update(Vector<T> measurement) Folds a measurement into the estimate.