#include <ekf.hpp>

Inheritance diagram for EKF:

Collaboration diagram for EKF:

Public Member Functions
	EKF (const VEParameters &params)

void	imu_callback (const common_lib::sensor_data::ImuData &imu_data) override
	Callback function for the IMU data that should be called by adapters when new IMU data is available.

void	wss_callback (const common_lib::sensor_data::WheelEncoderData &wss_data) override
	Callback function for the wheel speed sensor data that should be called by adapters when new wheel speed sensor data is available.

void	motor_rpm_callback (double motor_rpm) override
	Callback function for the motor RPM data that should be called by adapters when new motor RPM data is available.

void	steering_callback (double steering_angle) override
	Callback function for the steering angle data that should be called by adapters when new steering angle data is available.

common_lib::structures::Velocities	get_velocities () override

Private Member Functions
void	predict (Eigen::Vector3d &state, Eigen::Matrix3d &covariance, const Eigen::Matrix3d &process_noise_matrix, const rclcpp::Time last_update, common_lib::sensor_data::ImuData &imu_data)
	Predict velocities at the next index based on IMU measurements and current state.

void	correct_wheels (Eigen::Vector3d &state, Eigen::Matrix3d &covariance, common_lib::sensor_data::WheelEncoderData &wss_data, double motor_rpm, double steering_angle)
	Correct the state estimate based on wheel speed sensor, resolver, and steering data.

void	correct_imu (Eigen::Vector3d &state, Eigen::Matrix3d &covariance, common_lib::sensor_data::ImuData &imu_data)

Private Attributes
rclcpp::Time	_last_update_

Eigen::Vector3d	_state_ = Eigen::Vector3d::Zero()

Eigen::Matrix3d	_covariance_ = Eigen::Matrix3d::Identity()

Eigen::Matrix3d	_process_noise_matrix_

Eigen::MatrixXd	_wheels_measurement_noise_matrix_

Eigen::MatrixXd	_imu_measurement_noise_matrix_

common_lib::sensor_data::ImuData	imu_data_

common_lib::sensor_data::WheelEncoderData	wss_data_

double	motor_rpm_

double	steering_angle_

bool	_has_made_prediction_ = false

bool	imu_data_received_ = false

bool	wss_data_received_ = false

bool	motor_rpm_received_ = false

bool	steering_angle_received_ = false

common_lib::car_parameters::CarParameters	car_parameters_

std::shared_ptr< VEObservationModel >	observation_model_

std::shared_ptr< BaseVelocityProcessModel >	process_model

Detailed Description

Definition at line 17 of file ekf.hpp.

Constructor & Destructor Documentation

◆ EKF()

EKF::EKF ( const VEParameters & params )

Definition at line 3 of file ekf.cpp.

Member Function Documentation

◆ correct_imu()

void EKF::correct_imu	(	Eigen::Vector3d &	state,
		Eigen::Matrix3d &	covariance,
		common_lib::sensor_data::ImuData &	imu_data
	)

private

Definition at line 164 of file ekf.cpp.

Here is the caller graph for this function:

◆ correct_wheels()

void EKF::correct_wheels	(	Eigen::Vector3d &	state,
		Eigen::Matrix3d &	covariance,
		common_lib::sensor_data::WheelEncoderData &	wss_data,
		double	motor_rpm,
		double	steering_angle
	)

private

Correct the state estimate based on wheel speed sensor, resolver, and steering data.

This function updates the state and covariance estimates using measurements from the wheel speed sensor, resolver, and steering angle sensor. It corrects the predicted state to better match the observed measurements.

Parameters

state	Vector of velocities {velocity_x, velocity_y, rotational_velocity}
covariance	Covariance matrix representing the uncertainty in the state estimate.
wss_data	Wheel speed sensor data containing wheel speeds.
motor_rpm	data representing the motor's rpms.
steering_angle	data representing the steering angle.

Definition at line 130 of file ekf.cpp.

Here is the caller graph for this function:

◆ get_velocities()

common_lib::structures::Velocities EKF::get_velocities ( )

overridevirtual

Implements VelocityEstimator.

Definition at line 96 of file ekf.cpp.

◆ imu_callback()

void EKF::imu_callback ( const common_lib::sensor_data::ImuData & imu_data )

overridevirtual

Callback function for the IMU data that should be called by adapters when new IMU data is available.

Implements VelocityEstimator.

Definition at line 23 of file ekf.cpp.

Here is the call graph for this function:

◆ motor_rpm_callback()

void EKF::motor_rpm_callback ( double motor_rpm )

overridevirtual

Callback function for the motor RPM data that should be called by adapters when new motor RPM data is available.

Implements VelocityEstimator.

Definition at line 64 of file ekf.cpp.

Here is the call graph for this function:

◆ predict()

