$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/gravity.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 00032 namespace hector_pose_estimation { 00033 00034 GravityModel::GravityModel() 00035 : MeasurementModel(MeasurementDimension) 00036 , gravity_(0.0) 00037 { 00038 parameters().add("stddev", stddev_, 10.0); 00039 } 00040 00041 bool GravityModel::init() 00042 { 00043 NoiseCovariance noise = 0.0; 00044 noise(1,1) = noise(2,2) = noise(3,3) = pow(stddev_, 2); 00045 this->AdditiveNoiseSigmaSet(noise); 00046 return true; 00047 } 00048 00049 GravityModel::~GravityModel() {} 00050 00051 bool GravityModel::applyStatusMask(const SystemStatus &status) const { 00052 if (status & STATE_ROLLPITCH) return false; 00053 return true; 00054 } 00055 00056 SystemStatus GravityModel::getStatusFlags() const { 00057 return STATE_ROLLPITCH; 00058 } 00059 00060 ColumnVector GravityModel::ExpectedValueGet() const { 00061 const double qw = x_(QUATERNION_W); 00062 const double qx = x_(QUATERNION_X); 00063 const double qy = x_(QUATERNION_Y); 00064 const double qz = x_(QUATERNION_Z); 00065 00066 // y = q * [0 0 1] * q'; 00067 this->y_(1) = -gravity_ * (2*qx*qz - 2*qw*qy); 00068 this->y_(2) = -gravity_ * (2*qw*qx + 2*qy*qz); 00069 this->y_(3) = -gravity_ * (qw*qw - qx*qx - qy*qy + qz*qz); 00070 00071 return y_; 00072 } 00073 00074 Matrix GravityModel::dfGet(unsigned int i) const { 00075 if (i != 0) return Matrix(); 00076 00077 const double qw = x_(QUATERNION_W); 00078 const double qx = x_(QUATERNION_X); 00079 const double qy = x_(QUATERNION_Y); 00080 const double qz = x_(QUATERNION_Z); 00081 00082 C_(1,QUATERNION_W) = gravity_*2*qy; 00083 C_(1,QUATERNION_X) = -gravity_*2*qz; 00084 C_(1,QUATERNION_Y) = gravity_*2*qw; 00085 C_(1,QUATERNION_Z) = -gravity_*2*qx; 00086 C_(2,QUATERNION_W) = -gravity_*2*qx; 00087 C_(2,QUATERNION_X) = -gravity_*2*qw; 00088 C_(2,QUATERNION_Y) = -gravity_*2*qz; 00089 C_(2,QUATERNION_Z) = -gravity_*2*qy; 00090 C_(3,QUATERNION_W) = -gravity_*2*qw; 00091 C_(3,QUATERNION_X) = gravity_*2*qx; 00092 C_(3,QUATERNION_Y) = gravity_*2*qy; 00093 C_(3,QUATERNION_Z) = -gravity_*2*qz; 00094 00095 return C_; 00096 } 00097 00098 bool Gravity::beforeUpdate(PoseEstimation &estimator, const Update &update) { 00099 model_->setGravity(estimator.getSystemModel()->getGravity()); 00100 return true; 00101 } 00102 00103 } // namespace hector_pose_estimation