$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/magnetic.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 00032 namespace hector_pose_estimation { 00033 00034 MagneticModel::MagneticModel() 00035 : MeasurementModel(MeasurementDimension) 00036 , declination_(0.0), inclination_(60.0 * M_PI/180.0), magnitude_(0.0) 00037 , C_full_(3,StateDimension) 00038 { 00039 parameters().add("stddev", stddev_, 1.0); 00040 parameters().add("declination", declination_); 00041 parameters().add("inclination", inclination_); 00042 parameters().add("magnitude", magnitude_); 00043 00044 C_full_= 0.0; 00045 } 00046 00047 MagneticModel::~MagneticModel() {} 00048 00049 bool MagneticModel::init() 00050 { 00051 NoiseCovariance noise = 0.0; 00052 noise(1,1) = noise(2,2) = noise(3,3) = pow(stddev_, 2); 00053 this->AdditiveNoiseSigmaSet(noise); 00054 00055 updateMagneticField(); 00056 return true; 00057 } 00058 00059 SystemStatus MagneticModel::getStatusFlags() const { 00060 return STATE_YAW; 00061 } 00062 00063 void MagneticModel::setReference(const GlobalReference::Heading &reference_heading) { 00064 magnetic_field_reference_(1) = reference_heading.cos * magnetic_field_north_(1) - reference_heading.sin * magnetic_field_north_(2); 00065 magnetic_field_reference_(2) = reference_heading.sin * magnetic_field_north_(1) + reference_heading.cos * magnetic_field_north_(2); 00066 magnetic_field_reference_(3) = magnetic_field_north_(3); 00067 } 00068 00069 ColumnVector MagneticModel::ExpectedValueGet() const { 00070 const double qw = x_(QUATERNION_W); 00071 const double qx = x_(QUATERNION_X); 00072 const double qy = x_(QUATERNION_Y); 00073 const double qz = x_(QUATERNION_Z); 00074 00075 y_(1) = (qw*qw+qx*qx-qy*qy-qz*qz) * magnetic_field_reference_(1) + (2.0*qx*qy+2.0*qw*qz) * magnetic_field_reference_(2) + (2.0*qx*qz-2.0*qw*qy) * magnetic_field_reference_(3); 00076 y_(2) = (2.0*qx*qy-2.0*qw*qz) * magnetic_field_reference_(1) + (qw*qw-qx*qx+qy*qy-qz*qz) * magnetic_field_reference_(2) + (2.0*qy*qz+2.0*qw*qx) * magnetic_field_reference_(3); 00077 y_(3) = (2.0*qx*qz+2.0*qw*qy) * magnetic_field_reference_(1) + (2.0*qy*qz-2.0*qw*qx) * magnetic_field_reference_(2) + (qw*qw-qx*qx-qy*qy+qz*qz) * magnetic_field_reference_(3); 00078 00079 return y_; 00080 } 00081 00082 Matrix MagneticModel::dfGet(unsigned int i) const { 00083 if (i != 0) return Matrix(); 00084 00085 const double qw = x_(QUATERNION_W); 00086 const double qx = x_(QUATERNION_X); 00087 const double qy = x_(QUATERNION_Y); 00088 const double qz = x_(QUATERNION_Z); 00089 00090 C_full_(1,QUATERNION_W) = 2.0*qw * magnetic_field_reference_(1) + 2.0*qz * magnetic_field_reference_(2) - 2.0*qy * magnetic_field_reference_(3); 00091 C_full_(1,QUATERNION_X) = 2.0*qx * magnetic_field_reference_(1) + 2.0*qy * magnetic_field_reference_(2) + 2.0*qz * magnetic_field_reference_(3); 00092 C_full_(1,QUATERNION_Y) = -2.0*qy * magnetic_field_reference_(1) + 2.0*qx * magnetic_field_reference_(2) - 2.0*qw * magnetic_field_reference_(3); 00093 C_full_(1,QUATERNION_Z) = -2.0*qz * magnetic_field_reference_(1) + 2.0*qw * magnetic_field_reference_(2) + 2.0*qx * magnetic_field_reference_(3); 00094 C_full_(2,QUATERNION_W) = -2.0*qz * magnetic_field_reference_(1) + 2.0*qw * magnetic_field_reference_(2) + 2.0*qx * magnetic_field_reference_(3); 00095 C_full_(2,QUATERNION_X) = 2.0*qy * magnetic_field_reference_(1) - 2.0*qx * magnetic_field_reference_(2) + 2.0*qw * magnetic_field_reference_(3); 00096 C_full_(2,QUATERNION_Y) = 2.0*qx * magnetic_field_reference_(1) + 2.0*qy * magnetic_field_reference_(2) + 2.0*qz * magnetic_field_reference_(3); 00097 C_full_(2,QUATERNION_Z) = -2.0*qw * magnetic_field_reference_(1) - 2.0*qz * magnetic_field_reference_(2) + 2.0*qy * magnetic_field_reference_(3); 00098 C_full_(3,QUATERNION_W) = 2.0*qy * magnetic_field_reference_(1) - 2.0*qx * magnetic_field_reference_(2) + 2.0*qw * magnetic_field_reference_(3); 00099 C_full_(3,QUATERNION_X) = 2.0*qz * magnetic_field_reference_(1) - 2.0*qw * magnetic_field_reference_(2) - 2.0*qx * magnetic_field_reference_(3); 00100 C_full_(3,QUATERNION_Y) = 2.0*qw * magnetic_field_reference_(1) + 2.0*qz * magnetic_field_reference_(2) - 2.0*qy * magnetic_field_reference_(3); 00101 C_full_(3,QUATERNION_Z) = 2.0*qx * magnetic_field_reference_(1) + 2.0*qy * magnetic_field_reference_(2) + 2.0*qz * magnetic_field_reference_(3); 00102 00103 // return C_full_; 00104 00105 // q = [qw qx qy qz]'; 00106 // dq/dyaw * dyaw*dq = 1/2 * [-qz -qy qx qw]' * 2 * [-qz; -qy; qx; qw] = 00107 // [ qz*qz qz*qy -qz*qx -qz*qw ; 00108 // qy*qz qy*qy -qy*qx -qy*qw ; 00109 // -qx*qz -qx*qy qx*qx qx*qw ; 00110 // -qw*qz -qw*qy qw*qx qw*qw ] 00111 00112 for(int i = 1; i <= 3; ++i) { 00113 C_(i,QUATERNION_W) = C_full_(i,QUATERNION_W) * qz*qz + C_full_(i,QUATERNION_X) * qy*qz - C_full_(i,QUATERNION_Y) * qx*qz - C_full_(i,QUATERNION_Z) * qw*qz; 00114 C_(i,QUATERNION_X) = C_full_(i,QUATERNION_W) * qz*qy + C_full_(i,QUATERNION_X) * qy*qy - C_full_(i,QUATERNION_Y) * qx*qy - C_full_(i,QUATERNION_Z) * qw*qy; 00115 C_(i,QUATERNION_Y) = -C_full_(i,QUATERNION_W) * qz*qx - C_full_(i,QUATERNION_X) * qy*qx + C_full_(i,QUATERNION_Y) * qx*qx + C_full_(i,QUATERNION_Z) * qw*qx; 00116 C_(i,QUATERNION_Z) = -C_full_(i,QUATERNION_W) * qz*qw - C_full_(i,QUATERNION_X) * qy*qw + C_full_(i,QUATERNION_Y) * qx*qw + C_full_(i,QUATERNION_Z) * qw*qw; 00117 } 00118 00119 return C_; 00120 } 00121 00122 double MagneticModel::getMagneticHeading(const MeasurementVector &y) const { 00123 return -(-atan2(y(2), y(1))); 00124 } 00125 00126 double MagneticModel::getTrueHeading(const MeasurementVector &y) const { 00127 return getMagneticHeading(y) + declination_; 00128 } 00129 00130 void MagneticModel::updateMagneticField() 00131 { 00132 double cos_inclination, sin_inclination; 00133 sincos(inclination_, &sin_inclination, &cos_inclination); 00134 00135 double cos_declination, sin_declination; 00136 sincos(declination_, &sin_declination, &cos_declination); 00137 00138 // return normalized magnetic field if magnitude is zero 00139 double magnitude = magnitude_; 00140 if (magnitude == 0.0) magnitude = 1.0; 00141 00142 magnetic_field_north_(1) = magnitude * (cos_inclination * cos_declination); 00143 magnetic_field_north_(2) = magnitude * (-sin_declination); 00144 magnetic_field_north_(3) = magnitude * (-sin_inclination * cos_declination); 00145 } 00146 00147 Magnetic::Magnetic(const std::string &name) 00148 : Measurement_<MagneticModel>(name) 00149 , auto_heading_(true) 00150 , reference_(0) 00151 { 00152 parameters().add("auto_heading", auto_heading_); 00153 } 00154 00155 void Magnetic::onReset() { 00156 reference_ = 0; 00157 } 00158 00159 const MagneticModel::MeasurementVector& Magnetic::getVector(const Magnetic::Update& update) { 00160 if (getModel()->hasMagnitude()) return Measurement_<MagneticModel>::getVector(update); 00161 00162 y_ = Measurement_<MagneticModel>::getVector(update); 00163 double c = 1.0 / y_.norm(); 00164 if (isinf(c)) { 00165 y_ = MeasurementVector(0.0); 00166 } else { 00167 y_ = y_ * c; 00168 } 00169 return y_; 00170 } 00171 00172 const MagneticModel::NoiseCovariance& Magnetic::getCovariance(const Magnetic::Update& update) { 00173 if (getModel()->hasMagnitude()) return Measurement_<MagneticModel>::getCovariance(update); 00174 00175 R_ = Measurement_<MagneticModel>::getCovariance(update); 00176 double c = 1.0 / Measurement_<MagneticModel>::getVector(update).norm(); 00177 if (isinf(c)) { 00178 R_ = NoiseCovariance(1.0); 00179 } else { 00180 R_ = R_ * (c*c); 00181 } 00182 return R_; 00183 } 00184 00185 bool Magnetic::beforeUpdate(PoseEstimation &estimator, const Magnetic::Update &update) { 00186 // reset reference position if Magnetic has not been updated for a while 00187 if (timedout()) reference_ = 0; 00188 00189 if (reference_ != estimator.globalReference()) { 00190 reference_ = estimator.globalReference(); 00191 00192 if (auto_heading_) { 00193 double yaw, pitch, roll; 00194 estimator.getOrientation(yaw, pitch, roll); 00195 reference_->setHeading(getModel()->getTrueHeading(update.getVector()) - (-yaw)); 00196 ROS_INFO("Set new reference heading to %.1f degress", reference_->heading() * 180.0 / M_PI); 00197 } 00198 } 00199 00200 getModel()->setReference(reference_->heading()); 00201 return true; 00202 } 00203 00204 } // namespace hector_pose_estimation