Fix CombinedImuFactor #882

varunagrawal · 2021-09-28T22:34:44Z

This PR updates the CombinedImuFactor to compute the preintegration covariance as per the updated ImuFactor.pdf document.

lucacarlone · 2021-11-28T22:28:08Z

@varunagrawal : how is it going on this? I'm happy to help if you want!

varunagrawal · 2021-11-28T22:29:02Z

I've actually figured this out but we're in the middle of a submission cycle, so I'll get back to this in a week. :)

lucacarlone · 2021-11-28T22:29:50Z

yay! great to hear!

varunagrawal · 2022-05-05T15:37:31Z

@lucacarlone I am requesting a review from you and I hope you will be the primary person for discussion to get this fix merged in.

I have updated the ImuFactor.lyx doc to demonstrate the math for the covariance propagation, and have made the necessary code updates based on that.

dellaert · 2022-05-05T16:51:07Z

@varunagrawal could you give some more context and summary of changes in the PR comment so @lucacarlone could sanity-check this? I'll also be happy to take a look. Also, consider pasting the relevant change from the PDF in the comments so this is easy to review without having to checkout the pdf.

lucacarlone · 2022-05-05T17:09:43Z

@varunagrawal @dellaert : I'm happy to review this!

varunagrawal · 2022-05-05T17:43:14Z

@dellaert @lucacarlone here are the relevant sections. The code should match the math. If something is unclear or the if there are better naming recommendations, I am happy to oblige.

dellaert

There are now changes in 808 files. Did you change the base? Reason it's not develop?

dellaert · 2022-05-05T17:59:43Z

Also. just pasting those sections does not make clear what changed. Please help us by providing that and the reason for the change..

varunagrawal · 2022-05-05T20:56:17Z

The main idea is that I re-derived the covariance update equations for the CombinedImuFactor using the details in ImuFactor.lyx as well as the notes that @lucacarlone provided to me. I clarified a lot of the error terms and by writing it up in ImuFactor.lyx I was able to identify missing/poorly-named covariance blocks and rectify them.

varunagrawal · 2022-05-05T20:57:30Z

There are now changes in 808 files. Did you change the base? Reason it's not develop?

Sorry about that. Had to merge in develop across multiple branches. This PR does not target develop since the main fix for #368 is in #874 but when fixing that, we identified some issues in the covariance propagation of the CombinedImuFactor and this PR isolates that.

varunagrawal · 2022-05-05T20:59:55Z

I would argue that this PR and #879 should be looked at together (this PR targets #879) but I kept it separate for ease of reviewing.

dellaert · 2022-05-13T17:56:21Z

examples/CombinedImuFactorsExample.cpp

@@ -123,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> 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;


Renaming breaks API and I really think it was meant to be "int" for "integration"

Nope, we have integrationCovariance in PreintegrationParams.h for integration. This was a typo that I fixed.

dellaert · 2022-05-13T17:56:31Z

examples/ImuFactorsExample.cpp

@@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> 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 =


dellaert · 2022-05-13T17:56:38Z

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]"


dellaert · 2022-05-13T17:56:51Z

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.


dellaert · 2022-05-13T17:58:03Z

gtsam/navigation/ScenarioRunner.cpp

@@ -145,7 +145,8 @@ Eigen::Matrix<double, 15, 15> 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());


Is this the only real change?

For this PR? CombinedImuFactor.cpp has the main changes.

dellaert

You are making a math change - in future make this as atomic as possible. Lot's of distractions that make this hard to review.

dellaert · 2022-05-13T18:17:58Z

gtsam/navigation/CombinedImuFactor.cpp

-  D_R_R(&G_measCov_Gt) = theta_H_biasOmega  //
-      * (wCov_updated / dt)  //
-      * (theta_H_biasOmega.transpose());
+  D_v_v(&G_measCov_Gt) =


Could you restore the order in which these terms are computed for easier review, and comment on what has changed and why?

This order would be more aligned with the math and should make verifying it easier.

I'm saying it's harder for review. It's also not arbitrary. Maybe re-arrange the math. The order of Navstate is "attitude", "position", "velocity".

