34 #include <boost/bind.hpp> 101 Eigen::Vector3d update_pose(update.
pose->pose.pose.position.x, update.
pose->pose.pose.position.y, update.
pose->pose.pose.position.z);
102 Eigen::Quaterniond update_orientation(update.
pose->pose.pose.orientation.w, update.
pose->pose.pose.orientation.x, update.
pose->pose.pose.orientation.y, update.
pose->pose.pose.orientation.z);
103 Eigen::Vector3d update_euler;
107 SymmetricMatrix6 information(Eigen::Map<const SymmetricMatrix6>(update.
pose->pose.covariance.data()));
109 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"PoseUpdate: x = [ " <<
filter()->state().getVector().transpose() <<
" ], P = [ " <<
filter()->state().getCovariance() <<
" ]" << std::endl
110 <<
"update: pose = [ " << update_pose.transpose() <<
" ], rpy = [ " << update_euler.transpose() <<
" ], information = [ " << information <<
" ]");
114 if (!update.
pose->header.stamp.isZero()) {
117 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Ignoring pose update as it has a negative time difference: dt = " << dt <<
"s");
121 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Ignoring pose update as the time difference is too large: dt = " << dt <<
"s");
133 update_pose = update_pose + state_velocity * dt;
136 update_orientation = update_orientation * Eigen::Quaterniond().fromRotationVector(state_rate * dt);
142 const Eigen::Quaterniond &q = update_orientation;
143 update_euler(0) = atan2(2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
144 update_euler(1) = -asin(2*(q.x()*q.z() - q.w()*q.y()));
145 update_euler(2) = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
149 if (information(0,0) > 0.0 || information(1,1) > 0.0) {
156 PositionXYModel::MeasurementVector
y(update_pose.segment<2>(0));
157 PositionXYModel::NoiseVariance Iy(information.block<2,2>(0,0));
173 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
" ==> xy = [" <<
x <<
"], Pxy = [ " << (H *
filter()->state().P() * H.transpose()) <<
" ], innovation = " << innovation);
179 if (information(2,2) > 0.0) {
186 PositionZModel::MeasurementVector
y(update_pose.segment<1>(2));
187 PositionZModel::NoiseVariance Iy(information.block<1,1>(2,2));
203 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
" ==> xy = " <<
x(0) <<
", Pxy = [ " << (H *
filter()->state().P() * H.transpose()) <<
" ], innovation = " << innovation);
209 if (information(5,5) > 0.0) {
215 YawModel::MeasurementVector
y(update_euler.tail<1>());
216 YawModel::NoiseVariance Iy(information.block<1,1>(5,5));
231 YawModel::MeasurementVector error(y -
x);
232 error(0) = error(0) - 2.0*M_PI * round(error(0) / (2.0*M_PI));
236 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
" ==> xy = " <<
x(0) * 180.0/M_PI <<
"°, Pxy = [ " << (H *
filter()->state().P() * H.transpose()) <<
" ], innovation = " << innovation);
244 while (update.
twist) {
246 Eigen::Vector3d update_linear(update.
twist->twist.twist.linear.x, update.
twist->twist.twist.linear.y, update.
twist->twist.twist.linear.z);
247 Eigen::Vector3d update_angular(update.
twist->twist.twist.angular.x, update.
twist->twist.twist.angular.y, update.
twist->twist.twist.angular.z);
251 SymmetricMatrix6 information(Eigen::Map<const SymmetricMatrix6>(update.
twist->twist.covariance.data()));
253 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"TwistUpdate: state = [ " <<
filter()->state().getVector().transpose() <<
" ], P = [ " <<
filter()->state().getCovariance() <<
" ]" << std::endl
254 <<
" update: linear = [ " << update_linear.transpose() <<
" ], angular = [ " << update_angular.transpose() <<
" ], information = [ " << information <<
" ]");
258 if (!update.
twist->header.stamp.isZero()) {
261 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Ignoring twist update as it has a negative time difference: dt = " << dt <<
"s");
265 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Ignoring twist update as the time difference is too large: dt = " << dt <<
"s");
283 TwistModel::NoiseVariance Iy(information);
284 y.segment<3>(0) = update_linear;
285 y.segment<3>(3) = update_angular;
289 ROS_DEBUG_NAMED(
"poseupdate",
"Twist updates with covariance matrices are currently not supported");
294 if (information(0,0) > 0.0 || information(0,0) > 0.0) {
299 for(
int i = 0; i < 6; ++i) Iy(0,i) = Iy(1,i) = Iy(i,0) = Iy(i,1) = 0.0;
305 if (information(2,2) > 0.0) {
310 for(
int i = 0; i < 6; ++i) Iy(2,i) = Iy(i,2) = 0.0;
316 if (information(3,3) > 0.0 || information(4,4) > 0.0) {
321 for(
int i = 0; i < 6; ++i) Iy(3,i) = Iy(3,i) = Iy(i,4) = Iy(i,4) = 0.0;
327 if (information(5,5) > 0.0) {
332 for(
int i = 0; i < 6; ++i) Iy(5,i) = Iy(i,5) = 0.0;
353 double tr_x = Ix.trace();
354 double tr_y = Iy.trace();
355 return tr_y / (tr_x + tr_y);
358 template <
typename MeasurementVector,
typename MeasurementMatrix,
typename NoiseVariance>
360 NoiseVariance H_Px_HT(H * state.
P() * H.transpose());
362 if (H_Px_HT.determinant() <= 0) {
363 ROS_DEBUG_STREAM(
"Ignoring poseupdate for " << text <<
" as the a-priori state covariance is zero!");
366 NoiseVariance Ix(H_Px_HT.inverse().eval());
369 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Ix = [" << Ix <<
"]");
372 if (alpha == 0.0 && beta == 0.0) {
382 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"alpha = " << alpha <<
", beta = " << beta);
384 if (max_error > 0.0) {
385 double error2 = (error.transpose() * Ix * (Ix + Iy).
inverse() * Iy * error)(0);
386 if (error2 > max_error * max_error) {
388 ROS_WARN_STREAM_NAMED(
"poseupdate",
"Ignoring poseupdate for " << text <<
" as the error [ " << error.transpose() <<
" ], |error| = " << sqrt(error2) <<
" sigma exceeds max_error!");
391 ROS_WARN_STREAM_NAMED(
"poseupdate",
"Update for " << text <<
" with error [ " << error.transpose() <<
" ], |error| = " << sqrt(error2) <<
" sigma exceeds max_error!");
392 jump_function(state, error);
403 NoiseVariance S_1(Ix - Ix * (Ix * alpha + Iy * beta).
inverse() * Ix);
405 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"P*HT = [" << (P_HT) <<
"]");
407 double innovation = S_1.determinant();
408 state.
P() = state.
P() - P_HT * S_1 * P_HT.transpose();
409 state.
P().assertSymmetric();
410 state.
update(P_HT * Iy * beta * error);
413 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"K = [" << (P_HT * Iy * beta) <<
"]");
414 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"dx = [" << (P_HT * Iy * beta * error).transpose() <<
"]");
434 state.
position()->vector().head<2>() += diff;
450 state.
position()->vector().segment<1>(
Z) += diff;
455 y_pred(0) = state.
getYaw();
475 S.block(state.
orientation()->getCovarianceIndex(), state.
orientation()->getCovarianceIndex(), 3, 3) = rotation.transpose();
480 S.block(state.
velocity()->getCovarianceIndex(), state.
velocity()->getCovarianceIndex(), 3, 3) = rotation.transpose();
481 state.
velocity()->vector() = rotation.transpose() * state.
velocity()->vector();
490 ROS_DEBUG_STREAM_NAMED(
"poseupdate",
"Jump yaw by " << (diff(0) * 180.0/M_PI) <<
" degrees. rotation = [" << rotation <<
"], S = [" << S <<
"].");
493 state.
P() = S * state.
P() * S.transpose();
498 y_pred.segment<3>(3) = state.
getRate();
508 if (init && state.
rate()) {
509 state.
rate()->cols(C)(3,
X) = 1.0;
510 state.
rate()->cols(C)(4,
Y) = 1.0;
511 state.
rate()->cols(C)(5,
Z) = 1.0;
virtual int getDimension() const
double max_velocity_xy_error_
virtual bool update(const MeasurementUpdate &update)
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())
#define ROS_DEBUG_STREAM_NAMED(name, args)
double max_position_z_error_
bool interpret_covariance_as_information_matrix_
virtual const boost::shared_ptr< OrientationStateType > & orientation() const
SystemStatus status_flags_
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
void updateState(State &state, const ColumnVector &diff) const
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
void updateState(State &state, const ColumnVector &diff) const
virtual ConstRateType getRate() const
virtual ParameterList & parameters()
virtual ConstVelocityType getVelocity() const
virtual const boost::shared_ptr< PositionStateType > & position() const
void updateState(State &state, const ColumnVector &diff) const
double max_position_xy_error_
virtual void updateOrientation(const ColumnVector3 &rotation_vector)
double fixed_position_xy_stddev_
double max_angular_rate_xy_error_
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual bool updateImpl(const MeasurementUpdate &update)
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
geometry_msgs::PoseWithCovarianceStampedConstPtr pose
virtual Filter * filter() const
virtual const State & state() const
ParameterList & add(const std::string &key, T &value, const T &default_value)
#define ROS_DEBUG_NAMED(name,...)
double max_time_difference_
double fixed_angular_rate_xy_stddev_
double fixed_position_z_stddev_
double fixed_velocity_z_stddev_
PositionZModel position_z_model_
SymmetricMatrix_< 6 >::type SymmetricMatrix6
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
virtual IndexType getCovarianceDimension() const
double fixed_angular_rate_z_stddev_
Matrix_< 3, 3 >::type Matrix3
TFSIMD_FORCE_INLINE const tfScalar & x() const
double calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy)
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)
ColumnVector_< Dynamic >::type ColumnVector
Eigen::Matrix< ScalarType, Rows, Cols,(Rows==1 &&Cols!=1?Eigen::RowMajor:Eigen::ColMajor),(Rows!=Dynamic?Rows:MaxMatrixRowsCols),(Cols!=Dynamic?Cols:MaxMatrixRowsCols) > type
#define ROS_DEBUG_STREAM(args)
virtual const boost::shared_ptr< VelocityStateType > & velocity() const
geometry_msgs::TwistWithCovarianceStampedConstPtr twist
double max_velocity_z_error_
virtual bool init(PoseEstimation &estimator, State &state)
boost::function< void(State &state, const ColumnVector &diff)> JumpFunction
const ros::Time & getTimestamp(const T &t)
virtual const boost::shared_ptr< RateStateType > & rate() const
VectorBlock< const Vector, 3 > ConstRateType
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
double max_angular_rate_z_error_
PositionXYModel position_xy_model_
virtual void update(const Vector &vector_update)
const ros::Time & getTimestamp() const
SymmetricMatrix_< Dynamic >::type SymmetricMatrix
ColumnVector_< 3 >::type ColumnVector3
virtual ConstPositionType getPosition() const
VectorBlock< const Vector, 3 > ConstVelocityType
double fixed_velocity_xy_stddev_
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
PoseUpdate(const std::string &name="poseupdate")
#define ROS_WARN_STREAM_NAMED(name, args)
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)