Extended Kalman Filter class. More...

#include <ekf.hpp>

Collaboration diagram for ExtendedKalmanFilter:

Public Member Functions
	ExtendedKalmanFilter (std::shared_ptr< ObservationModel > observation_model, std::shared_ptr< DataAssociationModel > data_association_model)
	Extended Kalman Filter constructor declaration.

void	update (std::shared_ptr< common_lib::structures::VehicleState > vehicle_state, std::shared_ptr< std::vector< common_lib::structures::Cone > > track_map)
	Updates vehicle state and map variables according to the state vector x_vector

void	prediction_step (const MotionUpdate &motion_update, const std::string &sensor_type)
	Prediction step:

void	correct_with_matched_ids (const std::vector< int > &matched_ids, const std::vector< Eigen::Vector2f > &matched_cone_positions)
	Correct the state vector with matched cones, 2 vector inputs the first is the id of the cone in the map matched and the second is the position (observed) of the cone.

void	correct_with_matched_ids_full_state (const std::vector< int > &matched_ids, const std::vector< Eigen::Vector2f > &matched_cone_positions)
	Correct the state vector with matched cones, 2 vector inputs the first is the id of the cone in the map matched and the second is the position (observed) of the cone.

void	augment_state (const std::vector< Eigen::Vector2f > &new_features)
	Augment the state vector with new features, vector input has the new features.

void	add_motion_model (const std::string &model_name, std::shared_ptr< MotionModel > motion_model)
	Add a motion model to the motion model map.

void	correction_step (const std::vector< common_lib::structures::Cone > &perception_map)
	Correction step:

Eigen::VectorXf	get_state () const
	Get the state vector.

Eigen::MatrixXf	get_covariance () const
	Get the state covariance matrix.

rclcpp::Time	get_last_update () const
	Get the last update timestamp.

Private Member Functions
Eigen::MatrixXf	get_kalman_gain (const Eigen::MatrixXf &H, const Eigen::MatrixXf &_p_matrix_, const Eigen::MatrixXf &Q) const
	Flags used to mark first prediction step.

Private Attributes
Eigen::VectorXf	_x_vector_
	Expected state vector (localization + mapping)

Eigen::MatrixXf	_p_matrix_ = Eigen::MatrixXf::Identity(6, 6)

rclcpp::Time	_last_update_ = rclcpp::Clock().now()
	Timestamp of last update.

std::map< std::string, std::shared_ptr< MotionModel > >	_motion_models_

std::shared_ptr< ObservationModel >	_observation_model_
	Observation Model chosen for correction step.

std::shared_ptr< DataAssociationModel >	_data_association_model_
	Data Association Model.

bool	_fixed_map = false
	Flag to indicate if the map is fixed.

bool	_first_prediction_ = true

Friends
class	PerformanceTest

Detailed Description

Extended Kalman Filter class.

The Extended Kalman Filter (EKF) is a recursive state estimator for nonlinear systems. It is a nonlinear version of the Kalman Filter (KF). This implementation is used to perform state estimation for the localization of the vehicle and the map.

Definition at line 28 of file ekf.hpp.

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter()

ExtendedKalmanFilter::ExtendedKalmanFilter	(	std::shared_ptr< ObservationModel >	observation_model,
		std::shared_ptr< DataAssociationModel >	data_association_model
	)

Extended Kalman Filter constructor declaration.

Parameters

motion_model	motion model chosen for prediction step
observation_model	observation model chosen for correction step

Definition at line 14 of file ekf.cpp.

Member Function Documentation

◆ add_motion_model()

void ExtendedKalmanFilter::add_motion_model	(	const std::string &	model_name,
		std::shared_ptr< MotionModel >	motion_model
	)

inline

Add a motion model to the motion model map.

Parameters

model_name	name of the motion model
motion_model	motion model

Definition at line 116 of file ekf.hpp.

◆ augment_state()

void ExtendedKalmanFilter::augment_state ( const std::vector< Eigen::Vector2f > & new_features )

Augment the state vector with new features, vector input has the new features.

Parameters

new_features vector of new features

Definition at line 114 of file ekf.cpp.

Here is the caller graph for this function:

◆ correct_with_matched_ids()

void ExtendedKalmanFilter::correct_with_matched_ids	(	const std::vector< int > &	matched_ids,
		const std::vector< Eigen::Vector2f > &	matched_cone_positions
	)

Correct the state vector with matched cones, 2 vector inputs the first is the id of the cone in the map matched and the second is the position (observed) of the cone.

Parameters

matched_ids	vector of matched ids
matched_cone_positions	vector of matched cone positions

Definition at line 73 of file ekf.cpp.

Here is the call graph for this function:

◆ correct_with_matched_ids_full_state()

void ExtendedKalmanFilter::correct_with_matched_ids_full_state	(	const std::vector< int > &	matched_ids,
		const std::vector< Eigen::Vector2f > &	matched_cone_positions
	)

Correct the state vector with matched cones, 2 vector inputs the first is the id of the cone in the map matched and the second is the position (observed) of the cone.

Parameters

matched_ids	vector of matched ids
matched_cone_positions	vector of matched cone positions

