diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index da6b03019..367a62360 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -64,25 +64,22 @@ void PreintegrationBase::updatePreintegratedMeasurements( OptionalJacobian<9, 9> F) { const Matrix3 dRij = deltaRij_.matrix(); // expensive - const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame + const Vector3 i_acc = dRij * correctedAcc; // acceleration in i frame double dt22 = 0.5 * deltaT * deltaT; - deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc; - deltaVij_ += deltaT * j_acc; + deltaPij_ += deltaVij_ * deltaT + dt22 * i_acc; + deltaVij_ += deltaT * i_acc; Matrix3 R_i, F_angles_angles; - if (F) - R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); if (F) { - const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; + const Matrix3 F_vel_angles = -dRij * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_pos_angles; - F_pos_angles = 0.5 * F_vel_angles * deltaT; // pos vel angle *F << // - I_3x3, I_3x3 * deltaT, F_pos_angles, // pos + I_3x3, I_3x3 * deltaT, 0.5 * F_vel_angles * deltaT, // pos Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, Z_3x3, F_angles_angles; // angle }