Class FilterBase¶
Defined in File filter-base.h
Class Documentation¶
-
class maplab::FilterBase¶
Subclassed by maplab::Ukf
Public Functions
-
FilterBase()¶
Constructor for the FilterBase class.
-
virtual ~FilterBase()¶
Destructor for the FilterBase class.
-
void reset()¶
Resets filter to its unintialized state.
-
virtual bool correct(const LocalizationFilterMeasurement &measurement, const std::vector<size_t> &updateVector) = 0¶
Carries out the correct step in the predict/update cycle.
This method must be implemented by subclasses.
- Parameters
measurement – [in] - The measurement to fuse with the state estimate
-
const Eigen::MatrixXd &getEstimateErrorCovariance() const¶
Gets the estimate error covariance.
- Returns
A copy of the estimate error covariance matrix
-
bool getInitializedStatus() const¶
Gets the filter’s initialized status.
- Returns
True if we’ve received our first measurement, false otherwise
-
int64_t getLastMeasurementTime() const¶
Gets the most recent measurement time.
- Returns
The time at which we last received a measurement
-
const Eigen::VectorXd &getState() const¶
Gets the filter state.
- Returns
A constant reference to the current state
-
virtual void predict(const LocalizationFilterPrediction &odom_prediction = LocalizationFilterPrediction()) = 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.
- Parameters
delta_time_s – [in] - The time step [in seconds] over which to
odom_prediction – [in] - The predicted state from the odometry
-
virtual bool processMeasurement(const LocalizationFilterMeasurement &measurement, const LocalizationFilterPrediction &odom_prediction, const std::vector<size_t> &updateVector)¶
Does some final preprocessing, carries out the predict/update cycle.
- Parameters
measurement – [in] - The measurement object to fuse into the filter
-
void setEstimateErrorCovariance(const Eigen::MatrixXd &estimate_error_covariance)¶
Manually sets the filter’s estimate error covariance.
- Parameters
estimate_error_covariance – [in] - The state to set as the filter’s current state
-
void setLastMeasurementTime(const int64_t last_measurement_time)¶
Sets the filter’s last measurement time.
- Parameters
last_measurement_time – [in] - The last measurement time of the filter
-
void setMahalanobisThreshold(const double mahalanobis_threshold)¶
-
void setState(const Eigen::VectorXd &state)¶
Manually sets the filter’s state.
- Parameters
state – [in] - The state to set as the filter’s current state
-
void validateDelta(double &delta)¶
Ensures a given time delta is valid (helps with bag file playback issues)
- Parameters
delta – [in] - The time delta, in seconds, to validate
Protected Functions
-
virtual void wrapStateAngles()¶
Keeps the state Euler angles in the range [-pi, pi].
-
virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation, const Eigen::MatrixXd &innovation_covariance, const double n_sigmas)¶
Tests if innovation is within N-sigmas of covariance.
Returns true if passed the test.
- Parameters
innovation – [in] - The difference between the measurement and the state
innovation_covariance – [in] - The innovation error
n_sigmas – [in] - Number of standard deviations that are considered acceptable
Protected Attributes
-
bool initialized_¶
Whether or not we’ve received any measurements.
-
int64_t last_measurement_time_ns_¶
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.
-
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 covariance_epsilon_¶
Covariance matrices can be incredibly unstable.
We can add a small value to it at each iteration to help maintain its positive-definite property.
-
Eigen::MatrixXd estimate_error_covariance_¶
This matrix stores the total error in our position estimate (the state_ variable).
-
double mahalanobis_distance_threshold_¶
-
FilterBase()¶