Go to the documentation of this file.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/gps.h>
00030 #include <hector_pose_estimation/global_reference.h>
00031 #include <hector_pose_estimation/filter/set_filter.h>
00032
00033 namespace hector_pose_estimation {
00034
00035 template class Measurement_<GPSModel>;
00036
00037 GPSModel::GPSModel()
00038 {
00039 position_stddev_ = 10.0;
00040 velocity_stddev_ = 1.0;
00041 parameters().add("position_stddev", position_stddev_);
00042 parameters().add("velocity_stddev", velocity_stddev_);
00043 }
00044
00045 GPSModel::~GPSModel() {}
00046
00047 void GPSModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00048 {
00049 if (init) {
00050 R(0,0) = R(1,1) = pow(position_stddev_, 2);
00051 R(2,2) = R(3,3) = pow(velocity_stddev_, 2);
00052 }
00053 }
00054
00055 bool GPSModel::prepareUpdate(State &state, const MeasurementUpdate &update)
00056 {
00057 state.getRotationMatrix(R);
00058 return true;
00059 }
00060
00061 void GPSModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00062 {
00063 y_pred(0) = state.getPosition().x();
00064 y_pred(1) = state.getPosition().y();
00065 y_pred(2) = state.getVelocity().x();
00066 y_pred(3) = state.getVelocity().y();
00067 }
00068
00069 void GPSModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool init)
00070 {
00071 if (!init) return;
00072
00073 if (state.position()) {
00074 state.position()->cols(C)(0,X) = 1.0;
00075 state.position()->cols(C)(1,Y) = 1.0;
00076 }
00077
00078 if (state.velocity()) {
00079 state.velocity()->cols(C)(2,X) = 1.0;
00080 state.velocity()->cols(C)(3,Y) = 1.0;
00081 }
00082 }
00083
00084 GPS::GPS(const std::string &name)
00085 : Measurement_<GPSModel>(name)
00086 , auto_reference_(true)
00087 , y_(4)
00088 {
00089 parameters().add("auto_reference", auto_reference_);
00090 }
00091
00092 GPS::~GPS()
00093 {}
00094
00095 void GPS::onReset() {
00096 reference_.reset();
00097 }
00098
00099 GPSModel::MeasurementVector const& GPS::getVector(const GPSUpdate &update, const State&) {
00100 if (!reference_) {
00101 y_.setConstant(0.0/0.0);
00102 return y_;
00103 }
00104
00105 reference_->fromWGS84(update.latitude, update.longitude, y_(0), y_(1));
00106 reference_->fromNorthEast(update.velocity_north, update.velocity_east, y_(2), y_(3));
00107
00108 return y_;
00109 }
00110
00111 bool GPS::prepareUpdate(State &state, const Update &update) {
00112
00113 if (timedout()) reference_.reset();
00114
00115
00116 if (reference_ != GlobalReference::Instance()) {
00117 reference_ = GlobalReference::Instance();
00118 if (!auto_reference_ && !reference_->hasPosition()) {
00119 ROS_ERROR("Cannot use GPS measurements if no reference latitude/longitude is set and %s/auto_reference parameter is false.", name_.c_str());
00120 return false;
00121 }
00122 if (auto_reference_) reference_->setCurrentPosition(state, update.latitude, update.longitude);
00123 }
00124
00125 return true;
00126 }
00127
00128 }