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_IMU_EKF_H
00039 #define IMU_FILTER_IMU_EKF_H
00040
00041 #include <Eigen/Core>
00042 #include <Eigen/Geometry>
00043
00044 #include <opencv/cv.hpp>
00045
00046 #include <ros/ros.h>
00047
00048 #include <imu_filter/imu_measurement.h>
00049 #include <imu_filter/imu_state.h>
00050 #include <imu_filter/observation.h>
00051
00052 namespace imu_filter
00053 {
00054
00060 class ImuEkf
00061 {
00062 public:
00063 typedef Eigen::Matrix<double, 12, 12> ProcessNoiseMatrix;
00064
00069 ImuEkf();
00070
00074 ~ImuEkf();
00075
00090 void propagate( const ImuMeasurement & measurement, double deltaT );
00091
00099 void correction( const Observation & observation );
00100
00106 const ImuState & state() const { return state_; }
00107 ImuState & state() { return state_; }
00108
00121 void initialize( const Eigen::Quaterniond & rot,
00122 const Eigen::Vector3d & pos,
00123 const Eigen::Quaterniond & calibRot,
00124 const Eigen::Vector3d & calibPos,
00125 const Eigen::Vector3d & measuredAcceleration,
00126 const ImuState::CovarianceMatrix & covMat,
00127 bool constrainGravity );
00128
00133 void setStateCovariance( const ImuState::CovarianceMatrix & covMat ) { state_.covariance() = covMat; }
00134
00139 void setStateCovariance( const ImuState::StateVector & diag ) { state_.covariance() = diag.asDiagonal(); }
00140
00147 void setProcessNoise( const ProcessNoiseMatrix & noiseMat ) { Q_ = noiseMat; }
00148
00155 void setExtrinsicTransform( const Eigen::Quaterniond & rot, const Eigen::Vector3d & trans );
00156
00157
00166 void objectTransformation( Eigen::Quaterniond & rot, Eigen::Vector3d & trans, Eigen::Vector3d & grav );
00167
00172 void setEstimateGravity( bool v ) { state_.setEstimateGravity( v ); }
00173
00178 void setEstimateGyroBias( bool v ) { state_.setEstimateGyroBias( v ); }
00179
00184 void setEstimateAccBias( bool v ) { state_.setEstimateAccBias( v ); }
00185
00190 void setVelocityDamping( double v ) { velocity_damping_ = v; }
00191
00192 private:
00193 ImuState state_;
00194 cv::RNG rng_;
00195
00196
00197
00198 Eigen::Matrix<double, ImuState::Dimension, ImuState::Dimension> F_;
00199
00200
00201 Eigen::Matrix<double, ImuState::Dimension, 12> G_;
00202
00203
00204 ProcessNoiseMatrix Q_;
00205
00206
00207
00208 Observation::Vector measResidual_;
00209 ImuState::StateVector stateUpdate_;
00210 Eigen::Matrix<double, ImuState::Dimension, 7> kalmanGain_;
00211 ImuState::CovarianceMatrix covarianceUpdate_;
00212 double velocity_damping_;
00213
00214
00215
00216 void updateProcessNoise( const ImuMeasurement & measurment, double deltaT );
00217
00218 void propagateCovariance();
00219
00220 void propagateOrientation( Eigen::Quaterniond & qnew,
00221 const ImuState & state,
00222 const ImuMeasurement & measurement,
00223 double deltaT );
00224
00225 void propagateTranslation( Eigen::Vector3d & tnew,
00226 const Eigen::Quaterniond & rot,
00227 const ImuState & state,
00228 const ImuMeasurement & measurement,
00229 double deltaT );
00230
00231 void propagateVelocity( Eigen::Vector3d & vnew,
00232 const Eigen::Quaterniond & rot,
00233 const ImuState & state,
00234 const ImuMeasurement & measurement,
00235 double deltaT );
00236
00237
00238 void updatePerturbationJacobian( const Eigen::Quaterniond & d, double deltaT );
00239
00240 void numericalJacobians( const ImuState & state,
00241 const ImuMeasurement & measurement,
00242 double deltaT,
00243 Eigen::Matrix<double, 19, 19> & F,
00244 Eigen::Matrix<double, 19, 12> & G );
00245
00246 void numericalJacobians2( const ImuState & state,
00247 const ImuMeasurement & measurement,
00248 double deltaT,
00249 Eigen::Matrix<double, 19, 19> & F,
00250 Eigen::Matrix<double, 19, 12> & G );
00251
00252 void noiseJacobian( const ImuState & state,
00253 const ImuMeasurement & meas,
00254 double deltaT,
00255 double step,
00256 Eigen::Matrix<double, 19,12> & G );
00257
00258 void stateJacobian( const ImuState & state,
00259 const ImuMeasurement & meas,
00260 double deltaT,
00261 double step,
00262 Eigen::Matrix<double, 19, 19> & F );
00263
00264 void propagateState( const ImuState & oldState,
00265 const ImuMeasurement & meas,
00266 double deltaT, ImuState & newState );
00267
00268 void normalizeOrientationObservation( Eigen::Quaterniond & q )
00269 {
00270 q.normalize();
00271
00272
00273 Eigen::Vector4d a, b;
00274 a = state_.orientation().coeffs();
00275 b = q.coeffs();
00276 if( ( a - b ).norm() > ( a + b ).norm() ) {
00277 q.coeffs() *= -1;
00278 }
00279 }
00280 };
00281
00282 }
00283
00284 #endif