void EKF::predict	(	Eigen::Vector3d &	state,
		Eigen::Matrix3d &	covariance,
		const Eigen::Matrix3d &	process_noise_matrix,
		const rclcpp::Time	last_update,
		common_lib::sensor_data::ImuData &	imu_data
	)

private

Predict velocities at the next index based on IMU measurements and current state.

Parameters

state	Vector of velocities {velocity_x, velocity_y, rotational_velocity}
covariance	Covariance matrix representing the uncertainty in the state estimate.
process_noise_matrix	Process noise matrix representing the uncertainty in the process model.
last_update	Time point of the last update.
imu_data	IMU data containing acceleration and rotational velocity measurements.

Definition at line 108 of file ekf.cpp.

Here is the caller graph for this function:

◆ steering_callback()

void EKF::steering_callback ( double steering_angle )

overridevirtual

Callback function for the steering angle data that should be called by adapters when new steering angle data is available.

Implements VelocityEstimator.

Definition at line 80 of file ekf.cpp.

Here is the call graph for this function:

◆ wss_callback()

void EKF::wss_callback ( const common_lib::sensor_data::WheelEncoderData & wss_data )

overridevirtual

Callback function for the wheel speed sensor data that should be called by adapters when new wheel speed sensor data is available.

Implements VelocityEstimator.

Definition at line 48 of file ekf.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ _covariance_

Eigen::Matrix3d EKF::_covariance_ = Eigen::Matrix3d::Identity()

private

Definition at line 20 of file ekf.hpp.

◆ _has_made_prediction_

bool EKF::_has_made_prediction_ = false

private

Definition at line 30 of file ekf.hpp.

◆ _imu_measurement_noise_matrix_

Eigen::MatrixXd EKF::_imu_measurement_noise_matrix_

private

Definition at line 23 of file ekf.hpp.

◆ _last_update_

rclcpp::Time EKF::_last_update_

private

Definition at line 18 of file ekf.hpp.

◆ _process_noise_matrix_

Eigen::Matrix3d EKF::_process_noise_matrix_

private

Definition at line 21 of file ekf.hpp.

◆ _state_

Eigen::Vector3d EKF::_state_ = Eigen::Vector3d::Zero()

private

Definition at line 19 of file ekf.hpp.

◆ _wheels_measurement_noise_matrix_

Eigen::MatrixXd EKF::_wheels_measurement_noise_matrix_

private

Definition at line 22 of file ekf.hpp.

◆ car_parameters_

common_lib::car_parameters::CarParameters EKF::car_parameters_

private

Definition at line 40 of file ekf.hpp.

◆ imu_data_

common_lib::sensor_data::ImuData EKF::imu_data_

private

Definition at line 25 of file ekf.hpp.

◆ imu_data_received_

bool EKF::imu_data_received_ = false

private

Definition at line 35 of file ekf.hpp.

◆ motor_rpm_

double EKF::motor_rpm_

private

Definition at line 27 of file ekf.hpp.

◆ motor_rpm_received_

bool EKF::motor_rpm_received_ = false

private

Definition at line 37 of file ekf.hpp.

◆ observation_model_

std::shared_ptr<VEObservationModel> EKF::observation_model_

private

Definition at line 41 of file ekf.hpp.

◆ process_model

std::shared_ptr<BaseVelocityProcessModel> EKF::process_model

private

Definition at line 42 of file ekf.hpp.

◆ steering_angle_

double EKF::steering_angle_

private

Definition at line 28 of file ekf.hpp.

◆ steering_angle_received_

bool EKF::steering_angle_received_ = false

private

Definition at line 38 of file ekf.hpp.

◆ wss_data_

common_lib::sensor_data::WheelEncoderData EKF::wss_data_

private

Definition at line 26 of file ekf.hpp.

◆ wss_data_received_

bool EKF::wss_data_received_ = false

private

Definition at line 36 of file ekf.hpp.

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

src/velocity_estimation/include/estimators/ekf.hpp
src/velocity_estimation/src/estimators/ekf.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ EKF()

Member Function Documentation

◆ correct_imu()

◆ correct_wheels()

◆ get_velocities()

◆ imu_callback()

◆ motor_rpm_callback()

◆ predict()

◆ steering_callback()

◆ wss_callback()

Member Data Documentation

◆ _covariance_

◆ _has_made_prediction_

◆ _imu_measurement_noise_matrix_

◆ _last_update_

◆ _process_noise_matrix_

◆ _state_

◆ _wheels_measurement_noise_matrix_

◆ car_parameters_

◆ imu_data_

◆ imu_data_received_

◆ motor_rpm_

◆ motor_rpm_received_

◆ observation_model_

◆ process_model

◆ steering_angle_

◆ steering_angle_received_

◆ wss_data_

◆ wss_data_received_