$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/gps.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 00032 namespace hector_pose_estimation { 00033 00034 GPSModel::GPSModel() 00035 : MeasurementModel(MeasurementDimension) 00036 { 00037 position_stddev_ = 10.0; 00038 velocity_stddev_ = 1.0; 00039 parameters().add("position_stddev", position_stddev_); 00040 parameters().add("velocity_stddev", velocity_stddev_); 00041 } 00042 00043 bool GPSModel::init() 00044 { 00045 NoiseCovariance noise = 0.0; 00046 noise(1,1) = noise(2,2) = pow(position_stddev_, 2); 00047 noise(3,3) = noise(4,4) = pow(velocity_stddev_, 2); 00048 this->AdditiveNoiseSigmaSet(noise); 00049 return true; 00050 } 00051 00052 GPSModel::~GPSModel() {} 00053 00054 SystemStatus GPSModel::getStatusFlags() const { 00055 return STATE_XY_VELOCITY | STATE_XY_POSITION; 00056 } 00057 00058 ColumnVector GPSModel::ExpectedValueGet() const { 00059 this->y_(1) = x_(POSITION_X); 00060 this->y_(2) = x_(POSITION_Y); 00061 this->y_(3) = x_(VELOCITY_X); 00062 this->y_(4) = x_(VELOCITY_Y); 00063 return y_; 00064 } 00065 00066 Matrix GPSModel::dfGet(unsigned int i) const { 00067 if (i != 0) return Matrix(); 00068 C_(1,POSITION_X) = 1.0; 00069 C_(2,POSITION_Y) = 1.0; 00070 C_(3,VELOCITY_X) = 1.0; 00071 C_(4,VELOCITY_Y) = 1.0; 00072 return C_; 00073 } 00074 00075 GPS::GPS(const std::string &name) 00076 : Measurement_<GPSModel,GPSUpdate>(name) 00077 , reference_(0) 00078 , y_(4) 00079 { 00080 } 00081 00082 GPS::~GPS() 00083 {} 00084 00085 void GPS::onReset() { 00086 reference_ = 0; 00087 } 00088 00089 GPSModel::MeasurementVector const& GPS::getVector(const GPSUpdate &update) { 00090 if (!reference_) { 00091 y_(1) = y_(2) = y_(3) = y_(4) = 0.0/0.0; 00092 return y_; 00093 } 00094 00095 reference_->fromWGS84(update.latitude, update.longitude, y_(1), y_(2)); 00096 reference_->fromNorthEast(update.velocity_north, update.velocity_east, y_(3), y_(4)); 00097 00098 last_ = update; 00099 return y_; 00100 } 00101 00102 bool GPS::beforeUpdate(PoseEstimation &estimator, const GPSUpdate &update) { 00103 // reset reference position if GPS has not been updated for a while 00104 if (timedout()) reference_ = 0; 00105 00106 // find new reference position 00107 if (reference_ != estimator.globalReference()) { 00108 reference_ = estimator.globalReference(); 00109 reference_->setPosition(update.latitude, update.longitude); 00110 00111 StateVector state = estimator.getState(); 00112 double new_latitude, new_longitude; 00113 reference_->toWGS84(-state(POSITION_X), -state(POSITION_Y), new_latitude, new_longitude); 00114 reference_->setPosition(new_latitude, new_longitude); 00115 00116 ROS_INFO("Set new GPS reference position to %f/%f", reference_->position().latitude * 180.0/M_PI, reference_->position().longitude * 180.0/M_PI); 00117 } 00118 00119 return true; 00120 } 00121 00122 } // namespace hector_pose_estimation