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