kalman_filter

Overview

This package contains the kalman filter with time delay and the calculation of the kalman filter.

Design

The Kalman filter is a recursive algorithm used to estimate the state of a dynamic system. The Time Delay Kalman filter is based on the standard Kalman filter and takes into account possible time delays in the measured values.

Standard Kalman Filter

System Model

Assume that the system can be represented by the following linear discrete model:

\[x_{k} = A x_{k-1} + B u_{k} \\ y_{k} = C x_{k-1}\]

where,

• $x_k$ is the state vector at time $k$.
• $u_k$ is the control input vector at time $k$.
• $y_k$ is the measurement vector at time $k$.
• $A$ is the state transition matrix.
• $B$ is the control input matrix.
• $C$ is the measurement matrix.

Prediction Step

The prediction step consists of updating the state and covariance matrices:

\[x_{k|k-1} = A x_{k-1|k-1} + B u_{k} \\ P_{k|k-1} = A P_{k-1|k-1} A^{T} + Q\]

where,

• $x_{k

  k-1}$ is the priori state estimate.

• $P_{k

  k-1}$ is the priori covariance matrix.

Update Step

When the measurement value ( y_k ) is received, the update steps are as follows:

\[K_k = P_{k|k-1} C^{T} (C P_{k|k-1} C^{T} + R)^{-1} \\ x_{k|k} = x_{k|k-1} + K_k (y_{k} - C x_{k|k-1}) \\ P_{k|k} = (I - K_k C) P_{k|k-1}\]

where,

• $K_k$ is the Kalman gain.

• $x_{k

  k}$ is the posterior state estimate.

• $P_{k

  k}$ is the posterior covariance matrix.

Extension to Time Delay Kalman Filter

For the Time Delay Kalman filter, it is assumed that there may be a maximum delay of $d$ steps in the measured value (max_delay_step $= d$). To handle this delay, we extend the state vector to include past states. The valid delay step range is $0, 1, \ldots, d-1$.

Augmented State Vector:

\[(x_{k})_e = \begin{bmatrix} x_k \\ x_{k-1} \\ \vdots \\ x_{k-d+1} \end{bmatrix}\]

Augmented Transition Matrix ($A_e$) and Process Noise ($Q_e$):

\[A_e = \begin{bmatrix} A & 0 & 0 & \cdots & 0 \\ I & 0 & 0 & \cdots & 0 \\ 0 & I & 0 & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 0 \end{bmatrix}, \quad Q_e = \begin{bmatrix} Q & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 0 \end{bmatrix}\]

Prediction Step

The prediction step shifts the history of states and predicts the new current state.

Update extended state:

File truncated at 100 lines see the full file

Changelog for package autoware_kalman_filter

1.1.0 (2025-05-01)

• fix(autoware_kalman_filter): fixed clang-tidy error (#379)

  • fix(autoware_kalman_filter): fixed clang-tidy error

  * remove comment ---------

• refactor(autoware_kalman_filter): rewrite using modern C++ without API breakage (#346)

  • refactor using modern c++
  • remove ctor/dtor
  • precommit
  • use eigen methods

  * Update common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp ---------

• chore(autoware_kalman_filter): add maintainer (#381)

  • chore(autoware_kalman_filter): add maintainer
  • removed the maintainer with an invalid email address.
  • added members of the Localization / Mapping team as maintainers.
  • removed the duplicate entry.

  * fixed the deletion as the wrong entry was removed ---------

• Contributors: RyuYamamoto, Yutaka Kondo

1.9.0 (2026-06-24)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• perf(autoware_kalman_filter): use Cholesky solve in update() with full guard-branch test coverage (#1094)

  * test(autoware_kalman_filter): cover validation and guard branches Add unit tests for the previously-untested KalmanFilter dimension validation FALSE branches (init/predict/update), the state and covariance getters, and a closed-form numerical-equivalence update check on a correlated covariance. Add TimeDelayKalmanFilter::updateWithDelay tests for the non-column-vector, non-square-R, R/C-row-mismatch, and non-positive-definite (LLT failure) rejection paths, capturing stderr so test output stays clean.

  * perf(autoware_kalman_filter): solve innovation covariance via Cholesky in update() Mirror the optimization already applied to TimeDelayKalmanFilter::updateWithDelay in the base KalmanFilter::update: compute the Kalman gain with a Cholesky (LLT) solve of the innovation covariance S = C P C\^T + R instead of an explicit matrix inverse. This is faster and numerically more stable, and a failed decomposition now rejects updates whose innovation covariance is not positive definite, which the explicit inverse silently accepted with a meaningless gain (potentially corrupting downstream state in autoware_ekf_localizer). Also use noalias() in predict(x_next, A, Q) to drop the intermediate Eigen temporaries of A P A\^T in that hot per-cycle path. The public bool API is unchanged; behavior is identical for valid inputs (verified against the closed-form solution) and fails safe for non-positive-definite ones.

  • test(autoware_kalman_filter): address review feedback
  • assert update against hand-computed numeric literals instead of a re-typed copy of the solver formula

  - strengthen the two-arg init test to verify stored state/covariance feed the filter math, keeping getXelement coverage Refs: autowarefoundation/autoware_core#1096

  * test(autoware_kalman_filter): de-tautologize the kf end-to-end test (#78) Replace the recomputed-with-the-same-Kalman-formula expectations in TEST(kalman_filter, kf) with independent hand-computed numeric literals for the predict and update steps (A=B=C=I, Q=0.01I, R=0.09I), with the derivations written out in comments. The previous expectations re-typed the implementation's own predict/update expressions, so a wrong formula would have passed in both places. Refs: autowarefoundation/autoware_core#1096

  * test(autoware_kalman_filter): drop trivial init getter/setter test (#88) interimadd flagged the two-argument init test as covering only straightforward getters/setters and empty values, with no logic to break. The follow-up commit had reworked it into init_state_cov_feed_into_filter_math, but it still ultimately

File truncated at 100 lines see the full file