I'm a bit confused. It's already in attitude-position-velocity order (D_R_R for attitude, D_t_t for position and D_v_v fro velocity). The prior code had it position-velocity-attitude which isn't the expected order.

@varunagrawal : besides the order of the pre-integrated measurements, can you point out what changed with respect to my (terrible) writeup? I think you added a second-order term in the position and changed how the covariances are discretized. were there other bugs you fixed with respect to my writeup? (i.e., was my writeup wrong, or only the implementation? or both? :-) )

also: is the order attitude-position-velocity now also used in the standard IMU factor? or only the combined?

I believe just the implementation was incorrect in certain places. @lucacarlone's overall writeup was pretty great and I just expanded upon that with additional information I picked up from other books and papers. :)
It's been a while since I've seen this math though so I may have added minor corrections where I found them which I don't remember of the top of my head.

As for the attitude-position-velocity order, this was not really an issue for ImuFactor since the expression for the jacobians was a lot simpler (G * C/dt * G')

I looked at the differences again: I also changed the noise variable order from
$$\epsilon_{int}, \epsilon_{a}, \epsilon_{\omega}, \epsilon_{b^{a}_{init}} $$

$$\epsilon_{b^{\omega}{init}}, \epsilon{\Delta b^{a}}, \epsilon_{\Delta b^{\omega}}$$

to
$$\epsilon_{\omega}, \epsilon_{a}, \epsilon_{\Delta b^{a}}, \epsilon_{\Delta b^{\omega}}$$

$$\epsilon_{int}, \epsilon_{b^{a}{init}}, \epsilon{b^{\omega}_{init}}$$

(Apologies for the rendering issues. Seems like Github's Math mode is a bit buggy.)

dellaert · 2022-05-13T18:18:58Z

gtsam/navigation/CombinedImuFactor.cpp

-  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) =


Same. And why get rid of temp?

I got rid of temp because the derivation is no longer D_v_R = temp and D_R_v = temp.transpose().

lucacarlone

@varunagrawal : I'm starting to review the PR - great job overall! I plan to add comments as I go, starting from the writeup

lucacarlone

This looks good to me overall - thanks for the great work @varunagrawal !! It would be amazing to have a numerical analysis / example showing that this leads to better results (or at least better bias estimates).

lucacarlone · 2022-07-02T16:36:14Z

doc/ImuFactor.lyx

 \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


near eq (17) it is worth remarking these are discrete-time covariances (and point to the section on covariance discretization)

lucacarlone · 2022-07-02T16:37:21Z

doc/ImuFactor.lyx

+
+The noise model associated with this factor is assumed to be zero-mean Gaussian
+ with a 
+\begin_inset Formula $9\times9$


slightly confusing, since it is 15x15 in the combined factor. I would rephrase this sentence to keep it more general.

Ok, though Frank wanted this to be specific since we have different sections for ImuFactor and CombinedFactor. I'll add a corresponding paragraph in the Combined IMU Factor subsection.

lucacarlone · 2022-07-02T16:38:22Z

doc/ImuFactor.lyx

+
+.
+ This covariance matrix is computed in the preintegrated measurement class,
+ of which there are two variants as discussed above.


here I suggest making the terminology clearer: are these continuous-time densities/covariances or discrete-time covariances?

lucacarlone · 2022-07-02T16:41:18Z

doc/ImuFactor.lyx

-
- : The covariance associated with the gyroscope bias random walk.
-\end_layout
-


below the reference "Murray84book" does not render correctly

lucacarlone · 2022-07-02T17:21:20Z

gtsam/navigation/CombinedImuFactor.cpp

-  D_R_R(&G_measCov_Gt) = theta_H_biasOmega  //
-      * (wCov_updated / dt)  //
-      * (theta_H_biasOmega.transpose());
+  D_v_v(&G_measCov_Gt) =


@varunagrawal : besides the order of the pre-integrated measurements, can you point out what changed with respect to my (terrible) writeup? I think you added a second-order term in the position and changed how the covariances are discretized. were there other bugs you fixed with respect to my writeup? (i.e., was my writeup wrong, or only the implementation? or both? :-) )

