#include <filter_base.h>

Inheritance diagram for RobotLocalization::FilterBase:

Public Member Functions
virtual void	enqueueMeasurement (const std::string &topicName, const Eigen::VectorXd &measurement, const Eigen::MatrixXd &measurementCovariance, const std::vector< int > &updateVector, const double time)
	Adds a measurement to the queue of measurements to be processed.
	FilterBase ()
	Constructor for the FilterBase class.
bool	getDebug ()
	Gets the value of the debug_ variable.
const Eigen::MatrixXd &	getEstimateErrorCovariance ()
	Gets the estimate error covariance.
bool	getInitializedStatus ()
	Gets the filter's initialized status.
double	getLastMeasurementTime ()
	Gets the most recent measurement time.
double	getLastUpdateTime ()
	Gets the filter's last update time.
const Eigen::MatrixXd &	getProcessNoiseCovariance ()
	Gets the filter's process noise covariance.
double	getSensorTimeout ()
	Gets the sensor timeout value (in seconds)
const Eigen::VectorXd &	getState ()
	Gets the filter state.
virtual void	integrateMeasurements (double currentTime, std::map< std::string, Eigen::VectorXd > &postUpdateStates)
	Processes all measurements in the measurement queue, in temporal order.
void	setDebug (const bool debug, std::ostream *outStream=NULL)
	Sets the filter into debug mode.
void	setEstimateErrorCovariance (const Eigen::MatrixXd &estimateErrorCovariance)
	Manually sets the filter's estimate error covariance.
void	setLastMeasurementTime (const double lastMeasurementTime)
	Sets the filter's last measurement time.
void	setLastUpdateTime (const double lastUpdateTime)
	Sets the filter's last update time.
void	setProcessNoiseCovariance (const Eigen::MatrixXd &processNoiseCovariance)
	Sets the process noise covariance for the filter.
void	setSensorTimeout (const double sensorTimeout)
	Sets the sensor timeout.
void	setState (const Eigen::VectorXd &state)
	Manually sets the filter's state.
	~FilterBase ()
	Destructor for the FilterBase class.
Protected Member Functions
virtual void	correct (const Measurement &measurement)=0
	Carries out the correct step in the predict/update cycle. This method must be implemented by subclasses.
virtual void	predict (const double delta)=0
	Carries out the predict step in the predict/update cycle. Projects the state and error matrices forward using a model of the vehicle's motion. This method must be implemented by subclasses.
virtual void	processMeasurement (const Measurement &measurement)
	Does some final preprocessing, carries out the predict/update cycle.
void	validateDelta (double &delta)
	Ensures a given time delta is valid (helps with bag file playback issues)
virtual void	wrapStateAngles ()
	Keeps the state angles to reasonable values.
Protected Attributes
Eigen::MatrixXd	covarianceEpsilon_
	Covariance matrices can be incredibly unstable. We can add a small value to it at each iteration to help maintain its positive-definite property.
std::ostream *	debugStream_
	Used for outputting debug messages.
Eigen::MatrixXd	estimateErrorCovariance_
	This matrix stores the total error in our position estimate (the state_ variable).
Eigen::MatrixXd	identity_
	We need the identity for a few operations. Better to store it.
bool	initialized_
	Whether or not we've received any measurements.
double	lastMeasurementTime_
	Tracks the time the filter was last updated using a measurement.
double	lastUpdateTime_
	Used for tracking the latest update time as determined by this class.
std::priority_queue < Measurement, std::vector < Measurement >, Measurement >	measurementQueue_
	We process measurements based on their timestamp.
const double	pi_
	Commonly used constants.
Eigen::MatrixXd	processNoiseCovariance_
	As we move through the world, we follow a predict/update cycle. If one were to imagine a scenario where all we did was make predictions without correcting, the error in our position estimate would grow without bound. This error is stored in the stateEstimateCovariance_ matrix. However, this matrix doesn't answer the question of how much our error should grow for each time step. That's where the processNoiseCovariance matrix comes in. When we make a prediction using the transfer function, we add this matrix (times deltaT) to the state estimate covariance matrix.
