autoware::common::state_estimation Namespace Reference

Namespaces
	detail

Classes
struct	convert_to
	A default template structure used for message conversion. Only its specializations are to be used. More...

struct	convert_to< PoseMeasurementXYZ64 >
	A specialization for PoseMeasurementXYZ64. More...

struct	convert_to< PoseMeasurementXYZRPY64 >
	A specialization for PoseMeasurementXYZRPY64. More...

struct	convert_to< Stamped< MeasurementT > >
	A specialization of `convert_to` to convert to any `Stamped` measurement. More...

class	History
	This class encapsulates a history of events used with EKF. More...

class	KalmanFilter
	A Kalman filter implementation. More...

class	KalmanFilterWrapper
	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...

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

struct	MeasurementInterface
	A CRTP interface to any measurement. More...

class	NoiseInterface
	A CRTP interface for implementing noise models used for providing motion model noise covariance. More...

struct	number_of_acceleration_components
	A trait that defines the number of acceleration components. More...

struct	number_of_acceleration_components< common::state_vector::ConstAccelerationXY< ScalarT > >
	A specialization of the number_of_acceleration_components trait for common::state_vector::ConstAccelerationXY. More...

struct	number_of_acceleration_components< common::state_vector::ConstAccelerationXYYaw< ScalarT > >
	A specialization of the number_of_acceleration_components trait for common::state_vector::ConstAccelerationXYYaw. More...

struct	number_of_acceleration_components< common::state_vector::ConstAccelerationXYZ< ScalarT > >
	A specialization of the number_of_acceleration_components trait for common::state_vector::ConstAccelerationXYZ. More...

struct	number_of_acceleration_components< common::state_vector::ConstAccelerationXYZRPY< ScalarT > >
	A specialization of the number_of_acceleration_components trait for common::state_vector::ConstAccelerationXYZRPY. More...

struct	number_of_acceleration_components< common::state_vector::ConstAccelerationXYZYaw< ScalarT > >
	A specialization of the number_of_acceleration_components trait for common::state_vector::ConstAccelerationXYZYaw. More...

struct	PredictionEvent
	An event to indicate a prediction step. More...

struct	ResetEvent
	An event to reset the state of the filter. More...

struct	Stamped

class	StateEstimationInterface
	Interface for a filter that can be used to track an object. More...

class	StateEstimationNode

class	SteadyTimeGrid
	This is a utility class that allows querying timestamps on a 1D grid. More...

class	UniformNoise
	This class contains the logic for a uniform noise model. More...

class	WienerNoise
	A class that describes the Wiener process noise. More...

Typedefs
template<typename ScalarT >
using	PoseMeasurementXYZ = LinearMeasurement< state_vector::GenericState< ScalarT, state_vector::variable::X, state_vector::variable::Y, state_vector::variable::Z > >

using	PoseMeasurementXYZ32 = PoseMeasurementXYZ< common::types::float32_t >

using	PoseMeasurementXYZ64 = PoseMeasurementXYZ< common::types::float64_t >

template<typename ScalarT >
using	PoseMeasurementXYZRPY = LinearMeasurement< state_vector::GenericState< ScalarT, state_vector::variable::X, state_vector::variable::Y, state_vector::variable::Z, state_vector::variable::ROLL, state_vector::variable::PITCH, state_vector::variable::YAW > >

using	PoseMeasurementXYZRPY32 = PoseMeasurementXYZRPY< common::types::float32_t >

using	PoseMeasurementXYZRPY64 = PoseMeasurementXYZRPY< common::types::float64_t >

using	ConstAccelerationKalmanFilterXY = KalmanFilter< motion_model::LinearMotionModel< state_vector::ConstAccelerationXY32 >, WienerNoise< common::state_vector::ConstAccelerationXY32 > >

using	ConstAccelerationKalmanFilterXYZRPY = KalmanFilter< motion_model::LinearMotionModel< state_vector::ConstAccelerationXYZRPY32 >, WienerNoise< common::state_vector::ConstAccelerationXYZRPY32 > >

using	ConstantAccelerationFilterWrapperXY = KalmanFilterWrapper< ConstAccelerationKalmanFilterXY >

using	ConstantAccelerationFilterWrapperXYZRPY = KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY >

Enumerations
enum	DataStorageOrder { DataStorageOrder::kRowMajor, DataStorageOrder::kColumnMajor }
	This class describes a data storage order. More...

Functions
template<std::int32_t kStateDimensionality, typename FloatT >
static constexpr Eigen::Transform< FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry >	downscale_isometry (const Eigen::Transform< FloatT, 3, Eigen::TransformTraits::Isometry > &isometry)
	Downscale the isometry to a lower dimension if needed. More...

