diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 0b23d299c..4ee8f17de 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -66,8 +66,7 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) { + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion @@ -117,21 +116,6 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - - // F_test and G_test are used for testing purposes and are not needed by the factor - if (F_test) { - (*F_test) << F; - } - if (G_test) { - // This is for testing & documentation - ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - (*G_test) << // - -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // - Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; - } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5ac6d8a7e..6ed382966 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -143,9 +143,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { * frame) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT, - OptionalJacobian<15, 15> F_test = boost::none, - OptionalJacobian<15, 21> G_test = boost::none); + const Vector3& measuredOmega, double deltaT); /// methods to access class variables Matrix preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 279c95d3c..90a369bb1 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -48,21 +48,9 @@ void PreintegratedImuMeasurements::resetIntegration() { preintMeasCov_.setZero(); } -//------------------------------------------------------------------------------ -// sugar for derivative blocks -#define D_R_R(H) (H)->block<3,3>(0,0) -#define D_R_t(H) (H)->block<3,3>(0,3) -#define D_R_v(H) (H)->block<3,3>(0,6) -#define D_t_R(H) (H)->block<3,3>(3,0) -#define D_t_t(H) (H)->block<3,3>(3,3) -#define D_t_v(H) (H)->block<3,3>(3,6) -#define D_v_R(H) (H)->block<3,3>(6,0) -#define D_v_t(H) (H)->block<3,3>(6,3) -#define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, - OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) { + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); @@ -70,7 +58,8 @@ void PreintegratedImuMeasurements::integrateMeasurement( Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 G1, G2; Matrix3 D_incrR_integratedOmega; - updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &F, &G1, &G2); + updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &F, &G1, &G2); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF @@ -82,10 +71,8 @@ void PreintegratedImuMeasurements::integrateMeasurement( + G1 * (p().accelerometerCovariance / dt) * G1.transpose() + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); - - if (outF) *outF = F; - if (G) *G << G2, Gi*dt, G1; // NOTE(frank): order here is R,P,V } + //------------------------------------------------------------------------------ PreintegratedImuMeasurements::PreintegratedImuMeasurements( const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 1d32623bd..36251a246 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -97,12 +97,9 @@ public: * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement - * @param F, F Jacobians used internally (only needed for testing) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt, // - OptionalJacobian<9, 9> F = boost::none, // - OptionalJacobian<9, 9> G = boost::none); + const Vector3& measuredOmega, double dt); /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 8da1cc3e7..d473207da 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -40,40 +40,6 @@ using symbol_shorthand::V; using symbol_shorthand::B; namespace { -/* ************************************************************************* */ -// Auxiliary functions to test Jacobians F and G used for -// covariance propagation during preintegration -/* ************************************************************************* */ -Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT) { - - Matrix3 dRij = deltaRij_old.matrix(); - Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; - Vector3 deltaPij_new; - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old - * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); - Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more - imuBias::ConstantBias bias_new(bias_old); - Vector result(15); - result << logDeltaRij_new, deltaPij_new, deltaVij_new, bias_new.vector(); - return result; -} - -Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT) { - - Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, - deltaT); - - return Rot3::Expmap(result.segment < 3 > (6)); -} // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ @@ -334,158 +300,6 @@ TEST(CombinedImuFactor, PredictRotation) { EXPECT(assert_equal(expectedPose, x2, tol)); } -/* ************************************************************************* */ -TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { - // Linearization point - imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - // Actual pim values - CombinedImuFactor::CombinedPreintegratedMeasurements pim = - evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, - deltaTs); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); - const double newDeltaT = 0.01; - const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = pim.preintMeasCov(); - - Matrix Factual, Gactual; - pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, Factual, - Gactual); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected F wrt positions (15,3) - Matrix df_dpos = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT - ), deltaPij_old); - // rotation part has to be done properly, on manifold - df_dpos.block<3, 3>(0, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT - ), deltaPij_old); - EXPECT(assert_equal(df_dpos, Factual.block<15, 3>(0, 3), 1e-5)); - - // Compute expected F wrt velocities (15,3) - Matrix df_dvel = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT - ), deltaVij_old); - // rotation part has to be done properly, on manifold - df_dvel.block<3, 3>(0, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT - ), deltaVij_old); - EXPECT(assert_equal(df_dvel, Factual.block<15, 3>(0, 6), 1e-5)); - - // Compute expected F wrt angles (15,3) - Matrix df_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT), deltaRij_old); - // rotation part has to be done properly, on manifold - df_dangle.block<3, 3>(0, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT), deltaRij_old); - EXPECT(assert_equal(df_dangle, Factual.block<15, 3>(0, 0), 1e-5)); - - // Compute expected F wrt biases (15,6) - Matrix df_dbias = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT), bias_old); - // rotation part has to be done properly, on manifold - df_dbias.block<3, 6>(0, 0) = - numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT), bias_old); - EXPECT(assert_equal(df_dbias, Factual.block<15, 6>(0, 9), 1e-5)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected G wrt integration noise - Matrix df_dintNoise(15, 3); - df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; - - // Compute expected G wrt acc noise (15,3) - Matrix df_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT - ), newMeasuredAcc); - // rotation part has to be done properly, on manifold - df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT - ), newMeasuredAcc); - - // Compute expected G wrt gyro noise (15,3) - Matrix df_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT - ), newMeasuredOmega); - // rotation part has to be done properly, on manifold - df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT - ), newMeasuredOmega); - - // Compute expected G wrt bias random walk noise (15,6) - Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries - df_rwBias.setZero(); - df_rwBias.block<6, 6>(9, 0) = I_6x6; - - // Compute expected G wrt gyro noise (15,6) - Matrix df_dinitBias = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT), bias_old); - // rotation part has to be done properly, on manifold - df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT), bias_old); - df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows - - Matrix Gexpected(15, 21); - Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose(); - - Matrix newPreintCovarianceActual = pim.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index fdf9fd04c..341f2d29c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -53,30 +53,6 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } -// Auxiliary functions to test Jacobians F and G used for -// covariance propagation during preintegration -/* ************************************************************************* */ -Vector6 updatePreintegratedPosVel(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, - const double deltaT) { - Matrix3 dRij = deltaRij_old.matrix(); - Vector3 temp = dRij * correctedAcc * deltaT; - Vector3 deltaPij_new; - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - Vector3 deltaVij_new = deltaVij_old + temp; - - Vector6 result; - result << deltaPij_new, deltaVij_new; - return result; -} - -Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, - const Vector3& correctedOmega, const double deltaT) { - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); - return deltaRij_new; -} - // Define covariance matrices /* ************************************************************************* */ double accNoiseVar = 0.01; @@ -565,227 +541,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); } -/* ************************************************************************* */ -TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { - // Measurements - const Vector3 measuredAcc(0.1, 0.0, 0.0); - const Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); - const double deltaT = 0.01; - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(measuredAcc); - measuredOmegas.push_back(measuredOmega); - deltaTs.push_back(deltaT); - measuredAccs.push_back(measuredAcc); - measuredOmegas.push_back(measuredOmega); - deltaTs.push_back(deltaT); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(deltaT); - } - // Actual preintegrated values - PreintegratedImuMeasurements pim = evaluatePreintegratedMeasurements( - kZeroBias, measuredAccs, measuredOmegas, deltaTs); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = pim.preintMeasCov(); - - Matrix Factual, Gactual; - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, - Factual, Gactual); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected f_pos_vel wrt positions - boost::function f = - boost::bind(&updatePreintegratedPosVel, _1, _2, _3, measuredAcc, - measuredOmega, deltaT); - Matrix63 dfpv_dpos = numericalDerivative31(f, deltaPij_old, deltaVij_old, - deltaRij_old); - - // Compute expected f_pos_vel wrt velocities - Matrix63 dfpv_dvel = numericalDerivative32(f, deltaPij_old, deltaVij_old, - deltaRij_old); - - // Compute expected f_pos_vel wrt angles - Matrix63 dfpv_dangle = numericalDerivative33(f, deltaPij_old, deltaVij_old, - deltaRij_old); - - // Compute expected f_rot wrt angles - Matrix3 dfr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, _1, measuredOmega, deltaT), - deltaRij_old); - - Matrix Fexpected(9, 9); - Fexpected << // - dfr_dangle, Z_3x3, Z_3x3, // - dfpv_dangle, dfpv_dpos, dfpv_dvel; - - EXPECT(assert_equal(Fexpected, Factual)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6, 3); - dgpv_dintNoise << I_3x3 * deltaT, Z_3x3; - - // Compute jacobian wrt acc noise - Matrix63 dgpv_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, measuredOmega, deltaT), measuredAcc); - - // Compute expected F wrt gyro noise - Matrix63 dgpv_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, measuredAcc, _1, deltaT), measuredOmega); - - // Compute expected f_rot wrt gyro noise - Matrix3 dgr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, deltaRij_old, _1, deltaT), - measuredOmega); - - Matrix Gexpected(9, 9); - Gexpected << // - dgr_dangle, Z_3x3, Z_3x3, // - dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix9 measurementCovariance; - measurementCovariance << // - omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, // - Z_3x3, intNoiseVar * I_3x3, Z_3x3, // - Z_3x3, Z_3x3, accNoiseVar * I_3x3; - - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() - + (1 / deltaT) * Gactual * measurementCovariance * Gactual.transpose(); - - Matrix newPreintCovarianceActual = pim.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); -} - -/* ************************************************************************* */ -TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - // Actual preintegrated values - PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, - deltaTs); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); - const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = preintegrated.preintMeasCov(); - - Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, Factual, Gactual); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old); - - // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old); - - // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old); - - // Compute expected f_rot wrt angles - Matrix dfr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), - deltaRij_old); - - Matrix Fexpected(9, 9); - Fexpected << // - dfr_dangle, Z_3x3, Z_3x3, // - dfpv_dangle, dfpv_dpos, dfpv_dvel; - - EXPECT(assert_equal(Fexpected, Factual)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6, 3); - dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; - - // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc); - - // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega); - - // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), - newMeasuredOmega); - - Matrix Gexpected(9, 9); - Gexpected << // - dgr_dangle, Z_3x3, Z_3x3, // - dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix9 measurementCovariance; - measurementCovariance << // - omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, // - Z_3x3, intNoiseVar * I_3x3, Z_3x3, // - Z_3x3, Z_3x3, accNoiseVar * I_3x3; - - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() - + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); - - Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); -} - /* ************************************************************************* */ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim, const NavState& state, const imuBias::ConstantBias& bias, double dt,