double	sensorTimeout_
	The updates to the filter - both predict and correct - are driven by measurements. If we get a gap in measurements for some reason, we want the filter to continue estimating. When this gap occurs, as specified by this timeout, we will continue to call predict() at the filter's frequency.
Eigen::VectorXd	state_
	This is the robot's state vector, which is what we are trying to filter. The values in this vector are what get reported by the node.
const double	tau_
Eigen::MatrixXd	transferFunction_
	The Kalman filter transfer function.
Eigen::MatrixXd	transferFunctionJacobian_
	The Kalman filter transfer function Jacobian.
Private Attributes
bool	debug_
	Whether or not the filter is in debug mode.

Detailed Description

Definition at line 85 of file filter_base.h.

Constructor & Destructor Documentation

RobotLocalization::FilterBase::FilterBase ( )

Constructor for the FilterBase class.

Definition at line 43 of file filter_base.cpp.

RobotLocalization::FilterBase::~FilterBase ( )

Destructor for the FilterBase class.

Definition at line 105 of file filter_base.cpp.

Member Function Documentation

virtual void RobotLocalization::FilterBase::correct ( const Measurement & measurement ) [protected, pure virtual]

Carries out the correct step in the predict/update cycle. This method must be implemented by subclasses.

Parameters:

[in] measurement - The measurement to fuse with the state estimate

Implemented in RobotLocalization::Ekf, RobotLocalization::FilterDerived2, and RobotLocalization::FilterDerived.

void RobotLocalization::FilterBase::enqueueMeasurement	(	const std::string &	topicName,
		const Eigen::VectorXd &	measurement,
		const Eigen::MatrixXd &	measurementCovariance,
		const std::vector< int > &	updateVector,
		const double	time
	)		`[virtual]`

Adds a measurement to the queue of measurements to be processed.

Parameters:

[in]	measurement	- The measurement to enqueue
[in]	measurementCovariance	- The covariance of the measurement
[in]	updateVector	- The boolean vector that specifies which variables to update from this measurement
[in]	time	- The time of arrival (in seconds)

Definition at line 109 of file filter_base.cpp.

bool RobotLocalization::FilterBase::getDebug ( )

Gets the value of the debug_ variable.

Returns:: True if in debug mode, false otherwise

Definition at line 199 of file filter_base.cpp.

const Eigen::MatrixXd & RobotLocalization::FilterBase::getEstimateErrorCovariance ( )

Gets the estimate error covariance.

Returns:: A copy of the estimate error covariance matrix

Definition at line 204 of file filter_base.cpp.

bool RobotLocalization::FilterBase::getInitializedStatus ( )

Gets the filter's initialized status.

Returns:: True if we've received our first measurement, false otherwise

Definition at line 209 of file filter_base.cpp.

double RobotLocalization::FilterBase::getLastMeasurementTime ( )

Gets the most recent measurement time.

Returns:: The time at which we last received a measurement

Definition at line 214 of file filter_base.cpp.

double RobotLocalization::FilterBase::getLastUpdateTime ( )

Gets the filter's last update time.

Returns:: The time at which we last updated the filter

Definition at line 219 of file filter_base.cpp.

const Eigen::MatrixXd & RobotLocalization::FilterBase::getProcessNoiseCovariance ( )

Gets the filter's process noise covariance.

Returns:: A copy of the process noise covariance

Definition at line 224 of file filter_base.cpp.

double RobotLocalization::FilterBase::getSensorTimeout ( )

Gets the sensor timeout value (in seconds)

Returns:: The sensor timeout value

Definition at line 229 of file filter_base.cpp.

const Eigen::VectorXd & RobotLocalization::FilterBase::getState ( )

Gets the filter state.

Returns:: A copy of the current state

Definition at line 234 of file filter_base.cpp.

void RobotLocalization::FilterBase::integrateMeasurements	(	double	currentTime,
		std::map< std::string, Eigen::VectorXd > &	postUpdateStates
	)		`[virtual]`

Processes all measurements in the measurement queue, in temporal order.

