diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 7d6fdcc11..2859429db 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -182,8 +182,7 @@ void AHRSFactor::print(const string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -191,24 +190,23 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, +Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = // - _PIM_.predict(bias, H3); + const Vector3 theta_biascorrected = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = // - _PIM_.integrateCoriolis(rot_i, omegaCoriolis_); + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Matrix3 coriolisHat = skewSymmetric(coriolis); const Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); // Get error between actual and prediction - const Rot3 actualRij = rot_i.between(rot_j); + const Rot3 actualRij = Ri.between(Rj); const Rot3 fRhat = deltaRij_corrected.between(actualRij); Vector3 fR = Rot3::Logmap(fRhat); @@ -219,7 +217,7 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; (*H1) << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e3eb3d51a..d6a6c8dd9 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -92,12 +92,6 @@ public: Vector3 predict(const Vector3& bias, boost::optional H = boost::none) const; - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij(); - } - // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 1b21978fb..e84096859 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -112,6 +112,12 @@ public: return theta_biascorrected; } + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i, + const Vector3& omegaCoriolis) const { + return rot_i.transpose() * omegaCoriolis * deltaTij(); + } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 6cf6770c4..110d48118 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -176,24 +176,26 @@ public: const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); + const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij() - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + - coriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij() // Coriolis term + - 2 * coriolisHat * vel_i * deltaTij() // Coriolis term + gravity * deltaTij()); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity + pos_j += - 0.5 * coriolisHat * coriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - coriolisHat * coriolisHat * pos_i * deltaTij(); // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); @@ -201,11 +203,11 @@ public: // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Vector3 theta_corrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + const Rot3 deltaRij_corrected = + Rot3::Expmap( theta_corrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_corrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant @@ -221,13 +223,13 @@ public: boost::optional H3, boost::optional H4, boost::optional H5) const { - // We need the mistmatch w.r.t. the biases used for preintegration + // We need the mismatch w.r.t. the biases used for preintegration const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); // we give some shorter name to rotations and translations - const Rot3& Rot_i = pose_i.rotation(); - const Rot3& Rot_j = pose_j.rotation(); + const Rot3& Ri = pose_i.rotation(); + const Rot3& Rj = pose_j.rotation(); const Vector3& pos_j = pose_j.translation().vector(); // Jacobian computation @@ -235,19 +237,20 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Vector3 theta_biascorrected = - biascorrectedThetaRij(biasOmegaIncr, H5); + Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); + // Prediction + const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); // TODO: these are not always needed - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis * deltaTij()); + const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Ri.inverse().matrix() * omegaCoriolis * deltaTij()); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { @@ -256,26 +259,26 @@ public: Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij()*deltaTij(); - dfVdPi = skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij(); + dfPdPi = - Ri.matrix() + 0.5 * coriolisHat * coriolisHat * Ri.matrix() * deltaTij()*deltaTij(); + dfVdPi = coriolisHat * coriolisHat * Ri.matrix() * deltaTij(); } else{ - dfPdPi = - Rot_i.matrix(); + dfPdPi = - Ri.matrix(); dfVdPi = Z_3x3; } (*H1) << // dfP/dRi - Rot_i.matrix() * skewSymmetric(deltaPij() + Ri.matrix() * skewSymmetric(deltaPij() + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), // dfP/dPi dfPdPi, // dfV/dRi - Rot_i.matrix() * skewSymmetric(deltaVij() + Ri.matrix() * skewSymmetric(deltaVij() + delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr), // dfV/dPi dfVdPi, // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + Jrinv_fRhat * (- Rj.between(Ri).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi Z_3x3; } @@ -284,10 +287,10 @@ public: (*H2) << // dfP/dVi - I_3x3 * deltaTij() - + skewSymmetric(omegaCoriolis) * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + + coriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - I_3x3 - + 2 * skewSymmetric(omegaCoriolis) * deltaTij(), // Coriolis term + + 2 * coriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } @@ -295,7 +298,7 @@ public: H3->resize(9,6); (*H3) << // dfP/dPosej - Z_3x3, Rot_j.matrix(), + Z_3x3, Rj.matrix(), // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej @@ -317,11 +320,11 @@ public: H5->resize(9,6); (*H5) << // dfP/dBias - - Rot_i.matrix() * delPdelBiasAcc(), - - Rot_i.matrix() * delPdelBiasOmega(), + - Ri.matrix() * delPdelBiasAcc(), + - Ri.matrix() * delPdelBiasOmega(), // dfV/dBias - - Rot_i.matrix() * delVdelBiasAcc(), - - Rot_i.matrix() * delVdelBiasOmega(), + - Ri.matrix() * delVdelBiasAcc(), + - Ri.matrix() * delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);