#include <filter_base.h>

Inheritance diagram for RobotLocalization::FilterBase:

Public Member Functions
void	computeDynamicProcessNoiseCovariance (const Eigen::VectorXd &state, const double delta)
	Computes a dynamic process noise covariance matrix using the parameterized state. More...

virtual void	correct (const Measurement &measurement)=0
	Carries out the correct step in the predict/update cycle. This method must be implemented by subclasses. More...

	FilterBase ()
	Constructor for the FilterBase class. More...

const Eigen::VectorXd &	getControl ()
	Returns the control vector currently being used. More...

double	getControlTime ()
	Returns the time at which the control term was issued. More...

bool	getDebug ()
	Gets the value of the debug_ variable. More...

const Eigen::MatrixXd &	getEstimateErrorCovariance ()
	Gets the estimate error covariance. More...

bool	getInitializedStatus ()
	Gets the filter's initialized status. More...

double	getLastMeasurementTime ()
	Gets the most recent measurement time. More...

const Eigen::VectorXd &	getPredictedState ()
	Gets the filter's predicted state, i.e., the state estimate before correct() is called. More...

const Eigen::MatrixXd &	getProcessNoiseCovariance ()
	Gets the filter's process noise covariance. More...

double	getSensorTimeout ()
	Gets the sensor timeout value (in seconds) More...

const Eigen::VectorXd &	getState ()
	Gets the filter state. More...

