Autoware.Auto
autoware::common::state_estimation::LinearMeasurement< StateT > Class Template Reference

A class that represents a linear measurement. More...

#include <linear_measurement.hpp>

Inheritance diagram for autoware::common::state_estimation::LinearMeasurement< StateT >:
Collaboration diagram for autoware::common::state_estimation::LinearMeasurement< StateT >:

Public Types

using State = StateT
 

Public Member Functions

 LinearMeasurement ()=default
 Default constructor. More...
 
 LinearMeasurement (const typename StateT::Vector &measurement, const typename StateT::Matrix &covariance)
 Construct a new instance of the measurement from a state vector. More...
 
template<typename NewScalarT >
auto cast () const noexcept
 Cast to another scalar type. More...
 
- Public Member Functions inherited from autoware::common::state_estimation::MeasurementInterface< Derived >
auto & state ()
 Get measurement as a state vector. More...
 
const auto & state () const
 Get measurement as a state vector. More...
 
auto & covariance ()
 Get covariance of the measurement as a matrix. More...
 
const auto & covariance () const
 
template<typename OtherStateT >
auto create_new_instance_from (const OtherStateT &other_state) const
 Create a new instance of the measurement from another state. More...
 
template<typename OtherStateT >
OtherStateT map_into (const OtherStateT &other_state) const
 Map this measurement into another state space. More...
 
template<typename OtherStateT >
auto mapping_matrix_from (const OtherStateT &other_state) const
 Get a matrix that maps a state to this measurement's state space. More...
 

Static Public Member Functions

static LinearMeasurement create_with_stddev (const typename StateT::Vector &measurement, const typename StateT::Vector &standard_deviation)
 Convenience factory function to construct a measurement from a vector of standard deviations. More...
 

Protected Member Functions

StateT & crtp_state () noexcept
 Get state vector function that is to be called by the CRTP interface. More...
 
const StateT & crtp_state () const noexcept
 Get state vector function that is to be called by the CRTP interface. More...
 
StateT::Matrix & crtp_covariance () noexcept
 Get covariance function that is to be called by the CRTP interface. More...
 
const StateT::Matrix & crtp_covariance () const noexcept
 Get covariance function that is to be called by the CRTP interface. More...
 
template<typename OtherStateT >
StateT crtp_create_new_instance_from (const OtherStateT &other_state) const
 Create a new instance of the measurement from another state. More...
 
template<typename OtherStateT >
OtherStateT crtp_map_into (const OtherStateT &other_state) const noexcept
 Copy this measurement into another state space. More...
 
template<typename OtherStateT >
MappingMatrixFrom< OtherStateT > crtp_mapping_matrix_from (const OtherStateT &) const
 Get the mapping matrix between the two states called by the CRTP interface. More...
 
- Protected Member Functions inherited from autoware::common::helper_functions::crtp< Derived >
const Derived & impl () const
 
Derived & impl ()
 

Protected Attributes

friend MeasurementInterface< LinearMeasurement< StateT > >
 

Friends

bool operator== (const LinearMeasurement &lhs, const LinearMeasurement &rhs)
 An equality operator. More...
 

Detailed Description

template<typename StateT>
class autoware::common::state_estimation::LinearMeasurement< StateT >

A class that represents a linear measurement.

A linear measurement is one that measures directly variables from some unknown real state, thus requiring no measurement Jacobian computation. The mapping matrix will only have 1s and 0s in it.

Template Parameters
StateTA measured state.

Member Typedef Documentation

◆ State

template<typename StateT >
using autoware::common::state_estimation::LinearMeasurement< StateT >::State = StateT

Constructor & Destructor Documentation

◆ LinearMeasurement() [1/2]

template<typename StateT >
autoware::common::state_estimation::LinearMeasurement< StateT >::LinearMeasurement ( )
default

Default constructor.

◆ LinearMeasurement() [2/2]

template<typename StateT >
autoware::common::state_estimation::LinearMeasurement< StateT >::LinearMeasurement ( const typename StateT::Vector &  measurement,
const typename StateT::Matrix &  covariance 
)
inlineexplicit

