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 step ($d$) in the measured value. To handle this delay, we extend the state vector to:

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

The corresponding state transition matrix ($A_e$) and process noise covariance matrix ($Q_e$) are also expanded:

\[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 consists of updating the extended state and covariance matrices.

Update extension status:

$$ (x_{k|k-1})_e = \begin{bmatrix}

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.0.0 (2025-03-31)

0.3.0 (2025-03-21)

• chore: rename from [autoware.core]{.title-ref} to [autoware_core]{.title-ref} (#290)
• test(autoware_kalman_filter): add tests for missed lines (#263)
• Contributors: NorahXiong, Yutaka Kondo

0.2.0 (2025-02-07)

• unify version to 0.1.0
• update changelog
• feat: port autoware_kalman_filter from autoware_universe (#141) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>> Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>>
• Contributors: Yutaka Kondo, cyn-liu
• feat: port autoware_kalman_filter from autoware_universe (#141) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>> Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>>
• Contributors: cyn-liu

0.0.0 (2024-12-02)