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 #include <imu_filter/imu_ekf.h>
00038 #include <imu_filter/quaternion_helper.h>
00039 #include <imu_filter/marker_visualization.h>
00040
00041 #include <imu_filter/debug_helper.h>
00042 #include <imu_filter/imu_tools.h>
00043
00044 namespace imu_filter
00045 {
00046
00047 ImuEkf::ImuEkf() :
00048 rng_( time( NULL ) ),
00049 F_( Eigen::Matrix<double, 19, 19>::Identity() ),
00050 G_( Eigen::Matrix<double, 19, 12>::Zero() ),
00051 Q_( Eigen::Matrix<double, 12, 12>::Zero() ),
00052 velocity_damping_( 1.0 )
00053 {
00054 G_.block<3, 3>( 9, 6 ) = Eigen::Matrix3d::Identity();
00055 G_.block<3, 3>( 12, 9 ) = Eigen::Matrix3d::Identity();
00056 }
00057
00058 ImuEkf::~ImuEkf()
00059 {
00060 }
00061
00062 void ImuEkf::propagate( const ImuMeasurement & measurement, double deltaT )
00063 {
00064 Eigen::Quaterniond qNew;
00065 Eigen::Vector3d posNew, velNew;
00066
00067 propagateOrientation( qNew, state_, measurement, deltaT );
00068 propagateTranslation( posNew, qNew, state_, measurement, deltaT );
00069 propagateVelocity( velNew, qNew, state_, measurement, deltaT );
00070 state_.gyroBias() += measurement.gyroBiasPerturbation;
00071 state_.accBias() += measurement.accBiasPerturbation;
00072
00073
00074 numericalJacobians( state_, measurement, deltaT, F_, G_ );
00075
00076 updateProcessNoise( measurement, deltaT );
00077 propagateCovariance();
00078
00079 state_.setOrientation( qNew );
00080 state_.setPosition( posNew );
00081 state_.setVelocity( velNew );
00082
00083
00084 MarkerVisualization & visualizer = MarkerVisualization::instance();
00085 visualizer.addImuMarker( state_, measurement.linearAcceleration );
00086 visualizer.publish();
00087 }
00088
00089 void ImuEkf::updateProcessNoise( const ImuMeasurement & measurement, double deltaT )
00090 {
00091 Q_.block<3, 3>( 0, 0 ) = measurement.angularRateCovariance;
00092 Q_.block<3, 3>( 3, 3 ) = measurement.linearAccelerationCovariance;
00093
00094
00095 Q_.block<3, 3>( 6, 6 ) = measurement.gyroBiasCovariance * deltaT;
00096
00097
00098 Q_.block<3, 3>( 9, 9 ) = measurement.accBiasCovariance * deltaT;
00099 }
00100
00101 void ImuEkf::propagateCovariance()
00102 {
00103 state_.covariance() = F_ * state_.covariance() * F_.transpose() + G_ * Q_ * G_.transpose();
00104 }
00105
00106 void ImuEkf::correction( const Observation & observation )
00107 {
00108
00109
00110 const Observation::Vector & obs = observation.vector();
00111 Eigen::Quaterniond qObs;
00112 qObs.coeffs() = obs.head<4>();
00113
00114 normalizeOrientationObservation( qObs );
00115
00116 measResidual_.head<4>() = qObs.coeffs() - state_.orientation().coeffs();
00117 measResidual_.tail<3>() = obs.tail<3>() - state_.position();
00118
00119 const Observation::JacMat & measJac = observation.jacobian();
00120
00121
00122 Eigen::Matrix<double, 7, 7> S = measJac * state_.covariance() * measJac.transpose() + observation.covariance();
00123
00124 Eigen::Matrix<double, 7, 7> SInv = S.inverse();
00125
00126 kalmanGain_ = state_.covariance() * measJac.transpose() * SInv;
00127
00128
00129 stateUpdate_ = kalmanGain_ * measResidual_;
00130
00131 if( stateUpdate_.head<4>().norm() > 0.5 ){
00132 std::cout << "\n\n\nSUSPICIOUS!!!!!\n" << std::endl;
00133 }
00134
00135 state_.update( stateUpdate_ );
00136
00137
00138 covarianceUpdate_ = kalmanGain_ * measJac;
00139 state_.covariance() -= covarianceUpdate_ * state_.covariance();
00140
00141
00142 state_.fixCovariance();
00143 }
00144
00145
00146 void ImuEkf::initialize( const Eigen::Quaterniond & rot,
00147 const Eigen::Vector3d & pos,
00148 const Eigen::Quaterniond & calibRot,
00149 const Eigen::Vector3d & calibPos,
00150 const Eigen::Vector3d & measuredAcceleration,
00151 const ImuState::CovarianceMatrix & covMat,
00152 bool constrainGravity )
00153 {
00154
00155 setExtrinsicTransform( calibRot, calibPos );
00156
00157
00158 state_.setOrientation( rot * calibRot );
00159 state_.setPosition( rot * calibPos + pos );
00160
00161 state_.covariance() = covMat;
00162
00163
00164 Eigen::Vector3d gravity, accBias;
00165 if( constrainGravity ){
00166 double s = sqrt( utils::square( EARTH_GRAVITY ) / measuredAcceleration.squaredNorm() );
00167 gravity = -s * measuredAcceleration;
00168 } else {
00169 gravity = -measuredAcceleration;
00170 }
00171
00172
00173 accBias = measuredAcceleration + gravity;
00174 state_.setAccBias( accBias );
00175
00176
00177 gravity = state_.orientation() * gravity;
00178 state_.setGravity( gravity );
00179
00180 state_.setGyroBias( Eigen::Vector3d::Zero() );
00181 state_.setVelocity( Eigen::Vector3d::Zero() );
00182 }
00183
00184
00185 void ImuEkf::setExtrinsicTransform( const Eigen::Quaterniond & rot, const Eigen::Vector3d & trans )
00186 {
00187 state_.setBody2ImuRotation( rot );
00188 state_.setBody2ImuTranslation( trans );
00189 }
00190
00191
00192 void ImuEkf::objectTransformation( Eigen::Quaterniond & rot, Eigen::Vector3d & trans, Eigen::Vector3d & grav )
00193 {
00194
00195 rot = state_.body2ImuRotation().inverse() * state_.orientation();
00196
00197
00198 trans = state_.body2ImuRotation().inverse() * ( state_.position() - state_.body2ImuTranslation() );
00199
00200
00201 grav = state_.body2ImuRotation().inverse() * state_.gravity();
00202 }
00203
00204 void ImuEkf::propagateOrientation( Eigen::Quaterniond & qnew,
00205 const ImuState & state,
00206 const ImuMeasurement & measurement,
00207 double deltaT )
00208 {
00209 Eigen::Vector3d truePerturbation;
00210 truePerturbation = deltaT * ( measurement.angularRate - state.gyroBias() + measurement.angularRatePerturbation );
00211
00212 qnew = state.orientation();
00213 tools::perturb_orientation( truePerturbation, qnew );
00214 }
00215
00216 void ImuEkf::propagateTranslation( Eigen::Vector3d & tnew,
00217 const Eigen::Quaterniond & rot,
00218 const ImuState & state,
00219 const ImuMeasurement & measurement,
00220 double deltaT )
00221 {
00222 Eigen::Vector3d worldAcc;
00223 imu_filter::tools::remove_gravity( rot,
00224 measurement.linearAcceleration,
00225 state.accBias(),
00226 state.gravity(),
00227 worldAcc );
00228
00229 worldAcc += measurement.linearAccelerationPerturbation;
00230
00231 tnew = state.position() + state.velocity() * deltaT + worldAcc * deltaT * deltaT;
00232 }
00233
00234 void ImuEkf::propagateVelocity( Eigen::Vector3d & vnew,
00235 const Eigen::Quaterniond & rot,
00236 const ImuState & state,
00237 const ImuMeasurement & measurement,
00238 double deltaT )
00239 {
00240 Eigen::Vector3d worldAcc;
00241
00242 imu_filter::tools::remove_gravity( rot,
00243 measurement.linearAcceleration,
00244 state.accBias(),
00245 state.gravity(),
00246 worldAcc );
00247 worldAcc += measurement.linearAccelerationPerturbation;
00248
00249
00250 vnew = velocity_damping_ * state.velocity() + worldAcc * deltaT;
00251 }
00252
00253 void ImuEkf::propagateState( const ImuState & oldState, const ImuMeasurement & meas, double deltaT, ImuState & newState )
00254 {
00255 propagateOrientation( newState.orientation(), oldState, meas, deltaT );
00256 propagateTranslation( newState.position(), newState.orientation(), oldState, meas, deltaT );
00257 propagateVelocity( newState.velocity(), newState.orientation(), oldState, meas, deltaT );
00258 newState.gyroBias() = oldState.gyroBias() + meas.gyroBiasPerturbation;
00259 newState.accBias() = oldState.accBias() + meas.accBiasPerturbation;
00260 newState.gravity() = oldState.gravity();
00261 }
00262
00263 void ImuEkf::stateJacobian( const ImuState & state,
00264 const ImuMeasurement & meas,
00265 double deltaT,
00266 double step,
00267 Eigen::Matrix<double, 19, 19> & F )
00268 {
00269 ImuState newState;
00270 ImuState changedState( state );
00271 ImuState::StateVector stateVector, stateOrig;
00272 propagateState( state, meas, deltaT, newState );
00273 newState.toVector( stateOrig );
00274
00275 size_t i = 0;
00276
00277
00278 changedState.orientation().x() += step;
00279 propagateState( changedState, meas, deltaT, newState );
00280 newState.toVector( stateVector );
00281 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00282 changedState.orientation() = state.orientation();
00283
00284
00285 changedState.orientation().y() += step;
00286 propagateState( changedState, meas, deltaT, newState );
00287 newState.toVector( stateVector );
00288 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00289 changedState.orientation() = state.orientation();
00290
00291
00292 changedState.orientation().z() += step;
00293 propagateState( changedState, meas, deltaT, newState );
00294 newState.toVector( stateVector );
00295 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00296 changedState.orientation() = state.orientation();
00297
00298
00299 changedState.orientation().w() += step;
00300 propagateState( changedState, meas, deltaT, newState );
00301 newState.toVector( stateVector );
00302 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00303 changedState.orientation() = state.orientation();
00304
00305
00306 changedState.position().x() += step;
00307 propagateState( changedState, meas, deltaT, newState );
00308 newState.toVector( stateVector );
00309 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00310 changedState.position().x() -= step;
00311
00312
00313 changedState.position().y() += step;
00314 propagateState( changedState, meas, deltaT, newState );
00315 newState.toVector( stateVector );
00316 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00317 changedState.position().y() -= step;
00318
00319
00320 changedState.position().z() += step;
00321 propagateState( changedState, meas, deltaT, newState );
00322 newState.toVector( stateVector );
00323 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00324 changedState.position().z() -= step;
00325
00326
00327 changedState.velocity().x() += step;
00328 propagateState( changedState, meas, deltaT, newState );
00329 newState.toVector( stateVector );
00330 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00331 changedState.velocity().x() -= step;
00332
00333
00334 changedState.velocity().y() += step;
00335 propagateState( changedState, meas, deltaT, newState );
00336 newState.toVector( stateVector );
00337 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00338 changedState.velocity().y() -= step;
00339
00340
00341 changedState.velocity().z() += step;
00342 propagateState( changedState, meas, deltaT, newState );
00343 newState.toVector( stateVector );
00344 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00345 changedState.velocity().z() -= step;
00346
00347
00348 changedState.gyroBias().x() += step;
00349 propagateState( changedState, meas, deltaT, newState );
00350 newState.toVector( stateVector );
00351 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00352 changedState.gyroBias().x() -= step;
00353
00354
00355 changedState.gyroBias().y() += step;
00356 propagateState( changedState, meas, deltaT, newState );
00357 newState.toVector( stateVector );
00358 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00359 changedState.gyroBias().y() -= step;
00360
00361
00362 changedState.gyroBias().z() += step;
00363 propagateState( changedState, meas, deltaT, newState );
00364 newState.toVector( stateVector );
00365 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00366 changedState.gyroBias().z() -= step;
00367
00368
00369 changedState.accBias().x() += step;
00370 propagateState( changedState, meas, deltaT, newState );
00371 newState.toVector( stateVector );
00372 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00373 changedState.accBias().x() -= step;
00374
00375
00376 changedState.accBias().y() += step;
00377 propagateState( changedState, meas, deltaT, newState );
00378 newState.toVector( stateVector );
00379 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00380 changedState.accBias().y() -= step;
00381
00382
00383 changedState.accBias().z() += step;
00384 propagateState( changedState, meas, deltaT, newState );
00385 newState.toVector( stateVector );
00386 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00387 changedState.accBias().z() -= step;
00388
00389
00390 changedState.gravity().x() += step;
00391 propagateState( changedState, meas, deltaT, newState );
00392 newState.toVector( stateVector );
00393 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00394 changedState.gravity().x() -= step;
00395
00396
00397 changedState.gravity().y() += step;
00398 propagateState( changedState, meas, deltaT, newState );
00399 newState.toVector( stateVector );
00400 F.col( i++ ) = ( stateVector - stateOrig ) / step;
00401 changedState.gravity().y() -= step;
00402
00403
00404 changedState.gravity().z() += step;
00405 propagateState( changedState, meas, deltaT, newState );
00406 newState.toVector( stateVector );
00407 F.col( i ) = ( stateVector - stateOrig ) / step;
00408 changedState.gravity().z() -= step;
00409 }
00410
00411
00412 void ImuEkf::noiseJacobian( const ImuState & state,
00413 const ImuMeasurement & meas,
00414 double deltaT,
00415 double step,
00416 Eigen::Matrix<double, 19,12> & G )
00417 {
00418 ImuState newState;
00419 ImuState changedState( state );
00420
00421 ImuState::StateVector stateVector, stateOrig;
00422 propagateState( state, meas, deltaT, newState );
00423 newState.toVector( stateOrig );
00424
00425 ImuMeasurement measCopy( meas );
00426
00427 size_t i = 0;
00428
00429
00430 measCopy.angularRatePerturbation.x() += step;
00431 propagateState( state, measCopy, deltaT, newState );
00432 newState.toVector( stateVector );
00433 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00434 measCopy.angularRatePerturbation.x() -= step;
00435
00436 measCopy.angularRatePerturbation.y() += step;
00437 propagateState( state, measCopy, deltaT, newState );
00438 newState.toVector( stateVector );
00439 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00440 measCopy.angularRatePerturbation.y() -= step;
00441
00442 measCopy.angularRatePerturbation.z() += step;
00443 propagateState( state, measCopy, deltaT, newState );
00444 newState.toVector( stateVector );
00445 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00446 measCopy.angularRatePerturbation.z() -= step;
00447
00448
00449 measCopy.linearAccelerationPerturbation.x() += step;
00450 propagateState( state, measCopy, deltaT, newState );
00451 newState.toVector( stateVector );
00452 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00453 measCopy.linearAccelerationPerturbation.x() -= step;
00454
00455 measCopy.linearAccelerationPerturbation.y() += step;
00456 propagateState( state, measCopy, deltaT, newState );
00457 newState.toVector( stateVector );
00458 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00459 measCopy.linearAccelerationPerturbation.y() -= step;
00460
00461 measCopy.linearAccelerationPerturbation.z() += step;
00462 propagateState( state, measCopy, deltaT, newState );
00463 newState.toVector( stateVector );
00464 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00465 measCopy.linearAccelerationPerturbation.z() -= step;
00466
00467
00468 measCopy.gyroBiasPerturbation.x() += step;
00469 propagateState( state, measCopy, deltaT, newState );
00470 newState.toVector( stateVector );
00471 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00472 measCopy.gyroBiasPerturbation.x() -= step;
00473
00474 measCopy.gyroBiasPerturbation.y() += step;
00475 propagateState( state, measCopy, deltaT, newState );
00476 newState.toVector( stateVector );
00477 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00478 measCopy.gyroBiasPerturbation.y() -= step;
00479
00480 measCopy.gyroBiasPerturbation.z() += step;
00481 propagateState( state, measCopy, deltaT, newState );
00482 newState.toVector( stateVector );
00483 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00484 measCopy.gyroBiasPerturbation.z() -= step;
00485
00486
00487 measCopy.accBiasPerturbation.x() += step;
00488 propagateState( state, measCopy, deltaT, newState );
00489 newState.toVector( stateVector );
00490 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00491 measCopy.accBiasPerturbation.x() -= step;
00492
00493 measCopy.accBiasPerturbation.y() += step;
00494 propagateState( state, measCopy, deltaT, newState );
00495 newState.toVector( stateVector );
00496 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00497 measCopy.accBiasPerturbation.y() -= step;
00498
00499 measCopy.accBiasPerturbation.z() += step;
00500 propagateState( state, measCopy, deltaT, newState );
00501 newState.toVector( stateVector );
00502 G.col( i++ ) = ( stateVector - stateOrig ) / step;
00503 measCopy.accBiasPerturbation.z() -= step;
00504
00505 }
00506
00507
00508 void ImuEkf::numericalJacobians( const ImuState & state,
00509 const ImuMeasurement & meas,
00510 double deltaT,
00511 Eigen::Matrix<double, 19, 19> & F,
00512 Eigen::Matrix<double, 19, 12> & G )
00513 {
00514 ImuState newState;
00515 ImuState changedState( state );
00516
00517 ImuState::StateVector stateVector, stateOrig;
00518 propagateState( state, meas, deltaT, newState );
00519 newState.toVector( stateOrig );
00520
00521 double step = 1e-9;
00522
00523 stateJacobian( state, meas, deltaT, step, F );
00524 noiseJacobian( state, meas, deltaT, step, G );
00525 }
00526
00527 }