Autoware.Auto
autoware::common::state_estimation::KalmanFilterWrapper< FilterT > Class Template Reference

This class provides a high level interface to the Kalman Filter allowing to predict the state of the filter with time and observe it by receiving ROS messages. More...

#include <kalman_filter_wrapper.hpp>

Public Types

using State = typename FilterT::State
 

Public Member Functions

 KalmanFilterWrapper (const typename FilterT::MotionModel motion_model, const typename FilterT::NoiseModel noise_model, const typename FilterT::State::Matrix initial_state_covariance, const std::chrono::nanoseconds &expected_dt, const std::string &frame_id, const std::chrono::nanoseconds &history_duration=std::chrono::milliseconds{5000}, common::types::float32_t mahalanobis_threshold=std::numeric_limits< common::types::float32_t >::max())
 Create an EKF wrapper. More...
 
template<typename MeasurementT >
void add_reset_event_to_history (const MeasurementT &measurement)
 
void add_reset_event_to_history (const State &state, const typename State::Matrix &initial_covariance, const std::chrono::system_clock::time_point &event_timestamp)
 
common::types::bool8_t add_next_temporal_update_to_history ()
 
template<typename MeasurementT >
common::types::bool8_t add_observation_to_history (const MeasurementT &measurement)
 
common::types::bool8_t is_initialized () const noexcept
 Check if the filter is is_initialized with a state. More...
 
nav_msgs::msg::Odometry get_state () const
 Get the current state of the system as an odometry message. More...
 

Detailed Description

template<typename FilterT>
class autoware::common::state_estimation::KalmanFilterWrapper< FilterT >

This class provides a high level interface to the Kalman Filter allowing to predict the state of the filter with time and observe it by receiving ROS messages.

Template Parameters
FilterTType of filter used internally.

Member Typedef Documentation

◆ State

template<typename FilterT >
using autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::State = typename FilterT::State

Constructor & Destructor Documentation

◆ KalmanFilterWrapper()

template<typename FilterT >
autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::KalmanFilterWrapper ( const typename FilterT::MotionModel  motion_model,
const typename FilterT::NoiseModel  noise_model,
const typename FilterT::State::Matrix  initial_state_covariance,
const std::chrono::nanoseconds &  expected_dt,
const std::string &  frame_id,
const std::chrono::nanoseconds &  history_duration = std::chrono::milliseconds{5000},
common::types::float32_t  mahalanobis_threshold = std::numeric_limits<common::types::float32_t>::max() 
)
inline

Create an EKF wrapper.

Parameters
[in]motion_modelThe motion model that is to be used.
[in]noise_modelThe noise model that is to be used.
[in]initial_state_covarianceThe initial covariances for the state. This is usually a diagonal matrix with sigmas squared for each state dimension on the diagonal.
[in]expected_dtExpected time difference between updates of the filter.
[in]frame_idThe frame id in which tracking takes place.
[in]history_durationLength of the history of events.
[in]mahalanobis_thresholdThe threshold on the Mahalanobis distance for outlier rejection.

Member Function Documentation

◆ add_next_temporal_update_to_history()

template<typename FilterT >
common::types::bool8_t autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::add_next_temporal_update_to_history ( )
inline

Predict state of filter at the next timestep defined by the period of this node.

Returns
true if the update was successful and false otherwise. In case false is returned, this update had no effect on the state of the filter.

◆ add_observation_to_history()

template<typename FilterT >
template<typename MeasurementT >
common::types::bool8_t autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::add_observation_to_history ( const MeasurementT &  measurement)
inline

Update the filter state with a measurement.

Parameters
[in]measurementThe measurement. It is expected to be a concrete instantiation of the Measurement class.
Template Parameters
MeasurementTMeasurement type that is a concrete template specialization of the Measurement class.
Returns
true if the observation was successful, false otherwise. In case of an unsuccessful update, the state of the underlying filter has not been changed.

◆ add_reset_event_to_history() [1/2]

template<typename FilterT >
template<typename MeasurementT >
void autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::add_reset_event_to_history ( const MeasurementT &  measurement)
inline

Reset the filter state using the default covariance and state derived from the measurement.

Parameters
[in]measurementThe measurement from which we initialize the state.
Template Parameters
MeasurementTType of measurement.

◆ add_reset_event_to_history() [2/2]

template<typename FilterT >
void autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::add_reset_event_to_history ( const State state,
const typename State::Matrix &  initial_covariance,
const std::chrono::system_clock::time_point &  event_timestamp 
)
inline

Reset the filter state. This must be called at least once to start / tracking.

Parameters
[in]stateThe full state to set the system to.
[in]initial_covarianceThe initial covariance.
[in]event_timestampThe event timestamp. Ideally this should be in the same clock as the one that timestamps the messages.

◆ get_state()

template<typename FilterT >
nav_msgs::msg::Odometry autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::get_state

Get the current state of the system as an odometry message.

◆ is_initialized()

template<typename FilterT >
common::types::bool8_t autoware::common::state_estimation::KalmanFilterWrapper< FilterT >::is_initialized ( ) const
inlinenoexcept

Check if the filter is is_initialized with a state.


The documentation for this class was generated from the following files: