33 #ifndef ROBOT_LOCALIZATION_FILTER_BASE_H 34 #define ROBOT_LOCALIZATION_FILTER_BASE_H 39 #include <Eigen/Dense> 50 #include <boost/shared_ptr.hpp> 93 return (*
this)(*(a.get()), *(b.get()));
104 latestControlTime_(0.0),
106 mahalanobisThresh_(
std::numeric_limits<double>::
max())
143 estimateErrorCovariance_(),
145 lastMeasurementTime_(0.0),
146 latestControlTime_(0.0)
172 void computeDynamicProcessNoiseCovariance(
const Eigen::VectorXd &state,
const double delta);
179 virtual void correct(
const Measurement &measurement) = 0;
185 const Eigen::VectorXd& getControl();
191 double getControlTime();
203 const Eigen::MatrixXd& getEstimateErrorCovariance();
209 bool getInitializedStatus();
215 double getLastMeasurementTime();
222 const Eigen::VectorXd& getPredictedState();
228 const Eigen::MatrixXd& getProcessNoiseCovariance();
234 double getSensorTimeout();
240 const Eigen::VectorXd& getState();
249 virtual void predict(
const double referenceTime,
const double delta) = 0;
255 virtual void processMeasurement(
const Measurement &measurement);
262 void setControl(
const Eigen::VectorXd &control,
const double controlTime);
273 void setControlParams(
const std::vector<int> &updateVector,
const double controlTimeout,
274 const std::vector<double> &accelerationLimits,
const std::vector<double> &accelerationGains,
275 const std::vector<double> &decelerationLimits,
const std::vector<double> &decelerationGains);
287 void setDebug(
const bool debug, std::ostream *outStream = NULL);
293 void setUseDynamicProcessNoiseCovariance(
const bool dynamicProcessNoiseCovariance);
299 void setEstimateErrorCovariance(
const Eigen::MatrixXd &estimateErrorCovariance);
305 void setLastMeasurementTime(
const double lastMeasurementTime);
315 void setProcessNoiseCovariance(
const Eigen::MatrixXd &processNoiseCovariance);
322 void setSensorTimeout(
const double sensorTimeout);
328 void setState(
const Eigen::VectorXd &state);
334 void validateDelta(
double &delta);
347 const double accelerationGain,
const double decelerationLimit,
const double decelerationGain)
349 FB_DEBUG(
"---------- FilterBase::computeControlAcceleration ----------\n");
351 const double error = control - state;
352 const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01);
353 const double setPoint = (sameSign ? control : 0.0);
354 const bool decelerating = ::fabs(setPoint) < ::fabs(state);
355 double limit = accelerationLimit;
356 double gain = accelerationGain;
360 limit = decelerationLimit;
361 gain = decelerationGain;
364 const double finalAccel = std::min(std::max(gain * error, -limit), limit);
366 FB_DEBUG(
"Control value: " << control <<
"\n" <<
367 "State value: " << state <<
"\n" <<
368 "Error: " << error <<
"\n" <<
369 "Same sign: " << (sameSign ?
"true" :
"false") <<
"\n" <<
370 "Set point: " << setPoint <<
"\n" <<
371 "Decelerating: " << (decelerating ?
"true" :
"false") <<
"\n" <<
372 "Limit: " << limit <<
"\n" <<
373 "Gain: " << gain <<
"\n" <<
374 "Final is " << finalAccel <<
"\n");
381 virtual void wrapStateAngles();
388 virtual bool checkMahalanobisThreshold(
const Eigen::VectorXd &innovation,
389 const Eigen::MatrixXd &invCovariance,
390 const double nsigmas);
396 void prepareControl(
const double referenceTime,
const double predictionDelta);
538 #endif // ROBOT_LOCALIZATION_FILTER_BASE_H
std::vector< double > decelerationGains_
Gains applied to deceleration derived from control term.
Eigen::MatrixXd transferFunctionJacobian_
The Kalman filter transfer function Jacobian.
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) ...
boost::shared_ptr< FilterState > FilterStatePtr
Eigen::MatrixXd estimateErrorCovariance_
This matrix stores the total error in our position estimate (the state_ variable).
bool operator()(const Measurement &a, const Measurement &b)
Structure used for storing and comparing measurements (for priority queues)
Eigen::VectorXd predictedState_
Holds the last predicted state of the filter.
Eigen::MatrixXd transferFunction_
The Kalman filter transfer function.
Eigen::MatrixXd covariance_
Eigen::MatrixXd covarianceEpsilon_
Covariance matrices can be incredibly unstable. We can add a small value to it at each iteration to h...
double lastMeasurementTime_
Eigen::VectorXd latestControl_
Eigen::MatrixXd estimateErrorCovariance_
Eigen::VectorXd latestControl_
Latest control term.
Eigen::MatrixXd identity_
We need the identity for a few operations. Better to store it.
double latestControlTime_
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.
Eigen::VectorXd latestControl_
boost::shared_ptr< Measurement > MeasurementPtr
double latestControlTime_
The time of reception of the most recent control term.
double mahalanobisThresh_
bool debug_
Whether or not the filter is in debug mode.
Eigen::MatrixXd dynamicProcessNoiseCovariance_
Gets updated when useDynamicProcessNoise_ is true.
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.
double latestControlTime_
bool useDynamicProcessNoiseCovariance_
If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a ...
Eigen::VectorXd measurement_
bool operator()(const FilterState &a, const FilterState &b)
std::vector< double > accelerationGains_
Gains applied to acceleration derived from control term.
double lastMeasurementTime_
Tracks the time the filter was last updated using a measurement.
bool initialized_
Whether or not we've received any measurements.
Structure used for storing and comparing filter states.
Eigen::VectorXd controlAcceleration_
Variable that gets updated every time we process a measurement and we have a valid control...
Eigen::VectorXd state_
This is the robot's state vector, which is what we are trying to filter. The values in this vector ar...
std::vector< double > accelerationLimits_
Caps the acceleration we apply from control input.
Eigen::MatrixXd processNoiseCovariance_
As we move through the world, we follow a predict/update cycle. If one were to imagine a scenario whe...
std::vector< double > decelerationLimits_
Caps the deceleration we apply from control input.
double max(double a, double b)
std::vector< int > updateVector_
std::ostream * debugStream_
Used for outputting debug messages.
bool useControl_
Whether or not we apply the control term.
bool operator()(const boost::shared_ptr< Measurement > &a, const boost::shared_ptr< Measurement > &b)