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/gravity.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 #include <hector_pose_estimation/filter/set_filter.h> 00032 00033 namespace hector_pose_estimation { 00034 00035 template class Measurement_<GravityModel>; 00036 00037 GravityModel::GravityModel() 00038 : gravity_(MeasurementVector::Zero()) 00039 { 00040 parameters().add("stddev", stddev_, 1.0); 00041 parameters().add("use_bias", use_bias_, std::string("accelerometer_bias")); 00042 } 00043 00044 GravityModel::~GravityModel() {} 00045 00046 bool GravityModel::init(PoseEstimation &estimator, Measurement &measurement, State &state) { 00047 if (!use_bias_.empty()) { 00048 bias_ = state.getSubState<3,3>(use_bias_); 00049 if (!bias_) { 00050 ROS_ERROR("Could not find bias substate '%s' during initialization of gravity measurement '%s'.", use_bias_.c_str(), measurement.getName().c_str()); 00051 return false; 00052 } 00053 } else { 00054 bias_.reset(); 00055 } 00056 00057 setGravity(estimator.parameters().getAs<double>("gravity_magnitude")); 00058 return true; 00059 } 00060 00061 00062 void GravityModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init) 00063 { 00064 if (init) { 00065 R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2); 00066 } 00067 } 00068 00069 void GravityModel::getExpectedValue(MeasurementVector& y_pred, const State& state) 00070 { 00071 const State::RotationMatrix &R = state.R(); 00072 y_pred = -R.row(2).transpose() * gravity_.z(); 00073 if (bias_) { 00074 y_pred += bias_->getVector(); 00075 } 00076 } 00077 00078 void GravityModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool) 00079 { 00080 const State::RotationMatrix &R = state.R(); 00081 if (state.orientation()) { 00082 // C(0,state.getOrientationCovarianceIndex() + W) = gravity_.z()*2*q.y(); 00083 // C(0,state.getOrientationCovarianceIndex() + X) = -gravity_.z()*2*q.z(); 00084 // C(0,state.getOrientationCovarianceIndex() + Y) = gravity_.z()*2*q.w(); 00085 // C(0,state.getOrientationCovarianceIndex() + Z) = -gravity_.z()*2*q.x(); 00086 // C(1,state.getOrientationCovarianceIndex() + W) = -gravity_.z()*2*q.x(); 00087 // C(1,state.getOrientationCovarianceIndex() + X) = -gravity_.z()*2*q.w(); 00088 // C(1,state.getOrientationCovarianceIndex() + Y) = -gravity_.z()*2*q.z(); 00089 // C(1,state.getOrientationCovarianceIndex() + Z) = -gravity_.z()*2*q.y(); 00090 // C(2,state.getOrientationCovarianceIndex() + W) = -gravity_.z()*2*q.w(); 00091 // C(2,state.getOrientationCovarianceIndex() + X) = gravity_.z()*2*q.x(); 00092 // C(2,state.getOrientationCovarianceIndex() + Y) = gravity_.z()*2*q.y(); 00093 // C(2,state.getOrientationCovarianceIndex() + Z) = -gravity_.z()*2*q.z(); 00094 00095 state.orientation()->cols(C)(X,X) = -gravity_.z() * R(1,0); 00096 state.orientation()->cols(C)(X,Y) = gravity_.z() * R(0,0); 00097 state.orientation()->cols(C)(Y,X) = -gravity_.z() * R(1,1); 00098 state.orientation()->cols(C)(Y,Y) = gravity_.z() * R(0,1); 00099 state.orientation()->cols(C)(Z,X) = -gravity_.z() * R(1,2); 00100 state.orientation()->cols(C)(Z,Y) = gravity_.z() * R(0,2); 00101 } 00102 00103 // Only the bias component in direction of the gravity is observable, under the assumption that we do not accelerate vertically. 00104 if (bias_) { 00105 bias_->cols(C) = R.row(2).transpose() * R.row(2); 00106 } 00107 } 00108 00109 } // namespace hector_pose_estimation