hololib::PoseEKF Class Reference

Extended Kalman Filter implementation for maintaining the robot's pose. More...

#include <PoseEKF.hpp>

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	PoseEKF (float initial_x, float initial_y, float initial_theta)
	Construct a new Pose EKF object.
void	setProcessNoise (float xNoise, float yNoise, float thetaNoise, float measNoise)
	Set the process noise values for the Kalman filter matrices.
void	predict (float dx_local, float dy_local, float dtheta)
	The prediction step of the EKF based on local relative movements.
void	updateIMU (float measured_theta, float dynamic_R)
	Update the state using absolute heading measured by an IMU.
void	updateTrackingWheels (const std::vector< TrackingWheelConfig > &configs, const Eigen::VectorXf &measured_deltas, float dx_local, float dy_local, float dtheta, float wheel_noise)
	Update the state using unpowered tracking wheels data.
void	setTrackingWheelNoise (float xNoise, float yNoise, float thetaNoise)
	Set specific process noises when using tracking wheels.
float	getX () const
	Get current X position.
float	getY () const
	Get current Y position.
float	getTheta () const
	Get current heading angle.
void	setPose (float new_x, float new_y, float new_theta)
	Reset the state to a specific pose.

Detailed Description

Extended Kalman Filter implementation for maintaining the robot's pose.

Fuses IMU heading and tracking wheel (or motor encoder) deltas to estimate X, Y, and Theta coordinates on the field.

Definition at line 18 of file PoseEKF.hpp.

Constructor & Destructor Documentation

PoseEKF()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW hololib::PoseEKF::PoseEKF	(	float	initial_x,
		float	initial_y,
		float	initial_theta
	)

Construct a new Pose EKF object.

Parameters

initial_x	Starting X position.
initial_y	Starting Y position.
initial_theta	Starting heading.

Member Function Documentation

setProcessNoise()

void hololib::PoseEKF::setProcessNoise	(	float	xNoise,
		float	yNoise,
		float	thetaNoise,
		float	measNoise
	)

Set the process noise values for the Kalman filter matrices.

Parameters

xNoise	Process noise in the X direction.
yNoise	Process noise in the Y direction.
thetaNoise	Process noise for the heading.
measNoise	Measurement noise parameter.

predict()

void hololib::PoseEKF::predict	(	float	dx_local,
		float	dy_local,
		float	dtheta
	)

The prediction step of the EKF based on local relative movements.

Parameters

dx_local	Change in local X position.
dy_local	Change in local Y position.
dtheta	Change in heading.

updateIMU()

void hololib::PoseEKF::updateIMU	(	float	measured_theta,
		float	dynamic_R
	)

Update the state using absolute heading measured by an IMU.

Parameters

measured_theta	Heading read from the IMU.
dynamic_R	Dynamic measurement noise.

updateTrackingWheels()

void hololib::PoseEKF::updateTrackingWheels	(	const std::vector< TrackingWheelConfig > &	configs,
		const Eigen::VectorXf &	measured_deltas,
		float	dx_local,
		float	dy_local,
		float	dtheta,
		float	wheel_noise
	)

Update the state using unpowered tracking wheels data.

Parameters

configs	Vector of tracking wheel configurations.
measured_deltas	Measurement differences for each wheel since the last step.
dx_local	Estimated local X change.
dy_local	Estimated local Y change.
dtheta	Estimated heading change.
wheel_noise	Sensor noise for the tracking wheels.

setTrackingWheelNoise()

void hololib::PoseEKF::setTrackingWheelNoise	(	float	xNoise,
		float	yNoise,
		float	thetaNoise
	)

Set specific process noises when using tracking wheels.

Parameters

xNoise	Process noise in X direction.
yNoise	Process noise in Y direction.
thetaNoise	Process noise for the heading.

getX()

float hololib::PoseEKF::getX ( ) const

inline

Get current X position.

Returns

float The X position.

Definition at line 92 of file PoseEKF.hpp.

getY()

float hololib::PoseEKF::getY ( ) const

inline

Get current Y position.

Returns

float The Y position.

Definition at line 98 of file PoseEKF.hpp.

getTheta()

float hololib::PoseEKF::getTheta ( ) const

inline

Get current heading angle.

Returns

float The heading in radians.

Definition at line 104 of file PoseEKF.hpp.

setPose()

void hololib::PoseEKF::setPose	(	float	new_x,
		float	new_y,
		float	new_theta
	)

Reset the state to a specific pose.

Parameters

new_x	New X position.
new_y	New Y position.
new_theta	New heading angle.