C++ example of an Equivariant filter #2103

darshan-17 · 2025-04-18T03:15:53Z

An implementation of the Equivariant Filter (EqF) for attitude estimation with both gyroscope bias and sensor extrinsic calibration, based on the paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration" by Fornasier et al.

Uses the same dataset from the paper but implemented through built-in GTSAM functions to reproduce the results.

Authored by Darshan Rajasekaran & Jennifer Oum

dellaert

OK, so, many comments :-)
One main organizational comment that I think will make the transition to a templated version easier:

Put Group, State in ABC.h, no cpp file, everything inlined. Use template size_T and std:array as I suggested earlier
In ABC_EQF.*, only put filter stuff. Can still be specific and include ABC.h, but try to extract as much ABC-specific things you can into ABC.h
Everything else (data loading is biggest part) put in Demo file.

gtsam/CMakeLists.txt

gtsam/3rdparty/metis/CMakeLists.txt

.github/workflows/build-linux.yml

examples/ABC_EQF.h

dellaert · 2025-04-19T16:54:11Z

examples/ABC_EQF.cpp

+ * @param m Number of sensors
+ * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse()
+ */
+EqF::EqF(const Matrix& Sigma, int n, int m)


To me, the argument n causes a lot of issues! In the ABC.h code I shared with you I sketched how to use a size_t template argument, ijn which case the dimensions of G and State are known at compile time. I think that's much preferred for an example=, rather than introducing variable dimenions (and hence dynamically allocated matrices) everywhere.

examples/ABC_EQF.cpp

…l directory

…n METIS CMakeLists.txt & removed variables that were not used

…nge Input class to struct, remove changes to target

…nctions to be inline with the state class

dellaert

Awesome, great progresss!

I would still simplify much more. Get rid of “create” where possible, don’t check covariance matrices, and move all things that are ABC specific (except filter) to a separate file ABC.h.

And I would template on size_t, which will simplify the code even more. Remember this is an example, so we do not need to support reading data files with varying number of calibrations.

examples/ABC_EQF.h

dellaert · 2025-04-22T11:20:15Z

examples/ABC_EQF.h

+    Matrix3 W() const {      /// Return w as a skew symmetric matrix
+        return Rot3::Hat(w);
+    }
+    static Input create(const Vector3& w, const Matrix& Sigma) { /// Initialize w and Sigma


Still seems overkill, just use brace init, don’t check covariance

examples/ABC_EQF.h

dellaert · 2025-04-22T11:21:35Z

examples/ABC_EQF.h

+    Unit3 d;        /// Known direction in global frame
+    Matrix3 Sigma;      /// Covariance matrix of the measurement
+    int cal_idx = -1;   /// Calibration index (-1 for calibrated sensor)
+    static Measurement create(const Unit3& y_vec, const Unit3& d_vec, /// Initialize measurement


Same: kill and use brace init. Covariance checking is overkill in an example (and is not very relevant when we switch to noise models)

dellaert · 2025-04-22T11:22:01Z

examples/ABC_EQF.h

+
+/// State class representing the state of the Biased Attitude System
+
+class State {


Move to ABC.h and template on size_t

examples/ABC_EQF.h

dellaert · 2025-04-22T11:23:18Z

examples/ABC_EQF.h

+ * Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
+ * Each element of the B list is associated with a calibration state
+ */
+class G {


Move to ABC.h and template on size_t

dellaert · 2025-04-22T11:24:32Z

examples/ABC_EQF.h

+class EqF {
+private:
+    int dof;                 // Degrees of freedom
+    int n_cal;               // Number of calibration states


Template EqF on size_t as well.

…ordinate to exponential, separate filter and demo specific functions, rename stateAction to operator *, fix brace initialization

dellaert · 2025-04-25T20:35:29Z

examples/ABC.h

+
+/// Check if a vector is a unit vector
+
+bool checkNorm(const Vector3& x, double tol = 1e-3);


dellaert · 2025-04-25T20:36:36Z

examples/ABC.h

+
+/// Check if vector contains NaN values
+
+bool hasNaN(const Vector3& vec);


maybe kill this too if you don;t use it

dellaert · 2025-04-25T20:37:30Z

examples/ABC.h

+
+/// Create a block diagonal matrix from two matrices
+
+Matrix blockDiag(const Matrix& A, const Matrix& B);


kill declaration

dellaert · 2025-04-25T20:37:39Z

examples/ABC.h

+ * @return A single consolidated matrix with A in the top left and B in the
+ * bottom right
+ */
+Matrix blockDiag(const Matrix& A, const Matrix& B) {


decide whether this is ABC-specific, or Eqf specific. If latter, move it.

dellaert · 2025-04-25T20:41:29Z

examples/ABC.h

+ * @param n Number of times to be repeated
+ * @return Block diag matrix with A repeated 'n' times
+ */
+Matrix repBlock(const Matrix& A, int n) {


dellaert · 2025-04-25T20:52:23Z

Next PR should have:

Abc.h
EquivariantFilter.h (does not include Abc.h or Sphere.h)
AbcEquivariantFilterExample.cpp (instead of demo)
Sphere.h
SphereEquivariantFilterExample.h (does not include Abc.h)

dellaert reviewed Apr 19, 2025

View reviewed changes

darshan-17 and others added 8 commits April 20, 2025 20:12

Implement ABC_EQF in cpp - Inputs are simulated- Use with caution

6733365

c++ implementation - use with caution

d9cd905

Adding a function to translate output data into csv and store in loca…

9aefd92

…l directory

Used GTSAM ExpmapDerivative to calculate Left Jacobian(-1)

d1673b7

Update wedge and vee to Rot3 Hat and Vee for EqF

39a7a5b

C++ implementation of the EqF to estimate bias.

cd1782e

Removed older linux dependencies, updated cmake version requirement i…

d48bf56

…n METIS CMakeLists.txt & removed variables that were not used

Address some pr comments - remove ABC_EQF.cpp, clean up comments, cha…

e5f4978

…nge Input class to struct, remove changes to target

jenniferoum force-pushed the feature/equivariant-filter branch from 32bcc3a to e5f4978 Compare April 21, 2025 03:14

darshan-17 and others added 2 commits April 20, 2025 21:41

Resolved the direction class -> Unit3, and local_coords, local_inv fu…

925e94e

…nctions to be inline with the state class

Inline measurement, G, and State functions, use brace initialization

51e20ec

dellaert reviewed Apr 22, 2025

View reviewed changes

Template on size_t, Darshan's updates to cleanup comments, default co…

17bf752

…ordinate to exponential, separate filter and demo specific functions, rename stateAction to operator *, fix brace initialization

dellaert approved these changes Apr 25, 2025

View reviewed changes

dellaert merged commit deb4045 into borglab:develop Apr 26, 2025
36 checks passed


		/// State class representing the state of the Biased Attitude System

		class State {


		/// Check if a vector is a unit vector

		bool checkNorm(const Vector3& x, double tol = 1e-3);


		/// Check if vector contains NaN values

		bool hasNaN(const Vector3& vec);


		/// Create a block diagonal matrix from two matrices

		Matrix blockDiag(const Matrix& A, const Matrix& B);

C++ example of an Equivariant filter #2103

C++ example of an Equivariant filter #2103

Uh oh!

Conversation

darshan-17 commented Apr 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

dellaert commented Apr 25, 2025

Uh oh!

Uh oh!

Uh oh!

darshan-17 commented Apr 18, 2025 •

edited

Loading