virtual void	predict (const double referenceTime, 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. More...

virtual void	processMeasurement (const Measurement &measurement)
	Does some final preprocessing, carries out the predict/update cycle. More...

void	reset ()
	Resets filter to its unintialized state. More...

void	setControl (const Eigen::VectorXd &control, const double controlTime)
	Sets the most recent control term. More...

void	setControlParams (const std::vector< int > &updateVector, const double controlTimeout, const std::vector< double > &accelerationLimits, const std::vector< double > &accelerationGains, const std::vector< double > &decelerationLimits, const std::vector< double > &decelerationGains)
	Sets the control update vector and acceleration limits. More...

void	setDebug (const bool debug, std::ostream *outStream=NULL)
	Sets the filter into debug mode. More...

void	setEstimateErrorCovariance (const Eigen::MatrixXd &estimateErrorCovariance)
	Manually sets the filter's estimate error covariance. More...

void	setLastMeasurementTime (const double lastMeasurementTime)
	Sets the filter's last measurement time. More...

void	setProcessNoiseCovariance (const Eigen::MatrixXd &processNoiseCovariance)
	Sets the process noise covariance for the filter. More...

void	setSensorTimeout (const double sensorTimeout)
	Sets the sensor timeout. More...

void	setState (const Eigen::VectorXd &state)
	Manually sets the filter's state. More...

void	setUseDynamicProcessNoiseCovariance (const bool dynamicProcessNoiseCovariance)
	Enables dynamic process noise covariance calculation. More...

void	validateDelta (double &delta)
	Ensures a given time delta is valid (helps with bag file playback issues) More...

virtual	~FilterBase ()
	Destructor for the FilterBase class. More...

Protected Member Functions
virtual bool	checkMahalanobisThreshold (const Eigen::VectorXd &innovation, const Eigen::MatrixXd &invCovariance, const double nsigmas)
	Tests if innovation is within N-sigmas of covariance. Returns true if passed the test. More...

double	computeControlAcceleration (const double state, const double control, const double accelerationLimit, const double accelerationGain, const double decelerationLimit, const double decelerationGain)
	Method for settings bounds on acceleration values derived from controls. More...

void	prepareControl (const double referenceTime, const double predictionDelta)
	Converts the control term to an acceleration to be applied in the prediction step. More...

virtual void	wrapStateAngles ()
	Keeps the state Euler angles in the range [-pi, pi]. More...

Protected Attributes
std::vector< double >	accelerationGains_
	Gains applied to acceleration derived from control term. More...

std::vector< double >	accelerationLimits_
	Caps the acceleration we apply from control input. More...

Eigen::VectorXd	controlAcceleration_
	Variable that gets updated every time we process a measurement and we have a valid control. More...

double	controlTimeout_
	Timeout value, in seconds, after which a control is considered stale. More...

std::vector< int >	controlUpdateVector_
	Which control variables are being used (e.g., not every vehicle is controllable in Y or Z) More...

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. More...

std::ostream *	debugStream_
	Used for outputting debug messages. More...

std::vector< double >	decelerationGains_
	Gains applied to deceleration derived from control term. More...

std::vector< double >	decelerationLimits_
	Caps the deceleration we apply from control input. More...

Eigen::MatrixXd	dynamicProcessNoiseCovariance_
	Gets updated when useDynamicProcessNoise_ is true. More...

Eigen::MatrixXd	estimateErrorCovariance_
	This matrix stores the total error in our position estimate (the state_ variable). More...

Eigen::MatrixXd	identity_
	We need the identity for a few operations. Better to store it. More...

bool	initialized_
	Whether or not we've received any measurements. More...

double	lastMeasurementTime_
	Tracks the time the filter was last updated using a measurement. More...

Eigen::VectorXd	latestControl_
	Latest control term. More...

double	latestControlTime_
	The time of reception of the most recent control term. More...

Eigen::VectorXd	predictedState_
	Holds the last predicted state of the filter. More...

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. More...

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. More...

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. More...

Eigen::MatrixXd	transferFunction_
	The Kalman filter transfer function. More...

Eigen::MatrixXd	transferFunctionJacobian_
	The Kalman filter transfer function Jacobian. More...

bool	useControl_
	Whether or not we apply the control term. More...

bool	useDynamicProcessNoiseCovariance_
	If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a dynamic process noise covariance matrix. More...

Private Attributes
bool	debug_
	Whether or not the filter is in debug mode. More...

Detailed Description

Definition at line 151 of file filter_base.h.

Constructor & Destructor Documentation

RobotLocalization::FilterBase::FilterBase ( )

Constructor for the FilterBase class.

Definition at line 44 of file filter_base.cpp.

RobotLocalization::FilterBase::~FilterBase ( )

virtual

Destructor for the FilterBase class.

Definition at line 70 of file filter_base.cpp.

Member Function Documentation

bool RobotLocalization::FilterBase::checkMahalanobisThreshold	(	const Eigen::VectorXd &	innovation,
		const Eigen::MatrixXd &	invCovariance,
		const double	nsigmas
	)

protectedvirtual

Tests if innovation is within N-sigmas of covariance. Returns true if passed the test.

Parameters

[in]	innovation	- The difference between the measurement and the state
[in]	invCovariance	- The innovation error
[in]	nsigmas	- Number of standard deviations that are considered acceptable

Definition at line 380 of file filter_base.cpp.

double RobotLocalization::FilterBase::computeControlAcceleration	(	const double	state,
		const double	control,
		const double	accelerationLimit,
		const double	accelerationGain,
		const double	decelerationLimit,
		const double	decelerationGain
	)

inlineprotected

Method for settings bounds on acceleration values derived from controls.

Parameters

[in]	state	- The current state variable (e.g., linear X velocity)
[in]	control	- The current control commanded velocity corresponding to the state variable
[in]	accelerationLimit	- Limit for acceleration (regardless of driving direction)
[in]	accelerationGain	- Gain applied to acceleration control error
[in]	decelerationLimit	- Limit for deceleration (moving towards zero, regardless of driving direction)
[in]	decelerationGain	- Gain applied to deceleration control error

Returns: a usable acceleration estimate for the control vector

Definition at line 346 of file filter_base.h.

void RobotLocalization::FilterBase::computeDynamicProcessNoiseCovariance	(	const Eigen::VectorXd &	state,
		const double	delta
	)

Computes a dynamic process noise covariance matrix using the parameterized state.

This allows us to, e.g., not increase the pose covariance values when the vehicle is not moving

Parameters

[in] state - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance

Definition at line 132 of file filter_base.cpp.

virtual void RobotLocalization::FilterBase::correct ( const Measurement & measurement )

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::Ukf, and RobotLocalization::Ekf.

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

Returns the control vector currently being used.

Returns: The control vector

Definition at line 151 of file filter_base.cpp.

double RobotLocalization::FilterBase::getControlTime ( )

Returns the time at which the control term was issued.

Returns: The time the control vector was issued

Definition at line 156 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 161 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 166 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 171 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 176 of file filter_base.cpp.

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

Gets the filter's predicted state, i.e., the state estimate before correct() is called.

Returns: A constant reference to the predicted state

Definition at line 181 of file filter_base.cpp.

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

Gets the filter's process noise covariance.

Returns: A constant reference to the process noise covariance

Definition at line 186 of file filter_base.cpp.

double RobotLocalization::FilterBase::getSensorTimeout ( )

Gets the sensor timeout value (in seconds)

Returns: The sensor timeout value

Definition at line 191 of file filter_base.cpp.

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

Gets the filter state.

Returns: A constant reference to the current state

Definition at line 196 of file filter_base.cpp.

virtual void RobotLocalization::FilterBase::predict	(	const double	referenceTime,
		const double	delta
	)

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]	referenceTime	- The time at which the prediction is being made
[in]	delta	- The time step over which to predict.

Implemented in RobotLocalization::Ukf, and RobotLocalization::Ekf.

void RobotLocalization::FilterBase::prepareControl	(	const double	referenceTime,
		const double	predictionDelta
	)

protected

Converts the control term to an acceleration to be applied in the prediction step.

Parameters

[in]	referenceTime	- The time of the update (measurement used in the prediction step)
[in]	predictionDelta	- The amount of time over which we are carrying out our prediction

Definition at line 347 of file filter_base.cpp.

void RobotLocalization::FilterBase::processMeasurement ( const Measurement & measurement )

