$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #include <hector_pose_estimation/measurements/poseupdate.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 #include <hector_pose_estimation/bfl_conversions.h> 00032 #include <Eigen/Geometry> 00033 00034 namespace hector_pose_estimation { 00035 00036 PoseUpdate::PoseUpdate(const std::string& name) 00037 : Measurement(name) 00038 { 00039 fixed_alpha_ = 0.0; 00040 fixed_beta_ = 0.0; 00041 interpret_covariance_as_information_matrix_ = true; 00042 00043 fixed_position_xy_stddev_ = 0.0; 00044 fixed_position_z_stddev_ = 0.0; 00045 fixed_yaw_stddev_ = 0.0; 00046 00047 fixed_velocity_xy_stddev_ = 0.0; 00048 fixed_velocity_z_stddev_ = 0.0; 00049 fixed_angular_rate_xy_stddev_ = 0.0; 00050 fixed_angular_rate_z_stddev_ = 0.0; 00051 00052 max_time_difference_ = 1.0; 00053 max_position_xy_error_ = 3.0; // 3 sigma 00054 max_position_z_error_ = 3.0; // 3 sigma 00055 max_yaw_error_ = 3.0; // 3 sigma 00056 00057 max_velocity_xy_error_ = 3.0; // 3 sigma 00058 max_velocity_z_error_ = 3.0; // 3 sigma 00059 max_angular_rate_xy_error_ = 3.0; // 3 sigma 00060 max_angular_rate_z_error_ = 3.0; // 3 sigma 00061 00062 jump_on_max_error_ = true; 00063 00064 parameters().add("fixed_alpha", fixed_alpha_); 00065 parameters().add("fixed_beta", fixed_beta_); 00066 parameters().add("interpret_covariance_as_information_matrix", interpret_covariance_as_information_matrix_); 00067 00068 parameters().add("fixed_position_xy_stddev", fixed_position_xy_stddev_); 00069 parameters().add("fixed_position_z_stddev", fixed_position_z_stddev_); 00070 parameters().add("fixed_yaw_stddev", fixed_yaw_stddev_); 00071 parameters().add("fixed_velocity_xy_stddev", fixed_velocity_xy_stddev_); 00072 parameters().add("fixed_velocity_z_stddev", fixed_velocity_z_stddev_); 00073 parameters().add("fixed_angular_rate_xy_stddev", fixed_angular_rate_xy_stddev_); 00074 parameters().add("fixed_angular_rate_z_stddev", fixed_angular_rate_z_stddev_); 00075 parameters().add("max_time_difference", max_time_difference_); 00076 parameters().add("max_position_xy_error", max_position_xy_error_ ); 00077 parameters().add("max_position_z_error", max_position_z_error_); 00078 parameters().add("max_yaw_error", max_yaw_error_); 00079 parameters().add("max_velocity_xy_error", max_velocity_xy_error_ ); 00080 parameters().add("max_velocity_z_error", max_velocity_z_error_); 00081 parameters().add("max_angular_rate_xy_error", max_angular_rate_xy_error_ ); 00082 parameters().add("max_angular_rate_z_error", max_angular_rate_z_error_); 00083 parameters().add("jump_on_max_error", jump_on_max_error_); 00084 } 00085 00086 PoseUpdate::~PoseUpdate() 00087 { 00088 } 00089 00090 bool PoseUpdate::update(PoseEstimation &estimator, const MeasurementUpdate &update_) { 00091 Update const &update = static_cast<Update const &>(update_); 00092 00093 // fetch current state 00094 ColumnVector state = estimator.getState(); 00095 SymmetricMatrix covariance = estimator.getCovariance(); 00096 00097 while (update.pose) { 00098 // convert incoming update information to Eigen 00099 Eigen::Vector3d update_pose(update.pose->pose.pose.position.x, update.pose->pose.pose.position.y, update.pose->pose.pose.position.z); 00100 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); 00101 Eigen::Vector3d update_euler(update_orientation.toRotationMatrix().eulerAngles(2,1,0)); 00102 00103 // information is the information matrix if interpret_covariance_as_information_matrix_ is true and a covariance matrix otherwise 00104 // zero elements are counted as zero information in any case 00105 SymmetricMatrix information(6); 00106 covarianceMsgToBfl(update.pose->pose.covariance, information); 00107 00108 // forward state vector to the individual measurement models 00109 position_xy_model_.ConditionalArgumentSet(0,state); 00110 position_z_model_.ConditionalArgumentSet(0,state); 00111 yaw_model_.ConditionalArgumentSet(0,state); 00112 00113 ROS_DEBUG_STREAM_NAMED("poseupdate", "PoseUpdate: state = [ " << state.transpose() << " ], P = [ " << covariance << " ]" << std::endl 00114 << "update: pose = [ " << update_pose.transpose() << " ], euler = [ " << update_euler.transpose() << " ], information = [ " << information << " ]"); 00115 ROS_DEBUG_STREAM_NAMED("poseupdate", "dt = " << (estimator.getTimestamp() - update.pose->header.stamp).toSec() << " s"); 00116 00117 // predict update pose using the estimated velocity and degrade information 00118 if (!update.pose->header.stamp.isZero()) { 00119 double dt = (estimator.getTimestamp() - update.pose->header.stamp).toSec(); 00120 if (dt < 0.0) { 00121 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring pose update as it has a negative time difference: dt = " << dt << "s"); 00122 break; 00123 00124 } else if (max_time_difference_ > 0.0 && dt >= max_time_difference_) { 00125 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring pose update as the time difference is too large: dt = " << dt << "s"); 00126 break; 00127 00128 } else if (max_time_difference_ > 0.0){ 00129 if (interpret_covariance_as_information_matrix_) 00130 information = information * (1.0 - dt/max_time_difference_); 00131 else 00132 information = information / (1.0 - dt/max_time_difference_); 00133 } 00134 00135 Eigen::Vector3d state_velocity(state.sub(VELOCITY_X, VELOCITY_Z)); 00136 update_pose = update_pose + dt * state_velocity; 00137 #ifdef USE_RATE_SYSTEM_MODEL 00138 Eigen::Vector3d state_rate(state.sub(RATE_X,RATE_Z)); 00139 Eigen::AngleAxisd state_angle_offset(state_rate.norm() * dt, state_rate.normalized()); 00140 update_orientation = state_angle_offset * update_orientation; 00141 #endif 00142 } 00143 00144 // update PositionXY 00145 if (information(1,1) > 0.0 || information(2,2) > 0.0) { 00146 // fetch observation matrix H 00147 Matrix H = position_xy_model_.dfGet(0); 00148 ColumnVector x(position_xy_model_.ExpectedValueGet()); 00149 ColumnVector y(2); 00150 SymmetricMatrix Iy(information.sub(1,2,1,2)); 00151 y(1) = update_pose.x(); 00152 y(2) = update_pose.y(); 00153 00154 // invert Iy if information is a covariance matrix 00155 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse(); 00156 00157 // fixed_position_xy_stddev_ = 1.0; 00158 if (fixed_position_xy_stddev_ != 0.0) { 00159 Iy = 0.0; 00160 Iy(1,1) = Iy(2,2) = 1.0 / (fixed_position_xy_stddev_*fixed_position_xy_stddev_); 00161 } 00162 00163 ROS_DEBUG_STREAM_NAMED("poseupdate", "Position Update: "); 00164 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = [" << x.transpose() << "], H = [ " << H << " ], Px = [" << (H*covariance*H.transpose()) << "], Ix = [ " << (H*covariance*H.transpose()).inverse() << "]"); 00165 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = [" << y.transpose() << "], Iy = [ " << Iy << " ]"); 00166 double innovation = updateInternal(covariance, state, Iy, y - x, H, covariance, state, "position_xy", max_position_xy_error_); 00167 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = [" << position_xy_model_.PredictionGet(ColumnVector(), state).transpose() << "], Pxy = [ " << (H*covariance*H.transpose()) << " ], innovation = " << innovation); 00168 00169 status_flags_ |= STATE_XY_POSITION; 00170 } 00171 00172 // update PositionZ 00173 if (information(3,3) > 0.0) { 00174 // fetch observation matrix H 00175 Matrix H = position_z_model_.dfGet(0); 00176 ColumnVector x(position_z_model_.ExpectedValueGet()); 00177 ColumnVector y(1); y(1) = update_pose.z(); 00178 SymmetricMatrix Iy(information.sub(3,3,3,3)); 00179 00180 // invert Iy if information is a covariance matrix 00181 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse(); 00182 00183 // fixed_position_z_stddev_ = 1.0; 00184 if (fixed_position_z_stddev_ != 0.0) { 00185 Iy = 0.0; 00186 Iy(1,1) = 1.0 / (fixed_position_z_stddev_*fixed_position_z_stddev_); 00187 } 00188 00189 ROS_DEBUG_STREAM_NAMED("poseupdate", "Height Update: "); 00190 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = " << x(1) << ", H = [ " << H << " ], Px = [" << (H*covariance*H.transpose()) << "], Ix = [ " << (H*covariance*H.transpose()).inverse() << "]"); 00191 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = " << y(1) << ", Iy = [ " << Iy << " ]"); 00192 double innovation = updateInternal(covariance, state, Iy, y - x, H, covariance, state, "position_z", max_position_z_error_); 00193 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = " << position_z_model_.PredictionGet(ColumnVector(), state) << ", Pxy = [ " << (H*covariance*H.transpose()) << " ], innovation = " << innovation); 00194 00195 status_flags_ |= STATE_Z_POSITION; 00196 } 00197 00198 // update Yaw 00199 if (information(6,6) > 0.0) { 00200 // fetch observation matrix H 00201 Matrix H = yaw_model_.dfGet(0); 00202 ColumnVector x(yaw_model_.ExpectedValueGet()); 00203 ColumnVector y(1); y(1) = update_euler(0); 00204 SymmetricMatrix Iy(information.sub(6,6,6,6)); 00205 00206 // invert Iy if information is a covariance matrix 00207 if (!interpret_covariance_as_information_matrix_) Iy = Iy.inverse(); 00208 00209 // fixed_yaw_stddev_ = 5.0 * M_PI/180.0; 00210 if (fixed_yaw_stddev_ != 0.0) { 00211 Iy = 0.0; 00212 Iy(1,1) = 1.0 / (fixed_yaw_stddev_*fixed_yaw_stddev_); 00213 } 00214 00215 ROS_DEBUG_STREAM_NAMED("poseupdate", "Yaw Update: "); 00216 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = " << x(1) * 180.0/M_PI << "°, H = [ " << H << " ], Px = [" << (H*covariance*H.transpose()) << "], Ix = [ " << (H*covariance*H.transpose()).inverse() << "]"); 00217 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = " << y(1) * 180.0/M_PI << "°, Iy = [ " << Iy << " ]"); 00218 00219 ColumnVector error(y - x); 00220 error(1) = error(1) - 2.0*M_PI * round(error(1) / (2.0*M_PI)); 00221 00222 double innovation = updateInternal(covariance, state, Iy, error, H, covariance, state, "yaw", max_yaw_error_); 00223 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = " << yaw_model_.PredictionGet(ColumnVector(), state) * 180.0/M_PI << "°, Pxy = [ " << (H*covariance*H.transpose()) << " ], innovation = " << innovation); 00224 00225 status_flags_ |= STATE_YAW; 00226 } 00227 00228 break; 00229 } 00230 00231 while (update.twist) { 00232 // convert incoming update information to Eigen 00233 Eigen::Vector3d update_linear(update.twist->twist.twist.linear.x, update.twist->twist.twist.linear.y, update.twist->twist.twist.linear.z); 00234 Eigen::Vector3d update_angular(update.twist->twist.twist.angular.x, update.twist->twist.twist.angular.y, update.twist->twist.twist.angular.z); 00235 00236 // information is the information matrix if interpret_covariance_as_information_matrix_ is true and a covariance matrix otherwise 00237 // zero elements are counted as zero information in any case 00238 SymmetricMatrix information(6); 00239 covarianceMsgToBfl(update.twist->twist.covariance, information); 00240 00241 // forward state vector to the individual measurement models 00242 twist_model_.ConditionalArgumentSet(0,state); 00243 00244 ROS_DEBUG_STREAM_NAMED("poseupdate", "TwistUpdate: state = [ " << state.transpose() << " ], P = [ " << covariance << " ]" << std::endl 00245 << " update: linear = [ " << update_linear.transpose() << " ], angular = [ " << update_angular.transpose() << " ], information = [ " << information << " ]"); 00246 ROS_DEBUG_STREAM_NAMED("poseupdate", " dt = " << (estimator.getTimestamp() - update.twist->header.stamp).toSec() << " s"); 00247 00248 // degrade information if the time difference is too large 00249 if (!update.twist->header.stamp.isZero()) { 00250 double dt = (estimator.getTimestamp() - update.twist->header.stamp).toSec(); 00251 if (dt < 0.0) { 00252 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring twist update as it has a negative time difference: dt = " << dt << "s"); 00253 break; 00254 00255 } else if (max_time_difference_ > 0.0 && dt >= max_time_difference_) { 00256 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ignoring twist update as the time difference is too large: dt = " << dt << "s"); 00257 break; 00258 00259 } else if (max_time_difference_ > 0.0){ 00260 if (interpret_covariance_as_information_matrix_) 00261 information = information * (1.0 - dt/max_time_difference_); 00262 else 00263 information = information / (1.0 - dt/max_time_difference_); 00264 } 00265 } 00266 00267 // fetch observation matrix H 00268 Matrix H = twist_model_.dfGet(0); 00269 ColumnVector x(twist_model_.ExpectedValueGet()); 00270 TwistModel::NoiseCovariance Iy(information); 00271 TwistModel::MeasurementVector y; 00272 y(1) = update_linear.x(); 00273 y(2) = update_linear.y(); 00274 y(3) = update_linear.z(); 00275 y(4) = update_angular.x(); 00276 y(5) = update_angular.y(); 00277 y(6) = update_angular.z(); 00278 00279 // invert Iy if information is a covariance matrix 00280 if (!interpret_covariance_as_information_matrix_) { 00281 ROS_DEBUG_NAMED("poseupdate", "Twist updates with covariance matrices are currently not supported"); 00282 break; 00283 } 00284 00285 // update VelocityXY 00286 if (information(1,1) > 0.0 || information(2,2) > 0.0) { 00287 status_flags_ |= STATE_XY_VELOCITY; 00288 00289 // fixed_velocity_xy_stddev_ = 1.0; 00290 if (fixed_velocity_xy_stddev_ != 0.0) { 00291 for(int i = 1; i <= 6; ++i) Iy(1,i) = Iy(2,i) = Iy(i,1) = Iy(i,2) = 0.0; 00292 Iy(1,1) = Iy(2,2) = 1.0 / (fixed_velocity_xy_stddev_*fixed_velocity_xy_stddev_); 00293 } 00294 } 00295 00296 // update VelocityZ 00297 if (information(3,3) > 0.0) { 00298 status_flags_ |= STATE_Z_VELOCITY; 00299 00300 // fixed_velocity_z_stddev_ = 1.0; 00301 if (fixed_velocity_z_stddev_ != 0.0) { 00302 for(int i = 1; i <= 6; ++i) Iy(3,i) = Iy(i,3) = 0.0; 00303 Iy(3,3) = 1.0 / (fixed_velocity_z_stddev_*fixed_velocity_z_stddev_); 00304 } 00305 } 00306 00307 // update RateXY 00308 if (information(4,4) > 0.0 || information(5,5) > 0.0) { 00309 // fixed_angular_rate_xy_stddev_ = 1.0; 00310 if (fixed_angular_rate_xy_stddev_ != 0.0) { 00311 for(int i = 1; i <= 6; ++i) Iy(4,i) = Iy(4,i) = Iy(i,5) = Iy(i,5) = 0.0; 00312 Iy(4,4) = Iy(5,5) = 1.0 / (fixed_angular_rate_xy_stddev_*fixed_angular_rate_xy_stddev_); 00313 } 00314 } 00315 00316 // update RateZ 00317 if (information(6,6) > 0.0) { 00318 // fixed_angular_rate_z_stddev_ = 1.0; 00319 if (fixed_angular_rate_z_stddev_ != 0.0) { 00320 for(int i = 1; i <= 6; ++i) Iy(6,i) = Iy(i,6) = 0.0; 00321 Iy(6,6) = 1.0 / (fixed_angular_rate_z_stddev_*fixed_angular_rate_z_stddev_); 00322 } 00323 } 00324 00325 ROS_DEBUG_STREAM_NAMED("poseupdate", "Twist Update: "); 00326 ROS_DEBUG_STREAM_NAMED("poseupdate", " x = [" << x.transpose() << "], H = [ " << H << " ], Px = [" << (H*covariance*H.transpose()) << "], Ix = [ " << (H*covariance*H.transpose()).inverse() << "]"); 00327 ROS_DEBUG_STREAM_NAMED("poseupdate", " y = [" << y.transpose() << "], Iy = [ " << Iy << " ]"); 00328 double innovation = updateInternal(covariance, state, Iy, y - x, H, covariance, state, "twist", 0.0); 00329 ROS_DEBUG_STREAM_NAMED("poseupdate", " ==> xy = [" << twist_model_.PredictionGet(ColumnVector(), state).transpose() << "], Pxy = [ " << (H*covariance*H.transpose()) << " ], innovation = " << innovation); 00330 00331 break; 00332 } 00333 00334 estimator.setState(state); 00335 estimator.setCovariance(covariance); 00336 estimator.updated(); 00337 updated(); 00338 00339 return true; 00340 } 00341 00342 double PoseUpdate::calculateOmega(const SymmetricMatrix &Ix, const SymmetricMatrix &Iy) const { 00343 double tr_x = static_cast<EigenMatrix>(Ix).trace(); 00344 double tr_y = static_cast<EigenMatrix>(Iy).trace(); 00345 return tr_y / (tr_x + tr_y); 00346 } 00347 00348 double PoseUpdate::updateInternal(const SymmetricMatrix &Px, const ColumnVector &x, const SymmetricMatrix &Iy, const ColumnVector &error, const Matrix &H, SymmetricMatrix &Pxy, ColumnVector &xy, const std::string& text, const double max_error) { 00349 Matrix HT(H.transpose()); 00350 SymmetricMatrix H_Px_HT(H*Px*HT); 00351 00352 if (H_Px_HT.determinant() <= 0) { 00353 ROS_WARN_STREAM("Ignoring poseupdate for " << text << " as the a-priori state covariance is zero!"); 00354 return 0.0; 00355 } 00356 SymmetricMatrix Ix(H_Px_HT.inverse()); 00357 00358 ROS_DEBUG_STREAM_NAMED("poseupdate", "H = [" << H << "]"); 00359 ROS_DEBUG_STREAM_NAMED("poseupdate", "Ix = [" << Ix << "]"); 00360 00361 double alpha = fixed_alpha_, beta = fixed_beta_; 00362 if (alpha == 0.0 && beta == 0.0) { 00363 beta = calculateOmega(Ix, Iy); 00364 alpha = 1.0 - beta; 00365 00366 // if (beta > 0.8) { 00367 // ROS_DEBUG_STREAM("Reducing update variance for " << text << " due to high information difference between Ix = [" << Ix << "] and Iy = [" << Iy << "]"); 00368 // beta = 0.8; 00369 // alpha = 1.0 - beta; 00370 // } 00371 } 00372 ROS_DEBUG_STREAM_NAMED("poseupdate", "alpha = " << alpha << ", beta = " << beta); 00373 00374 if (max_error > 0.0) { 00375 double error2 = error.transpose() * Ix * (Ix + Iy).inverse() * Iy * error; 00376 if (error2 > max_error * max_error) { 00377 if (!jump_on_max_error_) { 00378 ROS_WARN_STREAM_NAMED("poseupdate", "Ignoring poseupdate for " << text << " as the error [ " << error.transpose() << " ], |error| = " << sqrt(error2) << " sigma exceeds max_error!"); 00379 return 0.0; 00380 } else { 00381 ROS_WARN_STREAM_NAMED("poseupdate", "Update for " << text << " with error [ " << error.transpose() << " ], |error| = " << sqrt(error2) << " sigma exceeds max_error!"); 00382 alpha = 0.0; 00383 beta = 1.0; 00384 } 00385 } 00386 } 00387 00388 // SymmetricMatrix Ii(Ix * (alpha - 1) + Iy * beta); 00389 // double innovation = Ii.determinant(); 00390 // ROS_DEBUG_STREAM_NAMED("poseupdate", "Ii = [" << Ii << "], innovation = " << innovation); 00391 00392 // S_1 is equivalent to S^(-1) = (H*P*H^T + R)^(-1) in the standard Kalman gain 00393 SymmetricMatrix S_1(Ix - Ix * (Ix * alpha + Iy * beta).inverse() * Ix); 00394 double innovation = S_1.determinant(); 00395 00396 Pxy = Px - Px * HT * S_1 * H * Px; // may invalidate Px if &Pxy == &Px 00397 xy = x + Pxy * HT * Iy * beta * error; 00398 00399 ROS_DEBUG_STREAM_NAMED("poseupdate", "K = [" << (Pxy * HT * Iy * beta) << "]"); 00400 ROS_DEBUG_STREAM_NAMED("poseupdate", "dx = [" << ( Pxy * HT * Iy * beta * error).transpose() << "]"); 00401 00402 return innovation; 00403 } 00404 00405 ColumnVector PositionXYModel::ExpectedValueGet() const { 00406 y_(1) = x_(POSITION_X); 00407 y_(2) = x_(POSITION_Y); 00408 return y_; 00409 } 00410 00411 Matrix PositionXYModel::dfGet(unsigned int i) const { 00412 if (i != 0) return Matrix(); 00413 C_(1,POSITION_X) = 1.0; 00414 C_(2,POSITION_Y) = 1.0; 00415 return C_; 00416 } 00417 00418 ColumnVector PositionZModel::ExpectedValueGet() const { 00419 y_(1) = x_(POSITION_Z); 00420 return y_; 00421 } 00422 00423 Matrix PositionZModel::dfGet(unsigned int i) const { 00424 if (i != 0) return Matrix(); 00425 C_(1,POSITION_Z) = 1.0; 00426 return C_; 00427 } 00428 00429 ColumnVector YawModel::ExpectedValueGet() const { 00430 const double qw = x_(QUATERNION_W); 00431 const double qx = x_(QUATERNION_X); 00432 const double qy = x_(QUATERNION_Y); 00433 const double qz = x_(QUATERNION_Z); 00434 00435 y_(1) = atan2(2*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz); 00436 00437 return y_; 00438 } 00439 00440 Matrix YawModel::dfGet(unsigned int i) const { 00441 if (i != 0) return Matrix(); 00442 00443 const double qw = x_(QUATERNION_W); 00444 const double qx = x_(QUATERNION_X); 00445 const double qy = x_(QUATERNION_Y); 00446 const double qz = x_(QUATERNION_Z); 00447 const double t1 = qw*qw + qx*qx - qy*qy - qz*qz; 00448 const double t2 = 2*(qx*qy + qw*qz); 00449 const double t3 = 1.0 / (t1*t1 + t2*t2); 00450 00451 C_(1,QUATERNION_W) = 2.0 * t3 * (qz * t1 - qw * t2); 00452 C_(1,QUATERNION_X) = 2.0 * t3 * (qy * t1 - qx * t2); 00453 C_(1,QUATERNION_Y) = 2.0 * t3 * (qx * t1 + qy * t2); 00454 C_(1,QUATERNION_Z) = 2.0 * t3 * (qw * t1 + qz * t2); 00455 00456 return C_; 00457 } 00458 00459 ColumnVector TwistModel::ExpectedValueGet() const { 00460 y_(1) = x_(VELOCITY_X); 00461 y_(2) = x_(VELOCITY_Y); 00462 y_(3) = x_(VELOCITY_Z); 00463 #ifdef USE_RATE_SYSTEM_MODEL 00464 y_(4) = x_(RATE_X); 00465 y_(5) = x_(RATE_Y); 00466 y_(6) = x_(RATE_Z); 00467 #else // USE_RATE_SYSTEM_MODEL 00468 y_(4) = 0.0; 00469 y_(5) = 0.0; 00470 y_(6) = 0.0; 00471 #endif // USE_RATE_SYSTEM_MODEL 00472 return y_; 00473 } 00474 00475 Matrix TwistModel::dfGet(unsigned int i) const { 00476 if (i != 0) return Matrix(); 00477 00478 C_(1,VELOCITY_X) = 1.0; 00479 C_(2,VELOCITY_Y) = 1.0; 00480 C_(3,VELOCITY_Z) = 1.0; 00481 #ifdef USE_RATE_SYSTEM_MODEL 00482 C_(4,RATE_X) = 1.0; 00483 C_(5,RATE_Y) = 1.0; 00484 C_(6,RATE_Z) = 1.0; 00485 #endif // USE_RATE_SYSTEM_MODEL 00486 return C_; 00487 } 00488 00489 } // namespace hector_pose_estimation