template<std::int32_t kRows, std::int32_t kCols, typename ScalarT , std::size_t kSize>
Eigen::Matrix< ScalarT, kRows, kCols >	array_to_matrix (const std::array< ScalarT, kSize > &array, const std::int32_t start_index, const std::int32_t stride, const DataStorageOrder storage_order)
	Transform a given array to an Eigen matrix using the specified stride and a starting index within this array. More...

template<std::int32_t kRows, std::int32_t kCols, typename ScalarT , std::size_t kSize>
void	set_from_matrix (std::array< ScalarT, kSize > &array, const Eigen::Matrix< ScalarT, kRows, kCols > &matrix, const std::int32_t start_index, const std::int32_t stride, const DataStorageOrder storage_order)
	Sets data in an array from a given Eigen matrix. More...

template<typename ScalarT , std::int32_t kRows, std::int32_t kCols, std::size_t kSeqRowsSize, std::size_t kSeqColsSize>
Eigen::Matrix< ScalarT, static_cast< std::int32_t >kSeqRowsSize), static_cast< std::int32_t >kSeqColsSize)>	slice (const Eigen::Matrix< ScalarT, kRows, kCols > matrix, const std::array< Eigen::Index, kSeqRowsSize > &sequence_rows, const std::array< Eigen::Index, kSeqColsSize > &sequence_cols)
	Get a slice of a given matrix. More...

template<typename MeasurementT >
MeasurementT	transform_measurement (const MeasurementT &, const Eigen::Isometry3d &)
	Interface for transforming a measurement into a different coordinate system. More...

template<typename MeasurementT , typename MessageT >
MeasurementT	message_to_transformed_measurement (const MessageT &msg, const Eigen::Isometry3d &tf__world__frame_id)
	Convenience function for converting a message into a measurement and transforming it into a different coordinate frame. More...

template<>
MEASUREMENT_CONVERSION_PUBLIC PoseMeasurementXYZ64	transform_measurement (const PoseMeasurementXYZ64 &measurement, const Eigen::Isometry3d &tf__world__frame_id)
	Specialization of transform_measurement for pose measurement. More...

template<>
MEASUREMENT_CONVERSION_PUBLIC Stamped< PoseMeasurementXYZ64 >	transform_measurement (const Stamped< PoseMeasurementXYZ64 > &measurement, const Eigen::Isometry3d &tf__world__frame_id)
	Specialization of transform_measurement for stamped pose measurement. More...

template<typename MotionModelT , typename NoiseModelT >
auto	make_kalman_filter (const MotionModelT &motion_model, const NoiseModelT &noise_model, const typename MotionModelT::State &initial_state, const typename MotionModelT::State::Matrix &initial_covariance)
	A utility function that creates a Kalman filter. More...

template<typename StateT >
auto	make_correction_only_kalman_filter (const StateT &initial_state, const typename StateT::Matrix &initial_covariance)
	A utility function that creates a Kalman filter that is to be used for correction only, i.e., this Kalman filter cannot predict the state forward in time. More...

template<typename MotionModelT , typename NoiseModelT >
auto	make_kalman_filter (const MotionModelT &motion_model, const NoiseModelT &noise_model, const typename MotionModelT::State &initial_state, const std::vector< typename MotionModelT::State::Scalar > &initial_variances)
	A utility function that creates a Kalman filter from a vector of variances. More...

template<typename StateT , typename OtherScalarT >
auto	make_wiener_noise (const std::vector< OtherScalarT > &acceleration_variances)

template void	StateEstimationNode::create_subscriptions< StateEstimationNode::PoseMsgT > (const std::vector< std::string > &, std::vector< rclcpp::Subscription< PoseMsgT >::SharedPtr > *, CallbackFnT< StateEstimationNode::PoseMsgT >)

template void	StateEstimationNode::create_subscriptions< StateEstimationNode::RelativePosMsgT > (const std::vector< std::string > &, std::vector< rclcpp::Subscription< RelativePosMsgT >::SharedPtr > *, CallbackFnT< StateEstimationNode::RelativePosMsgT >)

Variables
template class STATE_ESTIMATION_NODES_PUBLIC	KalmanFilterWrapper< ConstAccelerationKalmanFilterXY >

template class STATE_ESTIMATION_NODES_PUBLIC	KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY >

Typedef Documentation

◆ ConstAccelerationKalmanFilterXY

using autoware::common::state_estimation::ConstAccelerationKalmanFilterXY = typedef KalmanFilter< motion_model::LinearMotionModel<state_vector::ConstAccelerationXY32>, WienerNoise<common::state_vector::ConstAccelerationXY32> >

