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/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
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
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 const double& m1 = magnetic_field_reference_(1);
00090 const double& m2 = magnetic_field_reference_(2);
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126 double temp1 = -m2*qw*qw + 2*m1*qw*qz - m2*qx*qx + 2*m1*qx*qy + m2*qy*qy + m2*qz*qz;
00127 double temp2 = m1*qw*qw + 2*m2*qw*qz - m1*qx*qx - 2*m2*qx*qy + m1*qy*qy - m1*qz*qz;
00128 double temp3 = m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz;
00129 C_(1,QUATERNION_W) = 2*qz*temp1;
00130 C_(1,QUATERNION_X) = 2*qy*temp1;
00131 C_(1,QUATERNION_Y) = -2*qx*temp1;
00132 C_(1,QUATERNION_Z) = -2*qw*temp1;
00133 C_(2,QUATERNION_W) = 2*qz*temp2;
00134 C_(2,QUATERNION_X) = 2*qy*temp2;
00135 C_(2,QUATERNION_Y) = -2*qx*temp2;
00136 C_(2,QUATERNION_Z) = -2*qw*temp2;
00137 C_(3,QUATERNION_W) = -4*qz*temp3;
00138 C_(3,QUATERNION_X) = -4*qy*temp3;
00139 C_(3,QUATERNION_Y) = 4*qx*temp3;
00140 C_(3,QUATERNION_Z) = 4*qw*temp3;
00141
00142 return C_;
00143 }
00144
00145 double MagneticModel::getMagneticHeading(const StateVector& state, const MeasurementVector &y) const {
00146 MeasurementVector y_nav;
00147 const double& qw = state(QUATERNION_W), qx = state(QUATERNION_X), qy = state(QUATERNION_Y), qz = state(QUATERNION_Z);
00148 y_nav(1) = y(1) * (qw*qw + qx*qx - qy*qy - qz*qz) + y(2) * (2*qx*qy - 2*qw*qz) + y(3) * (2*qw*qy + 2*qx*qz);
00149 y_nav(2) = y(1) * (2*qw*qz + 2*qx*qy) + y(2) * (qw*qw - qx*qx + qy*qy - qz*qz) + y(3) * (2*qy*qz - 2*qw*qx);
00150 double heading_nav = -atan2(2*qx*qy + 2*qw*qz, qw*qw + qx*qx - qy*qy - qz*qz);
00151
00152 return heading_nav - (-atan2(y_nav(2), y_nav(1)));
00153 }
00154
00155 double MagneticModel::getTrueHeading(const StateVector& state, const MeasurementVector &y) const {
00156 return getMagneticHeading(state, y) + declination_;
00157 }
00158
00159 void MagneticModel::updateMagneticField()
00160 {
00161 double cos_inclination, sin_inclination;
00162 sincos(inclination_, &sin_inclination, &cos_inclination);
00163
00164 double cos_declination, sin_declination;
00165 sincos(declination_, &sin_declination, &cos_declination);
00166
00167
00168 double magnitude = magnitude_;
00169 if (magnitude == 0.0) magnitude = 1.0;
00170
00171 magnetic_field_north_(1) = magnitude * (cos_inclination * cos_declination);
00172 magnetic_field_north_(2) = magnitude * (-sin_declination);
00173 magnetic_field_north_(3) = magnitude * (-sin_inclination * cos_declination);
00174 }
00175
00176 Magnetic::Magnetic(const std::string &name)
00177 : Measurement_<MagneticModel>(name)
00178 , auto_heading_(true)
00179 , reference_(0)
00180 , deviation_(3, 0.0)
00181 {
00182 parameters().add("auto_heading", auto_heading_);
00183 parameters().add("deviation", deviation_);
00184 }
00185
00186 void Magnetic::onReset() {
00187 reference_ = 0;
00188 }
00189
00190 const MagneticModel::MeasurementVector& Magnetic::getVector(const Magnetic::Update& update) {
00191 y_ = Measurement_<MagneticModel>::getVector(update) + deviation_;
00192 if (getModel()->hasMagnitude()) return y_;
00193
00194 double c = 1.0 / y_.norm();
00195 if (isinf(c)) {
00196 y_ = MeasurementVector(0.0);
00197 } else {
00198 y_ = y_ * c;
00199 }
00200 return y_;
00201 }
00202
00203 const MagneticModel::NoiseCovariance& Magnetic::getCovariance(const Magnetic::Update& update) {
00204 if (getModel()->hasMagnitude()) return Measurement_<MagneticModel>::getCovariance(update);
00205
00206 R_ = Measurement_<MagneticModel>::getCovariance(update);
00207 if (update.hasCovariance()) {
00208 double c = 1.0 / Measurement_<MagneticModel>::getVector(update).norm();
00209 if (isinf(c)) {
00210 R_ = 0.0;
00211 R_(1,1) = R_(2,2) = R_(3,3) = 1.0;
00212 } else {
00213 R_ = R_ * (c*c);
00214 }
00215 }
00216
00217 return R_;
00218 }
00219
00220 bool Magnetic::beforeUpdate(PoseEstimation &estimator, const Magnetic::Update &update) {
00221
00222 if (timedout()) reference_ = 0;
00223
00224 if (reference_ != estimator.globalReference()) {
00225 reference_ = estimator.globalReference();
00226 if (auto_heading_) reference_->setCurrentHeading(estimator, getModel()->getTrueHeading(estimator.getState(), getVector(update)));
00227 }
00228
00229 getModel()->setReference(reference_->heading());
00230 return true;
00231 }
00232
00233 }