33 #ifndef ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H 34 #define ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H 38 #include <boost/circular_buffer.hpp> 39 #include <Eigen/Dense> 80 return os <<
"state:\n - time_stamp: " << state.
time_stamp <<
81 "\n - state: \n" << state.
state <<
86 namespace EstimatorResults
100 namespace FilterTypes
125 FilterType filter_type,
126 const Eigen::MatrixXd& process_noise_covariance,
127 const std::vector<double>& filter_args = std::vector<double>());
148 EstimatorResult getState(
const double time,
EstimatorState &state)
const;
158 void setBufferCapacity(
const int capacity);
167 unsigned int getBufferCapacity()
const;
175 unsigned int getSize()
const;
180 for ( boost::circular_buffer<EstimatorState>::const_iterator it = rle.
state_buffer_.begin();
195 const double requested_time,
206 const double requested_time,
EstimatorState& state_at_req_time)
const;
222 #endif // ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H
const int STATE_SIZE
Global constants that define our state vector size and offsets to groups of values within that state...
friend std::ostream & operator<<(std::ostream &os, const RobotLocalizationEstimator &rle)
EstimatorResults::EstimatorResult EstimatorResult
friend std::ostream & operator<<(std::ostream &os, const EstimatorState &state)
Robot Localization Listener class.
double time_stamp
Time at which this state is/was achieved.
boost::circular_buffer< EstimatorState > state_buffer_
The buffer holding the system states that have come in. Interpolation and extrapolation is done start...
Eigen::VectorXd state
System state at time = time_stamp.
Eigen::MatrixXd covariance
System state covariance at time = time_stamp.
FilterBase * filter_
A pointer to the filter instance that is used for extrapolation.
Robot Localization Estimator State.
FilterTypes::FilterType FilterType