Class FilterBase

Inheritance Relationships

Derived Type

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_