◆ ConstAccelerationKalmanFilterXYZRPY

using autoware::common::state_estimation::ConstAccelerationKalmanFilterXYZRPY = typedef KalmanFilter< motion_model::LinearMotionModel<state_vector::ConstAccelerationXYZRPY32>, WienerNoise<common::state_vector::ConstAccelerationXYZRPY32> >

◆ ConstantAccelerationFilterWrapperXY

using autoware::common::state_estimation::ConstantAccelerationFilterWrapperXY = typedef KalmanFilterWrapper<ConstAccelerationKalmanFilterXY>

◆ ConstantAccelerationFilterWrapperXYZRPY

using autoware::common::state_estimation::ConstantAccelerationFilterWrapperXYZRPY = typedef KalmanFilterWrapper<ConstAccelerationKalmanFilterXYZRPY>

◆ PoseMeasurementXYZ

template<typename ScalarT >

using autoware::common::state_estimation::PoseMeasurementXYZ = typedef LinearMeasurement<state_vector::GenericState<ScalarT, state_vector::variable::X, state_vector::variable::Y, state_vector::variable::Z> >

◆ PoseMeasurementXYZ32

using autoware::common::state_estimation::PoseMeasurementXYZ32 = typedef PoseMeasurementXYZ<common::types::float32_t>

◆ PoseMeasurementXYZ64

using autoware::common::state_estimation::PoseMeasurementXYZ64 = typedef PoseMeasurementXYZ<common::types::float64_t>

◆ PoseMeasurementXYZRPY

template<typename ScalarT >

using autoware::common::state_estimation::PoseMeasurementXYZRPY = typedef LinearMeasurement<state_vector::GenericState<ScalarT, state_vector::variable::X, state_vector::variable::Y, state_vector::variable::Z, state_vector::variable::ROLL, state_vector::variable::PITCH, state_vector::variable::YAW> >

◆ PoseMeasurementXYZRPY32

using autoware::common::state_estimation::PoseMeasurementXYZRPY32 = typedef PoseMeasurementXYZRPY<common::types::float32_t>

◆ PoseMeasurementXYZRPY64

using autoware::common::state_estimation::PoseMeasurementXYZRPY64 = typedef PoseMeasurementXYZRPY<common::types::float64_t>

Enumeration Type Documentation

◆ DataStorageOrder

enum autoware::common::state_estimation::DataStorageOrder

strong

This class describes a data storage order.

Enumerator
kRowMajor
kColumnMajor

Function Documentation

◆ array_to_matrix()

template<std::int32_t kRows, std::int32_t kCols, typename ScalarT , std::size_t kSize>

Eigen::Matrix<ScalarT, kRows, kCols> autoware::common::state_estimation::array_to_matrix	(	const std::array< ScalarT, kSize > &	array,
		const std::int32_t	start_index,
		const std::int32_t	stride,
		const DataStorageOrder	storage_order
	)

Transform a given array to an Eigen matrix using the specified stride and a starting index within this array.

The function converts a given array into an Eigen matrix using the given starting index and stride. The function will take the number of rows and columns from the provided parameters and will iterate the given array as many times as needed to fill all elements of the resulting matrix. It is assumed that there are no gaps between the elements in a single row but that the columns are spaced stride apart.

Note: The storage order only refers to the storage order expected from the array, not from the Eigen matrix. The Eigen matrices are expected to be column-major as per default.

Exceptions

std::runtime_error if there is not enough elements in the array to perform a full conversion of all the asked elements.

Parameters

[in]	array	The given array (e.g. a covariance array from a message)
[in]	start_index	The start index of the first element in an array to be copied
[in]	stride	How big the step to the next row is in terms of indices in the array
[in]	storage_order	The storage order of the data in the input array (row-major for ROS)

Template Parameters

kRows	Number of rows in the resulting matrix
kCols	Number of columns in the resulting matrix
ScalarT	Scalar type (inferred)
kSize	Size of the input array (inferred)

Returns: An Eigen matrix that stores the requested data.

◆ downscale_isometry()

template<std::int32_t kStateDimensionality, typename FloatT >

static constexpr Eigen::Transform< FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry> autoware::common::state_estimation::downscale_isometry ( const Eigen::Transform< FloatT, 3, Eigen::TransformTraits::Isometry > & isometry )

staticconstexpr

Downscale the isometry to a lower dimension if needed.

Parameters

[in] isometry The isometry transform

Template Parameters

kStateDimensionality	Dimensionality of the space.
FloatT	Type of scalar.

Returns: Downscaled isometry.

