00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/measurements/poseupdate.h>
00030 #include <hector_pose_estimation/pose_estimation.h>
00031
00032 #include <Eigen/Core>
00033
00034 #include <boost/bind.hpp>
00035
00036 namespace hector_pose_estimation {
00037
00038 PoseUpdate::PoseUpdate(const std::string& name)
00039 : Measurement(name)
00040 {
00041 fixed_alpha_ = 0.0;
00042 fixed_beta_ = 0.0;
00043 interpret_covariance_as_information_matrix_ = true;
00044
00045 fixed_position_xy_stddev_ = 0.0;
00046 fixed_position_z_stddev_ = 0.0;
00047 fixed_yaw_stddev_ = 0.0;
00048
00049 fixed_velocity_xy_stddev_ = 0.0;
00050 fixed_velocity_z_stddev_ = 0.0;
00051 fixed_angular_rate_xy_stddev_ = 0.0;
00052 fixed_angular_rate_z_stddev_ = 0.0;
00053
00054 max_time_difference_ = 1.0;
00055 max_position_xy_error_ = 3.0;
00056 max_position_z_error_ = 3.0;
00057 max_yaw_error_ = 3.0;
00058
00059 max_velocity_xy_error_ = 3.0;
00060 max_velocity_z_error_ = 3.0;
00061 max_angular_rate_xy_error_ = 3.0;
00062 max_angular_rate_z_error_ = 3.0;
00063
00064 jump_on_max_error_ = true;
00065
00066 parameters().add("fixed_alpha", fixed_alpha_);
00067 parameters().add("fixed_beta", fixed_beta_);
00068 parameters().add("interpret_covariance_as_information_matrix", interpret_covariance_as_information_matrix_);
00069
00070 parameters().add("fixed_position_xy_stddev", fixed_position_xy_stddev_);
00071 parameters().add("fixed_position_z_stddev", fixed_position_z_stddev_);
00072 parameters().add("fixed_yaw_stddev", fixed_yaw_stddev_);
00073 parameters().add("fixed_velocity_xy_stddev", fixed_velocity_xy_stddev_);
00074 parameters().add("fixed_velocity_z_stddev", fixed_velocity_z_stddev_);
00075 parameters().add("fixed_angular_rate_xy_stddev", fixed_angular_rate_xy_stddev_);
00076 parameters().add("fixed_angular_rate_z_stddev", fixed_angular_rate_z_stddev_);
00077 parameters().add("max_time_difference", max_time_difference_);
00078 parameters().add("max_position_xy_error", max_position_xy_error_ );
00079 parameters().add("max_position_z_error", max_position_z_error_);
00080 parameters().add("max_yaw_error", max_yaw_error_);
00081 parameters().add("max_velocity_xy_error", max_velocity_xy_error_ );
00082 parameters().add("max_velocity_z_error", max_velocity_z_error_);
00083 parameters().add("max_angular_rate_xy_error", max_angular_rate_xy_error_ );
00084 parameters().add("max_angular_rate_z_error", max_angular_rate_z_error_);
00085 parameters().add("jump_on_max_error", jump_on_max_error_);
00086 }
00087
00088 PoseUpdate::~PoseUpdate()
00089 {
00090 }
00091
00092 bool PoseUpdate::updateImpl(const MeasurementUpdate &update_)
00093 {
00094 Update const &update = static_cast<Update const &>(update_);
00095
00096 while (update.pose) {
00097
00098 Eigen::Vector3d update_pose(update.pose->pose.pose.position.x, update.pose->pose.pose.position.y, update.pose->pose.pose.position.z);
00099 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);
00100 Eigen::Vector3d update_euler(update_orientation.toRotationMatrix().eulerAngles(2,1,0));
00101
00102
00103
00104 SymmetricMatrix_<6> information(SymmetricMatrix_<6>::ConstMap(update.pose->pose.covariance.data()));
00105
00106 ROS_DEBUG_STREAM_NAMED("poseupdate", "PoseUpdate: x = [ " << filter()->state().getVector().transpose() << " ], P = [ " << filter()->state().getCovariance() << " ]" << std::endl
00107 << "update: pose = [ " << update_pose.transpose() << " ], euler = [ " << update_euler.transpose() << " ], information = [ " << information << " ]");
00108 ROS_DEBUG_STREAM_NAMED("poseupdate", "dt = " << (filter()->state().getTimestamp() - update.pose->header.stamp).toSec() << " s");
00109
00110
00111 if (!update.pose->header.stamp.isZero()) {
00112 double dt = (filter()->state().getTimestamp() - update.pose->header.stamp).toSec();
00113 if (dt < 0.0) {
00114 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring pose update as it has a negative time difference: dt = " << dt << "s");
00115 break;
00116
00117 } else if (max_time_difference_ > 0.0 && dt >= max_time_difference_) {
00118 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring pose update as the time difference is too large: dt = " << dt << "s");
00119 break;
00120
00121 } else if (max_time_difference_ > 0.0){
00122 if (interpret_covariance_as_information_matrix_)
00123 information = information * (1.0 - dt/max_time_difference_);
00124 else
00125 information = information / (1.0 - dt/max_time_difference_);
00126 }
00127
00128 State::ConstVelocityType state_velocity(filter()->state().getVelocity());
00129 update_pose = update_pose + dt * state_velocity;
00130
00131 State::ConstRateType state_rate(filter()->state().getRate());
00132 Eigen::AngleAxisd state_angle_offset(state_rate.norm() * dt, state_rate.normalized());
00133 update_orientation = state_angle_offset * update_orientation;
00134 }
00135
00136
00137 if (information(0,0) > 0.0 || information(1,1) > 0.0) {
00138
00139 PositionXYModel::MeasurementMatrix H;
00140 PositionXYModel::MeasurementVector x;
00141 position_xy_model_.getStateJacobian(H, filter()->state(), true);
00142 position_xy_model_.getExpectedValue(x, filter()->state());
00143
00144 PositionXYModel::MeasurementVector y(update_pose.segment<2>(0));
00145 PositionXYModel::NoiseVariance Iy(information.block<2,2>(0,0));
00146
00147
00148 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse().eval();
00149
00150
00151 if (fixed_position_xy_stddev_ != 0.0) {
00152 Iy = 0.0;
00153 Iy(0,0) = Iy(1,1) = 1.0 / (fixed_position_xy_stddev_*fixed_position_xy_stddev_);
00154 }
00155
00156 ROS_DEBUG_STREAM_NAMED("poseupdate", "Position Update: ");
00157 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = [" << x.transpose() << "], H = [ " << H << " ], Px = [" << (H * filter()->state().P0() * H.transpose()) << "], Ix = [ " << (H * filter()->state().P0() * H.transpose()).inverse() << "]");
00158 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = [" << y.transpose() << "], Iy = [ " << Iy << " ]");
00159 double innovation = updateInternal(filter()->state(), Iy, y - x, H, "position_xy", max_position_xy_error_, boost::bind(&PositionXYModel::updateState, position_xy_model_, _1, _2));
00160 position_xy_model_.getExpectedValue(x, filter()->state());
00161 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = [" << x << "], Pxy = [ " << (H * filter()->state().P0() * H.transpose()) << " ], innovation = " << innovation);
00162
00163 status_flags_ |= STATE_POSITION_XY;
00164 }
00165
00166
00167 if (information(2,2) > 0.0) {
00168
00169 PositionZModel::MeasurementMatrix H;
00170 PositionZModel::MeasurementVector x;
00171 position_z_model_.getStateJacobian(H, filter()->state(), true);
00172 position_z_model_.getExpectedValue(x, filter()->state());
00173
00174 PositionZModel::MeasurementVector y(update_pose.segment<1>(2));
00175 PositionZModel::NoiseVariance Iy(information.block<1,1>(2,2));
00176
00177
00178 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse().eval();
00179
00180
00181 if (fixed_position_z_stddev_ != 0.0) {
00182 Iy = 0.0;
00183 Iy(0,0) = 1.0 / (fixed_position_z_stddev_*fixed_position_z_stddev_);
00184 }
00185
00186 ROS_DEBUG_STREAM_NAMED("poseupdate", "Height Update: ");
00187 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = " << x(0) << ", H = [ " << H << " ], Px = [" << (H * filter()->state().P0() * H.transpose()) << "], Ix = [ " << (H * filter()->state().P0() * H.transpose()).inverse() << "]");
00188 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = " << y(0) << ", Iy = [ " << Iy << " ]");
00189 double innovation = updateInternal(filter()->state(), Iy, y - x, H, "position_z", max_position_z_error_, boost::bind(&PositionZModel::updateState, position_z_model_, _1, _2));
00190 position_z_model_.getExpectedValue(x, filter()->state());
00191 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = " << x(0) << ", Pxy = [ " << (H * filter()->state().P0() * H.transpose()) << " ], innovation = " << innovation);
00192
00193 status_flags_ |= STATE_POSITION_Z;
00194 }
00195
00196
00197 if (information(5,5) > 0.0) {
00198
00199 YawModel::MeasurementMatrix H;
00200 YawModel::MeasurementVector x;
00201 yaw_model_.getStateJacobian(H, filter()->state(), true);
00202 yaw_model_.getExpectedValue(x, filter()->state());
00203
00204 YawModel::MeasurementVector y(update_euler(0));
00205 YawModel::NoiseVariance Iy(information.block<1,1>(5,5));
00206
00207
00208 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse().eval();
00209
00210
00211 if (fixed_yaw_stddev_ != 0.0) {
00212 Iy = 0.0;
00213 Iy(0,0) = 1.0 / (fixed_yaw_stddev_*fixed_yaw_stddev_);
00214 }
00215
00216 ROS_DEBUG_STREAM_NAMED("poseupdate", "Yaw Update: ");
00217 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = " << x(0) * 180.0/M_PI << "°, H = [ " << H << " ], Px = [" << (H * filter()->state().P0() * H.transpose()) << "], Ix = [ " << (H * filter()->state().P0() * H.transpose()).inverse() << "]");
00218 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = " << y(0) * 180.0/M_PI << "°, Iy = [ " << Iy << " ]");
00219
00220 YawModel::MeasurementVector error(y - x);
00221 error(0) = error(0) - 2.0*M_PI * round(error(0) / (2.0*M_PI));
00222
00223 double innovation = updateInternal(filter()->state(), Iy, error, H, "yaw", max_yaw_error_, boost::bind(&YawModel::updateState, yaw_model_, _1, _2));
00224 yaw_model_.getExpectedValue(x, filter()->state());
00225 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = " << x(0) * 180.0/M_PI << "°, Pxy = [ " << (H * filter()->state().P0() * H.transpose()) << " ], innovation = " << innovation);
00226
00227 status_flags_ |= STATE_YAW;
00228 }
00229
00230 break;
00231 }
00232
00233 while (update.twist) {
00234
00235 Eigen::Vector3d update_linear(update.twist->twist.twist.linear.x, update.twist->twist.twist.linear.y, update.twist->twist.twist.linear.z);
00236 Eigen::Vector3d update_angular(update.twist->twist.twist.angular.x, update.twist->twist.twist.angular.y, update.twist->twist.twist.angular.z);
00237
00238
00239
00240 SymmetricMatrix_<6> information(SymmetricMatrix_<6>::ConstMap(update.twist->twist.covariance.data()));
00241
00242 ROS_DEBUG_STREAM_NAMED("poseupdate", "TwistUpdate: state = [ " << filter()->state().getVector().transpose() << " ], P = [ " << filter()->state().getCovariance() << " ]" << std::endl
00243 << " update: linear = [ " << update_linear.transpose() << " ], angular = [ " << update_angular.transpose() << " ], information = [ " << information << " ]");
00244 ROS_DEBUG_STREAM_NAMED("poseupdate", " dt = " << (filter()->state().getTimestamp() - update.twist->header.stamp).toSec() << " s");
00245
00246
00247 if (!update.twist->header.stamp.isZero()) {
00248 double dt = (filter()->state().getTimestamp() - update.twist->header.stamp).toSec();
00249 if (dt < 0.0) {
00250 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring twist update as it has a negative time difference: dt = " << dt << "s");
00251 break;
00252
00253 } else if (max_time_difference_ > 0.0 && dt >= max_time_difference_) {
00254 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring twist update as the time difference is too large: dt = " << dt << "s");
00255 break;
00256
00257 } else if (max_time_difference_ > 0.0){
00258 if (interpret_covariance_as_information_matrix_)
00259 information = information * (1.0 - dt/max_time_difference_);
00260 else
00261 information = information / (1.0 - dt/max_time_difference_);
00262 }
00263 }
00264
00265
00266 TwistModel::MeasurementMatrix H;
00267 TwistModel::MeasurementVector x;
00268 twist_model_.getStateJacobian(H, filter()->state(), true);
00269 twist_model_.getExpectedValue(x, filter()->state());
00270
00271 TwistModel::MeasurementVector y;
00272 TwistModel::NoiseVariance Iy(information);
00273 y.segment<3>(0) = update_linear;
00274 y.segment<3>(3) = update_angular;
00275
00276
00277 if (!interpret_covariance_as_information_matrix_) {
00278 ROS_DEBUG_NAMED("poseupdate", "Twist updates with covariance matrices are currently not supported");
00279 break;
00280 }
00281
00282
00283 if (information(0,0) > 0.0 || information(0,0) > 0.0) {
00284 status_flags_ |= STATE_VELOCITY_XY;
00285
00286
00287 if (fixed_velocity_xy_stddev_ != 0.0) {
00288 for(int i = 0; i < 6; ++i) Iy(0,i) = Iy(1,i) = Iy(i,0) = Iy(i,1) = 0.0;
00289 Iy(0,0) = Iy(1,1) = 1.0 / (fixed_velocity_xy_stddev_*fixed_velocity_xy_stddev_);
00290 }
00291 }
00292
00293
00294 if (information(2,2) > 0.0) {
00295 status_flags_ |= STATE_VELOCITY_Z;
00296
00297
00298 if (fixed_velocity_z_stddev_ != 0.0) {
00299 for(int i = 0; i < 6; ++i) Iy(2,i) = Iy(i,2) = 0.0;
00300 Iy(2,2) = 1.0 / (fixed_velocity_z_stddev_*fixed_velocity_z_stddev_);
00301 }
00302 }
00303
00304
00305 if (information(3,3) > 0.0 || information(4,4) > 0.0) {
00306 status_flags_ |= STATE_RATE_XY;
00307
00308
00309 if (fixed_angular_rate_xy_stddev_ != 0.0) {
00310 for(int i = 0; i < 6; ++i) Iy(3,i) = Iy(3,i) = Iy(i,4) = Iy(i,4) = 0.0;
00311 Iy(4,4) = Iy(5,5) = 1.0 / (fixed_angular_rate_xy_stddev_*fixed_angular_rate_xy_stddev_);
00312 }
00313 }
00314
00315
00316 if (information(5,5) > 0.0) {
00317 status_flags_ |= STATE_RATE_Z;
00318
00319
00320 if (fixed_angular_rate_z_stddev_ != 0.0) {
00321 for(int i = 0; i < 6; ++i) Iy(5,i) = Iy(i,5) = 0.0;
00322 Iy(5,5) = 1.0 / (fixed_angular_rate_z_stddev_*fixed_angular_rate_z_stddev_);
00323 }
00324 }
00325
00326 ROS_DEBUG_STREAM_NAMED("poseupdate", "Twist Update: ");
00327 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = [" << x.transpose() << "], H = [ " << H << " ], Px = [" << (H * filter()->state().P0() * H.transpose()) << "], Ix = [ " << (H * filter()->state().P0() * H.transpose()).inverse() << "]");
00328 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = [" << y.transpose() << "], Iy = [ " << Iy << " ]");
00329 double innovation = updateInternal(filter()->state(), Iy, y - x, H, "twist", 0.0);
00330 twist_model_.getExpectedValue(x, filter()->state());
00331 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = [" << x.transpose() << "], Pxy = [ " << (H * filter()->state().P0() * H.transpose()) << " ], innovation = " << innovation);
00332
00333 break;
00334 }
00335
00336 filter()->state().updated();
00337 return true;
00338 }
00339
00340 double PoseUpdate::calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy) {
00341 double tr_x = Ix.trace();
00342 double tr_y = Iy.trace();
00343 return tr_y / (tr_x + tr_y);
00344 }
00345
00346 template <typename MeasurementVector, typename MeasurementMatrix, typename NoiseVariance>
00347 double PoseUpdate::updateInternal(State &state, const NoiseVariance &Iy, const MeasurementVector &error, const MeasurementMatrix &H, const std::string& text, const double max_error, JumpFunction jump_function) {
00348 NoiseVariance H_Px_HT(H * state.P0() * H.transpose());
00349
00350 if (H_Px_HT.determinant() <= 0) {
00351 ROS_WARN_STREAM("Ignoring poseupdate for " << text << " as the a-priori state covariance is zero!");
00352 return 0.0;
00353 }
00354 NoiseVariance Ix(H_Px_HT.inverse().eval());
00355
00356 ROS_DEBUG_STREAM_NAMED("poseupdate", "H = [" << H << "]");
00357 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ix = [" << Ix << "]");
00358
00359 double alpha = fixed_alpha_, beta = fixed_beta_;
00360 if (alpha == 0.0 && beta == 0.0) {
00361 beta = calculateOmega(Ix, Iy);
00362 alpha = 1.0 - beta;
00363
00364
00365
00366
00367
00368
00369 }
00370 ROS_DEBUG_STREAM_NAMED("poseupdate", "alpha = " << alpha << ", beta = " << beta);
00371
00372 if (max_error > 0.0) {
00373 double error2 = error.transpose() * Ix * (Ix + Iy).inverse() * Iy * error;
00374 if (error2 > max_error * max_error) {
00375 if (!jump_on_max_error_ || !jump_function) {
00376 ROS_WARN_STREAM_NAMED("poseupdate", "Ignoring poseupdate for " << text << " as the error [ " << error.transpose() << " ], |error| = " << sqrt(error2) << " sigma exceeds max_error!");
00377 return 0.0;
00378 } else {
00379 ROS_WARN_STREAM_NAMED("poseupdate", "Update for " << text << " with error [ " << error.transpose() << " ], |error| = " << sqrt(error2) << " sigma exceeds max_error!");
00380 jump_function(state, error);
00381 return 0.0;
00382 }
00383 }
00384 }
00385
00386
00387
00388
00389
00390
00391 NoiseVariance S_1(Ix - Ix * (Ix * alpha + Iy * beta).inverse() * Ix);
00392 Matrix_<State::Covariance::ColsAtCompileTime, MeasurementMatrix::RowsAtCompileTime> P_HT((H * state.P().template topRows<State::Dimension>()).transpose());
00393 ROS_DEBUG_STREAM_NAMED("poseupdate", "P*HT = [" << (P_HT) << "]");
00394
00395 double innovation = S_1.determinant();
00396 state.P() = state.P() - P_HT * S_1 * P_HT.transpose();
00397 state.x() = state.x() + P_HT * Iy * beta * error;
00398
00399 ROS_DEBUG_STREAM_NAMED("poseupdate", "K = [" << (P_HT * Iy * beta) << "]");
00400 ROS_DEBUG_STREAM_NAMED("poseupdate", "dx = [" << (P_HT * Iy * beta * error).transpose() << "]");
00401
00402 return innovation;
00403 }
00404
00405 void PositionXYModel::getExpectedValue(MeasurementVector &y_pred, const State &state) {
00406 y_pred = state.getPosition().segment<2>(0);
00407 }
00408
00409 void PositionXYModel::getStateJacobian(MeasurementMatrix &C, const State &state, bool init) {
00410 if (init) {
00411 if (state.getPositionIndex() >= 0) {
00412 C(0,State::POSITION_X) = 1.0;
00413 C(1,State::POSITION_Y) = 1.0;
00414 }
00415 }
00416 }
00417
00418 void PositionXYModel::updateState(State &state, const ColumnVector &diff) const {
00419 state.position().segment(0,2) += diff;
00420 }
00421
00422 void PositionZModel::getExpectedValue(MeasurementVector &y_pred, const State &state) {
00423 y_pred = state.getPosition().segment<1>(2);
00424 }
00425
00426 void PositionZModel::getStateJacobian(MeasurementMatrix &C, const State &state, bool init) {
00427 if (init && state.getPositionIndex() >= 0) {
00428 C(0,State::POSITION_Z) = 1.0;
00429 }
00430 }
00431
00432 void PositionZModel::updateState(State &state, const ColumnVector &diff) const {
00433 state.position().segment(2,1) += diff;
00434 }
00435
00436 void YawModel::getExpectedValue(MeasurementVector &y_pred, const State &state) {
00437 State::ConstOrientationType q(state.getOrientation());
00438 y_pred(0) = 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());
00439 }
00440
00441 void YawModel::getStateJacobian(MeasurementMatrix &C, const State &state, bool init) {
00442 State::ConstOrientationType q(state.getOrientation());
00443 if (init && state.getOrientationIndex() >= 0) {
00444 const double t1 = q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z();
00445 const double t2 = 2*(q.x()*q.y() + q.w()*q.z());
00446 const double t3 = 1.0 / (t1*t1 + t2*t2);
00447
00448 C(0,State::QUATERNION_W) = 2.0 * t3 * (q.z() * t1 - q.w() * t2);
00449 C(0,State::QUATERNION_X) = 2.0 * t3 * (q.y() * t1 - q.x() * t2);
00450 C(0,State::QUATERNION_Y) = 2.0 * t3 * (q.x() * t1 + q.y() * t2);
00451 C(0,State::QUATERNION_Z) = 2.0 * t3 * (q.w() * t1 + q.z() * t2);
00452 }
00453 }
00454
00455 void YawModel::updateState(State &state, const ColumnVector &diff) const {
00456 Eigen::Quaterniond rotation(Eigen::AngleAxisd(diff(0), Eigen::Vector3d::UnitZ()));
00457
00458 Eigen::MatrixXd S(state.getDimension(), state.getDimension()); S.setIdentity();
00459
00460 if (state.getOrientationIndex() >= 0) {
00461 S.block(state.getOrientationIndex(), state.getOrientationIndex(), 4, 4) <<
00462 rotation.w(), -rotation.z(), rotation.y(), rotation.x(),
00463 rotation.z(), rotation.w(), -rotation.x(), rotation.y(),
00464 -rotation.y(), rotation.x(), rotation.w(), rotation.z(),
00465 -rotation.x(), -rotation.y(), -rotation.z(), rotation.w();
00466 }
00467
00468 #ifdef VELOCITY_IN_WORLD_FRAME
00469 if (state.getVelocityIndex() >= 0) {
00470 S.block(state.getVelocityIndex(), state.getVelocityIndex(), 3, 3) = rotation.toRotationMatrix().transpose();
00471 }
00472 #endif // VELOCITY_IN_WORLD_FRAME
00473
00474 if (state.getRateIndex() >= 0) {
00475 S.block(state.getRateIndex(), state.getRateIndex(), 3, 3) = rotation.toRotationMatrix().transpose();
00476 }
00477
00478 ROS_DEBUG_STREAM_NAMED("poseupdate", "Jump yaw by " << (diff(0) * 180.0/M_PI) << " degrees. rotation = [" << rotation.coeffs().transpose() << "], S = [" << S << "].");
00479
00480 state.x() = S * state.x();
00481 state.P() = S * state.P() * S.transpose();
00482 }
00483
00484 void TwistModel::getExpectedValue(MeasurementVector &y_pred, const State &state) {
00485 y_pred.segment<3>(0) = state.getVelocity();
00486 y_pred.segment<3>(3) = state.getRate();
00487 }
00488
00489 void TwistModel::getStateJacobian(MeasurementMatrix &C, const State &state, bool init) {
00490 if (init && state.getVelocityIndex() >= 0) {
00491 C(0,State::VELOCITY_X) = 1.0;
00492 C(1,State::VELOCITY_Y) = 1.0;
00493 C(2,State::VELOCITY_Z) = 1.0;
00494 }
00495
00496 if (init && state.getRateIndex() >= 0) {
00497 C(3,State::RATE_X) = 1.0;
00498 C(4,State::RATE_Y) = 1.0;
00499 C(5,State::RATE_Z) = 1.0;
00500 }
00501 }
00502
00503 }