Robot Localization Listener class. More...
#include <robot_localization_estimator.h>
Public Member Functions | |
void | clearBuffer () |
Clears the internal state buffer. More... | |
unsigned int | getBufferCapacity () const |
Returns the buffer capacity. More... | |
unsigned int | getSize () const |
Returns the current buffer size. More... | |
EstimatorResult | getState (const double time, EstimatorState &state) const |
Returns the state at a given time. More... | |
RobotLocalizationEstimator (unsigned int buffer_capacity, FilterType filter_type, const Eigen::MatrixXd &process_noise_covariance, const std::vector< double > &filter_args=std::vector< double >()) | |
Constructor for the RobotLocalizationListener class. More... | |
void | setBufferCapacity (const int capacity) |
Sets the buffer capacity. More... | |
void | setState (const EstimatorState &state) |
Sets the current internal state of the listener. More... | |
virtual | ~RobotLocalizationEstimator () |
Destructor for the RobotLocalizationListener class. More... | |
Private Member Functions | |
void | extrapolate (const EstimatorState &boundary_state, const double requested_time, EstimatorState &state_at_req_time) const |
Extrapolates the given state to a requested time stamp. More... | |
void | interpolate (const EstimatorState &given_state_1, const EstimatorState &given_state_2, const double requested_time, EstimatorState &state_at_req_time) const |
Interpolates the given state to a requested time stamp. More... | |
Private Attributes | |
FilterBase * | filter_ |
A pointer to the filter instance that is used for extrapolation. More... | |
boost::circular_buffer< EstimatorState > | state_buffer_ |
The buffer holding the system states that have come in. Interpolation and extrapolation is done starting from these states. More... | |
Friends | |
std::ostream & | operator<< (std::ostream &os, const RobotLocalizationEstimator &rle) |
Robot Localization Listener class.
The Robot Localization Estimator class buffers states of and inputs to a system and can interpolate and extrapolate based on a given system model.
Definition at line 116 of file robot_localization_estimator.h.
|
explicit |
Constructor for the RobotLocalizationListener class.
[in] | args | - Generic argument container (not used here, but needed so that the ROS filters can pass arbitrary arguments to templated filter types). |
Definition at line 41 of file robot_localization_estimator.cpp.
|
virtual |
Destructor for the RobotLocalizationListener class.
Definition at line 61 of file robot_localization_estimator.cpp.
void RobotLocalization::RobotLocalizationEstimator::clearBuffer | ( | ) |
Clears the internal state buffer.
Definition at line 163 of file robot_localization_estimator.cpp.
|
private |
Extrapolates the given state to a requested time stamp.
[in] | boundary_state | - state from which to extrapolate |
[in] | requested_time | - time stamp to extrapolate to |
[out] | state_at_req_time | - predicted state at requested time |
Definition at line 178 of file robot_localization_estimator.cpp.
unsigned int RobotLocalization::RobotLocalizationEstimator::getBufferCapacity | ( | ) | const |
Returns the buffer capacity.
Returns the number of EstimatorState objects that can be pushed to the buffer before old ones are dropped. (The capacity of the buffer).
Definition at line 168 of file robot_localization_estimator.cpp.
unsigned int RobotLocalization::RobotLocalizationEstimator::getSize | ( | ) | const |
Returns the current buffer size.
Returns the number of EstimatorState objects currently in the buffer.
Definition at line 173 of file robot_localization_estimator.cpp.
EstimatorResult RobotLocalization::RobotLocalizationEstimator::getState | ( | const double | time, |
EstimatorState & | state | ||
) | const |
Returns the state at a given time.
Projects the current state and error matrices forward using a model of the robot's motion.
[in] | time | - The time to which the prediction is being made |
[out] | state | - The returned state at the given time |
Definition at line 87 of file robot_localization_estimator.cpp.
|
private |
Interpolates the given state to a requested time stamp.
[in] | given_state_1 | - last state update before requested time |
[in] | given_state_2 | - next state update after requested time |
[in] | requested_time | - time stamp to extrapolate to |
[out] | state_at_req_time | - predicted state at requested time |
Definition at line 199 of file robot_localization_estimator.cpp.
void RobotLocalization::RobotLocalizationEstimator::setBufferCapacity | ( | const int | capacity | ) |
Sets the buffer capacity.
[in] | capacity | - The new buffer capacity |
Definition at line 158 of file robot_localization_estimator.cpp.
void RobotLocalization::RobotLocalizationEstimator::setState | ( | const EstimatorState & | state | ) |
Sets the current internal state of the listener.
[in] | state | - The new state vector to set the internal state to |
Definition at line 66 of file robot_localization_estimator.cpp.
|
friend |
Definition at line 178 of file robot_localization_estimator.h.
|
private |
A pointer to the filter instance that is used for extrapolation.
Definition at line 217 of file robot_localization_estimator.h.
|
private |
The buffer holding the system states that have come in. Interpolation and extrapolation is done starting from these states.
Definition at line 212 of file robot_localization_estimator.h.