Parameters:

[in] currentTime - The time at which to carry out integration (the current time)

Reimplemented in RobotLocalization::FilterDerived2.

Definition at line 126 of file filter_base.cpp.

virtual void RobotLocalization::FilterBase::predict ( const double delta ) [protected, pure virtual]

Carries out the predict step in the predict/update cycle. Projects the state and error matrices forward using a model of the vehicle's motion. This method must be implemented by subclasses.

Parameters:

[in] delta - The time step over which to predict.

Implemented in RobotLocalization::Ekf, RobotLocalization::FilterDerived2, and RobotLocalization::FilterDerived.

void RobotLocalization::FilterBase::processMeasurement ( const Measurement & measurement ) [protected, virtual]

Does some final preprocessing, carries out the predict/update cycle.

Parameters:

[in] measurement - The measurement object to fuse into the filter

Reimplemented in RobotLocalization::FilterDerived2.

Definition at line 239 of file filter_base.cpp.

void RobotLocalization::FilterBase::setDebug	(	const bool	debug,
		std::ostream *	outStream = `NULL`
	)

Sets the filter into debug mode.

NOTE: this will generates a lot of debug output to the provided stream. The value must be a pointer to a valid ostream object.

Parameters:

[in]	debug	- Whether or not to place the filter in debug mode
[in]	outStream	- If debug is true, then this must have a valid pointer. If the pointer is invalid, the filter will not enter debug mode. If debug is false, outStream is ignored.

Definition at line 320 of file filter_base.cpp.

void RobotLocalization::FilterBase::setEstimateErrorCovariance ( const Eigen::MatrixXd & estimateErrorCovariance )

Manually sets the filter's estimate error covariance.

Parameters:

[in] estimateErrorCovariance - The state to set as the filter's current state

Definition at line 340 of file filter_base.cpp.

void RobotLocalization::FilterBase::setLastMeasurementTime ( const double lastMeasurementTime )

Sets the filter's last measurement time.

Parameters:

[in] lastMeasurementTime - The last measurement time of the filter

Definition at line 345 of file filter_base.cpp.

void RobotLocalization::FilterBase::setLastUpdateTime ( const double lastUpdateTime )

Sets the filter's last update time.

This is used mostly for initialization purposes, as the integrateMeasurements() function will update the filter's last update time as well.

Parameters:

[in] lastUpdateTime - The last update time of the filter

Definition at line 350 of file filter_base.cpp.

void RobotLocalization::FilterBase::setProcessNoiseCovariance ( const Eigen::MatrixXd & processNoiseCovariance )

Sets the process noise covariance for the filter.

This enables external initialization, which is important, as this matrix can be difficult to tune for a given implementation.

Parameters:

[in] processNoiseCovariance - The STATE_SIZExSTATE_SIZE process noise covariance matrix to use for the filter

Definition at line 355 of file filter_base.cpp.

void RobotLocalization::FilterBase::setSensorTimeout ( const double sensorTimeout )

Sets the sensor timeout.

Parameters:

[in] sensorTimeout - The time, in seconds, for a sensor to be considered having timed out

Definition at line 360 of file filter_base.cpp.

void RobotLocalization::FilterBase::setState ( const Eigen::VectorXd & state )

Manually sets the filter's state.

Parameters:

[in] state - The state to set as the filter's current state

Definition at line 365 of file filter_base.cpp.

void RobotLocalization::FilterBase::validateDelta ( double & delta ) [protected]

Ensures a given time delta is valid (helps with bag file playback issues)

Parameters:

[in] delta - The time delta, in seconds, to validate

Definition at line 370 of file filter_base.cpp.

void RobotLocalization::FilterBase::wrapStateAngles ( ) [protected, virtual]

Keeps the state angles to reasonable values.

Definition at line 384 of file filter_base.cpp.

Member Data Documentation

Eigen::MatrixXd RobotLocalization::FilterBase::covarianceEpsilon_ [protected]

Covariance matrices can be incredibly unstable. We can add a small value to it at each iteration to help maintain its positive-definite property.

