32 #ifndef REVOLUTEJOINTFILTER_H_ 33 #define REVOLUTEJOINTFILTER_H_ 38 #include <pdf/linearanalyticconditionalgaussian.h> 39 #include <pdf/analyticconditionalgaussian.h> 40 #include <pdf/analyticconditionalgaussian_additivenoise.h> 41 #include <wrappers/matrix/matrix_wrapper.h> 42 #include <filter/extendedkalmanfilter.h> 43 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h> 44 #include <model/analyticmeasurementmodel_gaussianuncertainty.h> 45 #include <model/measurementmodel.h> 58 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76 RevoluteJointFilterPtr
clone()
const 172 BFL::LinearAnalyticSystemModelGaussianUncertainty*
_sys_MODEL;
177 BFL::ExtendedKalmanFilter*
_ekf;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RevoluteJointFilter()
virtual RevoluteJointFilter * doClone() const
double _sigma_delta_meas_uncertainty_angular
void _initializeMeasurementModel()
virtual void setMinRotationRevolute(const double &value)
virtual std::vector< visualization_msgs::Marker > getJointMarkersInRRBFrame() const
BFL::LinearAnalyticConditionalGaussian * _sys_PDF
double _rev_max_joint_distance_for_ee
BFL::ExtendedKalmanFilter * _ekf
virtual JointFilterType getJointFilterType() const
virtual ~RevoluteJointFilter()
virtual void setMaxRadiusDistanceRevolute(const double &value)
virtual void initialize()
boost::shared_ptr< RevoluteJointFilter > RevoluteJointFilterPtr
virtual std::string getJointFilterTypeStr() const
virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame()
We use the kinematic structure to predict the next pose-twist of the second rigid body of the joint (...
BFL::AnalyticMeasurementModelGaussianUncertainty * _meas_MODEL
BFL::NonLinearRevoluteMeasurementPdf * _meas_PDF
void _initializeSystemModel()
virtual void setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular)
virtual void estimateMeasurementHistoryLikelihood()
virtual void estimateUnnormalizedModelProbability()
BFL::LinearAnalyticSystemModelGaussianUncertainty * _sys_MODEL
double _accumulated_rotation
virtual void predictState(double time_interval_ns)
First step when updating the filter. The next state is predicted from current state and system model...
RevoluteJointFilterPtr clone() const
virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame()
Generate a prediction about the change in pose of the second rigid body (SRB) and its covariance usin...
virtual void predictMeasurement()
virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame()
Generate a prediction about the velocity of the second rigid body (SRB) and its covariance using the ...
virtual void correctState()
double _rev_min_rot_for_ee