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 }
00041
00042 ZeroRateModel::~ZeroRateModel() {}
00043
00044 bool ZeroRateModel::init(PoseEstimation &estimator, State &state)
00045 {
00046 gyro_drift_ = state.addSubState<3>(this, "gyro");
00047
00048 if (!gyro_drift_ && state.getRateIndex() < 0) {
00049 ROS_WARN_NAMED("zerorate", "Updating with zero rate is a no-op, as the state does not contain rates and gyro drift estimation is disabled.");
00050
00051 }
00052
00053 return true;
00054 }
00055
00056 void ZeroRateModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00057 {
00058 if (init) {
00059 R = pow(stddev_, 2);
00060 }
00061 }
00062
00063 void ZeroRateModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00064 {
00065 y_pred(0) = state.getRate().z();
00066
00067 if (state.getRateIndex() < 0 && gyro_drift_) {
00068 y_pred(0) += gyro_drift_->getVector().z();
00069 }
00070 }
00071
00072 void ZeroRateModel::getStateJacobian(MeasurementMatrix& C0, SubMeasurementMatrix& C1, const State& state, bool)
00073 {
00074 if (state.getRateIndex() >= 0) {
00075 C0(0, State::RATE_Z) = 1.0;
00076 } else if (gyro_drift_) {
00077 C1(0, GyroModel::BIAS_GYRO_Z) = 1.0;
00078 }
00079 }
00080
00081 const ZeroRateModel::MeasurementVector* ZeroRateModel::getFixedMeasurementVector()
00082 {
00083 static MeasurementVector zero = 0.0;
00084 return &zero;
00085 }
00086
00087 }