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/zerorate.h>
00030 #include <hector_pose_estimation/system/imu_model.h>
00031 #include <hector_pose_estimation/filter/set_filter.h>
00032
00033 namespace hector_pose_estimation {
00034
00035 template class Measurement_<ZeroRateModel>;
00036
00037 ZeroRateModel::ZeroRateModel()
00038 {
00039 parameters().add("stddev", stddev_, 90.0*M_PI/180.0);
00040 parameters().add("use_bias", use_bias_, std::string("gyro_bias"));
00041 }
00042
00043 ZeroRateModel::~ZeroRateModel() {}
00044
00045 bool ZeroRateModel::init(PoseEstimation &estimator, Measurement &measurement, State &state)
00046 {
00047 if (!use_bias_.empty()) {
00048 bias_ = state.getSubState<3,3>(use_bias_);
00049 if (!bias_) {
00050 ROS_ERROR("Could not find bias substate '%s' during initialization of zero rate pseudo measurement '%s'.", use_bias_.c_str(), measurement.getName().c_str());
00051 return false;
00052 }
00053 } else {
00054 bias_.reset();
00055 }
00056
00057 if (!bias_ && !state.rate()) {
00058 ROS_WARN("Pseudo updating with zero rate is a no-op, as the state does not contain rates nor biases.");
00059
00060 }
00061
00062 return true;
00063 }
00064
00065 void ZeroRateModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00066 {
00067 if (init) {
00068 R(0,0) = pow(stddev_, 2);
00069 }
00070 }
00071
00072 void ZeroRateModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00073 {
00074 y_pred(0) = state.getRate().z();
00075
00076 if (!state.rate() && bias_) {
00077 y_pred(0) += bias_->getVector().z();
00078 }
00079 }
00080
00081 void ZeroRateModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00082 {
00083 if (state.rate()) {
00084 state.rate()->cols(C)(0,Z) = 1.0;
00085 } else if (bias_) {
00086 bias_->cols(C)(0,Z) = 1.0;
00087 }
00088 }
00089
00090 const ZeroRateModel::MeasurementVector* ZeroRateModel::getFixedMeasurementVector() const
00091 {
00092 static MeasurementVector zero(MeasurementVector::Zero());
00093 return &zero;
00094 }
00095
00096 }