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
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef IMU_FILTER_OBSERVATION_H
00039 #define IMU_FILTER_OBSERVATION_H
00040
00041 #include <ros/ros.h>
00042 #include <Eigen/Geometry>
00043 #include <imu_filter/imu_state.h>
00044
00045 namespace imu_filter
00046 {
00047
00048 class Observation
00049 {
00050 public:
00051 static const size_t Dimension = 7;
00052 typedef Eigen::Matrix<double, Dimension, 1> Vector;
00053 typedef Eigen::Matrix<double, Dimension, Dimension> CovMat;
00054 typedef Eigen::Matrix<double, Dimension, ImuState::Dimension> JacMat;
00055
00056 Observation() :
00057 observation_( Vector::Zero() ),
00058 measCov_( CovMat::Zero() ),
00059 measJacobian_( JacMat::Zero() ),
00060 body2ImuTransform_( Eigen::Affine3d::Identity() ),
00061 lastBodyTransform_( Eigen::Affine3d::Identity() ),
00062 lastWorld2Imu_( Eigen::Affine3d::Identity() ),
00063 deltaTransform_( Eigen::Affine3d::Identity() )
00064 {
00065 }
00066
00067 virtual ~Observation()
00068 {
00069 }
00070
00075 void set( const Vector & v ) { observation_ = v; }
00076
00080 const Vector & vector() const { return observation_; }
00081
00086 void setCovariance( const CovMat & covariance ) { measCov_ = covariance; }
00087
00092 void setCovarianceFromStdDev( const Vector & stdDevs )
00093 {
00094 measCov_.setZero();
00095 for( int i = 0; i < stdDevs.rows(); i++ )
00096 measCov_( i, i ) = utils::square( stdDevs[ i ] );
00097 }
00098
00102 const CovMat & covariance() const { return measCov_; }
00103
00107 const JacMat & jacobian() const { return measJacobian_; }
00108
00109
00115 void setBody2ImuTransform( const Eigen::Quaterniond & q, const Eigen::Vector3d & t )
00116 {
00117 body2ImuTransform_.linear() = q.toRotationMatrix();
00118 body2ImuTransform_.translation() = t;
00119 }
00120
00125 void setBody2ImuTransform( const Eigen::Affine3d & T ) { body2ImuTransform_ = T; }
00126
00130 const Eigen::Affine3d & body2ImuTransform() const { return body2ImuTransform_; }
00131
00139 void updateLastStateInformation( const ImuState & state )
00140 {
00141 lastWorld2Imu_.linear() = state.orientation().toRotationMatrix();
00142 lastWorld2Imu_.translation() = state.position();
00143 }
00144
00148 const Eigen::Affine3d & lastWorld2Imu() const { return lastWorld2Imu_; }
00149
00153 virtual void updateJacobian( const ImuState & state ) = 0;
00154
00155 protected:
00156 Vector observation_;
00157 CovMat measCov_;
00158 JacMat measJacobian_;
00159
00160 Eigen::Affine3d body2ImuTransform_;
00161
00162 Eigen::Affine3d lastBodyTransform_;
00163
00164
00165 Eigen::Affine3d lastWorld2Imu_;
00166
00167
00168 Eigen::Affine3d deltaTransform_;
00169
00170
00171 void computeDeltaFromAbsolute( const Eigen::Affine3d & currentObservation )
00172 {
00173
00174 deltaTransform_ = body2ImuTransform_.inverse() * lastBodyTransform_.inverse() * currentObservation * body2ImuTransform_;
00175 }
00176
00177 void updateObservationFromDelta()
00178 {
00179 Eigen::Affine3d pseudoTrans = lastWorld2Imu_ * deltaTransform_;
00180
00181 Eigen::Quaterniond pseudoQ;
00182 pseudoQ = pseudoTrans.rotation();
00183 pseudoQ.normalize();
00184
00185 observation_.head<4>() = pseudoQ.coeffs();
00186 observation_.tail<3>() = pseudoTrans.translation();
00187 }
00188 };
00189
00190 }
00191
00192 #endif