◆ make_correction_only_kalman_filter()

template<typename StateT >

auto autoware::common::state_estimation::make_correction_only_kalman_filter	(	const StateT &	initial_state,
		const typename StateT::Matrix &	initial_covariance
	)

A utility function that creates a Kalman filter that is to be used for correction only, i.e., this Kalman filter cannot predict the state forward in time.

Mostly this is needed to avoid passing the template parameters explicitly and let the compiler infer them from the objects passed into this function.

Parameters

[in]	initial_state	The initial state
[in]	initial_covariance	The initial covariance

Template Parameters

StateT	{ description }
MotionModelT	Type of the motion model.
NoiseModelT	Type of the noise model.

Returns: Returns a valid KalmanFilter instance.

◆ make_kalman_filter() [1/2]

template<typename MotionModelT , typename NoiseModelT >

auto autoware::common::state_estimation::make_kalman_filter	(	const MotionModelT &	motion_model,
		const NoiseModelT &	noise_model,
		const typename MotionModelT::State &	initial_state,
		const std::vector< typename MotionModelT::State::Scalar > &	initial_variances
	)

A utility function that creates a Kalman filter from a vector of variances.

        Mostly this is needed to avoid passing the template parameters explicitly and let
        the compiler infer them from the objects passed into this function.

Parameters

[in]	motion_model	A motion model.
[in]	noise_model	A noise model.
[in]	initial_state	Initial state.
[in]	initial_variances	Initial variances as a vector.

Template Parameters

MotionModelT	Type of the motion model.
NoiseModelT	Type of the noise model.

Returns: Returns a valid KalmanFilter instance.

◆ make_kalman_filter() [2/2]

template<typename MotionModelT , typename NoiseModelT >

auto autoware::common::state_estimation::make_kalman_filter	(	const MotionModelT &	motion_model,
		const NoiseModelT &	noise_model,
		const typename MotionModelT::State &	initial_state,
		const typename MotionModelT::State::Matrix &	initial_covariance
	)

A utility function that creates a Kalman filter.

Mostly this is needed to avoid passing the template parameters explicitly and let the compiler infer them from the objects passed into this function.

Parameters

[in]	motion_model	A motion model.
[in]	noise_model	A noise model.
[in]	initial_state	The initial state
[in]	initial_covariance	The initial covariance

Template Parameters

MotionModelT	Type of the motion model.
NoiseModelT	Type of the noise model.

Returns: Returns a valid KalmanFilter instance.

◆ make_wiener_noise()

template<typename StateT , typename OtherScalarT >

auto autoware::common::state_estimation::make_wiener_noise ( const std::vector< OtherScalarT > & acceleration_variances )

◆ message_to_transformed_measurement()

template<typename MeasurementT , typename MessageT >

MeasurementT autoware::common::state_estimation::message_to_transformed_measurement	(	const MessageT &	msg,
		const Eigen::Isometry3d &	tf__world__frame_id
	)

Convenience function for converting a message into a measurement and transforming it into a different coordinate frame.

Template Parameters

MeasurementT	Type of measurement.
MessageT	Type of ROS 2 message.

Returns: The measurement created from a message.

◆ set_from_matrix()

template<std::int32_t kRows, std::int32_t kCols, typename ScalarT , std::size_t kSize>

void autoware::common::state_estimation::set_from_matrix	(	std::array< ScalarT, kSize > &	array,
		const Eigen::Matrix< ScalarT, kRows, kCols > &	matrix,
		const std::int32_t	start_index,
		const std::int32_t	stride,
		const DataStorageOrder	storage_order
	)

Sets data in an array from a given Eigen matrix.

Note: The storage order only refers to the storage order expected from the array, not from the Eigen matrix. The Eigen matrices are expected to be column-major as per default.

Parameters

	array	The array for which the data is set
[in]	matrix	The matrix that has the data that is to be copied
[in]	start_index	The start index in the output data array
[in]	stride	The step that brings the index to the next row/column
[in]	storage_order	The storage order of the output data

Template Parameters

kRows	Number of rows in the input matrix, inferred by the compiler.
kCols	Number of columns in the input matrix, inferred by the compiler.
ScalarT	Type of underlying data elements, inferred by the compiler.
kSize	Size of the input matrix, inferred by the compiler.

◆ slice()

template<typename ScalarT , std::int32_t kRows, std::int32_t kCols, std::size_t kSeqRowsSize, std::size_t kSeqColsSize>

