Repository navigation
GalileanFactor ! - #2231
GalileanFactor !#2231
Conversation
182580b to
02b578e
Compare
- no IMU biases - preintegration covariance propagation is for RI error
dellaert
added
the
enhancement
ChatGPT Codex Connector
|
codex: This develops a Galilean IMU factor and supporting preintegration, tests, and build updates. Labeled enhancement because the primary purpose is to add a new navigation factor capability. |
|
Pulled it down, it’s easy to use. Thumbs up from me! Though I defer to @giuliodelama as the paper's author. I'm also thinking that adding Python bindings should be pretty straightforward for this: expose the new |
giuliodelama
left a comment
giuliodelama
left a comment
There was a problem hiding this comment.
This looks really nice! I've gone through the notebook and gtsam/navigation/GalileanImuFactor.*. I checked with Claude's help, and the implementation is correct and builds on the paper's idea of using Gal(3) together with the tangent group structure. The thing I most wanted to be sure of was A_LI and B_LI, so I checked them by numerically differentiating the exact nonlinear error map, keeping the notebook's own error coordinates. Numerical tests with Claude are all ok (I can share the scripts). I've left two small fixes on the test harness inline.
There is one thing I would rather discuss than request a change on, and it is about how §5 defines the error.
The notebook takes the tangent group Gal(3) |x_Ad gal(3) from the paper, and the lift it uses is exactly the one the paper's action induces, so that part is faithful. What changes is which group error gets formed. The paper builds X Xhat^-1; the notebook builds Xhat^-1 X, because GTSAM retracts on the right. But the paper's action on the state space is a right action, and under a right action only the right group error is invariant: acting on the true and the estimated state with the same group element leaves X Xhat^-1 untouched and turns Xhat^-1 X into Y^-1 (Xhat^-1 X) Y. So the object §5 works with is a coordinate chart rather than an equivariant error, and I could not find a symmetry of this system that would make it one.
The semidirect twist in the tangent group is there to transport between two frames. The right-invariant navigation error lives in the spatial frame while the bias lives in the body frame, and Ad_Upsilonhat carries you between them. In the left-invariant convention, the navigation error is already a body-frame quantity, in the same frame as the bias, so there is nothing left to transport. Keeping the twist anyway does not add geometry; it just puts ad_bhat into every block and flips the sign of the bias coordinate.
The alternative is the direct product Gal(3) x gal(3), with the bias as an ordinary abelian factor instead of a semidirect one. The system is equivariant under its left action (Gamma, c)·(Upsilon, b) = (Gamma·Upsilon, b + c) with w̃ -> w̃ + c, and the left-invariant error is simply
Xhat^-1 X = ( Upsilonhat^-1 · Upsilon , b − bhat )
with no adjoint anywhere. Its logarithm is (e, b − bhat) exactly, and the propagation collapses to
A = [ Ad_(V^-1) −C ; 0 I ] B = [ −C 0 ; 0 −h·I ]
which is the exact Ad_Upsilonhat mirror of Eqs. (36)-(37). In that chart the conditioned subspace §6 refers to is literally {d = 0} rather than the graph {d = ad_bhat·e}, and J_{k+1} = Ad_(V^-1)·J_k − C_k is just the accumulated (1,2) block. None of it touches the code: the conditioned 10x10 block and J_k come out identical either way.
|
@giuliodelama Could you please clarify your large comment? I Follow what you're saying, but If I'm correct, you're discussing multiple points at the same time. So maybe grouping them in a with some headings would just already clarify what you mean, And which points needs changes in code and which ones don't. |
Never mind - I think I got it. |
|
The Galilean formulation has now been rewritten around the direct-product symmetry suggested by @giuliodelama, while preserving GTSAM's right-retraction convention. We also don't need the virtual inputs anymore in this. Everything became so much simpler :-) I also worked with the Ai to rewrite everything in terms of the equivariant filter much more clearly. The basic construction is:
For a held IMU sample, let where and the 16D left-invariant linearization is The physical bias correction is consequently accumulated as so the correction is applied on the right. The factor finally removes deterministic time and permutes Gal3 The notebook now derives this construction in EqF terminology and includes an executable Python example using the new |
|
Added a reproducible four-backend NEES comparison in The notebook compares the actual GTSAM Manifold, Tangent, Lie-group, and Galilean preintegration implementations. All four receive the same zero-order-held IMU signal and paired noise samples. The experiment uses 3,000 trials over 1 s at 20 Hz, with simultaneous specific force For 3,000 trials, the exact aggregate 95% consistency interval for the mean 9D NEES is
The deterministic timestep sweep also shows the Galilean mean update at floating-point accuracy for this held-input trajectory, while the three conventional variants have the expected finite-step discretization error. Their deterministic mean-error norm is about The claim is intentionally scoped: this demonstrates the Galilean formulation's consistency and accuracy advantage for finite-rate, zero-order-held measurements with simultaneous rotation and acceleration. It is not a claim of universal superiority or lower runtime. Supporting changes add Python-only named bindings for the three existing preintegration backends, a wrapper test covering all four, and links from the navigation documentation and Galilean factor guide. Validation completed:
|
giuliodelama
left a comment
giuliodelama
left a comment
There was a problem hiding this comment.
That was exactly my point, looks good to me :)
computeErrorDynamicsMatrix() rejected ActionType::Left and directed callers to predictWithJacobian() with an explicit continuous-time A. That safeguard was added by borglab#2231 together with a guard test pinning the workaround; borglab#2753 tracks removing it. A = Dphi0 * D_lift is already correct for left actions, and no transport term has to be added by hand, provided the lift is an equivariant lift. That is the documented contract rather than a choice made here. The lift is defined by the lift condition Dphi_xi|_Id[Lambda(xi,u)] = f_u(xi); van Goor, Hamel and Mahony require in addition that Ad_X[Lambda(phi_X(xi), psi_X(u))] = Lambda(xi,u), which is what makes the error dynamics depend only on the error and the origin input, and so makes linearizing at a fixed origin valid. Both GTSAM's own EqF post and the class documentation already say the caller supplies "the input and output actions needed for equivariance". borglab#2753 observes A = 0 for a state-independent lift on the left-regular action. That lift does not satisfy either condition for the system in question, which has a body velocity: the equivariance residual is 2.1, not zero. The lift that does satisfy them is Lambda(xi,u) = Ad_xi u, and its differential at the origin is -ad_u, exactly the term the issue expects. Two independent derivations agree: LieGroupEKF::transitionMatrix() obtains the same -ad from Baker-Campbell-Hausdorff for a left-invariant error, and it is the classical left-invariant EKF result of Barrau and Bonnabel. The formula also holds when DimM != DimG, checked against SO(3) acting on S^2, transitive but not free -- a case where the alternative, an invariant lift corrected by -ad_Lambda, is not even well defined, since two lifts differing by a stabiliser element move the manifold identically but give different ad. Whether the lift carries an Ad is a frame question, not a left-or-right one. A left action's fundamental vector field is spatial and a right action's is a body velocity, so a constant lift is correct exactly when the system's velocity is stated in the matching frame. Every right-action fixture here has one because IMU inputs are body quantities and those actions are body-framed; a right action driven spatially would need Ad_{xi^-1} u just the same. The header and the new fixtures say so. What needed fixing is where each operation lands on the manifold. Two places get this wrong for a left action, and both are reachable on develop: only computeErrorDynamicsMatrix() carries the guard, while predictWithJacobian(), update() and updateWithVector() do not. Prediction always right-composes, X <- X Exp(Lambda dt). For a right action that places the motion at the current estimate, so the lift belongs there, which is what predictWithJacobian() uses. For a left action it places the motion at the reference instead, so the lift must be Ad_{X^-1} Lambda(xi_hat, u), equal by equivariance to Lambda(xi_ref, u_origin). Taking it at the estimate gives the wrong mean whenever the lift depends on the state. Only paths that receive the input orbit can correct this; predictWithJacobian() cannot, and says so. A measurement correction is different. InnovationLift_ is pinv(Dphi0_) with Dphi0_ taken at the reference, so Exp(dx) moves the reference by dxi and the composition has to place it there: Exp(dx) X does that for a right action, X Exp(dx) for a left one. update() used the right-action side unconditionally, which rotates the correction by Ad_{X^-1} -- invisible only when X is the identity. Both update paths now go through applyCorrection(). The left-hand side is correct for a right action and matches the dR_Xhat term in van Goor and Mahony's observer, which is derived for one. The Galilean construction in borglab#2231 uses the other side where it matters, accumulating its correction as Yhat(betahat + dbeta) ~= Yhat Exp(J dbeta). The note in doc/GalileanImuFactor.ipynb that the update composes the innovation on the left describes the generic code rather than that construction; GalileanImuFactor does not instantiate this class, so the two never met. ActionType::Left means a left group action. It is unrelated to a left-invariant error, the error form Xhat^-1 X induced by GTSAM's right retraction and used by LieGroupEKF and GalileanPreintegration, and to the left-trivialized tangent group, a group construction. Either error form pairs with either action type. The header says so, since all three appear here. No test exercised a non-zero D_lift. For the right-action fixtures that is correct rather than an oversight: their constant lift is equivariant because psi_Q(w) = Ad_{Q^-1} w carries the state dependence, so D_lift is genuinely zero. The left-action guard fixture paired a constant lift with a trivial input action, which is not equivariant, and its error is stationary only at X0 = Identity. Its input action becomes the adjoint action, which makes the lift equivariant, and the test now starts from a non-identity estimate. New tests, all with a non-identity initial group estimate: - Issue2753Counterexample: the issue's scenario in its own terms, asserting the generator -ad_u, the discrete transition Ad_Exp(-u h), the prediction Yhat Exp(u h) and the error flow Exp(-u h) E Exp(u h). - left-regular Rot3 on Rot3: A equals -ad_u, checked against the exact nonlinear error flow. - Rot3 on Unit3 (DimM = 2, DimG = 3): A compared with a numerically differentiated error generator. - mean propagation under a state-dependent lift, for predict() and predictWithTransition(). - update() moving the error by exactly the applied correction. For the left-action Galilean model of doc/GalileanImuFactor.ipynb, which pairs the direct product Gal(3) x R^6 with phi((Gamma,c),(Upsilon,beta)) = (Gamma Upsilon, beta + c), reference (I,0) and lift lambda((Upsilon,beta),u) = (q(u,beta), 0), the equivariant form Ad_Upsilon lambda gives [[-ad_q, -E],[0,0]], whose matrix exponential is [[Ad_{V^-1}, -J_R(q h) h E],[0,I]]. That is the transition GalileanPreintegration already builds by hand, as Ad_{V^-1} from increment.inverse().AdjointMap() and -J_R h E from the reordered right Jacobian. The notebook's remark that Dphi0 * D_lift omits the -ad transport is correct for the invariant lift it defines. Nothing shipped changes. Also adds predictWithTransition(), taking an already discretized Phi and Qd. transitionMatrix() discretizes by a truncated series, which converges to exp(A dt) but is only first order at the default K = 1; and some derivations are natively discrete and never define a continuous A at all, the discrete lift of Delama et al. among them. It takes the input orbit for the same reason predict() does. The lift is evaluated once per prediction: the value and the Jacobian come from the same call, and the reference-frame value is reused as the increment rather than recomputed. liftAtOrigin(), incrementIsAtOrigin(), applyCorrection() and propagate() are protected, so predictWithTransition() is the only added public API. Reported in borglab#2753.
computeErrorDynamicsMatrix() rejected ActionType::Left and directed callers to predictWithJacobian() with an explicit continuous-time A. That safeguard was added by borglab#2231 together with a guard test pinning the workaround; borglab#2753 tracks removing it. A = Dphi0 * D_lift is already correct for left actions, and no transport term has to be added by hand, provided the lift is an equivariant lift. That is the documented contract rather than a choice made here. The lift is defined by the lift condition Dphi_xi|_Id[Lambda(xi,u)] = f_u(xi); van Goor, Hamel and Mahony require in addition that Ad_X[Lambda(phi_X(xi), psi_X(u))] = Lambda(xi,u), which is what makes the error dynamics depend only on the error and the origin input, and so makes linearizing at a fixed origin valid. Both GTSAM's own EqF post and the class documentation already say the caller supplies "the input and output actions needed for equivariance". borglab#2753 observes A = 0 for a state-independent lift on the left-regular action. That lift does not satisfy either condition for the system in question, which has a body velocity: the equivariance residual is nonzero. The lift that does satisfy them is Lambda(xi,u) = Ad_xi u, and its differential at the origin is -ad_u, exactly the term the issue expects. Two independent derivations agree: LieGroupEKF::transitionMatrix() obtains the same -ad from Baker-Campbell-Hausdorff for a left-invariant error, and it is the classical left-invariant EKF result of Barrau and Bonnabel. The formula also holds when DimM != DimG, checked against SO(3) acting on S^2, transitive but not free -- a case where the alternative, an invariant lift corrected by -ad_Lambda, is not even well defined, since two lifts differing by a stabiliser element move the manifold identically but give different ad. Whether the lift carries an Ad is a frame question, not a left-or-right one. A left action's fundamental vector field is spatial and a right action's is a body velocity, so a constant lift is correct exactly when the system's velocity is stated in the matching frame. Every right-action fixture here has one because IMU inputs are body quantities and those actions are body-framed; a right action driven spatially would need Ad_{xi^-1} u just the same. The header and the new fixtures say so. What needed fixing is where each operation lands on the manifold. Two places get this wrong for a left action, and both are reachable on develop: only computeErrorDynamicsMatrix() carries the guard, while predictWithJacobian(), update() and updateWithVector() do not. Prediction always right-composes, X <- X Exp(Lambda dt). For a right action that places the motion at the current estimate, so the lift belongs there, which is what predictWithJacobian() uses. For a left action it places the motion at the reference instead, so the lift must be Ad_{X^-1} Lambda(xi_hat, u), equal by equivariance to Lambda(xi_ref, u_origin). Taking it at the estimate gives the wrong mean whenever the lift depends on the state. Only paths that receive the input orbit can correct this; predictWithJacobian() cannot, and says so. A measurement correction is different. InnovationLift_ is pinv(Dphi0_) with Dphi0_ taken at the reference, so Exp(dx) moves the reference by dxi and the composition has to place it there: Exp(dx) X does that for a right action, X Exp(dx) for a left one. update() used the right-action side unconditionally, which rotates the correction by Ad_{X^-1} -- invisible only when X is the identity. Both update paths now go through applyCorrection(). The left-hand side is correct for a right action and matches the dR_Xhat term in van Goor and Mahony's observer, which is derived for one. The Galilean construction in borglab#2231 uses the other side where it matters, accumulating its correction as Yhat(betahat + dbeta) ~= Yhat Exp(J dbeta). The note in doc/GalileanImuFactor.ipynb that the update composes the innovation on the left describes the generic code rather than that construction; GalileanImuFactor does not instantiate this class, so the two never met. ActionType::Left means a left group action. It is unrelated to a left-invariant error, the error form Xhat^-1 X induced by GTSAM's right retraction and used by LieGroupEKF and GalileanPreintegration, and to the left-trivialized tangent group, a group construction. Either error form pairs with either action type. The header says so, since all three appear here. No test exercised a non-zero D_lift. For the right-action fixtures that is correct rather than an oversight: their constant lift is equivariant because psi_Q(w) = Ad_{Q^-1} w carries the state dependence, so D_lift is genuinely zero. The left-action guard fixture paired a constant lift with a trivial input action, which is not equivariant, and its error is stationary only at X0 = Identity. Its input action becomes the adjoint action, which makes the lift equivariant, and the test now starts from a non-identity estimate. New tests, all with a non-identity initial group estimate: - Issue2753Counterexample: the issue's scenario in its own terms, asserting the generator -ad_u, the discrete transition Ad_Exp(-u h), the prediction Yhat Exp(u h) and the error flow Exp(-u h) E Exp(u h). - left-regular Rot3 on Rot3: A equals -ad_u, checked against the exact nonlinear error flow. - Rot3 on Unit3 (DimM = 2, DimG = 3): A compared with a numerically differentiated error generator. - mean propagation under a state-dependent lift, for predict() and predictWithTransition(). - update() moving the error by exactly the applied correction. For the left-action Galilean model of doc/GalileanImuFactor.ipynb, which pairs the direct product Gal(3) x R^6 with phi((Gamma,c),(Upsilon,beta)) = (Gamma Upsilon, beta + c), reference (I,0) and lift lambda((Upsilon,beta),u) = (q(u,beta), 0), the equivariant form Ad_Upsilon lambda gives [[-ad_q, -E],[0,0]], whose matrix exponential is [[Ad_{V^-1}, -J_R(q h) h E],[0,I]]. That is the transition GalileanPreintegration already builds by hand, as Ad_{V^-1} from increment.inverse().AdjointMap() and -J_R h E from the reordered right Jacobian. The notebook's remark that Dphi0 * D_lift omits the -ad transport is correct for the invariant lift it defines. Nothing shipped changes. Also adds predictWithTransition(), taking an already discretized Phi and Qd. transitionMatrix() discretizes by a truncated series, which converges to exp(A dt) but is only first order at the default K = 1; and some derivations are natively discrete and never define a continuous A at all, the discrete lift of Delama et al. among them. It takes the input orbit for the same reason predict() does. The lift is evaluated once per prediction: the value and the Jacobian come from the same call, and the reference-frame value is reused as the increment rather than recomputed. liftAtOrigin(), incrementIsAtOrigin(), applyCorrection() and propagate() are protected, so predictWithTransition() is the only added public API. Reported in borglab#2753.
computeErrorDynamicsMatrix() rejected ActionType::Left (a safeguard added in #2231) and directed callers to predictWithJacobian() with an explicit A. Remove the guard and make the left-action paths correct: - A = Dphi0 * D_lift holds for both action types provided the lift is equivariant, Lambda(phi_X(xi), psi_X(u)) = Ad_X Lambda(xi, u); D_lift then carries any transport term. The A = 0 observed in #2753 comes from a constant lift paired with a trivial input action, which is not an equivariant lift for the body-velocity system xi+ = xi Exp(u h). The equivariant lift Ad_xi u gives D_lift = -ad_u, the generator the issue expects and the same term LieGroupEKF::transitionMatrix() derives from Baker-Campbell-Hausdorff. Document the precondition and the invariant-lift alternative, for which the formula would omit -ad_Lambda. - Prediction always right-composes, X <- X Exp(Lambda dt). For a left action that places the increment at the reference state, so predict() evaluates the lift there, Lambda(xi_ref, u_origin), instead of at the estimate. The lift value and its Jacobian come from a single evaluation. - A measurement correction lives in error coordinates at the reference, so for a left action it must compose on the right, X Exp(dx). update() and updateWithVector() composed on the left unconditionally, which rotates the correction by Ad_{X^-1} whenever X is not the identity. Both now go through applyCorrection(). - Add predictWithTransition() for callers that already have a discretized Phi and Qd. Add the protected helpers liftAtOrigin(), propagate(), applyCorrection() and incrementIsAtOrigin(). Tests: the former guard fixture becomes a constant-lift fixture with the adjoint input action that makes it equivariant. New left-regular Rot3 and SO(3)-on-S^2 fixtures check A against the numerical Jacobian of the exact error flow, the mean, predictWithTransition() and update(), all with a non-identity group estimate and, for Rot3, a non-identity reference state. Update GalileanImuFactor.ipynb, which described the removed guard and the update side. Existing right-action users (ABC, EqVIO) are unaffected. Fixes #2753
5 participants
Stubs for @giuliodelama
@giuliodelama please continue to work on ths branch, but only
pushwhen you need review or want to run CI.