Definition at line 95 of file ekf.cpp.

Here is the caller graph for this function:

◆ correction_step()

void ExtendedKalmanFilter::correction_step ( const std::vector< common_lib::structures::Cone > & perception_map )

Correction step:

Calculate the expected measurement
Calculate the Kalman Gain
Calculate the expected measurement covariance matrix
Augment state to include the newly detected cones

Parameters

perception_map map from perception

Definition at line 53 of file ekf.cpp.

Here is the call graph for this function:

◆ get_covariance()

Eigen::MatrixXf ExtendedKalmanFilter::get_covariance ( ) const

inline

Get the state covariance matrix.

Returns: Eigen::MatrixXf covariance matrix

Definition at line 143 of file ekf.hpp.

◆ get_kalman_gain()

Eigen::MatrixXf ExtendedKalmanFilter::get_kalman_gain	(	const Eigen::MatrixXf &	H,
		const Eigen::MatrixXf &	_p_matrix_,
		const Eigen::MatrixXf &	Q
	)		const

private

Flags used to mark first prediction step.

Calculate the kalman gain

Parameters

H	jacobian observation model matrix
_p_matrix_	state covariance matrix
Q	measurement noise matrix

Returns: Eigen::MatrixXf kalman gain matrix

Definition at line 165 of file ekf.cpp.

Here is the caller graph for this function:

◆ get_last_update()

rclcpp::Time ExtendedKalmanFilter::get_last_update ( ) const

inline

Get the last update timestamp.

Returns: rclcpp::Time timestamp

Definition at line 151 of file ekf.hpp.

◆ get_state()

Eigen::VectorXf ExtendedKalmanFilter::get_state ( ) const

inline

Get the state vector.

Returns: Eigen::VectorXf state vector

Definition at line 136 of file ekf.hpp.

◆ prediction_step()

void ExtendedKalmanFilter::prediction_step	(	const MotionUpdate &	motion_update,
		const std::string &	sensor_type
	)

Prediction step:

Calculate the expected state regarding pose estimates
Calculate the expected state covariance matrix regarding pose estimates

Parameters

motion_update data of motion (velocities)

Definition at line 23 of file ekf.cpp.

◆ update()

void ExtendedKalmanFilter::update	(	std::shared_ptr< common_lib::structures::VehicleState >	vehicle_state,
		std::shared_ptr< std::vector< common_lib::structures::Cone > >	track_map
	)

Updates vehicle state and map variables according to the state vector x_vector

Parameters

vehicle_state	pose
track_map	map

Definition at line 178 of file ekf.cpp.

Here is the caller graph for this function:

Friends And Related Symbol Documentation

◆ PerformanceTest

friend class PerformanceTest

friend

Definition at line 153 of file ekf.hpp.

Member Data Documentation

◆ _data_association_model_

std::shared_ptr<DataAssociationModel> ExtendedKalmanFilter::_data_association_model_

private

Data Association Model.

Definition at line 37 of file ekf.hpp.

◆ _first_prediction_

bool ExtendedKalmanFilter::_first_prediction_ = true

private

Definition at line 40 of file ekf.hpp.

◆ _fixed_map

bool ExtendedKalmanFilter::_fixed_map = false

private

Flag to indicate if the map is fixed.

Definition at line 39 of file ekf.hpp.

◆ _last_update_

rclcpp::Time ExtendedKalmanFilter::_last_update_ = rclcpp::Clock().now()

private

Timestamp of last update.

Definition at line 32 of file ekf.hpp.

◆ _motion_models_

std::map<std::string, std::shared_ptr<MotionModel> > ExtendedKalmanFilter::_motion_models_

private

Definition at line 34 of file ekf.hpp.

◆ _observation_model_

std::shared_ptr<ObservationModel> ExtendedKalmanFilter::_observation_model_

private

Observation Model chosen for correction step.

Definition at line 36 of file ekf.hpp.

◆ _p_matrix_

Eigen::MatrixXf ExtendedKalmanFilter::_p_matrix_ = Eigen::MatrixXf::Identity(6, 6)

private

Definition at line 31 of file ekf.hpp.

◆ _x_vector_

Eigen::VectorXf ExtendedKalmanFilter::_x_vector_

private

Initial value:

=

Eigen::VectorXf::Zero(6)

Expected state vector (localization + mapping)

Definition at line 29 of file ekf.hpp.

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

src/ekf_state_est/include/kalman_filter/ekf.hpp
src/ekf_state_est/src/kalman_filter/ekf.cpp

Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ ExtendedKalmanFilter()

Member Function Documentation

◆ add_motion_model()

◆ augment_state()

◆ correct_with_matched_ids()

◆ correct_with_matched_ids_full_state()

◆ correction_step()

◆ get_covariance()

◆ get_kalman_gain()

◆ get_last_update()

◆ get_state()

◆ prediction_step()

◆ update()

Friends And Related Symbol Documentation

◆ PerformanceTest

Member Data Documentation

◆ _data_association_model_

◆ _first_prediction_

◆ _fixed_map

◆ _last_update_

◆ _motion_models_

◆ _observation_model_

◆ _p_matrix_

◆ _x_vector_