29 #ifndef HECTOR_POSE_ESTIMATION_POSEUPDATE_H 30 #define HECTOR_POSE_ESTIMATION_POSEUPDATE_H 33 #include <geometry_msgs/PoseWithCovarianceStamped.h> 34 #include <geometry_msgs/TwistWithCovarianceStamped.h> 36 #include <boost/function.hpp> 85 PoseUpdate(
const std::string& name =
"poseupdate");
91 Update(
const geometry_msgs::PoseWithCovarianceStamped& pose) : pose(new
geometry_msgs::PoseWithCovarianceStamped(pose)) {}
92 Update(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose) : pose(pose) {}
93 Update(
const geometry_msgs::TwistWithCovarianceStamped& twist) : twist(new
geometry_msgs::TwistWithCovarianceStamped(twist)) {}
94 Update(
const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : twist(twist) {}
95 Update(
const geometry_msgs::PoseWithCovarianceStamped& pose,
const geometry_msgs::TwistWithCovarianceStamped& twist) : pose(new
geometry_msgs::PoseWithCovarianceStamped(pose)), twist(new
geometry_msgs::TwistWithCovarianceStamped(twist)) {}
96 Update(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose,
const geometry_msgs::TwistWithCovarianceStampedConstPtr& twist) : pose(pose), twist(twist) {}
97 geometry_msgs::PoseWithCovarianceStampedConstPtr
pose;
98 geometry_msgs::TwistWithCovarianceStampedConstPtr
twist;
133 typedef boost::function<void(State &state, const ColumnVector &diff)>
JumpFunction;
136 template <
typename MeasurementVector,
typename MeasurementMatrix,
typename NoiseVariance>
137 double updateInternal(
State &state,
const NoiseVariance &Iy,
const MeasurementVector &error,
const MeasurementMatrix &H,
const std::string& text,
const double max_error = 0.0, JumpFunction jump_function = JumpFunction());
146 #endif // HECTOR_POSE_ESTIMATION_POSEUPDATE_H
double max_velocity_xy_error_
double max_position_z_error_
bool interpret_covariance_as_information_matrix_
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
void updateState(State &state, const ColumnVector &diff) const
virtual ~PositionZModel()
double max_position_xy_error_
Update(const geometry_msgs::TwistWithCovarianceStampedConstPtr &twist)
double fixed_position_xy_stddev_
double max_angular_rate_xy_error_
geometry_msgs::PoseWithCovarianceStampedConstPtr pose
double max_time_difference_
double fixed_angular_rate_xy_stddev_
double fixed_position_z_stddev_
double fixed_velocity_z_stddev_
Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose, const geometry_msgs::TwistWithCovarianceStampedConstPtr &twist)
Update(const geometry_msgs::PoseWithCovarianceStamped &pose, const geometry_msgs::TwistWithCovarianceStamped &twist)
PositionZModel position_z_model_
double fixed_angular_rate_z_stddev_
virtual ~PositionXYModel()
ColumnVector_< Dynamic >::type ColumnVector
geometry_msgs::TwistWithCovarianceStampedConstPtr twist
double max_velocity_z_error_
Update(const geometry_msgs::PoseWithCovarianceStamped &pose)
Update(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose)
boost::function< void(State &state, const ColumnVector &diff)> JumpFunction
virtual bool init(PoseEstimation &estimator, Measurement &measurement, State &state)
double max_angular_rate_z_error_
PositionXYModel position_xy_model_
Update(const geometry_msgs::TwistWithCovarianceStamped &twist)
SymmetricMatrix_< Dynamic >::type SymmetricMatrix
double fixed_velocity_xy_stddev_
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)