hector_pose_estimation_core: magnetic.cpp Source File

Go to the documentation of this file.
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/filter/set_filter.h>
00031 
00032 #include <Eigen/Geometry>
00033 
00034 namespace hector_pose_estimation {
00035 
00036 template class Measurement_<MagneticModel>;
00037 
00038 MagneticModel::MagneticModel()
00039   : declination_(0.0), inclination_(60.0 * M_PI/180.0), magnitude_(0.0)
00040 //  , C_full_(3,StateDimension)
00041 {
00042   parameters().add("stddev", stddev_, 1.0);
00043   parameters().add("declination", declination_);
00044   parameters().add("inclination", inclination_);
00045   parameters().add("magnitude", magnitude_);
00046 
00047 //  C_full_.setZero();
00048 }
00049 
00050 MagneticModel::~MagneticModel() {}
00051 
00052 bool MagneticModel::init(PoseEstimation &estimator, State &state)
00053 {
00054   updateMagneticField();
00055   return true;
00056 }
00057 
00058 void MagneticModel::setReference(const GlobalReference::Heading &reference_heading) {
00059   magnetic_field_reference_.x() = reference_heading.cos * magnetic_field_north_.x() - reference_heading.sin * magnetic_field_north_.y();
00060   magnetic_field_reference_.y() = reference_heading.sin * magnetic_field_north_.x() + reference_heading.cos * magnetic_field_north_.y();
00061   magnetic_field_reference_.z() = magnetic_field_north_.z();
00062 }
00063 
00064 void MagneticModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00065 {
00066   if (init) {
00067     R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2);
00068   }
00069 }
00070 
00071 void MagneticModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00072 {
00073   State::ConstOrientationType q(state.getOrientation());
00074 
00075   y_pred(0) = (q.w()*q.w()+q.x()*q.x()-q.y()*q.y()-q.z()*q.z()) * magnetic_field_reference_(0) + (2.0*q.x()*q.y()+2.0*q.w()*q.z())                 * magnetic_field_reference_.y() + (2.0*q.x()*q.z()-2.0*q.w()*q.y())                 * magnetic_field_reference_.z();
00076   y_pred(1) = (2.0*q.x()*q.y()-2.0*q.w()*q.z())                 * magnetic_field_reference_(0) + (q.w()*q.w()-q.x()*q.x()+q.y()*q.y()-q.z()*q.z()) * magnetic_field_reference_.y() + (2.0*q.y()*q.z()+2.0*q.w()*q.x())                 * magnetic_field_reference_.z();
00077   y_pred(2) = (2.0*q.x()*q.z()+2.0*q.w()*q.y())                 * magnetic_field_reference_(0) + (2.0*q.y()*q.z()-2.0*q.w()*q.x())                 * magnetic_field_reference_.y() + (q.w()*q.w()-q.x()*q.x()-q.y()*q.y()+q.z()*q.z()) * magnetic_field_reference_.z();
00078 }
00079 
00080 void MagneticModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00081 {
00082   State::ConstOrientationType q(state.getOrientation());
00083 
00084   if (state.getOrientationIndex() >= 0) {
00085     const double& m1 = magnetic_field_reference_.x();
00086     const double& m2 = magnetic_field_reference_.y();
00087 //    const double& m3 = magnetic_field_reference_.y();
00088 
00089 //    C_full_(0,State::QUATERNION_W) =  2.0*q.w() * magnetic_field_reference_.x() + 2.0*q.z() * magnetic_field_reference_.y() - 2.0*q.y() * magnetic_field_reference_.z();
00090 //    C_full_(0,State::QUATERNION_X) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();
00091 //    C_full_(0,State::QUATERNION_Y) = -2.0*q.y() * magnetic_field_reference_.x() + 2.0*q.x() * magnetic_field_reference_.y() - 2.0*q.w() * magnetic_field_reference_.z();
00092 //    C_full_(0,State::QUATERNION_Z) = -2.0*q.z() * magnetic_field_reference_.x() + 2.0*q.w() * magnetic_field_reference_.y() + 2.0*q.x() * magnetic_field_reference_.z();
00093 //    C_full_(1,State::QUATERNION_W) = -2.0*q.z() * magnetic_field_reference_.x() + 2.0*q.w() * magnetic_field_reference_.y() + 2.0*q.x() * magnetic_field_reference_.z();
00094 //    C_full_(1,State::QUATERNION_X) =  2.0*q.y() * magnetic_field_reference_.x() - 2.0*q.x() * magnetic_field_reference_.y() + 2.0*q.w() * magnetic_field_reference_.z();
00095 //    C_full_(1,State::QUATERNION_Y) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();
00096 //    C_full_(1,State::QUATERNION_Z) = -2.0*q.w() * magnetic_field_reference_.x() - 2.0*q.z() * magnetic_field_reference_.y() + 2.0*q.y() * magnetic_field_reference_.z();
00097 //    C_full_(2,State::QUATERNION_W) =  2.0*q.y() * magnetic_field_reference_.x() - 2.0*q.x() * magnetic_field_reference_.y() + 2.0*q.w() * magnetic_field_reference_.z();
00098 //    C_full_(2,State::QUATERNION_X) =  2.0*q.z() * magnetic_field_reference_.x() - 2.0*q.w() * magnetic_field_reference_.y() - 2.0*q.x() * magnetic_field_reference_.z();
00099 //    C_full_(2,State::QUATERNION_Y) =  2.0*q.w() * magnetic_field_reference_.x() + 2.0*q.z() * magnetic_field_reference_.y() - 2.0*q.y() * magnetic_field_reference_.z();
00100 //    C_full_(2,State::QUATERNION_Z) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();
00101 
00102     // return C_full_;
00103 
00104     // q = [qw qx qy qz]';
00105     // dq/dyaw * dyaw*dq = 1/2 * [-qz -qy qx qw]' * 2 * [-qz; -qy; qx; qw] =
00106     //  [ qz*qz  qz*qy -qz*qx -qz*qw ;
00107     //    qy*qz  qy*qy -qy*qx -qy*qw ;
00108     //   -qx*qz -qx*qy  qx*qx  qx*qw ;
00109     //   -qw*qz -qw*qy  qw*qx  qw*qw ]
00110 
00111 //    for(int i = 0; i <= 2; ++i) {
00112 //      C(i,State::QUATERNION_W) =  C_full_(i,State::QUATERNION_W) * q.z()*q.z() + C_full_(i,State::QUATERNION_X) * q.y()*q.z() - C_full_(i,State::QUATERNION_Y) * q.x()*q.z() - C_full_(i,State::QUATERNION_Z) * q.w()*q.z();
00113 //      C(i,State::QUATERNION_X) =  C_full_(i,State::QUATERNION_W) * q.z()*q.y() + C_full_(i,State::QUATERNION_X) * q.y()*q.y() - C_full_(i,State::QUATERNION_Y) * q.x()*q.y() - C_full_(i,State::QUATERNION_Z) * q.w()*q.y();
00114 //      C(i,State::QUATERNION_Y) = -C_full_(i,State::QUATERNION_W) * q.z()*q.x() - C_full_(i,State::QUATERNION_X) * q.y()*q.x() + C_full_(i,State::QUATERNION_Y) * q.x()*q.x() + C_full_(i,State::QUATERNION_Z) * q.w()*q.x();
00115 //      C(i,State::QUATERNION_Z) = -C_full_(i,State::QUATERNION_W) * q.z()*q.w() - C_full_(i,State::QUATERNION_X) * q.y()*q.w() + C_full_(i,State::QUATERNION_Y) * q.x()*q.w() + C_full_(i,State::QUATERNION_Z) * q.w()*q.w();
00116 //    }
00117 
00118 //
00119 // Simplified with symbolic Matlab toolbox:
00120 //
00121 // C = [  2*qz*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
00122 //        2*qy*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
00123 //       -2*qx*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
00124 //       -2*qw*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz);
00125 //        2*qz*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
00126 //        2*qy*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
00127 //       -2*qx*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
00128 //       -2*qw*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2);
00129 //       -4*qz*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
00130 //       -4*qy*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
00131 //        4*qx*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
00132 //        4*qw*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz) ]
00133 
00134   double temp1 = -m2*q.x()*q.x() + 2*m1*q.x()*q.y() + m2*q.z()*q.x() + m2*q.y()*q.y()   + m2*q.w()*q.y() + 2*m1*q.w()*q.z();
00135   double temp2 =  m1*q.w()*q.w() + 2*m2*q.w()*q.z() - m1*q.x()*q.x() - 2*m2*q.x()*q.y() + m1*q.y()*q.y() - m1*q.z()*q.z();
00136   double temp3 =  m1*q.w()*q.x() +   m2*q.w()*q.y() + m2*q.x()*q.z() -   m1*q.y()*q.z();
00137   C(0,State::QUATERNION_W) =  2*q.z()*temp1;
00138   C(0,State::QUATERNION_X) =  2*q.y()*temp1;
00139   C(0,State::QUATERNION_Y) = -2*q.x()*temp1;
00140   C(0,State::QUATERNION_Z) = -2*q.w()*temp1;
00141   C(1,State::QUATERNION_W) =  2*q.z()*temp2;
00142   C(1,State::QUATERNION_X) =  2*q.y()*temp2;
00143   C(1,State::QUATERNION_Y) = -2*q.x()*temp2;
00144   C(1,State::QUATERNION_Z) = -2*q.w()*temp2;
00145   C(2,State::QUATERNION_W) = -4*q.z()*temp3;
00146   C(2,State::QUATERNION_X) = -4*q.y()*temp3;
00147   C(2,State::QUATERNION_Y) =  4*q.x()*temp3;
00148   C(2,State::QUATERNION_Z) =  4*q.w()*temp3;
00149   }
00150 }
00151 
00152 double MagneticModel::getMagneticHeading(const State& state, const MeasurementVector &y) const {
00153   MeasurementVector y_nav;
00154   State::ConstOrientationType q(state.getOrientation());
00155   y_nav(0) = y(0) * (q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z()) + y(1) * (2*q.x()*q.y() - 2*q.w()*q.z())                         + y(2) * (2*q.w()*q.y() + 2*q.x()*q.z());
00156   y_nav(1) = y(0) * (2*q.w()*q.z() + 2*q.x()*q.y())                         + y(1) * (q.w()*q.w() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z()) + y(2) * (2*q.y()*q.z() - 2*q.w()*q.x());
00157   double heading_nav = -atan2(2*q.x()*q.y() + 2*q.w()*q.z(), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
00158 
00159   return heading_nav - (-atan2(y_nav(1), y_nav(0)));
00160 }
00161 
00162 double MagneticModel::getTrueHeading(const State& state, const MeasurementVector &y) const {
00163   return getMagneticHeading(state, y) + declination_;
00164 }
00165 
00166 void MagneticModel::updateMagneticField()
00167 {
00168   double cos_inclination, sin_inclination;
00169   sincos(inclination_, &sin_inclination, &cos_inclination);
00170 
00171   double cos_declination, sin_declination;
00172   sincos(declination_, &sin_declination, &cos_declination);
00173 
00174   // return normalized magnetic field if magnitude is zero
00175   double magnitude = magnitude_;
00176   if (magnitude == 0.0) magnitude = 1.0;
00177 
00178   magnetic_field_north_.x() = magnitude * (cos_inclination * cos_declination);
00179   magnetic_field_north_.y() = magnitude * (-sin_declination);
00180   magnetic_field_north_.z() = magnitude * (-sin_inclination * cos_declination);
00181 }
00182 
00183 Magnetic::Magnetic(const std::string &name)
00184   : Measurement_<MagneticModel>(name)
00185   , auto_heading_(true)
00186   , deviation_(3)
00187 {
00188   deviation_.setZero();
00189   parameters().add("auto_heading", auto_heading_);
00190   parameters().add("deviation", deviation_);
00191 }
00192 
00193 void Magnetic::onReset() {
00194   reference_.reset();
00195 }
00196 
00197 const MagneticModel::MeasurementVector& Magnetic::getVector(const Magnetic::Update& update, const State& state) {
00198   y_ = Measurement_<MagneticModel>::getVector(update, state) + deviation_;
00199   if (getModel()->hasMagnitude()) return y_;
00200 
00201   double c = 1.0 / y_.norm();
00202   if (isinf(c)) {
00203     y_ = MeasurementVector(0.0);
00204   } else {
00205     y_ = y_ * c;
00206   }
00207   return y_;
00208 }
00209 
00210 const MagneticModel::NoiseVariance& Magnetic::getVariance(const Magnetic::Update& update, const State& state) {
00211   if (getModel()->hasMagnitude()) return Measurement_<MagneticModel>::getVariance(update, state);
00212 
00213   R_ = Measurement_<MagneticModel>::getVariance(update, state);
00214   double c = 1.0 / Measurement_<MagneticModel>::getVector(update, state).norm();
00215   if (isinf(c)) {
00216     R_ = NoiseVariance(1.0);
00217   } else {
00218     R_ =  R_ * (c*c);
00219   }
00220   return R_;
00221 }
00222 
00223 bool Magnetic::prepareUpdate(State &state, const Update &update) {
00224   // reset reference position if Magnetic has not been updated for a while
00225   if (timedout()) reference_.reset();
00226 
00227   if (reference_ != GlobalReference::Instance()) {
00228     reference_ = GlobalReference::Instance();
00229     if (auto_heading_) reference_->setCurrentHeading(state, getModel()->getTrueHeading(state, update.getVector()));
00230   }
00231 
00232   getModel()->setReference(reference_->heading());
00233   return true;
00234 }
00235 
00236 } // namespace hector_pose_estimation