53 latestControlTime_(0.0),
65 useDynamicProcessNoiseCovariance_(false)
142 velocityMatrix.setIdentity();
148 velocityMatrix.transpose();
203 FB_DEBUG(
"------ FilterBase::processMeasurement (" << measurement.
topicName_ <<
") ------\n");
215 FB_DEBUG(
"Filter is already initialized. Carrying out predict/correct loop...\n" 216 "Measurement time is " << std::setprecision(20) << measurement.
time_ <<
234 FB_DEBUG(
"First measurement. Initializing filter.\n");
238 for (
size_t i = 0; i < measurementLength; ++i)
244 for (
size_t i = 0; i < measurementLength; ++i)
246 for (
size_t j = 0; j < measurementLength; ++j)
262 FB_DEBUG(
"------ /FilterBase::processMeasurement (" << measurement.
topicName_ <<
") ------\n");
272 const std::vector<double> &accelerationLimits,
const std::vector<double> &accelerationGains,
273 const std::vector<double> &decelerationLimits,
const std::vector<double> &decelerationGains)
288 if (outStream != NULL)
338 if (delta > 100000.0)
340 FB_DEBUG(
"Delta was very large. Suspect playing from bag file. Setting to 0.01\n");
357 FB_DEBUG(
"Control timed out. Reference time was " << referenceTime <<
", latest control time was " <<
361 for (
size_t controlInd = 0; controlInd <
TWIST_SIZE; ++controlInd)
381 const Eigen::MatrixXd &invCovariance,
382 const double nsigmas)
384 double sqMahalanobis = innovation.dot(invCovariance * innovation);
385 double threshold = nsigmas * nsigmas;
387 if (sqMahalanobis >= threshold)
389 FB_DEBUG(
"Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis <<
"\n" <<
390 "Threshold is: " << threshold <<
"\n" <<
391 "Innovation is: " << innovation <<
"\n" <<
392 "Innovation covariance is:\n" << invCovariance <<
"\n");
void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance)
Sets the process noise covariance for the filter.
std::vector< double > decelerationGains_
Gains applied to deceleration derived from control term.
Eigen::MatrixXd transferFunctionJacobian_
The Kalman filter transfer function Jacobian.
const int STATE_SIZE
Global constants that define our state vector size and offsets to groups of values within that state...
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) ...
const Eigen::VectorXd & getState()
Gets the filter state.
Eigen::MatrixXd estimateErrorCovariance_
This matrix stores the total error in our position estimate (the state_ variable).
FilterBase()
Constructor for the FilterBase class.
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_
void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance)
Manually sets the filter's estimate error covariance.
Eigen::MatrixXd covarianceEpsilon_
Covariance matrices can be incredibly unstable. We can add a small value to it at each iteration to h...
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 setLastMeasurementTime(const double lastMeasurementTime)
Sets the filter's last measurement time.
bool getDebug()
Gets the value of the debug_ variable.
const int POSITION_V_OFFSET
double getControlTime()
Returns the time at which the control term was issued.
Eigen::VectorXd latestControl_
Latest control term.
Eigen::MatrixXd identity_
We need the identity for a few operations. Better to store it.
double getLastMeasurementTime()
Gets the most recent measurement time.
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.
double latestControlTime_
The time of reception of the most recent control term.
void prepareControl(const double referenceTime, const double predictionDelta)
Converts the control term to an acceleration to be applied in the prediction step.
virtual ~FilterBase()
Destructor for the FilterBase class.
const int POSITION_OFFSET
const Eigen::VectorXd & getPredictedState()
Gets the filter's predicted state, i.e., the state estimate before correct() is called.
virtual void processMeasurement(const Measurement &measurement)
Does some final preprocessing, carries out the predict/update cycle.
void reset()
Resets filter to its unintialized state.
bool debug_
Whether or not the filter is in debug mode.
Eigen::MatrixXd dynamicProcessNoiseCovariance_
Gets updated when useDynamicProcessNoise_ is true.
void setDebug(const bool debug, std::ostream *outStream=NULL)
Sets the filter into debug mode.
virtual void wrapStateAngles()
Keeps the state Euler angles in the range [-pi, pi].
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.
void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance)
Enables dynamic process noise covariance calculation.
bool getInitializedStatus()
Gets the filter's initialized status.
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 clampRotation(double rotation)
Utility method keeping RPY angles in the range [-pi, pi].
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 forwa...
void setControl(const Eigen::VectorXd &control, const double controlTime)
Sets the most recent control term.
bool useDynamicProcessNoiseCovariance_
If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a ...
const Eigen::VectorXd & getControl()
Returns the control vector currently being used.
Eigen::VectorXd measurement_
void validateDelta(double &delta)
Ensures a given time delta is valid (helps with bag file playback issues)
virtual void correct(const Measurement &measurement)=0
Carries out the correct step in the predict/update cycle. This method must be implemented by subclass...
double getSensorTimeout()
Gets the sensor timeout value (in seconds)
std::vector< double > accelerationGains_
Gains applied to acceleration derived from control term.
void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta)
Computes a dynamic process noise covariance matrix using the parameterized state. ...
double lastMeasurementTime_
Tracks the time the filter was last updated using a measurement.
const Eigen::MatrixXd & getEstimateErrorCovariance()
Gets the estimate error covariance.
void setState(const Eigen::VectorXd &state)
Manually sets the filter's state.
void setSensorTimeout(const double sensorTimeout)
Sets the sensor timeout.
const Eigen::MatrixXd & getProcessNoiseCovariance()
Gets the filter's process noise covariance.
bool initialized_
Whether or not we've received any measurements.
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.
std::vector< int > updateVector_
std::ostream * debugStream_
Used for outputting debug messages.
bool useControl_
Whether or not we apply the control term.