virtual

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

Parameters

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

Definition at line 201 of file filter_base.cpp.

void RobotLocalization::FilterBase::reset ( )

Resets filter to its unintialized state.

Definition at line 74 of file filter_base.cpp.

void RobotLocalization::FilterBase::setControl	(	const Eigen::VectorXd &	control,
		const double	controlTime
	)

Sets the most recent control term.

Parameters

[in]	control	- The control term to be applied
[in]	controlTime	- The time at which the control in question was received

Definition at line 265 of file filter_base.cpp.

void RobotLocalization::FilterBase::setControlParams	(	const std::vector< int > &	updateVector,
		const double	controlTimeout,
		const std::vector< double > &	accelerationLimits,
		const std::vector< double > &	accelerationGains,
		const std::vector< double > &	decelerationLimits,
		const std::vector< double > &	decelerationGains
	)

Sets the control update vector and acceleration limits.

Parameters

[in]	updateVector	- The values the control term affects
[in]	controlTimeout	- Timeout value, in seconds, after which a control is considered stale
[in]	accelerationLimits	- The acceleration limits for the control variables
[in]	accelerationGains	- Gains applied to the control term-derived acceleration
[in]	decelerationLimits	- The deceleration limits for the control variables
[in]	decelerationGains	- Gains applied to the control term-derived deceleration

Definition at line 271 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 284 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 309 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 314 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 319 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 325 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 330 of file filter_base.cpp.

void RobotLocalization::FilterBase::setUseDynamicProcessNoiseCovariance ( const bool dynamicProcessNoiseCovariance )

Enables dynamic process noise covariance calculation.

Parameters

[in] dynamicProcessNoiseCovariance - Whether or not to compute dynamic process noise covariance matrices

Definition at line 304 of file filter_base.cpp.

void RobotLocalization::FilterBase::validateDelta ( double & delta )

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 335 of file filter_base.cpp.

void RobotLocalization::FilterBase::wrapStateAngles ( )

protectedvirtual

Keeps the state Euler angles in the range [-pi, pi].

Definition at line 373 of file filter_base.cpp.

Member Data Documentation

std::vector<double> RobotLocalization::FilterBase::accelerationGains_

protected

Gains applied to acceleration derived from control term.

Definition at line 400 of file filter_base.h.

std::vector<double> RobotLocalization::FilterBase::accelerationLimits_

protected

Caps the acceleration we apply from control input.

Definition at line 404 of file filter_base.h.

Eigen::VectorXd RobotLocalization::FilterBase::controlAcceleration_

protected

Variable that gets updated every time we process a measurement and we have a valid control.

Definition at line 408 of file filter_base.h.

double RobotLocalization::FilterBase::controlTimeout_

protected

Timeout value, in seconds, after which a control is considered stale.

Definition at line 428 of file filter_base.h.

std::vector<int> RobotLocalization::FilterBase::controlUpdateVector_

protected

Which control variables are being used (e.g., not every vehicle is controllable in Y or Z)

Definition at line 424 of file filter_base.h.

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 434 of file filter_base.h.

bool RobotLocalization::FilterBase::debug_

private

Whether or not the filter is in debug mode.

Definition at line 533 of file filter_base.h.

std::ostream* RobotLocalization::FilterBase::debugStream_

protected

Used for outputting debug messages.

Definition at line 438 of file filter_base.h.

std::vector<double> RobotLocalization::FilterBase::decelerationGains_

protected

Gains applied to deceleration derived from control term.

Definition at line 412 of file filter_base.h.

std::vector<double> RobotLocalization::FilterBase::decelerationLimits_

protected

Caps the deceleration we apply from control input.

Definition at line 416 of file filter_base.h.

Eigen::MatrixXd RobotLocalization::FilterBase::dynamicProcessNoiseCovariance_

protected

Gets updated when useDynamicProcessNoise_ is true.

Definition at line 442 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 447 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 451 of file filter_base.h.

bool RobotLocalization::FilterBase::initialized_

protected

Whether or not we've received any measurements.

Definition at line 455 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 462 of file filter_base.h.

Eigen::VectorXd RobotLocalization::FilterBase::latestControl_

protected

Latest control term.

Definition at line 420 of file filter_base.h.

double RobotLocalization::FilterBase::latestControlTime_

protected

The time of reception of the most recent control term.

Definition at line 466 of file filter_base.h.

Eigen::VectorXd RobotLocalization::FilterBase::predictedState_

protected

Holds the last predicted state of the filter.

Definition at line 470 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 482 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 489 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 494 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 509 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 519 of file filter_base.h.

bool RobotLocalization::FilterBase::useControl_

protected

Whether or not we apply the control term.

Definition at line 523 of file filter_base.h.

bool RobotLocalization::FilterBase::useDynamicProcessNoiseCovariance_

protected

If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a dynamic process noise covariance matrix.

Definition at line 528 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