35 template class Measurement_<GravityModel>;
38 : gravity_(MeasurementVector::Zero())
50 ROS_ERROR(
"Could not find bias substate '%s' during initialization of gravity measurement '%s'.",
use_bias_.c_str(), measurement.
getName().c_str());
65 R(0,0) = R(1,1) = R(2,2) = pow(
stddev_, 2);
72 y_pred = -R.row(2).transpose() *
gravity_.z();
105 bias_->
cols(C) = R.row(2).transpose() * R.row(2);
virtual void getMeasurementNoise(NoiseVariance &R, const State &, bool init)
boost::shared_ptr< SubState_< SubVectorDimension, SubCovarianceDimension > > getSubState(const Model *model) const
SubState_< 3 >::Ptr bias_
virtual const boost::shared_ptr< OrientationStateType > & orientation() const
virtual ParameterList & parameters()
Matrix_< 3, 3 >::type RotationMatrix
virtual void getStateJacobian(MeasurementMatrix &C, const State &state, bool init)
T & getAs(const std::string &key) const
ConstVectorSegment getVector() const
virtual const std::string & getName() const
Block< MatrixType, MatrixType::RowsAtCompileTime, CovarianceDimension > cols(MatrixType &matrix)
const State::RotationMatrix & R() const
MeasurementVector gravity_
virtual void setGravity(double gravity)
ParameterList & add(const std::string &key, T &value, const T &default_value)
ParameterList & parameters()
virtual bool init(PoseEstimation &estimator, Measurement &measurement, State &state)
virtual void getExpectedValue(MeasurementVector &y_pred, const State &state)