Kalman Filter

This documentation was generated with the assistance of AI. Please report any inaccuracies.

The Kalman filter is a recursive estimator for the state of a discrete-time linear system from a sequence of noisy measurements. It was introduced in Kalman, 1960, and is implemented as KalmanFilter in the com.irurueta.numerical.signal.processing package. The class’s own Javadoc states that "the source code, notation and formulae… are borrowed from the JKalman tutorial [Welch95]," referencing Welch and Bishop, 1995 directly by URL — its notation for the state transition matrix , control matrix , measurement matrix , and noise covariances / is exactly the notation this class’s API uses.

System model

The filter assumes the true state evolves linearly, and is observed only through a linear, noisy measurement:

where is the (unobserved) state at step , is an optional external control input, is the measurement actually taken, and and are independent, zero-mean Gaussian process and measurement noise. Neither noise term is observed directly — only their covariances and are assumed known (typically is estimated from real sensor data by Measurement Noise Covariance Estimation, while is chosen intuitively to reflect how much unmodeled dynamics the filter should tolerate).

Predict and correct

Each step alternates between two phases. Predict (time update) projects the previous state forward using only the system model, before any new measurement is available:

Correct (measurement update) then folds in the new measurement , weighted by the Kalman gain :

is the innovation — the discrepancy between the actual measurement and what the model predicted it would be. The gain balances how much to trust that discrepancy against how much to trust the prediction: a large (noisy sensor) shrinks , leaning on the model; a large (uncertain prediction) grows , leaning on the measurement. KalmanFilter.correct(Matrix) solves for via Utils.solve on rather than forming the matrix inverse explicitly, which is the more numerically stable way to reach the same result.

flowchart LR subgraph Predict["predict(control)"] A["x'_k = A * x_(k-1) + B * u_k"] --> B["P'_k = A * P_(k-1) * A^T + Q"] end subgraph Correct["correct(measurement)"] C["K_k = P'_k * H^T * inv(H * P'_k * H^T + R)"] --> D["x_k = x'_k + K_k * (z_k - H * x'_k)"] D --> E["P_k = (I - K_k * H) * P'_k"] end Start(["Initial state x_0, P_0"]) --> Predict Predict --> Correct Correct -->|"next time step"| Predict

Library implementation

KalmanFilter(dynamParams, measureParams, controlParams) allocates all matrices for a system with dynamParams state dimensions (dp) and measureParams measurement dimensions (mp); controlParams (cp) may be 0 (no control input, no B matrix) or omitted entirely via the two-argument constructor.

predict() / predict(Matrix control) — runs the time update, storing the result in statePre / errorCovPre and returning statePre.
correct(Matrix measurement) — runs the measurement update, storing the result in statePost / errorCovPost / gain and returning statePost.
Matrix accessors mirror the equations above directly: getTransitionMatrix()/setTransitionMatrix(Matrix) ( , dp×dp), getControlMatrix()/setControlMatrix(Matrix) ( , dp×cp), getMeasurementMatrix()/setMeasurementMatrix(Matrix) ( , mp×dp), getProcessNoiseCov()/setProcessNoiseCov(Matrix) ( , dp×dp, must be symmetric), getMeasurementNoiseCov()/setMeasurementNoiseCov(Matrix) ( , mp×mp, must be symmetric), getErrorCovPre()/getErrorCovPost() ( / ), and getGain() ( ).
getStatePre()/setStatePre(Matrix) and getStatePost()/setStatePost(Matrix) expose the state vectors directly, letting a caller seed the filter’s initial state before the first predict()/correct() call.
Defaults: transitionMatrix and errorCovPost start as identity, processNoiseCov as DEFAULT_PROCESS_NOISE_VARIANCE (1e-6) times identity, and measurementNoiseCov as DEFAULT_MEASUREMENT_NOISE_VARIANCE (1e-1) times identity — all meant to be overridden once the actual system’s dynamics and sensor noise are known.

A lower makes the filter trust its own model more, producing a smoother but slower-to-react state estimate; a higher lets it follow the true state more quickly at the cost of noisier output — the same trade-off applies to , in the opposite direction, since a KalmanFilter weighs prediction against measurement purely through the ratio between the two.

Example

Estimating a constant scalar value ( , , no control) from repeated noisy measurements — the simplest possible instance of the model, and the introductory example used by Welch and Bishop, 1995 themselves:

final var filter = new KalmanFilter(1, 1);
filter.setStatePost(new Matrix(1, 1)); // initial guess: 0.0

final var processNoise = new Matrix(1, 1);
processNoise.setElementAtIndex(0, 1e-5); // small: the true value does not change
filter.setProcessNoiseCov(processNoise);

final var measurementNoise = new Matrix(1, 1);
measurementNoise.setElementAtIndex(0, 0.01); // from the sensor's known noise variance
filter.setMeasurementNoiseCov(measurementNoise);

for (final var z : measurements) { // measurements: double[] of noisy readings
    filter.predict();

    final var measurement = new Matrix(1, 1);
    measurement.setElementAtIndex(0, z);
    filter.correct(measurement);
}

final var estimate = filter.getStatePost().getElementAtIndex(0);