Construct a new instance of the measurement from a state vector.

Parameters
[in]measurementThe measurement
[in]covarianceThe covariance

Member Function Documentation

◆ cast()

template<typename StateT >
template<typename NewScalarT >
auto autoware::common::state_estimation::LinearMeasurement< StateT >::cast ( ) const
inlinenoexcept

Cast to another scalar type.

Returns
The measurement with a different scalar type.

◆ create_with_stddev()

template<typename StateT >
static LinearMeasurement autoware::common::state_estimation::LinearMeasurement< StateT >::create_with_stddev ( const typename StateT::Vector &  measurement,
const typename StateT::Vector &  standard_deviation 
)
inlinestatic

Convenience factory function to construct a measurement from a vector of standard deviations.

This results in a diagonal covariance matrix, with the squared standard deviations as entries. In a diagonal covariance matrix, the variables are uncorrelated.

Parameters
[in]measurementThe measurement
[in]standard_deviationThe standard deviation for each variable.

◆ crtp_covariance() [1/2]

template<typename StateT >
const StateT::Matrix& autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_covariance ( ) const
inlineprotectednoexcept

Get covariance function that is to be called by the CRTP interface.

◆ crtp_covariance() [2/2]

template<typename StateT >
StateT::Matrix& autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_covariance ( )
inlineprotectednoexcept

Get covariance function that is to be called by the CRTP interface.

◆ crtp_create_new_instance_from()

template<typename StateT >
template<typename OtherStateT >
StateT autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_create_new_instance_from ( const OtherStateT &  other_state) const
inlineprotected

Create a new instance of the measurement from another state.

This is a linear measurement, i.e., it directly observes the variables of another state, thus this mapping function is linear and just copied values for the matching variables in both states.

Note
It is expected that the measurement StateT is a direct sub-state of the OtherStateT.
Parameters
[in]other_stateThe other state
Template Parameters
OtherStateTState to which we map the internal state StateT.
Returns
A vector that represents a mapping of another state to this measurement's frame.

◆ crtp_map_into()

template<typename StateT >
template<typename OtherStateT >
OtherStateT autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_map_into ( const OtherStateT &  other_state) const
inlineprotectednoexcept

Copy this measurement into another state space.

Note
Values present in other_state but not in this measurement will stay intact.
Parameters
[in]other_stateThe other state
Template Parameters
OtherStateTState to which we map the internal state StateT.
Returns
A vector that represents a mapping of another state to this measurement's frame.

◆ crtp_mapping_matrix_from()

template<typename StateT >
template<typename OtherStateT >
MappingMatrixFrom<OtherStateT> autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_mapping_matrix_from ( const OtherStateT &  ) const
inlineprotected

Get the mapping matrix between the two states called by the CRTP interface.

As this is a linear measurement (one that directly measures variables of some state) this function only fills the entries of the mapping matrix with 1s and 0s.

Note
The OtherStateT input parameter is not used in this implementation. It is required for other implementation that must map into the measurement state space in a non-linear way.
Template Parameters
OtherStateTState to which we map the internal state StateT.
Returns
A matrix M, such that other_state = M * this_state.

◆ crtp_state() [1/2]

template<typename StateT >
const StateT& autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_state ( ) const
inlineprotectednoexcept

Get state vector function that is to be called by the CRTP interface.

◆ crtp_state() [2/2]

template<typename StateT >
StateT& autoware::common::state_estimation::LinearMeasurement< StateT >::crtp_state ( )
inlineprotectednoexcept

Get state vector function that is to be called by the CRTP interface.

Friends And Related Function Documentation

◆ operator==

template<typename StateT >
bool operator== ( const LinearMeasurement< StateT > &  lhs,
const LinearMeasurement< StateT > &  rhs 
)
friend

An equality operator.

Member Data Documentation

◆ MeasurementInterface< LinearMeasurement< StateT > >

template<typename StateT >
friend autoware::common::state_estimation::LinearMeasurement< StateT >::MeasurementInterface< LinearMeasurement< StateT > >
protected

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