#include <no_rear_wss_ekf.hpp>

Inheritance diagram for NoRearWSSEKF:

Collaboration diagram for NoRearWSSEKF:

Public Member Functions
	NoRearWSSEKF (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() * 0.05

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 no_rear_wss_ekf.hpp.

Constructor & Destructor Documentation

◆ NoRearWSSEKF()

NoRearWSSEKF::NoRearWSSEKF ( const VEParameters & params )

Definition at line 3 of file no_rear_wss_ekf.cpp.

Member Function Documentation

◆ correct_imu()

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

private

Definition at line 169 of file no_rear_wss_ekf.cpp.

Here is the caller graph for this function:

◆ correct_wheels()

void NoRearWSSEKF::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 125 of file no_rear_wss_ekf.cpp.

Here is the caller graph for this function:

◆ get_velocities()

common_lib::structures::Velocities NoRearWSSEKF::get_velocities ( )

overridevirtual

Implements VelocityEstimator.

Definition at line 94 of file no_rear_wss_ekf.cpp.

◆ imu_callback()

void NoRearWSSEKF::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 21 of file no_rear_wss_ekf.cpp.

Here is the call graph for this function:

◆ motor_rpm_callback()

void NoRearWSSEKF::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 62 of file no_rear_wss_ekf.cpp.

Here is the call graph for this function:

◆ predict()

void NoRearWSSEKF::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 106 of file no_rear_wss_ekf.cpp.

◆ steering_callback()

void NoRearWSSEKF::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 78 of file no_rear_wss_ekf.cpp.

Here is the call graph for this function:

◆ wss_callback()

void NoRearWSSEKF::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 46 of file no_rear_wss_ekf.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ _covariance_

Eigen::Matrix3d NoRearWSSEKF::_covariance_ = Eigen::Matrix3d::Identity() * 0.05

private

Definition at line 20 of file no_rear_wss_ekf.hpp.

◆ _has_made_prediction_

bool NoRearWSSEKF::_has_made_prediction_ = false

private

Definition at line 30 of file no_rear_wss_ekf.hpp.

◆ _imu_measurement_noise_matrix_

Eigen::MatrixXd NoRearWSSEKF::_imu_measurement_noise_matrix_

private

Definition at line 23 of file no_rear_wss_ekf.hpp.

◆ _last_update_

rclcpp::Time NoRearWSSEKF::_last_update_

private

Definition at line 18 of file no_rear_wss_ekf.hpp.

◆ _process_noise_matrix_

Eigen::Matrix3d NoRearWSSEKF::_process_noise_matrix_

private

Definition at line 21 of file no_rear_wss_ekf.hpp.

◆ _state_

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

private

Definition at line 19 of file no_rear_wss_ekf.hpp.

◆ _wheels_measurement_noise_matrix_

Eigen::MatrixXd NoRearWSSEKF::_wheels_measurement_noise_matrix_

private

Definition at line 22 of file no_rear_wss_ekf.hpp.

◆ car_parameters_

common_lib::car_parameters::CarParameters NoRearWSSEKF::car_parameters_

private

Definition at line 40 of file no_rear_wss_ekf.hpp.

◆ imu_data_

common_lib::sensor_data::ImuData NoRearWSSEKF::imu_data_

private

Definition at line 25 of file no_rear_wss_ekf.hpp.

◆ imu_data_received_

bool NoRearWSSEKF::imu_data_received_ = false

private

Definition at line 35 of file no_rear_wss_ekf.hpp.

◆ motor_rpm_

double NoRearWSSEKF::motor_rpm_

private

Definition at line 27 of file no_rear_wss_ekf.hpp.

◆ motor_rpm_received_

bool NoRearWSSEKF::motor_rpm_received_ = false

private

Definition at line 37 of file no_rear_wss_ekf.hpp.

◆ observation_model_

std::shared_ptr<VEObservationModel> NoRearWSSEKF::observation_model_

private

Definition at line 41 of file no_rear_wss_ekf.hpp.

◆ process_model

std::shared_ptr<BaseVelocityProcessModel> NoRearWSSEKF::process_model

private

Definition at line 42 of file no_rear_wss_ekf.hpp.

◆ steering_angle_

double NoRearWSSEKF::steering_angle_

private

Definition at line 28 of file no_rear_wss_ekf.hpp.

◆ steering_angle_received_

bool NoRearWSSEKF::steering_angle_received_ = false

private

Definition at line 38 of file no_rear_wss_ekf.hpp.

◆ wss_data_

common_lib::sensor_data::WheelEncoderData NoRearWSSEKF::wss_data_

private

Definition at line 26 of file no_rear_wss_ekf.hpp.

◆ wss_data_received_

bool NoRearWSSEKF::wss_data_received_ = false

private

Definition at line 36 of file no_rear_wss_ekf.hpp.

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

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NoRearWSSEKF()

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_