Autoware.Auto
autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT > Class Template Reference

A Kalman filter implementation. More...

#include <kalman_filter.hpp>

Inheritance diagram for autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >:
Collaboration diagram for autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >:

Public Types

using State = typename MotionModelT::State
 
using StateMatrix = typename State::Matrix
 
using MotionModel = MotionModelT
 
using NoiseModel = NoiseModelT
 

Public Member Functions

 KalmanFilter (MotionModelT motion_model, NoiseModelT noise_model, const State &initial_state, const StateMatrix &initial_covariance)
 Constructs a new instance of a Kalman filter. More...
 
State crtp_predict (const std::chrono::nanoseconds &dt)
 Predict next state. More...
 
template<typename MeasurementT >
State crtp_correct (const MeasurementT &measurement)
 Correct the predicted state given a measurement. More...
 
void crtp_reset (const State &state, const StateMatrix &covariance)
 Reset the state of the filter to a given state and covariance. More...
 
auto & crtp_state ()
 Get current state. More...
 
const auto & crtp_state () const
 Get current state. More...
 
auto & crtp_covariance ()
 Get current covariance. More...
 
const auto & crtp_covariance () const
 Get current covariance. More...
 
- Public Member Functions inherited from autoware::common::state_estimation::StateEstimationInterface< KalmanFilter< MotionModelT, NoiseModelT > >
auto predict (const std::chrono::nanoseconds &dt)
 Predict the state by dt into the future. More...
 
auto correct (const MeasurementT &measurement)
 Correct the state with a measurement. More...
 
void reset (const StateT &state, const typename StateT::Matrix &covariance)
 Reset the internal state of the vector to the given state and covariance. More...
 
auto & state ()
 Return current state. More...
 
const auto & state () const
 Return current state. More...
 
auto & covariance ()
 Return current covariance. More...
 
const auto & covariance () const
 Return current covariance. More...
 

Additional Inherited Members

- Protected Member Functions inherited from autoware::common::helper_functions::crtp< KalmanFilter< MotionModelT, NoiseModelT > >
const KalmanFilter< MotionModelT, NoiseModelT > & impl () const
 
KalmanFilter< MotionModelT, NoiseModelT > & impl ()
 

Detailed Description

template<typename MotionModelT, typename NoiseModelT>
class autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >

A Kalman filter implementation.

Template Parameters
MotionModelTType of the motion model.
NoiseModelTType of the noise model.

Member Typedef Documentation

◆ MotionModel

template<typename MotionModelT , typename NoiseModelT >
using autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::MotionModel = MotionModelT

◆ NoiseModel

template<typename MotionModelT , typename NoiseModelT >
using autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::NoiseModel = NoiseModelT

◆ State

template<typename MotionModelT , typename NoiseModelT >
using autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::State = typename MotionModelT::State

◆ StateMatrix

template<typename MotionModelT , typename NoiseModelT >
using autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::StateMatrix = typename State::Matrix

Constructor & Destructor Documentation

◆ KalmanFilter()

template<typename MotionModelT , typename NoiseModelT >
autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::KalmanFilter ( MotionModelT  motion_model,
NoiseModelT  noise_model,
const State initial_state,
const StateMatrix initial_covariance 
)
inlineexplicit

Constructs a new instance of a Kalman filter.

Parameters
[in]motion_modelThe motion model to be used to predict the movement.
[in]noise_modelThe noise model that models the motion noise.
[in]initial_stateThe initial state of the filter.
[in]initial_covarianceThe initial state covariance.

Member Function Documentation

◆ crtp_correct()

template<typename MotionModelT , typename NoiseModelT >
template<typename MeasurementT >
State autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_correct ( const MeasurementT &  measurement)
inline

Correct the predicted state given a measurement.

Note
It is expected that a prediction step was done right before the correction.
Parameters
[in]measurementCurrent measurement.
Template Parameters
MeasurementTMeasurement type.
Returns
State corrected with the measurement.

◆ crtp_covariance() [1/2]

template<typename MotionModelT , typename NoiseModelT >
auto& autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_covariance ( )
inline

Get current covariance.

◆ crtp_covariance() [2/2]

template<typename MotionModelT , typename NoiseModelT >
const auto& autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_covariance ( ) const
inline

Get current covariance.

◆ crtp_predict()

template<typename MotionModelT , typename NoiseModelT >
State autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_predict ( const std::chrono::nanoseconds &  dt)
inline

Predict next state.

Parameters
[in]dtTime difference to the time at which prediction is needed.
Returns
Predicted state.

◆ crtp_reset()

template<typename MotionModelT , typename NoiseModelT >
void autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_reset ( const State state,
const StateMatrix covariance 
)
inline

Reset the state of the filter to a given state and covariance.

Parameters
[in]stateThe new state that overwrites one stored in the filter.
[in]covarianceThe new covariance that overwrites one stored in the filter.

◆ crtp_state() [1/2]

template<typename MotionModelT , typename NoiseModelT >
auto& autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_state ( )
inline

Get current state.

◆ crtp_state() [2/2]

template<typename MotionModelT , typename NoiseModelT >
const auto& autoware::common::state_estimation::KalmanFilter< MotionModelT, NoiseModelT >::crtp_state ( ) const
inline

Get current state.


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