Class FilterBase

• Defined in File filter_base.hpp

Inheritance Relationships

Derived Types

• public robot_localization::Ekf (Class Ekf)

• public robot_localization::Ukf (Class Ukf)

Class Documentation

class FilterBase

Subclassed by robot_localization::Ekf, robot_localization::Ukf

Public Functions

FilterBase()

Constructor for the FilterBase class.

virtual ~FilterBase()

Destructor for the FilterBase class.

void reset()

Resets filter to its unintialized state.

void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state)

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.

Parameters:

state – [in] - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance

virtual void correct(const Measurement &measurement) = 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::VectorXd &getControl()

Returns the control vector currently being used.

Returns:

The control vector

const rclcpp::Time &getControlTime()

Returns the time at which the control term was issued.

Returns:

The time the control vector was issued

bool getDebug()

Gets the value of the debug_ variable.

Returns:

True if in debug mode, false otherwise

const Eigen::MatrixXd &getEstimateErrorCovariance()

Gets the estimate error covariance.

Returns:

A copy of the estimate error covariance matrix

bool getInitializedStatus()

Gets the filter’s initialized status.

Returns:

True if we’ve received our first measurement, false otherwise

const rclcpp::Time &getLastMeasurementTime()

Gets the most recent measurement time.

Returns:

The time at which we last received a measurement

const Eigen::VectorXd &getPredictedState()

Gets the filter’s predicted state, i.e., the state estimate before correct() is called.

Returns:

A constant reference to the predicted state

const Eigen::MatrixXd &getProcessNoiseCovariance()

Gets the filter’s process noise covariance.

Returns:

A constant reference to the process noise covariance

const rclcpp::Duration &getSensorTimeout()

Gets the sensor timeout value (in seconds)

Returns:

The sensor timeout value

const Eigen::VectorXd &getState()

Gets the filter state.

Returns:

A constant reference to the current state

virtual void predict(const rclcpp::Time &reference_time, const rclcpp::Duration &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.

Parameters:

• reference_time – [in] - The time at which the prediction is being made

• delta – [in] - The time step over which to predict.

virtual void processMeasurement(const Measurement &measurement)

Does some final preprocessing, carries out the predict/update cycle.

Parameters:

measurement – [in] - The measurement object to fuse into the filter

void setControl(const Eigen::VectorXd &control, const rclcpp::Time &control_time)

Sets the most recent control term.

Parameters:

• control – [in] - The control term to be applied

• control_time – [in] - The time at which the control in question was received

void setControlParams(const std::vector<bool> &update_vector, const rclcpp::Duration &control_timeout, const std::vector<double> &acceleration_limits, const std::vector<double> &acceleration_gains, const std::vector<double> &deceleration_limits, const std::vector<double> &deceleration_gains)

Sets the control update vector and acceleration limits.

Parameters:

• update_vector – [in] - The values the control term affects

• control_timeout – [in] - Timeout value, in seconds, after which a control is considered stale

• acceleration_limits – [in] - The acceleration limits for the control variables

• acceleration_gains – [in] - Gains applied to the control term-derived acceleration

• deceleration_limits – [in] - The deceleration limits for the control variables

• deceleration_gains – [in] - Gains applied to the control term-derived deceleration

void setDebug(const bool debug, std::ostream *out_stream = 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.

Parameters:

• debug – [in] - Whether or not to place the filter in debug mode

• out_stream – [in] - 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.

void setUseDynamicProcessNoiseCovariance(const bool dynamic_process_noise_covariance)

Enables dynamic process noise covariance calculation.

Parameters:

dynamic_process_noise_covariance – [in] - Whether or not to compute dynamic process noise covariance matrices

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 rclcpp::Time &last_measurement_time)

Sets the filter’s last measurement time.

Parameters:

last_measurement_time – [in] - The last measurement time of the filter

void setProcessNoiseCovariance(const Eigen::MatrixXd &process_noise_covariance)

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.

Parameters:

process_noise_covariance – [in] - The STATE_SIZExSTATE_SIZE process noise covariance matrix to use for the filter

void setSensorTimeout(const rclcpp::Duration &sensor_timeout)

Sets the sensor timeout.

Parameters:

sensor_timeout – [in] - The time, in seconds, for a sensor to be considered having timed out

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(rclcpp::Duration &delta)

Ensures a given time delta is valid (helps with bag file playback issues)

Parameters:

delta – [inout] - The time delta to validate

Protected Functions

double computeControlAcceleration(const double state, const double control, const double acceleration_limit, const double acceleration_gain, const double deceleration_limit, const double deceleration_gain)

Method for settings bounds on acceleration values derived from controls.

Parameters:

• state – [in] - The current state variable (e.g., linear X velocity)

• control – [in] - The current control commanded velocity corresponding to the state variable

• acceleration_limit – [in] - Limit for acceleration (regardless of driving direction)

• acceleration_gain – [in] - Gain applied to acceleration control error

• deceleration_limit – [in] - Limit for deceleration (moving towards zero, regardless of driving direction)

• deceleration_gain – [in] - Gain applied to deceleration control error

Returns:

a usable acceleration estimate for the control vector

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

void prepareControl(const rclcpp::Time &reference_time, const rclcpp::Duration&)

Converts the control term to an acceleration to be applied in the prediction step.

Parameters:

reference_time – [in] - The time of the update (measurement used in the prediction step)

Protected Attributes

bool initialized_

Whether or not we’ve received any measurements.

bool use_control_

Whether or not we apply the control term.

bool use_dynamic_process_noise_covariance_

If true, uses the robot’s vehicle state and the static process noise covariance matrix to generate a dynamic process noise covariance matrix.

rclcpp::Duration control_timeout_

Timeout value, in seconds, after which a control is considered stale.

rclcpp::Time last_measurement_time_

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.

rclcpp::Time latest_control_time_

The time of reception of the most recent control term.

rclcpp::Duration sensor_timeout_

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.

std::ostream *debug_stream_

Used for outputting debug messages.

std::vector<double> acceleration_gains_

Gains applied to acceleration derived from control term.

std::vector<double> acceleration_limits_

Caps the acceleration we apply from control input.

std::vector<double> deceleration_gains_

Gains applied to deceleration derived from control term.

std::vector<double> deceleration_limits_

Caps the deceleration we apply from control input.

std::vector<bool> control_update_vector_

Which control variables are being used (e.g., not every vehicle is controllable in Y or Z)

Eigen::VectorXd control_acceleration_

Variable that gets updated every time we process a measurement and we have a valid control.

Eigen::VectorXd latest_control_

Latest control term.

Eigen::VectorXd predicted_state_

Holds the last predicted state of the filter.

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 dynamic_process_noise_covariance_

Gets updated when useDynamicProcessNoise_ is true.

Eigen::MatrixXd estimate_error_covariance_

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.

Eigen::MatrixXd process_noise_covariance_

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 delta_t) to the state estimate covariance matrix.

Eigen::MatrixXd transfer_function_

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.

Eigen::MatrixXd transfer_function_jacobian_

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.