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/rate.h>
00030 #include <hector_pose_estimation/filter/set_filter.h>
00031
00032 namespace hector_pose_estimation {
00033
00034 template class Measurement_<RateModel>;
00035
00036 RateModel::RateModel()
00037 {
00038 parameters().add("stddev", stddev_, 10.0 * M_PI/180.0);
00039 parameters().add("use_bias", use_bias_, std::string("gyro_bias"));
00040 }
00041
00042 RateModel::~RateModel() {}
00043
00044 bool RateModel::init(PoseEstimation &estimator, Measurement &measurement, State &state)
00045 {
00046 if (!use_bias_.empty()) {
00047 bias_ = state.getSubState<3,3>(use_bias_);
00048 if (!bias_) {
00049 ROS_ERROR("Could not find bias substate '%s' during initialization of rate measurement '%s'.", use_bias_.c_str(), measurement.getName().c_str());
00050 return false;
00051 }
00052 } else {
00053 bias_.reset();
00054 }
00055
00056 return true;
00057 }
00058
00059 void RateModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00060 {
00061 if (init) {
00062 R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2);
00063 }
00064 }
00065
00066 void RateModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00067 {
00068 y_pred = state.getRate();
00069
00070 if (bias_) {
00071 y_pred += bias_->getVector();
00072 }
00073 }
00074
00075 void RateModel::getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
00076 {
00077 if (!init) return;
00078
00079 if (state.rate()) {
00080 state.rate()->cols(C).setIdentity();
00081 }
00082
00083 if (bias_) {
00084 bias_->cols(C).setIdentity();
00085 }
00086 }
00087
00088 }