38 orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>(
"orientation");
39 rate_ = addSubState<RateStateType::VectorDimension,RateStateType::CovarianceDimension>(
"rate");
40 position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>(
"position");
41 velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>(
"velocity");
49 orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>(
"orientation");
50 position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>(
"position");
51 velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>(
"velocity");
59 orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>(
"orientation");
67 position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>(
"position");
68 velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>(
"velocity");
125 R = q.toRotationMatrix();
142 return atan2(2*q.x()*q.y() + 2*q.w()*q.z(), q.x()*q.x() + q.w()*q.w() - q.z()*q.z() - q.y()*q.y());
148 roll = atan2(2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
149 pitch = -asin(2*(q.x()*q.z() - q.w()*q.y()));
150 yaw = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
156 getEuler(euler(0), euler(1), euler(2));
164 int orientation_index, orientation_size;
166 orientation_index =
orientation()->getCovarianceIndex();
167 orientation_size =
orientation()->getCovarianceDimension();
170 orientation_index =
orientation()->getVectorIndex();
171 orientation_size =
orientation()->getVectorDimension();
175 if (orientation_index > 0) {
176 int length = orientation_index;
177 x().head(length) += vector_update.head(length);
181 if (orientation_index + orientation_size < vector_update.size()) {
182 int length = vector_update.size() - orientation_index - orientation_size;
183 x().tail(length) += vector_update.tail(length);
191 x() += vector_update;
204 Eigen::Quaterniond q(
orientation()->vector().data());
205 q = Eigen::Quaterniond().fromRotationVector(rotation_vector) * q;
220 if (!(*it)(new_status))
return false;
270 roll = atan2(2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
271 pitch = -asin(2*(q.x()*q.z() - q.w()*q.y()));
278 fake_orientation_ =
Quaternion(Eigen::AngleAxis<ScalarType>(yaw, ColumnVector3::UnitZ()) * Eigen::AngleAxis<ScalarType>(pitch, ColumnVector3::UnitY()) * Eigen::AngleAxis<ScalarType>(roll, ColumnVector3::UnitX())).coeffs();
284 double yaw = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
291 fake_orientation_ =
Quaternion(Eigen::AngleAxis<ScalarType>(yaw, ColumnVector3::UnitZ()) * Eigen::AngleAxis<ScalarType>(euler(1), ColumnVector3::UnitY()) * Eigen::AngleAxis<ScalarType>(euler(2), ColumnVector3::UnitX())).coeffs();
virtual ~OrientationOnlyState()
OrientationPositionVelocityState()
void setOrientation(const Quaternion &orientation)
virtual const boost::shared_ptr< OrientationStateType > & orientation() const
std::string getSystemStatusString(const SystemStatus &status, const SystemStatus &asterisk_status=0)
Matrix_< 3, 3 >::type RotationMatrix
VectorBlock< const Vector, 3 > ConstAccelerationType
boost::function< bool(SystemStatus &)> SystemStatusCallback
virtual ConstRateType getRate() const
void setYaw(const Quaternion &orientation)
const State::RotationMatrix & R() const
virtual ConstVelocityType getVelocity() const
SystemStatus measurement_status_
virtual const boost::shared_ptr< PositionStateType > & position() const
virtual ConstOrientationType getOrientation() const
virtual void updateOrientation(const ColumnVector3 &rotation_vector)
ColumnVector3 getEuler() const
boost::shared_ptr< PositionStateType > position_
Vector fake_acceleration_
Eigen::Quaternion< ScalarType > Quaternion
virtual ~OrientationPositionVelocityState()
boost::shared_ptr< BaseState > base_
virtual IndexType getCovarianceDimension() const
virtual const Vector & getVector() const
boost::shared_ptr< VelocityStateType > velocity_
std::vector< SystemStatusCallback > status_callbacks_
virtual bool inSystemStatus(SystemStatus test_status) const
unsigned int SystemStatus
boost::shared_ptr< RateStateType > rate_
virtual const boost::shared_ptr< VelocityStateType > & velocity() const
virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear)
SystemStatus system_status_
virtual IndexType getVectorDimension() const
virtual SystemStatus getSystemStatus() const
virtual bool setMeasurementStatus(SystemStatus new_status)
virtual bool valid() const
virtual ConstAccelerationType getAcceleration() const
VectorBlock< const Vector, 4 > ConstOrientationType
void getRotationMatrix(RotationMatrix &R) const
#define ROS_INFO_STREAM(args)
virtual bool setSystemStatus(SystemStatus new_status)
virtual const boost::shared_ptr< RateStateType > & rate() const
VectorBlock< const Vector, 3 > ConstRateType
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void setRollPitch(const Quaternion &orientation)
virtual ~PositionVelocityState()
VectorBlock< const Vector, 3 > ConstPositionType
virtual void update(const Vector &vector_update)
virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear)
ColumnVector_< 3 >::type ColumnVector3
boost::shared_ptr< OrientationStateType > orientation_
virtual ConstPositionType getPosition() const
VectorBlock< const Vector, 3 > ConstVelocityType
virtual const boost::shared_ptr< AccelerationStateType > & acceleration() const
virtual void addSystemStatusCallback(const SystemStatusCallback &callback)