12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added const and changed name in updatePreintegratedMeasurements to "omegaCoriolisHat" to comply with notation in "predict"

release/4.3a0
Luca 2014-12-06 19:04:41 -05:00
parent 943a18b124
commit d46224e8a1
2 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/PreintegratedRotation.h
View File

@ -74,7 +74,7 @@ public:
} }
/// Update preintegrated measurements /// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, double deltaT){ void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT){
deltaRij_ = deltaRij_ * incrR; deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT; deltaTij_ += deltaT;
} }

20
gtsam/navigation/PreintegrationBase.h
View File

@ -116,7 +116,7 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, double deltaT) { const Rot3& incrR, const double deltaT) {
Matrix3 dRij = deltaRij(); // expensive Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
@ -176,7 +176,7 @@ public:
const Rot3& Rot_i = pose_i.rotation(); const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_i = pose_i.translation().vector();
const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
@ -184,23 +184,22 @@ public:
+ delPdelBiasAcc() * biasAccIncr + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr) + delPdelBiasOmega() * biasOmegaIncr)
+ vel_i * deltaTij() + vel_i * deltaTij()
- coriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - omegaCoriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij(); + 0.5 * gravity * deltaTij()*deltaTij();
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij()
+ delVdelBiasAcc() * biasAccIncr + delVdelBiasAcc() * biasAccIncr
+ delVdelBiasOmega() * biasOmegaIncr) + delVdelBiasOmega() * biasOmegaIncr)
- 2 * coriolisHat * vel_i * deltaTij() // Coriolis term - 2 * omegaCoriolisHat * vel_i * deltaTij() // Coriolis term
+ gravity * deltaTij()); + gravity * deltaTij());
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
pos_j += - 0.5 * coriolisHat * coriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position pos_j += - 0.5 * omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - coriolisHat * coriolisHat * pos_i * deltaTij(); // 2nd order term for velocity vel_j += - omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij(); // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// TODO Frank says comment below does not reflect what was in code // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_corrected = theta_biascorrected - Vector3 theta_corrected = theta_biascorrected -
@ -240,9 +239,8 @@ public:
Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 theta_corrected = theta_biascorrected - coriolis; Vector3 theta_corrected = theta_biascorrected - coriolis;
// Prediction // Prediction
@ -251,7 +249,7 @@ public:
// TODO: these are not always needed // TODO: these are not always needed
const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
const Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) { if(H1) {