also: is the order attitude-position-velocity now also used in the standard IMU factor? or only the combined?

lucacarlone · 2022-07-02T17:35:38Z

tests/testImuPreintegration.cpp

@@ -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;


if not there yet, I suggest adding a unit test on the block-wise implementation of G * cov * G'

This will have to be an MCMC test similar to ImuFactor.

lucacarlone

@varunagrawal : this is good to go to me, but do you have stats to show how it compares with the standard imu factor?

varunagrawal · 2022-07-05T02:06:31Z

@lucacarlone that is going to have to be another PR since I will need to implement the CombinedImuFactor equivalent of the ScenarioRunner. I can and will do that but I am keen to land this PR. I'll create an issue for it so we can track it.

varunagrawal added 3 commits September 30, 2021 09:05

undo name change from 984a90

af714cd

detailed implementation of CombinedImuFactor noise propagation

e38ea50

test passes

3cee1b7

varunagrawal force-pushed the fix/combined-imu branch from c20fe7d to 3cee1b7 Compare September 30, 2021 13:06

varunagrawal linked an issue Sep 30, 2021 that may be closed by this pull request

Details about IMU Integration Covariance #878

Open

lyx update

dfa32e5

varunagrawal self-assigned this May 5, 2022

formatting

40e6d8b

varunagrawal requested review from dellaert and lucacarlone May 5, 2022 15:36

varunagrawal marked this pull request as ready for review May 5, 2022 15:36

update PDF doc

995710f

varunagrawal mentioned this pull request May 5, 2022

Fix Preintegration Covariance of CombinedImuFactor #879

Merged

Merge branch 'develop' into fix/combined-imu

2d3859d

dellaert reviewed May 5, 2022

View reviewed changes

Merge branch 'fix/combined-imu-cov' into fix/combined-imu

f0be857

varunagrawal requested a review from dellaert May 5, 2022 20:57

varunagrawal added 2 commits May 6, 2022 16:45

fix test

8dbbb1f

fix test

ce7c71b

minor refactor to follow the math better

a17134d

dellaert reviewed May 13, 2022

View reviewed changes

use prior naming scheme for bias jacobians

cb75d92

lucacarlone reviewed Jul 2, 2022

View reviewed changes

lucacarlone self-requested a review July 2, 2022 16:30

lucacarlone reviewed Jul 2, 2022

View reviewed changes

update Lyx document based on Luca's review

bbd1e3f

varunagrawal requested a review from lucacarlone July 3, 2022 20:02

varunagrawal added the bugfix Fixes an issue or bug label Jul 4, 2022

lucacarlone approved these changes Jul 4, 2022

View reviewed changes

varunagrawal merged commit 4244345 into fix/combined-imu-cov Jul 5, 2022

varunagrawal deleted the fix/combined-imu branch July 5, 2022 02:06


		: The covariance associated with the gyroscope bias random walk.
		\end_layout

Fix CombinedImuFactor #882

Fix CombinedImuFactor #882

Conversation

varunagrawal commented Sep 28, 2021 • edited Loading

lucacarlone commented Nov 28, 2021

varunagrawal commented Nov 28, 2021

lucacarlone commented Nov 28, 2021

varunagrawal commented May 5, 2022

dellaert commented May 5, 2022

lucacarlone commented May 5, 2022

varunagrawal commented May 5, 2022

dellaert left a comment

Choose a reason for hiding this comment

dellaert commented May 5, 2022

varunagrawal commented May 5, 2022

varunagrawal commented May 5, 2022

varunagrawal commented May 5, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

varunagrawal Jul 3, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

lucacarlone left a comment

Choose a reason for hiding this comment

lucacarlone left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

lucacarlone left a comment

Choose a reason for hiding this comment

varunagrawal commented Jul 5, 2022

varunagrawal commented Sep 28, 2021 •

edited

Loading

varunagrawal Jul 3, 2022 •

edited

Loading

lucacarlone left a comment •

edited

Loading