00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 {
00041 parameters().add("stddev", stddev_, 1.0);
00042 parameters().add("declination", declination_);
00043 parameters().add("inclination", inclination_);
00044 parameters().add("magnitude", magnitude_);
00045 }
00046
00047 MagneticModel::~MagneticModel() {}
00048
00049 bool MagneticModel::init(PoseEstimation &estimator, Measurement &measurement, State &state)
00050 {
00051 updateMagneticField();
00052 return true;
00053 }
00054
00055 void MagneticModel::setReference(const GlobalReference::Heading &reference_heading) {
00056 magnetic_field_reference_.x() = reference_heading.cos * magnetic_field_north_.x() - reference_heading.sin * magnetic_field_north_.y();
00057 magnetic_field_reference_.y() = reference_heading.sin * magnetic_field_north_.x() + reference_heading.cos * magnetic_field_north_.y();
00058 magnetic_field_reference_.z() = magnetic_field_north_.z();
00059 }
00060
00061 void MagneticModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00062 {
00063 if (init) {
00064 R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2);
00065 }
00066 }
00067
00068 void MagneticModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00069 {
00070 const State::RotationMatrix &R = state.R();
00071 y_pred = R.transpose() * magnetic_field_reference_;
00072 }
00073
00074 void MagneticModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00075 {
00076 if (state.orientation()) {
00077 const State::RotationMatrix &R = state.R();
00078 state.orientation()->cols(C)(X,Z) = R(0,0) * magnetic_field_reference_.y() - R(1,0) * magnetic_field_reference_.x();
00079 state.orientation()->cols(C)(Y,Z) = R(0,1) * magnetic_field_reference_.y() - R(1,1) * magnetic_field_reference_.x();
00080 state.orientation()->cols(C)(Z,Z) = R(0,2) * magnetic_field_reference_.y() - R(1,2) * magnetic_field_reference_.x();
00081 }
00082 }
00083
00084 double MagneticModel::getMagneticHeading(const State& state, const MeasurementVector &y) const {
00085 MeasurementVector y_nav;
00086 y_nav = state.R() * y;
00087 return atan2(y_nav.y(), y_nav.x()) - state.getYaw();
00088 }
00089
00090 double MagneticModel::getTrueHeading(const State& state, const MeasurementVector &y) const {
00091 return getMagneticHeading(state, y) + declination_;
00092 }
00093
00094 void MagneticModel::updateMagneticField()
00095 {
00096 double cos_inclination, sin_inclination;
00097 sincos(inclination_, &sin_inclination, &cos_inclination);
00098
00099 double cos_declination, sin_declination;
00100 sincos(declination_, &sin_declination, &cos_declination);
00101
00102
00103 double magnitude = magnitude_;
00104 if (magnitude == 0.0) magnitude = 1.0;
00105
00106 magnetic_field_north_.x() = magnitude * (cos_inclination * cos_declination);
00107 magnetic_field_north_.y() = magnitude * (cos_inclination * (-sin_declination));
00108 magnetic_field_north_.z() = magnitude * (-sin_inclination);
00109 }
00110
00111 Magnetic::Magnetic(const std::string &name)
00112 : Measurement_<MagneticModel>(name)
00113 , auto_heading_(true)
00114 , deviation_(3)
00115 {
00116 deviation_.setZero();
00117 parameters().add("auto_heading", auto_heading_);
00118 parameters().add("deviation", deviation_);
00119 }
00120
00121 void Magnetic::onReset() {
00122 reference_.reset();
00123 }
00124
00125 const MagneticModel::MeasurementVector& Magnetic::getVector(const Magnetic::Update& update, const State& state) {
00126 y_ = Measurement_<MagneticModel>::getVector(update, state) + deviation_;
00127 if (getModel()->hasMagnitude()) return y_;
00128
00129 double norm = y_.norm();
00130 if (norm < 1e-5) {
00131 y_.setZero();
00132 } else {
00133 y_ = y_ / norm;
00134 }
00135 return y_;
00136 }
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151 bool Magnetic::prepareUpdate(State &state, const Update &update) {
00152
00153 if (timedout()) reference_.reset();
00154
00155 if (reference_ != GlobalReference::Instance()) {
00156 reference_ = GlobalReference::Instance();
00157 if (auto_heading_) reference_->setCurrentHeading(state, getModel()->getTrueHeading(state, update.getVector()));
00158 }
00159
00160 getModel()->setReference(reference_->heading());
00161 return true;
00162 }
00163
00164 }