Template EqF on Group and Manifold #2197

jenniferoum · 2025-07-10T04:28:42Z

Updates to template equivariant filter on group and manifold

Also template on geometry structure to allow functions to be called in EquivariantFilter.h
Reorganize files:
- ABC.h contains geometry specific definitions
- EquivariantFilter.h contains fitler specific functions
- Renamed ABC_EQF_Demo to AbbcEquivariantFilterExample.cpp

dellaert

Nice progress! Some initial comments.

gtsam_unstable/geometry/ABC.h

examples/ABC.h

examples/EquivariantFilter.h

…equire for template, change EqF to take in initial state, use traits for group

gtsam/navigation/EquivariantFilter.h

dellaert · 2025-08-29T17:47:37Z

examples/EquivariantFilter.h

+  Matrix Q = Geometry::processNoise(u);  // Replaces blockDiag(...)
+
+  X_hat = traits<G>::Compose(X_hat, traits<G>::Expmap(L * dt));
+  Sigma = Phi * Sigma * Phi.transpose() + Bt * Q * Bt.transpose() * dt;


This is already done in the ManifoldEKF, specialized to groups in the LieGroupEKF, which is the main reason to derive form that class.

dellaert · 2025-08-29T17:48:44Z

examples/EquivariantFilter.h

+  Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result;
+  Matrix Dt = Geometry::outputMatrixDt(y.cal_idx, X_hat);
+
+  Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();


You'll also find this somewhere in the existing hierarchy.

AlessandroFornasier · 2025-09-12T09:47:32Z

Nice to see this one in gtsam 👍🏻

dellaert · 2025-09-12T20:52:23Z

Thans @AlessandroFornasier !We're going to substantially rewrite and base on LieGroupEKF (or derived), we'll let you know :-)

dellaert

Good meeting. Trying to give some more feedback here. The goal is whether we can get rid of Geometry altogether.

gtsam_unstable/geometry/ABC.h

gtsam/navigation/EquivariantFilter.h

dellaert · 2025-10-24T21:36:14Z

gtsam/navigation/EquivariantFilter.h

+  void update(const typename Geometry::Measurement& y);
+
+  static constexpr int DOF = gtsam::traits<G>::dimension;
+  static constexpr int n_cal = Geometry::n_cal;


Also out of place in a generic filter.

dellaert · 2025-10-24T21:41:41Z

gtsam_unstable/geometry/ABC.h

+   * @return Linearized state matrix
+   * Uses Matrix zero and Identity functions
+   */
+  static Matrix stateMatrixA(const GType& X_hat, const Input& u) {


Is this actually used in the filter?

dellaert · 2025-10-24T21:48:21Z

gtsam/navigation/EquivariantFilter.h

+  Matrix Q   = Geometry::processNoise(u);
+
+
+  this->X_ = traits<G>::Compose(this->X_, traits<G>::Expmap(L * dt));


This makes me think $\Phi = Ad_{U^{-1}}$ with $U=L dt$. If so we can then kill it from Geometry - and I bet this holds for any equivariant filter that updates the state this way. Look at the InvariantEKF.

dellaert · 2025-10-24T21:50:27Z

gtsam/navigation/EquivariantFilter.h

+  Vector Delta = InnovationLift * K * delta_vec;
+  this->X_ = traits<G>::Compose(traits<G>::Expmap(Delta), this->X_);
+  this->P_ = (Matrix::Identity(DOF, DOF) - K * Ct) * this->P_;
+}


Try to see whether you can do this update by constructing the parameters of update in the base class. It will be informative.

dellaert · 2025-10-24T21:54:11Z

gtsam_unstable/geometry/ABC.h

+
+/// Input struct for the Biased Attitude System
+
+struct Input {


I would split this file up: ABC.h is pure geometry. All the other data structures (Input, etc) should be moved to the example - there should be zero “infra” code in this geometry code.

refactored to remove dependency on Geometry some comments

…lter-refactor

…actor Equivariant Filter Optimizations (?)

- abc namespace - use gtsam::diag - format files

Refactor Geometry into state, input, and output actions

dellaert reviewed Jul 10, 2025

View reviewed changes

jenniferoum added 2 commits July 30, 2025 09:30

Template EqF on Group and Manifold

eb95739

Address EqF template comments, add compile time check for functions r…

4ee2efe

…equire for template, change EqF to take in initial state, use traits for group

jenniferoum force-pushed the feature/equivariant-filter branch from 73215df to 4ee2efe Compare July 30, 2025 17:20

Remove abc_eqf_lib from EquivariantFilter.h

7e12b62

dellaert reviewed Aug 29, 2025

View reviewed changes

jenniferoum added 5 commits September 26, 2025 09:29

Set up EqF to inherit from LieGroupEKF

11a2459

Move Eqf files and add unit test for ABC.h

742c4e5

Fix AbcEqF include paths

c7da938

Add filter test

70c723c

Fix naming conventions, add prediction test

4c1f40d

dellaert reviewed Oct 24, 2025

View reviewed changes

rohan-bansal and others added 16 commits November 7, 2025 03:09

addressed input/inputtype disparity, small-n

deb979d

Generalize phi state transition matrix, code cleanup

4e35967

constructed parameters of update in base class, updated predict step

9f9ad34

refactored to remove dependency on Geometry some comments

Merge branch 'feature/equivariant-filter' into feature/equivariant-fi…

49486be

…lter-refactor

Squashed commit

a70bb78

Merge pull request #4 from jenniferoum/feature/equivariant-filter-ref…

b9cbad6

…actor Equivariant Filter Optimizations (?)

Cleanup

0425db9

- abc namespace - use gtsam::diag - format files

InputData gone, Measurement in Example only

0d46f12

Spelling/formatting

9dbde14

Inlined definitions

a53e1b6

Fix traits issue

9ad8989

Define Group via MatrixLieGroup

1de2c70

Clean up product

9d2ba91

Added PowerLieGroup

911bdac

Add test for AdjointMap

c7cfe92

Use size_t

7b5ee6c

dellaert and others added 14 commits November 15, 2025 15:23

Made groups Testable

725312b

Define Calibrations as power group

4a2df97

Define Group as product

f8fdd70

Move everything back into Geometry

43fadb9

Dphi0_ not needed

f4c59a9

Manifold traits

0c6323c

StateAction functor

0a301e3

InputAction functor

46f7876

OutputAction functor

eac04bf

Lift functor

78fc700

Template output on OutputAction

1b3c65f

Use most basic predict

b7c97d8

update takes applied OutputAction

1d38d41

Merge pull request #5 from jenniferoum/feature/EqF

1a589b3

Refactor Geometry into state, input, and output actions

		Matrix Q = Geometry::processNoise(u);


		this->X_ = traits<G>::Compose(this->X_, traits<G>::Expmap(L * dt));


		/// Input struct for the Biased Attitude System

		struct Input {

Template EqF on Group and Manifold #2197

Are you sure you want to change the base?

Template EqF on Group and Manifold #2197

Uh oh!

Conversation

jenniferoum commented Jul 10, 2025

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

dellaert Aug 29, 2025

Choose a reason for hiding this comment

Uh oh!

dellaert Aug 29, 2025

Choose a reason for hiding this comment

Uh oh!

AlessandroFornasier commented Sep 12, 2025

Uh oh!

dellaert commented Sep 12, 2025

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

dellaert Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

dellaert Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

dellaert Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

dellaert Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

dellaert Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants