#include <filter_base.h>
Public Member Functions | |
void | computeDynamicProcessNoiseCovariance (const Eigen::VectorXd &state, const double delta) |
Computes a dynamic process noise covariance matrix using the parameterized state. | |
virtual void | correct (const Measurement &measurement)=0 |
Carries out the correct step in the predict/update cycle. This method must be implemented by subclasses. | |
FilterBase () | |
Constructor for the FilterBase class. | |
const Eigen::VectorXd & | getControl () |
Returns the control vector currently being used. | |
double | getControlTime () |
Returns the time at which the control term was issued. | |
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::VectorXd & | getPredictedState () |
Gets the filter's predicted state, i.e., the state estimate before correct() is called. | |
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 | 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. | |
virtual void | processMeasurement (const Measurement &measurement) |
Does some final preprocessing, carries out the predict/update cycle. | |
void | reset () |
Resets filter to its unintialized state. | |
void | setControl (const Eigen::VectorXd &control, const double controlTime) |
Sets the most recent control term. | |
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. | |
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. | |
void | setUseDynamicProcessNoiseCovariance (const bool dynamicProcessNoiseCovariance) |
Enables dynamic process noise covariance calculation. | |
void | validateDelta (double &delta) |
Ensures a given time delta is valid (helps with bag file playback issues) | |
virtual | ~FilterBase () |
Destructor for the FilterBase class. | |
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. | |
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. | |
void | prepareControl (const double referenceTime, const double predictionDelta) |
Converts the control term to an acceleration to be applied in the prediction step. | |
virtual void | wrapStateAngles () |
Keeps the state Euler angles in the range [-pi, pi]. | |
Protected Attributes | |
std::vector< double > | accelerationGains_ |
Gains applied to acceleration derived from control term. | |
std::vector< double > | accelerationLimits_ |
Caps the acceleration we apply from control input. | |
Eigen::VectorXd | controlAcceleration_ |
Variable that gets updated every time we process a measurement and we have a valid control. | |
double | controlTimeout_ |
Timeout value, in seconds, after which a control is considered stale. | |
std::vector< int > | controlUpdateVector_ |
Which control variables are being used (e.g., not every vehicle is controllable in Y or Z) | |
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. | |
std::vector< double > | decelerationGains_ |
Gains applied to deceleration derived from control term. | |
std::vector< double > | decelerationLimits_ |
Caps the deceleration we apply from control input. | |
Eigen::MatrixXd | dynamicProcessNoiseCovariance_ |
Gets updated when useDynamicProcessNoise_ is true. | |
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. | |
Eigen::VectorXd | latestControl_ |
Latest control term. | |
double | latestControlTime_ |
The time of reception of the most recent control term. | |
Eigen::VectorXd | predictedState_ |
Holds the last predicted state of the filter. | |
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. | |
Eigen::MatrixXd | transferFunction_ |
The Kalman filter transfer function. | |
Eigen::MatrixXd | transferFunctionJacobian_ |
The Kalman filter transfer function Jacobian. | |
bool | useControl_ |
Whether or not we apply the control term. | |
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. | |
Private Attributes | |
bool | debug_ |
Whether or not the filter is in debug mode. |
Definition at line 151 of file filter_base.h.
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.
bool RobotLocalization::FilterBase::checkMahalanobisThreshold | ( | const Eigen::VectorXd & | innovation, |
const Eigen::MatrixXd & | invCovariance, | ||
const double | nsigmas | ||
) | [protected, virtual] |
Tests if innovation is within N-sigmas of covariance. Returns true if passed the test.
[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 398 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 | ||
) | [inline, protected] |
Method for settings bounds on acceleration values derived from controls.
[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 |
Definition at line 362 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
[in] | state | - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance |
Definition at line 133 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.
[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.
Definition at line 152 of file filter_base.cpp.
Returns the time at which the control term was issued.
Definition at line 157 of file filter_base.cpp.
Gets the value of the debug_ variable.
Definition at line 162 of file filter_base.cpp.
const Eigen::MatrixXd & RobotLocalization::FilterBase::getEstimateErrorCovariance | ( | ) |
Gets the estimate error covariance.
Definition at line 167 of file filter_base.cpp.
Gets the filter's initialized status.
Definition at line 172 of file filter_base.cpp.
Gets the most recent measurement time.
Definition at line 177 of file filter_base.cpp.
Gets the filter's last update time.
Definition at line 182 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.
Definition at line 187 of file filter_base.cpp.
const Eigen::MatrixXd & RobotLocalization::FilterBase::getProcessNoiseCovariance | ( | ) |
Gets the filter's process noise covariance.
Definition at line 192 of file filter_base.cpp.
Gets the sensor timeout value (in seconds)
Definition at line 197 of file filter_base.cpp.
const Eigen::VectorXd & RobotLocalization::FilterBase::getState | ( | ) |
Gets the filter state.
Definition at line 202 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.
[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.
[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 365 of file filter_base.cpp.
void RobotLocalization::FilterBase::processMeasurement | ( | const Measurement & | measurement | ) | [virtual] |
Does some final preprocessing, carries out the predict/update cycle.
[in] | measurement | - The measurement object to fuse into the filter |
Definition at line 207 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.
[in] | control | - The control term to be applied |
[in] | controlTime | - The time at which the control in question was received |
Definition at line 278 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.
[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 284 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.
[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 297 of file filter_base.cpp.
void RobotLocalization::FilterBase::setEstimateErrorCovariance | ( | const Eigen::MatrixXd & | estimateErrorCovariance | ) |
Manually sets the filter's estimate error covariance.
[in] | estimateErrorCovariance | - The state to set as the filter's current state |
Definition at line 322 of file filter_base.cpp.
void RobotLocalization::FilterBase::setLastMeasurementTime | ( | const double | lastMeasurementTime | ) |
Sets the filter's last measurement time.
[in] | lastMeasurementTime | - The last measurement time of the filter |
Definition at line 327 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.
[in] | lastUpdateTime | - The last update time of the filter |
Definition at line 332 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.
[in] | processNoiseCovariance | - The STATE_SIZExSTATE_SIZE process noise covariance matrix to use for the filter |
Definition at line 337 of file filter_base.cpp.
void RobotLocalization::FilterBase::setSensorTimeout | ( | const double | sensorTimeout | ) |
Sets the sensor timeout.
[in] | sensorTimeout | - The time, in seconds, for a sensor to be considered having timed out |
Definition at line 343 of file filter_base.cpp.
void RobotLocalization::FilterBase::setState | ( | const Eigen::VectorXd & | state | ) |
Manually sets the filter's state.
[in] | state | - The state to set as the filter's current state |
Definition at line 348 of file filter_base.cpp.
void RobotLocalization::FilterBase::setUseDynamicProcessNoiseCovariance | ( | const bool | dynamicProcessNoiseCovariance | ) |
Enables dynamic process noise covariance calculation.
[in] | dynamicProcessNoiseCovariance | - Whether or not to compute dynamic process noise covariance matrices |
Definition at line 317 of file filter_base.cpp.
void RobotLocalization::FilterBase::validateDelta | ( | double & | delta | ) |
Ensures a given time delta is valid (helps with bag file playback issues)
[in] | delta | - The time delta, in seconds, to validate |
Definition at line 353 of file filter_base.cpp.
void RobotLocalization::FilterBase::wrapStateAngles | ( | ) | [protected, virtual] |
Keeps the state Euler angles in the range [-pi, pi].
Definition at line 391 of file filter_base.cpp.
std::vector<double> RobotLocalization::FilterBase::accelerationGains_ [protected] |
Gains applied to acceleration derived from control term.
Definition at line 416 of file filter_base.h.
std::vector<double> RobotLocalization::FilterBase::accelerationLimits_ [protected] |
Caps the acceleration we apply from control input.
Definition at line 420 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 424 of file filter_base.h.
double RobotLocalization::FilterBase::controlTimeout_ [protected] |
Timeout value, in seconds, after which a control is considered stale.
Definition at line 444 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 440 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 450 of file filter_base.h.
bool RobotLocalization::FilterBase::debug_ [private] |
Whether or not the filter is in debug mode.
Definition at line 560 of file filter_base.h.
std::ostream* RobotLocalization::FilterBase::debugStream_ [protected] |
Used for outputting debug messages.
Definition at line 454 of file filter_base.h.
std::vector<double> RobotLocalization::FilterBase::decelerationGains_ [protected] |
Gains applied to deceleration derived from control term.
Definition at line 428 of file filter_base.h.
std::vector<double> RobotLocalization::FilterBase::decelerationLimits_ [protected] |
Caps the deceleration we apply from control input.
Definition at line 432 of file filter_base.h.
Eigen::MatrixXd RobotLocalization::FilterBase::dynamicProcessNoiseCovariance_ [protected] |
Gets updated when useDynamicProcessNoise_ is true.
Definition at line 458 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 463 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 467 of file filter_base.h.
bool RobotLocalization::FilterBase::initialized_ [protected] |
Whether or not we've received any measurements.
Definition at line 471 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 478 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 489 of file filter_base.h.
Eigen::VectorXd RobotLocalization::FilterBase::latestControl_ [protected] |
Latest control term.
Definition at line 436 of file filter_base.h.
double RobotLocalization::FilterBase::latestControlTime_ [protected] |
The time of reception of the most recent control term.
Definition at line 493 of file filter_base.h.
Eigen::VectorXd RobotLocalization::FilterBase::predictedState_ [protected] |
Holds the last predicted state of the filter.
Definition at line 497 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 509 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 516 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 521 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 536 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 546 of file filter_base.h.
bool RobotLocalization::FilterBase::useControl_ [protected] |
Whether or not we apply the control term.
Definition at line 550 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 555 of file filter_base.h.