diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index f76ede023f..c335e69496 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -82,7 +82,7 @@ \begin_body \begin_layout Title -The new IMU Factor +The New IMU Factor \end_layout \begin_layout Author @@ -227,16 +227,62 @@ preintegrated_ \begin_layout Standard The main function of a factor is to calculate an error. - The easiest case to look at is the NavState variant in ImuFactor2, which - is given as: + This is done exactly the same in both variants: \begin_inset Formula \begin{equation} -\Delta X_{ij}=X_{j}-\hat{X_{ij}}\label{eq:imu-factor-error} +e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error} \end{equation} \end_inset +where the predicted NavState +\begin_inset Formula $\widehat{X_{j}}$ +\end_inset + + at time +\begin_inset Formula $t_{j}$ +\end_inset + + is a function of the NavState +\begin_inset Formula $X_{i}$ +\end_inset + + at time +\begin_inset Formula $t_{i}$ +\end_inset + + and the preintegrated measurements +\begin_inset Formula $PIM$ +\end_inset + +: +\begin_inset Formula +\[ +\widehat{X_{j}}=f(X_{i},PIM) +\] + +\end_inset + +The noise model associated with this factor is assumed to be zero-mean Gaussian + with a +\begin_inset Formula $9\times9$ +\end_inset + covariance matrix +\begin_inset Formula $\Sigma_{ij}$ +\end_inset + +, which is defined in the tangent space +\begin_inset Formula $T_{X_{j}}\mathcal{N}$ +\end_inset + + of the NavState manifold at the NavState +\begin_inset Formula $X_{j}$ +\end_inset + +. + This (discrete-time) covariance matrix is computed in the preintegrated + measurement class, of which there are two variants as discussed above. \end_layout \begin_layout Subsubsection* @@ -263,6 +309,20 @@ CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU bias and the current NavState and IMU bias. \end_layout +\begin_layout Standard +Since the Combined IMU Factor has a larger state variable due to the inclusion + of IMU biases, the noise model associated with this factor is assumed to + be a zero mean Gaussian with a +\begin_inset Formula $15\times15$ +\end_inset + + covariance matrix +\begin_inset Formula $\Sigma$ +\end_inset + +, similarly defined on the tangent space of the NavState manifold. +\end_layout + \begin_layout Subsubsection* Covariance Matrices \end_layout @@ -282,6 +342,14 @@ Gyroscope Covariance : Measurement uncertainty of the gyroscope. \end_layout +\begin_layout Itemize +Gyroscope Bias Covariance +\begin_inset Formula $Q_{\Delta b^{\omega}}$ +\end_inset + + : The covariance associated with the gyroscope bias random walk. +\end_layout + \begin_layout Itemize Accelerometer Covariance \begin_inset Formula $Q_{acc}$ @@ -298,14 +366,6 @@ Accelerometer Bias Covariance : The covariance associated with the accelerometer bias random walk. \end_layout -\begin_layout Itemize -Gyroscope Bias Covariance -\begin_inset Formula $Q_{\Delta b^{\omega}}$ -\end_inset - - : The covariance associated with the gyroscope bias random walk. -\end_layout - \begin_layout Itemize Integration Covariance \begin_inset Formula $Q_{int}$ @@ -518,7 +578,15 @@ acceleration \end_inset in the body frame. - We know (from Murray84book) that the derivative of + We know (from +\begin_inset CommandInset citation +LatexCommand cite +key "Murray94book" +literal "false" + +\end_inset + +) that the derivative of \begin_inset Formula $R$ \end_inset @@ -1469,7 +1537,12 @@ Noise Propagation in IMU Factor \end_layout \begin_layout Standard -Even when we assume uncorrelated noise on +We wish to compute the ImuFactor covariance matrix +\begin_inset Formula $\Sigma_{ij}$ +\end_inset + +. + Even when we assume uncorrelated noise on \begin_inset Formula $\omega^{b}$ \end_inset @@ -1487,11 +1560,12 @@ Even when we assume uncorrelated noise on \end_inset appear in multiple places. - To model the noise propagation, let us define + To model the noise propagation, let us define the preintegrated navigation + state \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$ \end_inset - and rewrite Eqns. +, as a 9D vector on tangent space at and rewrite Eqns. ( \begin_inset CommandInset ref LatexCommand ref @@ -1566,6 +1640,42 @@ where \begin_inset Formula $\omega^{b}$ \end_inset +. + Note that +\begin_inset Formula $\Sigma_{k},$ +\end_inset + + +\begin_inset Formula $\Sigma_{\eta}^{ad}$ +\end_inset + +, and +\begin_inset Formula $\Sigma_{\eta}^{gd}$ +\end_inset + + are discrete time covariances with +\begin_inset Formula $\Sigma_{\eta}^{ad}$ +\end_inset + +, and +\begin_inset Formula $\Sigma_{\eta}^{gd}$ +\end_inset + +divided by +\begin_inset Formula $\Delta_{t}$ +\end_inset + +. + Please see the section on Covariance Discretization +\begin_inset CommandInset ref +LatexCommand vpageref +reference "subsec:Covariance-Discretization" +plural "false" +caps "false" +noprefix "false" + +\end_inset + . \end_layout @@ -1593,7 +1703,7 @@ It can be shown that for small we have \begin_inset Formula \[ -\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}} +\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} \] \end_inset @@ -1981,6 +2091,13 @@ which we can break into 3 matrices for clarity, representing the main diagonal \begin_layout Subsubsection* Covariance Discretization +\begin_inset CommandInset label +LatexCommand label +name "subsec:Covariance-Discretization" + +\end_inset + + \end_layout \begin_layout Standard diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index a4ec57fb7b..1823cbc4a6 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 49cdb68354..35ed387c40 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -107,7 +107,7 @@ boost::shared_ptr imuParams() { I_3x3 * 1e-8; // error committed in integrating position from velocities Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = + Matrix66 bias_acc_omega_init = I_6x6 * 1e-5; // error in the bias used for preintegration auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); @@ -123,7 +123,7 @@ boost::shared_ptr imuParams() { // PreintegrationCombinedMeasurements params: p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 38ee4c7c76..12167a19d1 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -94,7 +94,7 @@ boost::shared_ptr imuParams() { I_3x3 * 1e-8; // error committed in integrating position from velocities Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = + Matrix66 bias_acc_omega_init = I_6x6 * 1e-5; // error in the bias used for preintegration auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); @@ -110,7 +110,7 @@ boost::shared_ptr imuParams() { // PreintegrationCombinedMeasurements params: p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + p->biasAccOmegaInit = bias_acc_omega_init; return p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 15812c46b9..0813bbfd12 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const { << endl; cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]" << endl; - cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]" + cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]" << endl; } @@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth tol) && equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance, tol) && - equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol); + equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol); } //------------------------------------------------------------------------------ @@ -114,6 +114,10 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( Matrix3 pos_H_biasAcc = B.middleRows<3>(3); Matrix3 vel_H_biasAcc = B.bottomRows<3>(); + Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega; + Matrix3 pos_H_biasAccInit = -pos_H_biasAcc; + Matrix3 vel_H_biasAccInit = -vel_H_biasAcc; + // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; F.setZero(); @@ -131,37 +135,51 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( const Matrix3& aCov = p().accelerometerCovariance; const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& iCov = p().integrationCovariance; + const Matrix6& bInitCov = p().biasAccOmegaInit; // first order uncertainty propagation // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); - Matrix3 aCov_updated = aCov + p().biasAccOmegaInt.block<3, 3>(0, 0); - Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3); + const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt; + const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt; + const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt; + const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt; // BLOCK DIAGONAL TERMS - D_t_t(&G_measCov_Gt) = (pos_H_biasAcc // - * (aCov_updated / dt) // - * pos_H_biasAcc.transpose()) + (dt * iCov); - D_v_v(&G_measCov_Gt) = vel_H_biasAcc // - * (aCov_updated / dt) // - * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = + (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) // + + + (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose()); + + D_t_t(&G_measCov_Gt) = + (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) // + + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) // + + (dt * iCov); - D_R_R(&G_measCov_Gt) = theta_H_biasOmega // - * (wCov_updated / dt) // - * (theta_H_biasOmega.transpose()); + D_v_v(&G_measCov_Gt) = + (vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) // + + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) - * theta_H_biasOmega.transpose(); - D_v_R(&G_measCov_Gt) = temp; - D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose(); - D_R_v(&G_measCov_Gt) = temp.transpose(); - D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose(); + D_R_t(&G_measCov_Gt) = + theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose(); + D_R_v(&G_measCov_Gt) = + theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose(); + D_t_R(&G_measCov_Gt) = + pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); + D_t_v(&G_measCov_Gt) = + (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) + + (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); + D_v_R(&G_measCov_Gt) = + vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); + D_v_t(&G_measCov_Gt) = + (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) + + (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()); preintMeasCov_.noalias() += G_measCov_Gt; } @@ -271,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); return os; } -} - /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index dfdad1d3c7..28a3142472 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -62,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType; struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration + Matrix6 biasAccOmegaInit; ///< covariance of bias used as initial estimate. /// Default constructor makes uninitialized params struct. /// Used for serialization. PreintegrationCombinedParams() : biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), - biasAccOmegaInt(I_6x6) {} + biasAccOmegaInit(I_6x6) {} /// See two named constructors below for good values of n_gravity in body frame PreintegrationCombinedParams(const Vector3& n_gravity) : PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), - biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { + biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) { } @@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } - void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } + void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } + const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; } private: @@ -109,7 +109,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit); } public: diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 8840c34e9d..f6e9fccb8d 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i, state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, H1 || H3 ? &D_error_predict : 0); - if (H1) *H1 << D_error_predict* D_predict_state_i; + if (H1) *H1 << D_error_predict * D_predict_state_i; if (H2) *H2 << D_error_state_j; - if (H3) *H3 << D_error_predict* D_predict_bias_i; + if (H3) *H3 << D_error_predict * D_predict_bias_i; return error; } diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 3a447dcab6..9d3e258de9 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -145,7 +145,8 @@ Eigen::Matrix CombinedScenarioRunner::estimateCovariance( auto pim = integrate(T, estimatedBias, true); NavState sampled = predict(pim); Vector15 xi = Vector15::Zero(); - xi << sampled.localCoordinates(prediction), estimatedBias_.vector(); + xi << sampled.localCoordinates(prediction), + (estimatedBias_.vector() - estimatedBias.vector()); samples.col(i) = xi; sum += xi; } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 2f12983c37..48a89d7a05 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -230,20 +230,20 @@ TEST(CombinedImuFactor, CheckCovariance) { actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); Eigen::Matrix expected; - expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, // + expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, // 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01; // regression diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index c0a102d111..27d87d217f 100644 --- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -57,7 +57,7 @@ struct IMUHelper { p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous p->biasOmegaCovariance = I_3x3 * pow(0.001, 2.0); // gyro bias in continuous - p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; + p->biasAccOmegaInit = Matrix::Identity(6, 6) * 1e-5; // body to IMU rotation Rot3 iRb(0.036129, -0.998727, 0.035207, diff --git a/tests/testImuPreintegration.cpp b/tests/testImuPreintegration.cpp index 1f584be0ed..94e6fb89f2 100644 --- a/tests/testImuPreintegration.cpp +++ b/tests/testImuPreintegration.cpp @@ -43,7 +43,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) { double gyrNoiseSigma = 0.000208; double gyrBiasRwSigma = 0.000004; double integrationCovariance = 1e-8; - double biasAccOmegaInt = 1e-5; + double biasAccOmegaInit = 1e-5; double gravity = 9.81; double rate = 400.0; // Hz @@ -76,7 +76,7 @@ TEST(TestImuPreintegration, LoadedSimulationData) { imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2); imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2); imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance; - imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; + imuPreintegratedParams->biasAccOmegaInit = I_6x6 * biasAccOmegaInit; // Initial state Pose3 priorPose;