Eigen::Matrix< ScalarT, static_cast<std::int32_t>kSeqRowsSize), static_cast<std::int32_t>kSeqColsSize)> autoware::common::state_estimation::slice	(	const Eigen::Matrix< ScalarT, kRows, kCols >	matrix,
		const std::array< Eigen::Index, kSeqRowsSize > &	sequence_rows,
		const std::array< Eigen::Index, kSeqColsSize > &	sequence_cols
	)

Get a slice of a given matrix.

Given two sequences for rows and for columns generate a new matrix that contains only rows and columns in those sequences.

Parameters

[in]	matrix	The given matrix
[in]	sequence_rows	The sequence of rows to include in the slice
[in]	sequence_cols	The sequence of columns to include in the slice

Template Parameters

ScalarT	Type of underlying data
kRows	Number of rows in a query matrix
kCols	Number of columns in a query matrix
kSeqRowsSize	Number of rows in the slice
kSeqColsSize	Number of columns in the slice

Returns: A matrix representing a slice of the original matrix.

◆ StateEstimationNode::create_subscriptions< StateEstimationNode::PoseMsgT >()

template void autoware::common::state_estimation::StateEstimationNode::create_subscriptions< StateEstimationNode::PoseMsgT >	(	const std::vector< std::string > &	,
		std::vector< rclcpp::Subscription< PoseMsgT >::SharedPtr > *	,
		CallbackFnT< StateEstimationNode::PoseMsgT >
	)

◆ StateEstimationNode::create_subscriptions< StateEstimationNode::RelativePosMsgT >()

template void autoware::common::state_estimation::StateEstimationNode::create_subscriptions< StateEstimationNode::RelativePosMsgT >	(	const std::vector< std::string > &	,
		std::vector< rclcpp::Subscription< RelativePosMsgT >::SharedPtr > *	,
		CallbackFnT< StateEstimationNode::RelativePosMsgT >
	)

◆ transform_measurement() [1/3]

template<typename MeasurementT >

MeasurementT autoware::common::state_estimation::transform_measurement	(	const MeasurementT &	,
		const Eigen::Isometry3d &
	)

Interface for transforming a measurement into a different coordinate system.

Template Parameters

MeasurementT Type of measurement.

Returns: The measurement, but transformed

◆ transform_measurement() [2/3]

template<>

PoseMeasurementXYZ64 autoware::common::state_estimation::transform_measurement	(	const PoseMeasurementXYZ64 &	measurement,
		const Eigen::Isometry3d &	tf__world__frame_id
	)

Specialization of transform_measurement for pose measurement.

◆ transform_measurement() [3/3]

template<>

Stamped< PoseMeasurementXYZ64 > autoware::common::state_estimation::transform_measurement	(	const Stamped< PoseMeasurementXYZ64 > &	measurement,
		const Eigen::Isometry3d &	tf__world__frame_id
	)

Specialization of transform_measurement for stamped pose measurement.

Variable Documentation

◆ KalmanFilterWrapper< ConstAccelerationKalmanFilterXY >

template class STATE_ESTIMATION_NODES_PUBLIC autoware::common::state_estimation::KalmanFilterWrapper< ConstAccelerationKalmanFilterXY >

◆ KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY >

template class STATE_ESTIMATION_NODES_PUBLIC autoware::common::state_estimation::KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY >

Namespaces

Classes

Typedefs

Enumerations

Functions

Variables

Typedef Documentation

◆ ConstAccelerationKalmanFilterXY

◆ ConstAccelerationKalmanFilterXYZRPY

◆ ConstantAccelerationFilterWrapperXY

◆ ConstantAccelerationFilterWrapperXYZRPY

◆ PoseMeasurementXYZ

◆ PoseMeasurementXYZ32

◆ PoseMeasurementXYZ64

◆ PoseMeasurementXYZRPY

◆ PoseMeasurementXYZRPY32

◆ PoseMeasurementXYZRPY64

Enumeration Type Documentation

◆ DataStorageOrder

Function Documentation

◆ array_to_matrix()

◆ downscale_isometry()

◆ make_correction_only_kalman_filter()

◆ make_kalman_filter() [1/2]

◆ make_kalman_filter() [2/2]

◆ make_wiener_noise()

◆ message_to_transformed_measurement()

◆ set_from_matrix()

◆ slice()

◆ StateEstimationNode::create_subscriptions< StateEstimationNode::PoseMsgT >()

◆ StateEstimationNode::create_subscriptions< StateEstimationNode::RelativePosMsgT >()

◆ transform_measurement() [1/3]

◆ transform_measurement() [2/3]

◆ transform_measurement() [3/3]

Variable Documentation

◆ KalmanFilterWrapper< ConstAccelerationKalmanFilterXY >

◆ KalmanFilterWrapper< ConstAccelerationKalmanFilterXYZRPY >