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:

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:

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:

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:

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

Prediction Step

The prediction step consists of updating the extended state and covariance matrices.

Update extension status:

Update extended covariance matrix:

$\Longrightarrow$

where,

• $(x_{k

  k-1})_e$ is the priori extended state estimate.

• $(P_{k

  k-1})_e$ is the priori extended covariance matrix.

Update Step

When receiving the measurement value ( $y_{k}$ ) with a delay of ( $ds$ ), the update steps are as follows:

Update kalman gain:

Update extension status:

Update extended covariance matrix:

where,

• $K_k$ is the Kalman gain.

• $(x_{k

  k})_e$ is the posterior extended state estimate.

• $(P_{k

  k})_e$ is the posterior extended covariance matrix.

• $C$ is the measurement matrix, which only applies to the delayed state part.

Example Usage

This section describes Example Usage of KalmanFilter.

• Initialization

#include "autoware/kalman_filter/kalman_filter.hpp"

// Define system parameters
int dim_x = 2; // state vector dimensions
int dim_y = 1; // measure vector dimensions

// Initial state
Eigen::MatrixXd x0 = Eigen::MatrixXd::Zero(dim_x, 1);
x0 << 0.0, 0.0;

// Initial covariance matrix
Eigen::MatrixXd P0 = Eigen::MatrixXd::Identity(dim_x, dim_x);
P0 *= 100.0;

// Define state transition matrix
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x, dim_x);
A(0, 1) = 1.0;

// Define measurement matrix
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x);
C(0, 0) = 1.0;

// Define process noise covariance matrix
Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(dim_x, dim_x);
Q *= 0.01;

// Define measurement noise covariance matrix
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(dim_y, dim_y);
R *= 1.0;

// Initialize Kalman filter
autoware::kalman_filter::KalmanFilter kf;
kf.init(x0, P0);

• Predict step

const Eigen::MatrixXd x_next = A * x0;
kf.predict(x_next, A, Q);

• Update step

// Measured value
Eigen::MatrixXd y = Eigen::MatrixXd::Zero(dim_y, 1);
kf.update(y, C, R);

• Get the current estimated state and covariance matrix

Eigen::MatrixXd x_curr = kf.getX();
Eigen::MatrixXd P_curr = kf.getP();

Assumptions / Known limits

• Delay Step Check: Ensure that the delay_step provided during the update does not exceed the maximum delay steps set during initialization.

Changelog for package autoware_kalman_filter

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)