Definition at line 301 of file filter_base.h.

bool RobotLocalization::FilterBase::debug_ [private]

Whether or not the filter is in debug mode.

Definition at line 362 of file filter_base.h.

std::ostream* RobotLocalization::FilterBase::debugStream_ [protected]

Used for outputting debug messages.

Definition at line 260 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::estimateErrorCovariance_ [protected]

This matrix stores the total error in our position estimate (the state_ variable).

Definition at line 295 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::identity_ [protected]

We need the identity for a few operations. Better to store it.

Definition at line 317 of file filter_base.h.

bool RobotLocalization::FilterBase::initialized_ [protected]

Whether or not we've received any measurements.

Definition at line 256 of file filter_base.h.

double RobotLocalization::FilterBase::lastMeasurementTime_ [protected]

Tracks the time the filter was last updated using a measurement.

This value is used to monitor sensor readings with respect to the sensorTimeout_. We also use it to compute the time delta values for our prediction step.

Definition at line 356 of file filter_base.h.

double RobotLocalization::FilterBase::lastUpdateTime_ [protected]

Used for tracking the latest update time as determined by this class.

We assume that this class may receive measurements that occurred in the past, as may happen with sensors distributed on different machines on a network. This variable tracks when the filter was updated with respect to the executable in which this class was instantiated. We use this to determine if we have experienced a sensor timeout, i.e., if we haven't received any sensor data in a long time.

Definition at line 349 of file filter_base.h.

std::priority_queue<Measurement, std::vector<Measurement>, Measurement> RobotLocalization::FilterBase::measurementQueue_ [protected]

We process measurements based on their timestamp.

In the events that messages come in asynchronously and with no guarantee on order, we can use this to ensure that they are processed in sequence.

Definition at line 338 of file filter_base.h.

const double RobotLocalization::FilterBase::pi_ [protected]

Commonly used constants.

Definition at line 328 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::processNoiseCovariance_ [protected]

As we move through the world, we follow a predict/update cycle. If one were to imagine a scenario where all we did was make predictions without correcting, the error in our position estimate would grow without bound. This error is stored in the stateEstimateCovariance_ matrix. However, this matrix doesn't answer the question of *how much* our error should grow for each time step. That's where the processNoiseCovariance matrix comes in. When we make a prediction using the transfer function, we add this matrix (times deltaT) to the state estimate covariance matrix.

Definition at line 313 of file filter_base.h.

double RobotLocalization::FilterBase::sensorTimeout_ [protected]

The updates to the filter - both predict and correct - are driven by measurements. If we get a gap in measurements for some reason, we want the filter to continue estimating. When this gap occurs, as specified by this timeout, we will continue to call predict() at the filter's frequency.

Definition at line 324 of file filter_base.h.

Eigen::VectorXd RobotLocalization::FilterBase::state_ [protected]

This is the robot's state vector, which is what we are trying to filter. The values in this vector are what get reported by the node.

Definition at line 265 of file filter_base.h.

const double RobotLocalization::FilterBase::tau_ [protected]

Definition at line 330 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::transferFunction_ [protected]

The Kalman filter transfer function.

Kalman filters and extended Kalman filters project the current state forward in time. This is the "predict" part of the predict/correct cycle. A Kalman filter has a (typically constant) matrix A that defines how to turn the current state, x, into the predicted next state. For an EKF, this matrix becomes a function f(x). However, this function can still be expressed as a matrix to make the math a little cleaner, which is what we do here. Technically, each row in the matrix is actually a function. Some rows will contain many trigonometric functions, which are of course non-linear. In any case, you can think of this as the 'A' matrix in the Kalman filter formulation.

Definition at line 280 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::transferFunctionJacobian_ [protected]

The Kalman filter transfer function Jacobian.

The transfer function is allowed to be non-linear in an EKF, but for propagating (predicting) the covariance matrix, we need to linearize it about the current mean (i.e., state). This is done via a Jacobian, which calculates partial derivatives of each row of the transfer function matrix with respect to each state variable.

Definition at line 290 of file filter_base.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation