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.8.0 (2026-05-01)

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

• perf(autoware_kalman_filter): optimize [updateWithDelay]{.title-ref} by replacing sparse matrix operations (#739)

  • Optimize [updateWithDelay]{.title-ref} by replacing sparse matrix operations with block arithmetic
  • style(pre-commit): autofix
  • Add test
  • style(pre-commit): autofix
  • Update README.md for (Extended Kalman Filter)
  • style(pre-commit): autofix
  • Add more detailed comments
  • style(pre-commit): autofix
  • Handle negative delay step and refactor tests
  • Replace TDKF with full term
  • style(pre-commit): autofix
  • Remove LLM like comment
  • Address PR review comments: add input validation, NaN guard, and docs fix
  • Add dimension validation for y, R, and C matrices in updateWithDelay
  • Add NaN/Inf check on Kalman gain to match base KalmanFilter::update
  • Add [[unlikely]] branch hints on all error paths
  • Clarify "diagonal block" comment terminology
  • Fix README augmented state vector (x_{k-d} -> x_{k-d+1}) and clarify delay range
  • Add test for C matrix dimension mismatch
  • style(pre-commit): autofix

  * fix: remove [[unlikely]] C++20 attributes for C++17 compatibility The [[unlikely]] attribute is a C++20 feature that causes compilation errors under clang-tidy which enforces strict C++17 compliance. ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <<yamato.ando@tier4.jp>>

• refactor(autoware_core): add USE_SCOPED_HEADER_INSTALL_DIR to common and testing packages (#967) Co-authored-by: github-actions <<github-actions@github.com>> Co-authored-by: Junya Sasaki <<j2sasaki1990@gmail.com>>

• fix(autoware_kalman_filter): fix bugprone-implicit-widening-of-multiplication-result warnings (#910)

• fix(autoware_kalman_filter): fix bugprone-narrowing-conversions warnings (#930)

• Contributors: Keita Morisaki, NorahXiong, Vishal Chauhan, github-actions

1.7.0 (2026-02-14)

1.6.0 (2025-12-30)

File truncated at 100 lines see the full file